private static Vector2[] Search(Queue <SearchNode> nodeQueue, GridPosition goal, SearchMap searchMap)
{
if (nodeQueue.Count == 0)
{
return(null);
}
SearchNode node = nodeQueue.Dequeue();
if (node.position.x == goal.x && node.position.y == goal.y)
{
Stack <SearchNode> nodeStack = new Stack <SearchNode> ();
nodeStack.Push(node);
while (nodeStack.Peek().previousNode != null)
{
nodeStack.Push(nodeStack.Peek().previousNode);
}
Vector2[] path = new Vector2[nodeStack.Count];
for (int i = 0; i < path.Length; i++)
{
path [i] = nodeStack.Pop().position.ToVector2();
}
return(path);
}
GridPosition top = new GridPosition(node.position.x, node.position.y + 1);
GridPosition right = new GridPosition(node.position.x + 1, node.position.y);
GridPosition bottom = new GridPosition(node.position.x, node.position.y - 1);
GridPosition left = new GridPosition(node.position.x - 1, node.position.y);
if (!searchMap.CheckSearched(top))
{
searchMap.SetSearched(top);
nodeQueue.Enqueue(new SearchNode(top, node));
}
if (!searchMap.CheckSearched(right))
{
searchMap.SetSearched(right);
nodeQueue.Enqueue(new SearchNode(right, node));
}
if (!searchMap.CheckSearched(bottom))
{
searchMap.SetSearched(bottom);
nodeQueue.Enqueue(new SearchNode(bottom, node));
}
if (!searchMap.CheckSearched(left))
{
searchMap.SetSearched(left);
nodeQueue.Enqueue(new SearchNode(left, node));
}
return(Search(nodeQueue, goal, searchMap));
}
public static Vector2[] GetPath(Vector2 bottomLeft, Vector2 topRight, Vector2 start, Vector2 goal)
{
GridPosition startGrid = new GridPosition(start);
GridPosition goalGrid = new GridPosition(goal);
Queue <SearchNode> nodeQueue = new Queue <SearchNode> ();
nodeQueue.Enqueue(new SearchNode(startGrid, null));
SearchMap map = new SearchMap(new GridPosition(bottomLeft), new GridPosition(topRight));
map.SetSearched(startGrid);
Vector2[] gridPath = Search(nodeQueue, goalGrid, map);
/*
* Vector2[] fullPath = new Vector2[gridPath.Length + 2];
* fullPath [0] = start;
* fullPath [fullPath.Length - 1] = goal;
* for (int i = 0; i < gridPath.Length; i++)
* {
* fullPath [i + 1] = gridPath [i];
* }
*/
gridPath [0] = start;
gridPath [gridPath.Length - 1] = goal;
return(gridPath);
}