| 77 | |
| 78 | |
| 79 | void OpenQueue::Push( PathNode* pNode ) |
| 80 | { |
| 81 | |
| 82 | MPASSERT( pNode->inOpen == 0 ); |
| 83 | MPASSERT( pNode->inClosed == 0 ); |
| 84 | |
| 85 | #ifdef DEBUG_PATH_DEEP |
| 86 | printf( "Open Push: " ); |
| 87 | graph->PrintStateInfo( pNode->state ); |
| 88 | printf( " total=%.1f\n", pNode->totalCost ); |
| 89 | #endif |
| 90 | |
| 91 | // Add sorted. Lowest to highest cost path. Note that the sentinel has |
| 92 | // a value of FLT_MAX, so it should always be sorted in. |
| 93 | if (!(pNode->totalCost < FLT_MAX)) |
| 94 | return; |
| 95 | |
| 96 | MPASSERT( pNode->totalCost < FLT_MAX ); |
| 97 | PathNode* iter = sentinel->next; |
| 98 | while ( true ) |
| 99 | { |
| 100 | if ( pNode->totalCost < iter->totalCost ) { |
| 101 | iter->AddBefore( pNode ); |
| 102 | pNode->inOpen = 1; |
| 103 | break; |
| 104 | } |
| 105 | iter = iter->next; |
| 106 | } |
| 107 | MPASSERT( pNode->inOpen ); // make sure this was actually added. |
| 108 | #ifdef DEBUG |
| 109 | sentinel->CheckList(); |
| 110 | #endif |
| 111 | } |
| 112 | |
| 113 | PathNode* OpenQueue::Pop() |
| 114 | { |
no test coverage detected