/// <summary>
/// A* based pathing algorithim for finding the shortest path through a linked set of PathNodes.
/// There does not need to be a path between the two PathNodes.
/// </summary>
/// <param name="start">PathNode to start at.</param>
/// <param name="goal">PathNode that is the goal.</param>
/// <returns>True if a path was found and the PathNodes are set with the correct NextNode values, false otherwise.</returns>
private static bool AStarSearch(PathNode start, PathNode goal, PathType path, bool updateWorldPath)
{
//TODO: This algorithim is wrong. It doesnt update a node if it finds a better way to get there.
PathNode.resetPath.Invoke(); // mark all path nodes as unexplored
List <PathNodeStruct> unexplored = new List <PathNodeStruct>();
start.Visit();
unexplored.Add(new PathNodeStruct(start, 0, null));
int pathLength = 0;
while (unexplored.Count > 0) // while there are unexplored nodes
{
/*
* Each cycle should check the node with the best F value and explore from it.
* this does ignore the possibility that there is a better way to get to an already-explored node
* but I dont think that should come up in a square grid.
*/
PathNodeStruct cur = unexplored[0];
unexplored.RemoveAt(0);
if (cur.node.Equals(goal))
{
if (!updateWorldPath)
{
return(true); //If we just want to know if there is a path we are done now.
}
//backtrack up the nodes and set their pointers for nextNode
PathNodeStruct node = cur;
while (node.parent != null) //the start node has a null parent
{
node.parent.node.SetNextNode(node.node, path);
node = node.parent;
}
PathChanged.Invoke();
return(true); //found a path
}
foreach (PathNode p in cur.node.GetNeighbors())
{
if (p.IsVisited())
{
continue;
}
//add all unexplored neighbors as path nodes wth the curent distance as the g value.
unexplored.Add(new PathNodeStruct(p, pathLength, cur));
p.Visit();
}
unexplored.Sort(delegate(PathNodeStruct x, PathNodeStruct y)
{
return((x.node.Heuristic(goal) + x.gValue).CompareTo(y.node.Heuristic(goal) + y.gValue));
});
++pathLength;
}
Debug.Log("No path found from " + start + " to " + goal);
PathNotFound.Invoke();
return(false); // no path was found.
}
public PathNodeStruct(PathNode n, int g, PathNodeStruct p)
{
node = n;
gValue = g;
parent = p;
}