Started drafting path smoothing algorithm

WorldChunkRefactoring
Martin Felis 2023-07-01 09:50:41 +02:00
parent 0db9a54c52
commit 56ae5869e8
1 changed files with 42 additions and 0 deletions

View File

@ -177,6 +177,48 @@ public class NavigationComponent : Node
} }
bool SweptSphereHasCollision(Vector3 fromPosition, Vector3 toPosition, float radius)
{
if ((fromPosition - toPosition).LengthSquared() < 0.001)
{
return false;
}
Vector3 direction = (toPosition - fromPosition).Normalized();
// TODO: Complete Implementation
Debug.Assert(false);
return true;
}
public List<NavigationPoint> SmoothPath(List<NavigationPoint> navigationPoints)
{
List<NavigationPoint> smoothedPath = new List<NavigationPoint>();
int startIndex = 0;
smoothedPath.Add(navigationPoints[startIndex]);
int endIndex = navigationPoints.Count > 1 ? 1 : 0;
while (endIndex != navigationPoints.Count - 1)
{
Vector3 startPoint = navigationPoints[startIndex].WorldPosition;
Vector3 endPoint = navigationPoints[endIndex].WorldPosition;
if (SweptSphereHasCollision(startPoint, endPoint, 0.25f))
{
smoothedPath.Add(navigationPoints[endIndex-1]);
smoothedPath.Add(navigationPoints[endIndex]);
startIndex = endIndex;
endIndex += 1;
}
}
smoothedPath.Add(navigationPoints[endIndex]);
return smoothedPath;
}
public void PlanDirectPath(Transform fromTransformWorld, NavigationPoint navigationPoint) public void PlanDirectPath(Transform fromTransformWorld, NavigationPoint navigationPoint)
{ {
if (navigationPoint.Flags.HasFlag(NavigationPoint.NavigationFlags.Position) if (navigationPoint.Flags.HasFlag(NavigationPoint.NavigationFlags.Position)