1 |
root |
1.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 |
|
|
|