1 |
/* |
2 |
Copyright (c) 2000-2005 Lee Thomason (www.grinninglizard.com) |
3 |
|
4 |
Grinning Lizard Utilities. |
5 |
|
6 |
This software is provided 'as-is', without any express or implied |
7 |
warranty. In no event will the authors be held liable for any |
8 |
damages arising from the use of this software. |
9 |
|
10 |
Permission is granted to anyone to use this software for any |
11 |
purpose, including commercial applications, and to alter it and |
12 |
redistribute it freely, subject to the following restrictions: |
13 |
|
14 |
1. The origin of this software must not be misrepresented; you must |
15 |
not claim that you wrote the original software. If you use this |
16 |
software in a product, an acknowledgment in the product documentation |
17 |
would be appreciated but is not required. |
18 |
|
19 |
2. Altered source versions must be plainly marked as such, and |
20 |
must not be misrepresented as being the original software. |
21 |
|
22 |
3. This notice may not be removed or altered from any source |
23 |
distribution. |
24 |
*/ |
25 |
|
26 |
#ifdef _MSC_VER |
27 |
#pragma warning( disable : 4786 ) // Debugger truncating names. |
28 |
#pragma warning( disable : 4530 ) // Exception handler isn't used |
29 |
#endif |
30 |
|
31 |
#include <cstring> |
32 |
#include <cstdio> |
33 |
|
34 |
#include <vector> |
35 |
#include <map> |
36 |
#include <algorithm> |
37 |
#include <functional> |
38 |
|
39 |
//#define DEBUG_PATH |
40 |
//#define DEBUG_PATH_DEEP |
41 |
|
42 |
#include "micropather.h" |
43 |
|
44 |
using namespace std; |
45 |
using namespace micropather; |
46 |
|
47 |
template<class _Ty> |
48 |
struct PathNodeCompareTotalCost : binary_function<_Ty, _Ty, bool> { |
49 |
bool operator()(const _Ty& _X, const _Ty& _Y) const |
50 |
{return (_X->totalCost > _Y->totalCost ); } |
51 |
}; |
52 |
|
53 |
class OpenQueue |
54 |
{ |
55 |
public: |
56 |
OpenQueue( Graph* _graph ) |
57 |
{ |
58 |
graph = _graph; |
59 |
#ifdef USE_LIST |
60 |
sentinel = (PathNode*) sentinelMem; |
61 |
sentinel->Init( 0, 0, FLT_MAX, FLT_MAX, 0 ); |
62 |
sentinel->totalCost = FLT_MAX; |
63 |
sentinel->next = sentinel; |
64 |
sentinel->prev = sentinel; |
65 |
#ifdef DEBUG |
66 |
sentinel->CheckList(); |
67 |
#endif |
68 |
#endif |
69 |
} |
70 |
~OpenQueue() {} |
71 |
|
72 |
void Push( PathNode* pNode ) |
73 |
{ |
74 |
|
75 |
assert( pNode->inOpen == 0 ); |
76 |
assert( pNode->inClosed == 0 ); |
77 |
|
78 |
#ifdef DEBUG_PATH_DEEP |
79 |
printf( "Open Push: " ); |
80 |
graph->PrintStateInfo( pNode->state ); |
81 |
printf( " total=%.1f\n", pNode->totalCost ); |
82 |
#endif |
83 |
|
84 |
#ifdef USE_LIST |
85 |
// Add sorted. Lowest to highest cost path. Note that the sentinel has |
86 |
// a value of FLT_MAX, so it should always be sorted in. |
87 |
assert( pNode->totalCost < FLT_MAX ); |
88 |
PathNode* iter = sentinel->next; |
89 |
while ( true ) |
90 |
{ |
91 |
if ( pNode->totalCost < iter->totalCost ) { |
92 |
iter->AddBefore( pNode ); |
93 |
pNode->inOpen = 1; |
94 |
break; |
95 |
} |
96 |
iter = iter->next; |
97 |
} |
98 |
assert( pNode->inOpen ); // make sure this was actually added. |
99 |
#ifdef DEBUG |
100 |
sentinel->CheckList(); |
101 |
#endif |
102 |
#else |
103 |
heapVector.push_back( pNode ); |
104 |
#endif |
105 |
} |
106 |
|
107 |
PathNode* Pop() |
108 |
{ |
109 |
#ifdef USE_LIST |
110 |
assert( sentinel->next != sentinel ); |
111 |
PathNode* pNode = sentinel->next; |
112 |
pNode->Unlink(); |
113 |
#ifdef DEBUG |
114 |
sentinel->CheckList(); |
115 |
#endif |
116 |
#else |
117 |
PathNode* pNode = heapVector[0]; |
118 |
|
119 |
const int size = heapVector.size(); |
120 |
int found = 0; |
121 |
for( int i=1; i<size; ++i ) { |
122 |
if ( heapVector[i]->totalCost < pNode->totalCost ) { |
123 |
pNode = heapVector[i]; |
124 |
found = i; |
125 |
} |
126 |
} |
127 |
if ( found < size-1 ) |
128 |
memcpy( &heapVector[found], &heapVector[found+1], sizeof( PathNode* ) * (size-found-1) ); |
129 |
heapVector.pop_back(); |
130 |
#endif |
131 |
|
132 |
assert( pNode->inClosed == 0 ); |
133 |
assert( pNode->inOpen == 1 ); |
134 |
pNode->inOpen = 0; |
135 |
|
136 |
#ifdef DEBUG_PATH_DEEP |
137 |
printf( "Open Pop: " ); |
138 |
graph->PrintStateInfo( pNode->state ); |
139 |
printf( " total=%.1f\n", pNode->totalCost ); |
140 |
#endif |
141 |
|
142 |
return pNode; |
143 |
} |
144 |
|
145 |
void Update( PathNode* pNode ) |
146 |
{ |
147 |
#ifdef DEBUG_PATH_DEEP |
148 |
printf( "Open Update: " ); |
149 |
graph->PrintStateInfo( pNode->state ); |
150 |
printf( " total=%.1f\n", pNode->totalCost ); |
151 |
#endif |
152 |
|
153 |
#ifdef USE_LIST |
154 |
assert( pNode->inOpen ); |
155 |
|
156 |
// If the node now cost less than the one before it, |
157 |
// move it to the front of the list. |
158 |
if ( pNode->prev != sentinel && pNode->totalCost < pNode->prev->totalCost ) { |
159 |
pNode->Unlink(); |
160 |
sentinel->next->AddBefore( pNode ); |
161 |
} |
162 |
|
163 |
// If the node is too high, move to the right. |
164 |
if ( pNode->totalCost > pNode->next->totalCost ) { |
165 |
PathNode* it = pNode->next; |
166 |
pNode->Unlink(); |
167 |
|
168 |
while ( pNode->totalCost > it->totalCost ) |
169 |
it = it->next; |
170 |
|
171 |
it->AddBefore( pNode ); |
172 |
#ifdef DEBUG |
173 |
sentinel->CheckList(); |
174 |
#endif |
175 |
} |
176 |
#endif |
177 |
} |
178 |
|
179 |
#ifdef USE_LIST |
180 |
bool Empty() { return sentinel->next == sentinel; } |
181 |
#else |
182 |
bool Empty() { return heapVector.empty(); } |
183 |
#endif |
184 |
|
185 |
private: |
186 |
OpenQueue( const OpenQueue& ); // undefined and unsupported |
187 |
void operator=( const OpenQueue& ); |
188 |
|
189 |
#ifdef USE_LIST |
190 |
PathNode* sentinel; |
191 |
int sentinelMem[ ( sizeof( PathNode ) + sizeof( int ) ) / sizeof( int ) ]; |
192 |
#else |
193 |
vector< PathNode* > heapVector; |
194 |
#endif |
195 |
Graph* graph; // for debugging |
196 |
}; |
197 |
|
198 |
|
199 |
class ClosedSet |
200 |
{ |
201 |
public: |
202 |
ClosedSet( Graph* _graph ) { this->graph = _graph; } |
203 |
~ClosedSet() {} |
204 |
|
205 |
void Add( PathNode* pNode ) |
206 |
{ |
207 |
#ifdef DEBUG_PATH_DEEP |
208 |
printf( "Closed add: " ); |
209 |
graph->PrintStateInfo( pNode->state ); |
210 |
printf( " total=%.1f\n", pNode->totalCost ); |
211 |
#endif |
212 |
#ifdef DEBUG |
213 |
assert( pNode->inClosed == 0 ); |
214 |
assert( pNode->inOpen == 0 ); |
215 |
#endif |
216 |
pNode->inClosed = 1; |
217 |
} |
218 |
|
219 |
void Remove( PathNode* pNode ) |
220 |
{ |
221 |
#ifdef DEBUG_PATH_DEEP |
222 |
printf( "Closed remove: " ); |
223 |
graph->PrintStateInfo( pNode->state ); |
224 |
printf( " total=%.1f\n", pNode->totalCost ); |
225 |
#endif |
226 |
assert( pNode->inClosed == 1 ); |
227 |
assert( pNode->inOpen == 0 ); |
228 |
|
229 |
pNode->inClosed = 0; |
230 |
} |
231 |
|
232 |
private: |
233 |
ClosedSet( const ClosedSet& ); |
234 |
void operator=( const ClosedSet& ); |
235 |
Graph* graph; |
236 |
}; |
237 |
|
238 |
|
239 |
MicroPather::MicroPather( Graph* _graph, unsigned allocate ) |
240 |
: ALLOCATE( allocate ), |
241 |
BLOCKSIZE( allocate-1 ), |
242 |
graph( _graph ), |
243 |
pathNodeMem( 0 ), |
244 |
availMem( 0 ), |
245 |
pathNodeCount( 0 ), |
246 |
frame( 0 ), |
247 |
checksum( 0 ) |
248 |
{ |
249 |
memset( hashTable, 0, sizeof( PathNode* )*HASH_SIZE ); |
250 |
} |
251 |
|
252 |
|
253 |
MicroPather::~MicroPather() |
254 |
{ |
255 |
PathNode *temp; |
256 |
|
257 |
while( pathNodeMem ) { |
258 |
temp = pathNodeMem; |
259 |
pathNodeMem = pathNodeMem[ ALLOCATE-1 ].left; |
260 |
free( temp ); // c-allocation |
261 |
} |
262 |
} |
263 |
|
264 |
|
265 |
void MicroPather::DumpHashTable() |
266 |
{ |
267 |
for( int i=0; i<HASH_SIZE; ++i ) |
268 |
{ |
269 |
int count = 0; |
270 |
RecPathNodeWalk( hashTable[i], &count ); |
271 |
if ( count != 0 ) |
272 |
printf( "Hashtable[%3d] has %d items\n", i, count ); |
273 |
} |
274 |
} |
275 |
|
276 |
void MicroPather::RecPathNodeWalk( PathNode* node, int* count ) |
277 |
{ |
278 |
if ( node ) |
279 |
{ |
280 |
*count += 1; |
281 |
if ( node->left ) |
282 |
RecPathNodeWalk( node->left, count ); |
283 |
#ifdef USE_BINARY_HASH |
284 |
if ( node->right ) |
285 |
RecPathNodeWalk( node->right, count ); |
286 |
#endif |
287 |
} |
288 |
} |
289 |
|
290 |
|
291 |
void MicroPather::Reset() |
292 |
{ |
293 |
while( pathNodeMem ) { |
294 |
PathNode* temp = pathNodeMem; |
295 |
pathNodeMem = pathNodeMem[ ALLOCATE-1 ].left; |
296 |
free( temp ); // c-allocation |
297 |
} |
298 |
pathNodeMem = 0; |
299 |
memset( hashTable, 0, sizeof( PathNode* )*HASH_SIZE ); |
300 |
pathNodeMem = 0; |
301 |
availMem = 0; |
302 |
pathNodeCount = 0; |
303 |
frame = 0; |
304 |
checksum = 0; |
305 |
} |
306 |
|
307 |
|
308 |
//void MicroPather::StateCostChange( void* state ) |
309 |
//{ |
310 |
// PathNode* node = FindPathNode( state, true ); |
311 |
// if ( node ) { |
312 |
// node->numAdjacent = -1; // throw away any cached data. |
313 |
// } |
314 |
//} |
315 |
|
316 |
|
317 |
PathNode* MicroPather::AllocatePathNode() |
318 |
{ |
319 |
if ( availMem == 0 ) { |
320 |
PathNode* newBlock = (PathNode*) malloc( sizeof(PathNode) * ALLOCATE ); |
321 |
// set up the "next" pointer |
322 |
newBlock[ALLOCATE-1].left = pathNodeMem; |
323 |
pathNodeMem = newBlock; |
324 |
availMem = BLOCKSIZE; |
325 |
} |
326 |
assert( availMem ); |
327 |
|
328 |
PathNode* result = pathNodeMem + (BLOCKSIZE - availMem ); |
329 |
--availMem; |
330 |
++pathNodeCount; |
331 |
return result; |
332 |
} |
333 |
|
334 |
|
335 |
PathNode* MicroPather::NewPathNode( void* state, float costFromStart, float estToEnd, PathNode* parent ) |
336 |
{ |
337 |
// Try to find an existing node for this state. |
338 |
unsigned key = Hash( state ); //(HASH_SIZE-1) & ( (unsigned)state + (((unsigned)state)>>8) + (((unsigned)state)>>16) + (((unsigned)state)>>24) ); |
339 |
|
340 |
if ( !hashTable[key] ) { |
341 |
// There isn't even a hashtable yet - create and initialize the PathNode. |
342 |
hashTable[key] = AllocatePathNode(); |
343 |
hashTable[key]->Init( frame, state, costFromStart, estToEnd, parent ); |
344 |
return hashTable[key]; |
345 |
} |
346 |
|
347 |
PathNode* root = hashTable[key]; |
348 |
PathNode* up = 0; |
349 |
while ( root ) { |
350 |
up = root; |
351 |
if ( root->state == state ) { |
352 |
root->Reuse( frame, costFromStart, estToEnd, parent ); |
353 |
assert( root->state == state ); |
354 |
return root; |
355 |
} |
356 |
#ifdef USE_BINARY_HASH |
357 |
else if ( state > root->state ) { |
358 |
root = root->right; |
359 |
} |
360 |
#endif |
361 |
else { |
362 |
#ifdef USE_BINARY_HASH |
363 |
assert( state < root->state ); |
364 |
#endif |
365 |
root = root->left; |
366 |
} |
367 |
} |
368 |
|
369 |
assert( up ); |
370 |
PathNode* pNode = AllocatePathNode(); |
371 |
pNode->Init( frame, state, costFromStart, estToEnd, parent ); |
372 |
#ifdef USE_BINARY_HASH |
373 |
if ( state > up->state ) { |
374 |
assert( up->right == 0 ); |
375 |
up->right = pNode; |
376 |
} |
377 |
else { |
378 |
assert( up->left == 0 ); |
379 |
up->left = pNode; |
380 |
} |
381 |
#else |
382 |
up->left = pNode; |
383 |
#endif |
384 |
return pNode; |
385 |
} |
386 |
|
387 |
|
388 |
void MicroPather::GoalReached( PathNode* node, void* start, void* end, vector< void* > *path ) |
389 |
{ |
390 |
path->clear(); |
391 |
|
392 |
// We have reached the goal. |
393 |
// How long is the path? Used to allocate the vector which is returned. |
394 |
int count = 1; |
395 |
PathNode* it = node; |
396 |
while( it->parent ) |
397 |
{ |
398 |
++count; |
399 |
it = it->parent; |
400 |
} |
401 |
|
402 |
// Now that the path has a known length, allocate |
403 |
// and fill the vector that will be returned. |
404 |
if ( count < 3 ) |
405 |
{ |
406 |
// Handle the short, special case. |
407 |
path->resize(2); |
408 |
(*path)[0] = start; |
409 |
(*path)[1] = end; |
410 |
} |
411 |
else |
412 |
{ |
413 |
path->resize(count); |
414 |
|
415 |
(*path)[0] = start; |
416 |
(*path)[count-1] = end; |
417 |
|
418 |
count-=2; |
419 |
it = node->parent; |
420 |
|
421 |
while ( it->parent ) |
422 |
{ |
423 |
(*path)[count] = it->state; |
424 |
it = it->parent; |
425 |
--count; |
426 |
} |
427 |
} |
428 |
checksum = 0; |
429 |
#ifdef DEBUG_PATH |
430 |
printf( "Path: " ); |
431 |
int counter=0; |
432 |
#endif |
433 |
for ( unsigned k=0; k<path->size(); ++k ) |
434 |
{ |
435 |
checksum += ((UPTR)((*path)[k])) << (k%8); |
436 |
|
437 |
#ifdef DEBUG_PATH |
438 |
graph->PrintStateInfo( (*path)[k] ); |
439 |
printf( " " ); |
440 |
++counter; |
441 |
if ( counter == 8 ) |
442 |
{ |
443 |
printf( "\n" ); |
444 |
counter = 0; |
445 |
} |
446 |
#endif |
447 |
} |
448 |
#ifdef DEBUG_PATH |
449 |
printf( "Cost=%.1f Checksum %d\n", node->costFromStart, checksum ); |
450 |
#endif |
451 |
} |
452 |
|
453 |
|
454 |
NodeCost* MicroPather::GetNodeNeighbors( PathNode* node, std::vector< StateCost >* neighbor, std::vector< NodeCost >* neighborNode ) |
455 |
{ |
456 |
NodeCost* nodeCost = 0; |
457 |
|
458 |
if ( node->numAdjacent < 0 |
459 |
|| node->numAdjacent > PathNode::MAX_CACHE ) |
460 |
{ |
461 |
// It has not been computed yet (<0) or |
462 |
// can not be cached (>MAX) |
463 |
neighbor->resize( 0 ); |
464 |
graph->AdjacentCost( node->state, neighbor ); |
465 |
#ifdef DEBUG |
466 |
{ |
467 |
// If this assert fires, you have passed a state |
468 |
// as its own neighbor state. This is impossible -- |
469 |
// bad things will happen. |
470 |
for ( unsigned i=0; i<neighbor->size(); ++i ) |
471 |
assert( (*neighbor)[i].state != node->state ); |
472 |
} |
473 |
#endif |
474 |
node->numAdjacent = (int) neighbor->size(); |
475 |
|
476 |
// Now convert to pathNodes, and put in cache if possible. |
477 |
if ( node->numAdjacent <= PathNode::MAX_CACHE ) |
478 |
{ |
479 |
// Can fit in the cache: |
480 |
for( int i=0; i<node->numAdjacent; ++i ) |
481 |
{ |
482 |
node->adjacent[i].cost = (*neighbor)[i].cost; |
483 |
node->adjacent[i].node = FindPathNode( (*neighbor)[i].state ); |
484 |
if ( !node->adjacent[i].node ) |
485 |
{ |
486 |
node->adjacent[i].node = NewPathNode( (*neighbor)[i].state, FLT_BIG, FLT_BIG, 0 ); |
487 |
} |
488 |
} |
489 |
nodeCost = node->adjacent; |
490 |
|
491 |
#ifdef DEBUG_PATH_DEEP |
492 |
printf( "State " ); |
493 |
graph->PrintStateInfo( node->state ); |
494 |
printf( "--> cache\n" ); |
495 |
#endif |
496 |
} |
497 |
else { |
498 |
// Too big for cache. |
499 |
node->numAdjacent = (int) neighbor->size(); |
500 |
|
501 |
neighborNode->resize( neighbor->size() ); |
502 |
|
503 |
for( unsigned i=0; i<neighborNode->size(); ++i ) { |
504 |
(*neighborNode)[i].cost = (*neighbor)[i].cost; |
505 |
(*neighborNode)[i].node = FindPathNode( (*neighbor)[i].state ); |
506 |
if ( !(*neighborNode)[i].node ) |
507 |
{ |
508 |
(*neighborNode)[i].node = NewPathNode( (*neighbor)[i].state, FLT_BIG, FLT_BIG, 0 ); |
509 |
} |
510 |
} |
511 |
nodeCost = &(*neighborNode)[0]; |
512 |
#ifdef DEBUG_PATH_DEEP |
513 |
printf( "State " ); |
514 |
graph->PrintStateInfo( node->state ); |
515 |
printf( "no cache\n" ); |
516 |
#endif |
517 |
} |
518 |
} |
519 |
else { |
520 |
// In the cache! |
521 |
nodeCost = node->adjacent; |
522 |
|
523 |
for( int i=0; i<node->numAdjacent; ++i ) { |
524 |
if ( node->adjacent[i].node->frame != frame ) |
525 |
node->adjacent[i].node->Reuse( frame, FLT_BIG, FLT_BIG, 0 ); |
526 |
} |
527 |
#ifdef DEBUG_PATH_DEEP |
528 |
printf( "State " ); |
529 |
graph->PrintStateInfo( node->state ); |
530 |
printf( "cache HIT\n" ); |
531 |
#endif |
532 |
} |
533 |
assert( nodeCost ); |
534 |
|
535 |
return nodeCost; |
536 |
} |
537 |
|
538 |
|
539 |
#ifdef DEBUG |
540 |
void MicroPather::DumpStats() |
541 |
{ |
542 |
int hashTableEntries = 0; |
543 |
for( int i=0; i<HASH_SIZE; ++i ) |
544 |
if ( hashTable[i] ) |
545 |
++hashTableEntries; |
546 |
|
547 |
int pathNodeBlocks = 0; |
548 |
for( PathNode* node = pathNodeMem; node; node = node[ALLOCATE-1].left ) |
549 |
++pathNodeBlocks; |
550 |
|
551 |
printf( "HashTableEntries=%d/%d PathNodeBlocks=%d [%dk] PathNodes=%d SolverCalled=%d\n", |
552 |
hashTableEntries, HASH_SIZE, pathNodeBlocks, |
553 |
pathNodeBlocks*ALLOCATE*sizeof(PathNode)/1024, |
554 |
pathNodeCount, |
555 |
frame ); |
556 |
} |
557 |
#endif |
558 |
|
559 |
|
560 |
void MicroPather::StatesInPool( std::vector< void* >* stateVec ) |
561 |
{ |
562 |
stateVec->clear(); |
563 |
|
564 |
for ( PathNode* mem = pathNodeMem; mem; mem = mem[ALLOCATE-1].left ) |
565 |
{ |
566 |
unsigned count = BLOCKSIZE; |
567 |
if ( mem == pathNodeMem ) |
568 |
count = BLOCKSIZE - availMem; |
569 |
|
570 |
for( unsigned i=0; i<count; ++i ) |
571 |
{ |
572 |
if ( mem[i].frame == frame ) |
573 |
stateVec->push_back( mem[i].state ); |
574 |
} |
575 |
} |
576 |
} |
577 |
|
578 |
|
579 |
int MicroPather::Solve( void* startNode, void* endNode, vector< void* >* path, float* cost ) |
580 |
{ |
581 |
#ifdef DEBUG_PATH |
582 |
printf( "Path: " ); |
583 |
graph->PrintStateInfo( startNode ); |
584 |
printf( " --> " ); |
585 |
graph->PrintStateInfo( endNode ); |
586 |
printf( " min cost=%f\n", graph->LeastCostEstimate( startNode, endNode ) ); |
587 |
#endif |
588 |
|
589 |
*cost = 0.0f; |
590 |
|
591 |
if ( startNode == endNode ) |
592 |
return START_END_SAME; |
593 |
|
594 |
++frame; |
595 |
// // In "reuse" mode, reset the costs. |
596 |
// if ( pathNodeMem ) |
597 |
// ReuseAll(); |
598 |
|
599 |
OpenQueue open( graph ); |
600 |
ClosedSet closed( graph ); |
601 |
|
602 |
open.Push( NewPathNode( startNode, // node |
603 |
0, // cost from start |
604 |
graph->LeastCostEstimate( startNode, endNode ), |
605 |
0 ) ); |
606 |
|
607 |
//vector< StateCost > neighbor; |
608 |
//vector< NodeCost > neighborNode; |
609 |
neighborVec.resize(0); |
610 |
neighborNodeVec.resize(0); |
611 |
|
612 |
while ( !open.Empty() ) |
613 |
{ |
614 |
PathNode* node = open.Pop(); |
615 |
|
616 |
if ( node->state == endNode ) |
617 |
{ |
618 |
GoalReached( node, startNode, endNode, path ); |
619 |
*cost = node->costFromStart; |
620 |
#ifdef DEBUG_PATH |
621 |
DumpStats(); |
622 |
#endif |
623 |
return SOLVED; |
624 |
} |
625 |
else |
626 |
{ |
627 |
// We have not reached the goal - add the neighbors. |
628 |
NodeCost* nodeCost = GetNodeNeighbors( node, &neighborVec, &neighborNodeVec ); |
629 |
|
630 |
for( int i=0; i<node->numAdjacent; ++i ) |
631 |
{ |
632 |
if ( nodeCost[i].cost == FLT_MAX ) { |
633 |
continue; |
634 |
} |
635 |
|
636 |
float newCost = node->costFromStart + nodeCost[i].cost; |
637 |
|
638 |
PathNode* inOpen = nodeCost[i].node->inOpen ? nodeCost[i].node : 0; |
639 |
PathNode* inClosed = nodeCost[i].node->inClosed ? nodeCost[i].node : 0; |
640 |
assert( !( inOpen && inClosed ) ); |
641 |
PathNode* inEither = inOpen ? inOpen : inClosed; |
642 |
|
643 |
assert( inEither != node ); |
644 |
|
645 |
if ( inEither ) |
646 |
{ |
647 |
// Is this node is in use, and the cost is not an improvement, |
648 |
// continue on. |
649 |
if ( inEither->costFromStart <= newCost ) |
650 |
continue; // Do nothing. This path is not better than existing. |
651 |
|
652 |
// Groovy. We have new information or improved information. |
653 |
inEither->parent = node; |
654 |
inEither->costFromStart = newCost; |
655 |
inEither->estToGoal = graph->LeastCostEstimate( inEither->state, endNode ); |
656 |
inEither->totalCost = inEither->costFromStart + inEither->estToGoal; |
657 |
} |
658 |
|
659 |
if ( inClosed ) |
660 |
{ |
661 |
closed.Remove( inClosed ); |
662 |
open.Push( inClosed ); |
663 |
} |
664 |
else if ( inOpen ) |
665 |
{ |
666 |
// Need to update the sort! |
667 |
open.Update( inOpen ); |
668 |
} |
669 |
else if (!inEither) |
670 |
{ |
671 |
assert( !inEither ); |
672 |
assert( nodeCost[i].node ); |
673 |
|
674 |
PathNode* pNode = nodeCost[i].node; |
675 |
pNode->parent = node; |
676 |
pNode->costFromStart = newCost; |
677 |
pNode->estToGoal = graph->LeastCostEstimate( pNode->state, endNode ), |
678 |
pNode->totalCost = pNode->costFromStart + pNode->estToGoal; |
679 |
|
680 |
open.Push( pNode ); |
681 |
} |
682 |
} |
683 |
} |
684 |
closed.Add( node ); |
685 |
} |
686 |
#ifdef DEBUG_PATH |
687 |
DumpStats(); |
688 |
#endif |
689 |
return NO_SOLUTION; |
690 |
} |
691 |
|
692 |
|