| 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 |
|
