/// <summary>
/// Finds the shortest path from the start node to the goal node
/// </summary>
/// <param name="startPosition">Start position</param>
/// <param name="goalPosition">Goal position</param>
/// <param name="startMeshNode">Start mesh node</param>
/// <param name="goalMeshNode">Goal mesh node</param>
public List<NavmeshPolygon> FindPath(Vector3 startPosition, Vector3 goalPosition, NavmeshPolygon startPolygon, NavmeshPolygon goalPolygon)
{
//Debug.Log("AStar.FindPath");
//Debug.Log("startPosition == " + startPosition);
//Debug.Log("goalPosition == " + goalPosition);
StartPosition = startPosition;
GoalPosition = goalPosition;
GoalNode = new AStarNode(this, null, goalPolygon, 0);
StartNode = new AStarNode(this, null, startPolygon, 0);
Heap openList = new Heap();
Heap closedList = new Heap();
List<AStarNode> solution = new List<AStarNode>();
List<AStarNode> successors = new List<AStarNode>();
int printed = 0;
openList.Add(StartNode);
while (openList.Count > 0) {
//Debug.Log("AStar:main loop");
// Get the node with the lowest TotalSum
AStarNode nodeCurrent = (AStarNode)openList.Pop();
if (printed < 1000){
//Debug.Log("Current polygon: " + nodeCurrent.NavmeshPolygon.name + " - " + nodeCurrent.TotalCost);
printed++;
}
// If the node is the goal copy the path to the solution array
if (GoalNode.Equals(nodeCurrent)) {
//Debug.Log("AStar:finish loop");
while (nodeCurrent != null) {
solution.Insert(0, nodeCurrent);
nodeCurrent = nodeCurrent.Parent;
}
//convert solution
//Debug.Log("Path found");
return solution.ConvertAll(an => an.NavmeshPolygon);
}
// Get successors to the current node
successors.Clear();
nodeCurrent.GetSuccessors(successors);
foreach (AStarNode nodeSuccessor in successors) {
//Debug.Log("AStar:successor loop");
// Test if the currect successor node is on the open list, if it is and
// the TotalSum is higher, we will throw away the current successor.
//Debug.Log("AStarNode nodeOpen");
AStarNode nodeOpen;
if (openList.Contains(nodeSuccessor)) {
//Debug.Log("openList check: nodeSuccessor (" + nodeSuccessor.NavmeshPolygon.name + ") - " + nodeSuccessor.TotalCost);
//Debug.Log("openList contains nodeSuccessor");
//Debug.Log("nodeOpen = (AStarNode)openList[openList.IndexOf(nodeSuccessor)];");
nodeOpen = (AStarNode)openList.GetEqual(nodeSuccessor);
//Debug.Log("openList check: nodeOpen (" + nodeOpen.NavmeshPolygon.name + ") - " + nodeOpen.TotalCost);
//Debug.Log("if ((nodeOpen != null) && (nodeSuccessor.TotalCost > nodeOpen.TotalCost))");
if ((nodeOpen != null) && (nodeSuccessor.TotalCost > nodeOpen.TotalCost)){
//Debug.Log("continue;");
//Debug.Log("openList check: continued");
continue;
} else {
//Debug.Log("openList check: not continued");
}
}
// Test if the currect successor node is on the closed list, if it is and
// the TotalSum is higher, we will throw away the current successor.
//Debug.Log("AStarNode nodeClosed;");
AStarNode nodeClosed;
//Debug.Log("if (closedList.Contains(nodeSuccessor)) {");
if (closedList.Contains(nodeSuccessor)) {
//Debug.Log("closedList check: nodeSuccessor (" + nodeSuccessor.NavmeshPolygon.name + ") - " + nodeSuccessor.TotalCost);
//Debug.Log("closedList contains nodeSuccessor");
//Debug.Log("nodeClosed = (AStarNode)closedList[closedList.IndexOf(nodeSuccessor)];");
nodeClosed = (AStarNode)closedList.GetEqual(nodeSuccessor);
//Debug.Log("closedList check: nodeClosed (" + nodeClosed.NavmeshPolygon.name + ") - " + nodeClosed.TotalCost);
//Debug.Log("if ((nodeClosed != null) && (nodeSuccessor.TotalCost > nodeClosed.TotalCost))");
if ((nodeClosed != null) && (nodeSuccessor.TotalCost > nodeClosed.TotalCost)){
//Debug.Log("continue;");
continue;
}
}
// Remove the old successor from the open list
//Debug.Log("openList.Remove(nodeSuccessor);");
openList.Remove(nodeSuccessor);
// Remove the old successor from the closed list
//Debug.Log("closedList.Remove(nodeSuccessor);");
closedList.Remove(nodeSuccessor);
// Add the current successor to the open list
//Debug.Log("penList.Push(nodeSuccessor);");
if (printed < 1000){
//Debug.Log("Adding to openList: " + nodeSuccessor.NavmeshPolygon.name + " - " + nodeSuccessor.TotalCost);
}
openList.Push(nodeSuccessor);
//Debug.Log("AStar:successor loop finished");
}
// Add the current node to the closed list
//Debug.Log("closedList.Add(nodeCurrent);");
closedList.Add(nodeCurrent);
//Debug.Log("AStar:main loop finished");
}
Debug.Log("AStart.FindPath() Path not found.");
return null;
}
