public Vector2[] BuildPath(Vector2 endPoint, PathfindingSearchNode node)
{
int length = 1;
PathfindingSearchNode current = node;
while (current != null)
{
++length;
current = current.backNode;
}
Vector2[] result = new Vector2[length];
int index = length - 1;
result[index] = endPoint;
current = node;
while (current != null)
{
--index;
result[index] = current.startPoint;
current = current.backNode;
}
return(result);
}
public Vector2[] PathTo(Vector2 end)
{
PathfindingNode endNode = grid.CellAt(end);
while (nextNodes.Count > 0)
{
PathfindingSearchNode next = ExpandNext();
if (next != null && next.node == endNode)
{
return(BuildPath(end, next));
}
}
return(null);
}
public Vector2[] PathToNearest(IEnumerable <Vector2> ends)
{
Dictionary <PathfindingNode, List <Vector2> > endNodeMapping = new Dictionary <PathfindingNode, List <Vector2> >();
foreach (Vector2 end in ends)
{
PathfindingNode endNode = grid.CellAt(end);
if (!endNodeMapping.ContainsKey(endNode))
{
endNodeMapping[endNode] = new List <Vector2>();
}
endNodeMapping[endNode].Add(end);
}
while (nextNodes.Count > 0)
{
PathfindingSearchNode next = ExpandNext();
if (next != null && endNodeMapping.ContainsKey(next.node))
{
Vector2 end = endNodeMapping[next.node].Aggregate((shortest, nextTest) =>
{
if (Distance(nextTest, next.startPoint) < Distance(shortest, next.startPoint))
{
return(nextTest);
}
else
{
return(shortest);
}
});
return(BuildPath(end, next));
}
}
return(null);
}
public PathfindingSearchNode ExpandNext()
{
PathfindingSearchNode next = nextNodes.Dequeue();
if (expandedNodes.Contains(next.node))
{
return(null);
}
expandedNodes.Add(next.node);
foreach (PathfindingEdge edge in next.node.GetAdjacentNodes())
{
if (!expandedNodes.Contains(edge.to))
{
Vector2 nextPoint = NearestPoint(edge.edge, next.startPoint);
float distance = Distance(nextPoint, next.startPoint);
nextNodes.Enqueue(new PathfindingSearchNode(edge.to, nextPoint, next, next.currentDistance + distance));
}
}
return(next);
}
