//This function is intended as a much faster alternative to A*, for use at the lowest node level
//While it improves performance, it is also much less flexable than A*. It relies heavily on the assumption that any node on a rigidbody can be reached from any other node on that same rigidbody.
//Either A* or some alternative will be required to properly handle fully dynamic environments.
public static List <NodeIndex> GetNextDestNode(Node startNode, Node endNode, List <Node> nodesToSearch, Node topLevelNode)
{
List <NodeIndex> toReturn = new List <NodeIndex>();
//if the destination is on the current rigidbody simply return the destination index
if (endNode.index.GetParentIndex() == startNode.index.GetParentIndex())
{
toReturn.Add(endNode.index);
return(toReturn);
}
Node localDest = null;
Node foreignDest = null;
foreach (Node n in nodesToSearch)
{
if (localDest == null || (n.position - startNode.position).Length() < (localDest.position - startNode.position).Length())
{
foreach (Connection c in n.connections)
{
if (c.destination.EqualAtDepth(endNode.index, endNode.index.GetMaxDepth()))
{
localDest = n;
foreignDest = topLevelNode.GetNode(c.destination);
}
}
}
}
if (startNode.index != localDest.index)
{
toReturn.Add(localDest.index);
}
toReturn.Add(foreignDest.index);
return(toReturn);
}
public static List <NodeIndex> AstarPathfind(Node startNode, Node endNode, List <Node> nodesToSearch)
{
//ASSUMPTIONS:
// Start Node is on the same layer as the set of nodes to search
// End Node is on the same layer as the start Node, or one level above!
//reset the path of all nodes
foreach (Node n in nodesToSearch)
{
n.ResetPath();
}
WorldGraph fullAIGraph = WorldGraph.GetWorldGraph();
//create the open and closed lists
List <NodeIndex> openList = new List <NodeIndex>();
List <NodeIndex> closedList = new List <NodeIndex>();
//find the current layer being searched through
int nodeListDepth = startNode.index.GetMaxDepth();
bool endNodeOnSameLayer = nodeListDepth == endNode.index.GetMaxDepth();
//currentNode is the node being examined
NodeIndex currentNode;
NodeIndex nextNode;
openList.Add(startNode.index);
while (openList.Count() > 0)
{
//Get the cheapest node in the open list
currentNode = GetCheapestNode(openList, nodesToSearch, endNode.internalNodes);
//if the goal has been found then retrace the path and return it
if (currentNode.EqualAtDepth(endNode.index, endNode.index.GetMaxDepth()))
{
if (endNodeOnSameLayer)
{
//retrace the path from the end node
return(RetracePath(endNode, nodesToSearch));
}
else
{
return(RetracePath(endNode.GetNode(currentNode), nodesToSearch));
}
}
//if the goal has not been found then add the nodes at the end of all connections to the open list, and close the current node
foreach (Connection edge in nodesToSearch[currentNode.GetLowestIndex()].connections)
{
nextNode = edge.destination;
//ignore if the node has already been closed
if (!closedList.Contains(nextNode))
{
//calculate the cost by looking at the accumulated cost, the connection cost, and the heuristic
float totalCost = Math.Abs(nodesToSearch[currentNode.GetLowestIndex()].accumulatedCost + edge.traversalCost + fullAIGraph.topLevelNode.GetNode(nextNode).GetHeuristic(fullAIGraph.goalCoords));
Node tempNode = GetNodeFromLists(nextNode, nodesToSearch, endNode.internalNodes);
if (tempNode != null)
{
//if the node is not currently in the open list then add it to the open list
if (!openList.Contains(nextNode))
{
openList.Add(nextNode);
//set the parent Id and the current cost
tempNode.previousNode = currentNode;
tempNode.accumulatedCost = totalCost;
}
//update the node in the list if the new path is faster
else if (tempNode.accumulatedCost > totalCost)
{
//set the parent Id and the current cost
tempNode.previousNode = currentNode;
tempNode.accumulatedCost = totalCost;
}
}
}
}
closedList.Add(currentNode);
openList.Remove(currentNode);
}
return(null); // If the goal is not found, return null
}