public Vector2f PopLastEdge()
{
Vector2f ret = PathEdges[PathEdges.Count - 1];
PathEdges.RemoveAt(PathEdges.Count - 1);
return(ret);
}
public List <Vector2f> FindPath()
{
PathEdges.Clear();
List <WayPoint> wayPoints = new List <WayPoint>();
wayPoints.Add(new WayPoint(StartPoint));
WayPoint current = wayPoints[0];
WayPoint previous = null;
while (wayPoints.Count != 0)
{
previous = current;
current = FindBestWaypoint(wayPoints, current.Location, EndingPoint);
if (current == null)
{
Vector2f collision = FindCollision(previous.Location, EndingPoint);
int x = 0;
int y = 0;
if (EndingPoint.X < collision.X)
{
x++;
}
else if (EndingPoint.X > collision.X)
{
x--;
}
if (EndingPoint.Y < collision.Y)
{
y++;
}
else if (EndingPoint.Y > collision.Y)
{
y--;
}
EndingPoint = new Vector2f(collision.X + x, collision.Y + y);
WayPoint wayPoint = new WayPoint(FindCollision(previous.Location, EndingPoint));
wayPoint.Cost = previous.Cost + wayPoint.DistanceToTarget(EndingPoint);
wayPoint.PreviousWayPoint = previous;
wayPoints.Add(wayPoint);
current = FindBestWaypoint(wayPoints, previous.Location, EndingPoint);
}
else if (current.Location == EndingPoint)
{
foreach (var wayPoint in wayPoints)
{
wayPoint.Closed = false;
}
return(ReconstructPath(current));
}
current.Closed = true;
AddWayPoints(ref wayPoints, current, EndingPoint);
}
return(null);
}
public List <Vector2f> ReconstructPath(WayPoint current)
{
while (current.PreviousWayPoint != null)
{
Vector2f edge = new Vector2f(current.Location.X - current.PreviousWayPoint.Location.X,
current.Location.Y - current.PreviousWayPoint.Location.Y);
PathEdges.Add(edge);
current = current.PreviousWayPoint;
}
return(PathEdges);
}
