Started drafting path smoothing algorithm
parent
0db9a54c52
commit
56ae5869e8
|
@ -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)
|
||||
{
|
||||
if (navigationPoint.Flags.HasFlag(NavigationPoint.NavigationFlags.Position)
|
||||
|
|
Loading…
Reference in New Issue