int MicroPather::Solve( void* startNode, void* endNode, vector< void* >* path, float* cost )
{
#ifdef DEBUG_PATH
printf( "Path: " );
graph->PrintStateInfo( startNode );
printf( " --> " );
graph->PrintStateInfo( endNode );
printf( " min cost=%f\n", graph->LeastCostEstimate( startNode, endNode ) );
#endif
*cost = 0.0f;
if ( startNode == endNode )
return START_END_SAME;
++frame;
OpenQueue open( graph );
ClosedSet closed( graph );
PathNode* newPathNode = pathNodePool.GetPathNode( frame,
startNode,
0,
graph->LeastCostEstimate( startNode, endNode ),
0 );
open.Push( newPathNode );
stateCostVec.resize(0);
nodeCostVec.resize(0);
while ( !open.Empty() )
{
PathNode* node = open.Pop();
if ( node->state == endNode )
{
GoalReached( node, startNode, endNode, path );
*cost = node->costFromStart;
#ifdef DEBUG_PATH
DumpStats();
#endif
return SOLVED;
}
else
{
closed.Add( node );
// We have not reached the goal - add the neighbors.
GetNodeNeighbors( node, &nodeCostVec );
for( int i=0; i<node->numAdjacent; ++i )
{
// Not actually a neighbor, but useful. Filter out infinite cost.
if ( nodeCostVec[i].cost == FLT_MAX ) {
continue;
}
PathNode* child = nodeCostVec[i].node;
float newCost = node->costFromStart + nodeCostVec[i].cost;
PathNode* inOpen = child->inOpen ? child : 0;
PathNode* inClosed = child->inClosed ? child : 0;
PathNode* inEither = (PathNode*)( ((MP_UPTR)inOpen) | ((MP_UPTR)inClosed) );
MPASSERT( inEither != node );
MPASSERT( !( inOpen && inClosed ) );
if ( inEither ) {
if ( newCost < child->costFromStart ) {
child->parent = node;
child->costFromStart = newCost;
child->estToGoal = graph->LeastCostEstimate( child->state, endNode );
child->CalcTotalCost();
if ( inOpen ) {
open.Update( child );
}
}
}
else {
child->parent = node;
child->costFromStart = newCost;
child->estToGoal = graph->LeastCostEstimate( child->state, endNode ),
child->CalcTotalCost();
MPASSERT( !child->inOpen && !child->inClosed );
open.Push( child );
}
}
}
}
#ifdef DEBUG_PATH
DumpStats();
#endif
return NO_SOLUTION;
}
int MicroPather::Solve( void* startNode, void* endNode, MP_VECTOR< void* >* path, float* cost )
{
// Important to clear() in case the caller doesn't check the return code. There
// can easily be a left over path from a previous call.
path->clear();
#ifdef DEBUG_PATH
printf( "Path: " );
graph->PrintStateInfo( startNode );
printf( " --> " );
graph->PrintStateInfo( endNode );
printf( " min cost=%f\n", graph->LeastCostEstimate( startNode, endNode ) );
#endif
*cost = 0.0f;
if ( startNode == endNode )
return START_END_SAME;
if ( pathCache ) {
int cacheResult = pathCache->Solve( startNode, endNode, path, cost );
if ( cacheResult == SOLVED || cacheResult == NO_SOLUTION ) {
#ifdef DEBUG_CACHING
GLOUTPUT(( "PathCache hit. result=%s\n", cacheResult == SOLVED ? "solved" : "no_solution" ));
#endif
return cacheResult;
}
#ifdef DEBUG_CACHING
GLOUTPUT(( "PathCache miss\n" ));
#endif
}
++frame;
OpenQueue open( graph );
ClosedSet closed( graph );
PathNode* newPathNode = pathNodePool.GetPathNode( frame,
startNode,
0,
graph->LeastCostEstimate( startNode, endNode ),
0 );
open.Push( newPathNode );
stateCostVec.resize(0);
nodeCostVec.resize(0);
while ( !open.Empty() )
{
PathNode* node = open.Pop();
if ( node->state == endNode )
{
GoalReached( node, startNode, endNode, path );
*cost = node->costFromStart;
#ifdef DEBUG_PATH
DumpStats();
#endif
return SOLVED;
}
else
{
closed.Add( node );
// We have not reached the goal - add the neighbors.
GetNodeNeighbors( node, &nodeCostVec );
for( int i=0; i<node->numAdjacent; ++i )
{
// Not actually a neighbor, but useful. Filter out infinite cost.
if ( nodeCostVec[i].cost == FLT_MAX ) {
continue;
}
PathNode* child = nodeCostVec[i].node;
float newCost = node->costFromStart + nodeCostVec[i].cost;
PathNode* inOpen = child->inOpen ? child : 0;
PathNode* inClosed = child->inClosed ? child : 0;
PathNode* inEither = (PathNode*)( ((MP_UPTR)inOpen) | ((MP_UPTR)inClosed) );
MPASSERT( inEither != node );
MPASSERT( !( inOpen && inClosed ) );
if ( inEither ) {
if ( newCost < child->costFromStart ) {
child->parent = node;
child->costFromStart = newCost;
child->estToGoal = graph->LeastCostEstimate( child->state, endNode );
child->CalcTotalCost();
if ( inOpen ) {
open.Update( child );
}
}
}
else {
child->parent = node;
child->costFromStart = newCost;
child->estToGoal = graph->LeastCostEstimate( child->state, endNode ),
child->CalcTotalCost();
MPASSERT( !child->inOpen && !child->inClosed );
open.Push( child );
}
}
}
}
#ifdef DEBUG_PATH
DumpStats();
#endif
if ( pathCache ) {
// Could add a bunch more with a little tracking.
pathCache->AddNoSolution( endNode, &startNode, 1 );
}
return NO_SOLUTION;
}