void OpenQueue::Update( PathNode* pNode )
{
#ifdef DEBUG_PATH_DEEP
printf( "Open Update: " );
graph->PrintStateInfo( pNode->state );
printf( " total=%.1f\n", pNode->totalCost );
#endif
MPASSERT( pNode->inOpen );
// If the node now cost less than the one before it,
// move it to the front of the list.
if ( pNode->prev != sentinel && pNode->totalCost < pNode->prev->totalCost ) {
pNode->Unlink();
sentinel->next->AddBefore( pNode );
}
// If the node is too high, move to the right.
if ( pNode->totalCost > pNode->next->totalCost ) {
PathNode* it = pNode->next;
pNode->Unlink();
while ( pNode->totalCost > it->totalCost )
it = it->next;
it->AddBefore( pNode );
#ifdef DEBUG
sentinel->CheckList();
#endif
}
}
void OpenQueue::Push( PathNode* pNode )
{
MPASSERT( pNode->inOpen == 0 );
MPASSERT( pNode->inClosed == 0 );
#ifdef DEBUG_PATH_DEEP
printf( "Open Push: " );
graph->PrintStateInfo( pNode->state );
printf( " total=%.1f\n", pNode->totalCost );
#endif
// Add sorted. Lowest to highest cost path. Note that the sentinel has
// a value of FLT_MAX, so it should always be sorted in.
MPASSERT( pNode->totalCost < FLT_MAX );
PathNode* iter = sentinel->next;
while ( true )
{
if ( pNode->totalCost < iter->totalCost ) {
iter->AddBefore( pNode );
pNode->inOpen = 1;
break;
}
iter = iter->next;
}
MPASSERT( pNode->inOpen ); // make sure this was actually added.
#ifdef DEBUG
sentinel->CheckList();
#endif
}
PathNode* OpenQueue::Pop()
{
MPASSERT( sentinel->next != sentinel );
PathNode* pNode = sentinel->next;
pNode->Unlink();
#ifdef DEBUG
sentinel->CheckList();
#endif
MPASSERT( pNode->inClosed == 0 );
MPASSERT( pNode->inOpen == 1 );
pNode->inOpen = 0;
#ifdef DEBUG_PATH_DEEP
printf( "Open Pop: " );
graph->PrintStateInfo( pNode->state );
printf( " total=%.1f\n", pNode->totalCost );
#endif
return pNode;
}