public override double CalculateHeuristic(PathfinderNode end)
{
PathfinderTile other = (PathfinderTile)end;
int ox = other.X,
oy = other.Y;
// Check if it's faster to take a shortcut over the map's edge.
if (Map.WrapsX)
{
int dx = ox - X;
if (dx > Map.SizeX / 2)
{
ox -= Map.SizeX;
}
else if (dx < -Map.SizeX / 2)
{
ox += Map.SizeX;
}
}
if (Map.WrapsY)
{
int dy = oy - Y;
if (dy > Map.SizeY / 2)
{
oy -= Map.SizeY;
}
else if (dy < -Map.SizeY / 2)
{
dy += Map.SizeY;
}
}
return(1.2 * Vector2.Distance(VectorPosition, new Vector2(ox, oy)));
//return 1.2 * Vector2.DistanceSquared( VectorPosition, new Vector2(ox, oy) );
}
/// <summary>
/// Uses Dijkstra's algorithm to find and return all nodes whose total cost is below the specified number.
/// </summary>
/// <param name="start">From where the pathfinding should start.</param>
/// <param name="range">How many PathfinderLink.Cost-units away the tiles can be.</param>
/// <returns></returns>
public static List <PathfinderNode> FindNodesWithinRange(PathfinderNode start, int range, IPathfinderAgent agent = null)
{
CurrentRun++;
var closed = new List <PathfinderNode>();
var open = new BinaryHeap <PathfinderNode>();
start.LastVisit = CurrentRun;
start.PathCost = 0;
open.Add(start);
while (open.Count > 0)
{
PathfinderNode active = open.Remove();
closed.Add(active);
active.Status = PathfinderNodeStatus.Closed;
foreach (PathfinderLink link in active.GetNeighbors())
{
PathfinderNode neighbor = link.Target;
if (neighbor.LastVisit != CurrentRun) // Reset nodes that haven't been visited yet this run.
{
neighbor.Status = PathfinderNodeStatus.Unvisited;
neighbor.LastVisit = CurrentRun;
}
if (!neighbor.IsClosed)
{
double cost = active.PathCost + link.GetCost(agent);
if (cost <= range)
{
if (!neighbor.IsOpen)
{
neighbor.PathCost = cost;
neighbor.Status = PathfinderNodeStatus.Open;
open.Add(neighbor);
}
else if (cost < neighbor.PathCost)
{
neighbor.PathCost = cost;
}
}
}
}
}
return(closed);
}
/// <summary>
/// Calculates the lowest cost path with A* algorithm and returns the path. If no path is found, an empty list is returned.
/// </summary>
/// <param name="start">The node from which the path starts.</param>
/// <param name="end">The destination of the path.</param>
/// <param name="agent">The agent trying to find the path. Can be used to modify the cost of links.</param>
/// <param name="maximumSearchDepth">How many loops the pathfinder will go through before terminating.</param>
/// <returns></returns>
public static IEnumerable <PathfinderNode> FindPath(PathfinderNode start, PathfinderNode end, int maximumSearchDepth = int.MaxValue, IPathfinderAgent agent = null)
{
if (start == end)
{
return(new List <PathfinderNode>());
}
CurrentRun++;
var open = new BinaryHeap <PathfinderNode>();
start.LastVisit = CurrentRun;
start.PathCost = 0;
start.Previous = null;
open.Add(start);
for (int i = 0; open.Count > 0 && i < maximumSearchDepth; i++)
{
PathfinderNode active = open.Remove();
active.Status = PathfinderNodeStatus.Closed;
foreach (PathfinderLink link in active.GetNeighbors())
{
PathfinderNode neighbor = link.Target;
if (neighbor == end)
{
var path = new LinkedList <PathfinderNode>();
end.Previous = active;
active = end;
while (active != null)
{
path.AddFirst(active);
active = active.Previous;
}
return(path);
}
if (neighbor.LastVisit != CurrentRun) // Reset nodes that haven't been visited yet this run.
{
neighbor.Status = PathfinderNodeStatus.Unvisited;
neighbor.LastVisit = CurrentRun;
}
if (!neighbor.IsClosed)
{
double cost = active.PathCost + link.GetCost(agent);
if (!neighbor.IsOpen)
{
neighbor.Previous = active;
neighbor.PathCost = cost;
neighbor.Heuristic = neighbor.CalculateHeuristic(end);
neighbor.Status = PathfinderNodeStatus.Open;
open.Add(neighbor);
}
else if (cost < neighbor.PathCost)
{
neighbor.Previous = active;
neighbor.PathCost = cost;
}
}
}
}
return(new List <PathfinderNode>());
}