private void SetupRawPath()
{
m_nextPathNodeID = 0;
m_navCellToNodeMap = new Dictionary<uint, PathNode>();
m_nodeHeap = new PathNodeHeap();
PathNode pathNode =
new PathNode(
(int)m_nextPathNodeID,
null,
(uint)m_startNavRef.NavCellIndex,
0,
ComputeHeuristicCostEstimate(m_startPosition, m_endPosition));
m_nextPathNodeID++;
m_totalNodesSearched = 0;
m_nodeHeap.Push(pathNode);
m_navCellToNodeMap[pathNode.NavCellIndex] = pathNode;
m_state = eState.compute_raw_path;
}
public void Push(PathNode node)
{
m_heap.Add(node);
BubbleUp(m_heap.Count - 1);
}
private void ComputeRawPath()
{
int nodesSearchedThisUpdate = 0;
// Keep searching until we either find the end
// or we're not allowed to search anymore
while (!m_nodeHeap.Empty() &&
m_totalNodesSearched < MAX_TOTAL_NODES_SEARCHED_ALLOWED &&
(m_maxNodesSearchedPerUpdate == 0 || (m_maxNodesSearchedPerUpdate > 0 && nodesSearchedThisUpdate < m_maxNodesSearchedPerUpdate)))
{
PathNode currentNode = m_nodeHeap.Top();
// Keep track of how many nodes total we have visited so far
m_totalNodesSearched++;
// Keep track of how many nodes were searched this update
nodesSearchedThisUpdate++;
// Make sure we haven't hit the destination
if (currentNode.NavCellIndex != m_endNavRef.NavCellIndex)
{
// Remove the current node from the heap now that we're investigating it
m_nodeHeap.Pop();
// Add the current nav-cell to the closed set
// (since we just visited it and don't want to visit it again)
currentNode.MarkAsInClosedSet();
// See which neighbor, if any, looks best at this moment to search from
for (NavCellNeighborIterator iterator = new NavCellNeighborIterator(m_navMesh, currentNode.NavCellIndex);
iterator.Valid(); iterator.Next())
{
// See if we already have a search node for the neighbor nav-cell
uint neighborNavCellIndex = iterator.NeighborNavCellIndex;
PathNode neighborNode = null;
bool hasNeighbor = m_navCellToNodeMap.TryGetValue(neighborNavCellIndex, out neighborNode);
// Skip this neighbor if we already put it in the closed set
if (!hasNeighbor || !neighborNode.InClosedSet)
{
// Compute G(x): The path cost from the start to this neighbor
float netTraversalCostToNeighbor = currentNode.Cost +
ComputeTraversalCost(currentNode.NavCellIndex, neighborNavCellIndex);
// Add this neighbor into consideration if:
// A) We haven't seen it before (i.e. not in the open set)
// B) We have seen it before, but the net traversal cost to the neighbor
// from the start through the current node is cheaper than some previous
// traversal to the neighbor.
if (neighborNode == null || netTraversalCostToNeighbor < neighborNode.Cost)
{
// Compute F(x)= G(x) + H(x): The past cost to the neighbor plus the estimated distance to the end
Point3d neighborCenter = m_navMesh.ComputeNavCellCenter(neighborNavCellIndex);
float netTraversalPlusHeuristicCost =
netTraversalCostToNeighbor +
ComputeHeuristicCostEstimate(neighborCenter, m_endPosition);
// Make sure to add the neighbor to the open set if not already added
if (neighborNode == null)
{
neighborNode =
new PathNode(
(int)m_nextPathNodeID,
currentNode,
neighborNavCellIndex,
netTraversalCostToNeighbor,
netTraversalPlusHeuristicCost);
m_nextPathNodeID++;
m_navCellToNodeMap[neighborNavCellIndex] = neighborNode;
}
else
{
neighborNode.ParentNode = currentNode;
neighborNode.Cost = netTraversalCostToNeighbor;
neighborNode.Total = netTraversalPlusHeuristicCost;
}
// Add the neighbor into the heap for future consideration
m_nodeHeap.Push(neighborNode);
}
}
}
}
else
{
// Stop searching and compute the raw path from the search nodes
m_state = eState.finalize_raw_path;
break;
}
}
// Sanity check to make sure we never get in an infinite loop
if (m_nodeHeap.Empty())
{
m_resultCode = eResult.failed_raw_path_search;
m_state = eState.complete;
}
else if (m_totalNodesSearched >= MAX_TOTAL_NODES_SEARCHED_ALLOWED)
{
//Debug.log("PathComputer: Ran out of search node!", Debug.RED);
m_resultCode = eResult.failed_raw_path_search;
m_state = eState.complete;
}
}