stlastar.h (17416B)
1 /* 2 A* Algorithm Implementation using STL is 3 Copyright (C)2001-2005 Justin Heyes-Jones 4 5 Permission is given by the author to freely redistribute and 6 include this code in any program as long as this credit is 7 given where due. 8 9 COVERED CODE IS PROVIDED UNDER THIS LICENSE ON AN "AS IS" BASIS, 10 WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, 11 INCLUDING, WITHOUT LIMITATION, WARRANTIES THAT THE COVERED CODE 12 IS FREE OF DEFECTS, MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE 13 OR NON-INFRINGING. THE ENTIRE RISK AS TO THE QUALITY AND 14 PERFORMANCE OF THE COVERED CODE IS WITH YOU. SHOULD ANY COVERED 15 CODE PROVE DEFECTIVE IN ANY RESPECT, YOU (NOT THE INITIAL 16 DEVELOPER OR ANY OTHER CONTRIBUTOR) ASSUME THE COST OF ANY 17 NECESSARY SERVICING, REPAIR OR CORRECTION. THIS DISCLAIMER OF 18 WARRANTY CONSTITUTES AN ESSENTIAL PART OF THIS LICENSE. NO USE 19 OF ANY COVERED CODE IS AUTHORIZED HEREUNDER EXCEPT UNDER 20 THIS DISCLAIMER. 21 22 Use at your own risk! 23 24 */ 25 26 // used for text debugging 27 #include <iostream> 28 #include <stdio.h> 29 //#include <conio.h> 30 #include <assert.h> 31 32 // stl includes 33 #include <algorithm> 34 #include <set> 35 #include <vector> 36 37 using namespace std; 38 39 // fast fixed size memory allocator, used for fast node memory management 40 #include "fsa.h" 41 42 // Fixed size memory allocator can be disabled to compare performance 43 // Uses std new and delete instead if you turn it off 44 #define USE_FSA_MEMORY 1 45 46 // disable warning that debugging information has lines that are truncated 47 // occurs in stl headers 48 #pragma warning( disable : 4786 ) 49 50 // The AStar search class. UserState is the users state space type 51 template <class UserState> class AStarSearch 52 { 53 54 public: // data 55 56 enum 57 { 58 SEARCH_STATE_NOT_INITIALISED, 59 SEARCH_STATE_SEARCHING, 60 SEARCH_STATE_SUCCEEDED, 61 SEARCH_STATE_FAILED, 62 SEARCH_STATE_OUT_OF_MEMORY, 63 SEARCH_STATE_INVALID 64 }; 65 66 67 // A node represents a possible state in the search 68 // The user provided state type is included inside this type 69 70 public: 71 72 class Node 73 { 74 public: 75 76 Node *parent; // used during the search to record the parent of successor nodes 77 Node *child; // used after the search for the application to view the search in reverse 78 79 float g; // cost of this node + it's predecessors 80 float h; // heuristic estimate of distance to goal 81 float f; // sum of cumulative cost of predecessors and self and heuristic 82 83 Node() : 84 parent( 0 ), 85 child( 0 ), 86 g( 0.0f ), 87 h( 0.0f ), 88 f( 0.0f ) 89 { 90 } 91 92 UserState m_UserState; 93 }; 94 95 96 // For sorting the heap the STL needs compare function that lets us compare 97 // the f value of two nodes 98 99 class HeapCompare_f 100 { 101 public: 102 103 bool operator() ( const Node *x, const Node *y ) const 104 { 105 return x->f > y->f; 106 } 107 }; 108 109 110 public: // methods 111 112 113 // constructor just initialises private data 114 AStarSearch( int MaxNodes = 1000 ) : 115 m_AllocateNodeCount(0), 116 #if USE_FSA_MEMORY 117 m_FixedSizeAllocator( MaxNodes ), 118 #endif 119 m_State( SEARCH_STATE_NOT_INITIALISED ), 120 m_CurrentSolutionNode( NULL ), 121 m_CancelRequest( false ) 122 { 123 } 124 125 // call at any time to cancel the search and free up all the memory 126 void CancelSearch() 127 { 128 m_CancelRequest = true; 129 } 130 131 // Set Start and goal states 132 void SetStartAndGoalStates( UserState &Start, UserState &Goal ) 133 { 134 m_CancelRequest = false; 135 136 m_Start = AllocateNode(); 137 m_Goal = AllocateNode(); 138 139 assert((m_Start != NULL && m_Goal != NULL)); 140 141 m_Start->m_UserState = Start; 142 m_Goal->m_UserState = Goal; 143 144 m_State = SEARCH_STATE_SEARCHING; 145 146 // Initialise the AStar specific parts of the Start Node 147 // The user only needs fill out the state information 148 149 m_Start->g = 0; 150 m_Start->h = m_Start->m_UserState.GoalDistanceEstimate( m_Goal->m_UserState ); 151 m_Start->f = m_Start->g + m_Start->h; 152 m_Start->parent = 0; 153 154 // Push the start node on the Open list 155 156 m_OpenList.push_back( m_Start ); // heap now unsorted 157 158 // Sort back element into heap 159 push_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() ); 160 161 // Initialise counter for search steps 162 m_Steps = 0; 163 } 164 165 // Advances search one step 166 unsigned int SearchStep() 167 { 168 // Firstly break if the user has not initialised the search 169 assert( (m_State > SEARCH_STATE_NOT_INITIALISED) && 170 (m_State < SEARCH_STATE_INVALID) ); 171 172 // Next I want it to be safe to do a searchstep once the search has succeeded... 173 if( (m_State == SEARCH_STATE_SUCCEEDED) || 174 (m_State == SEARCH_STATE_FAILED) 175 ) 176 { 177 return m_State; 178 } 179 180 // Failure is defined as emptying the open list as there is nothing left to 181 // search... 182 // New: Allow user abort 183 if( m_OpenList.empty() || m_CancelRequest ) 184 { 185 FreeAllNodes(); 186 m_State = SEARCH_STATE_FAILED; 187 return m_State; 188 } 189 190 // Incremement step count 191 m_Steps ++; 192 193 // Pop the best node (the one with the lowest f) 194 Node *n = m_OpenList.front(); // get pointer to the node 195 pop_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() ); 196 m_OpenList.pop_back(); 197 198 // Check for the goal, once we pop that we're done 199 if( n->m_UserState.IsGoal( m_Goal->m_UserState ) ) 200 { 201 // The user is going to use the Goal Node he passed in 202 // so copy the parent pointer of n 203 m_Goal->parent = n->parent; 204 205 // A special case is that the goal was passed in as the start state 206 // so handle that here 207 if( false == n->m_UserState.IsSameState( m_Start->m_UserState ) ) 208 { 209 FreeNode( n ); 210 211 // set the child pointers in each node (except Goal which has no child) 212 Node *nodeChild = m_Goal; 213 Node *nodeParent = m_Goal->parent; 214 215 do 216 { 217 nodeParent->child = nodeChild; 218 219 nodeChild = nodeParent; 220 nodeParent = nodeParent->parent; 221 222 } 223 while( nodeChild != m_Start ); // Start is always the first node by definition 224 225 } 226 227 // delete nodes that aren't needed for the solution 228 FreeUnusedNodes(); 229 230 m_State = SEARCH_STATE_SUCCEEDED; 231 232 return m_State; 233 } 234 else // not goal 235 { 236 237 // We now need to generate the successors of this node 238 // The user helps us to do this, and we keep the new nodes in 239 // m_Successors ... 240 241 m_Successors.clear(); // empty vector of successor nodes to n 242 243 // User provides this functions and uses AddSuccessor to add each successor of 244 // node 'n' to m_Successors 245 bool ret = n->m_UserState.GetSuccessors( this, n->parent ? &n->parent->m_UserState : NULL ); 246 247 if( !ret ) 248 { 249 250 typename vector< Node * >::iterator successor; 251 252 // free the nodes that may previously have been added 253 for( successor = m_Successors.begin(); successor != m_Successors.end(); successor ++ ) 254 { 255 FreeNode( (*successor) ); 256 } 257 258 m_Successors.clear(); // empty vector of successor nodes to n 259 260 // free up everything else we allocated 261 FreeAllNodes(); 262 263 m_State = SEARCH_STATE_OUT_OF_MEMORY; 264 return m_State; 265 } 266 267 // Now handle each successor to the current node ... 268 for( typename vector< Node * >::iterator successor = m_Successors.begin(); successor != m_Successors.end(); successor ++ ) 269 { 270 271 // The g value for this successor ... 272 float newg = n->g + n->m_UserState.GetCost( (*successor)->m_UserState ); 273 274 // Now we need to find whether the node is on the open or closed lists 275 // If it is but the node that is already on them is better (lower g) 276 // then we can forget about this successor 277 278 // First linear search of open list to find node 279 280 typename vector< Node * >::iterator openlist_result; 281 282 for( openlist_result = m_OpenList.begin(); openlist_result != m_OpenList.end(); openlist_result ++ ) 283 { 284 if( (*openlist_result)->m_UserState.IsSameState( (*successor)->m_UserState ) ) 285 { 286 break; 287 } 288 } 289 290 if( openlist_result != m_OpenList.end() ) 291 { 292 293 // we found this state on open 294 295 if( (*openlist_result)->g <= newg ) 296 { 297 FreeNode( (*successor) ); 298 299 // the one on Open is cheaper than this one 300 continue; 301 } 302 } 303 304 typename vector< Node * >::iterator closedlist_result; 305 306 for( closedlist_result = m_ClosedList.begin(); closedlist_result != m_ClosedList.end(); closedlist_result ++ ) 307 { 308 if( (*closedlist_result)->m_UserState.IsSameState( (*successor)->m_UserState ) ) 309 { 310 break; 311 } 312 } 313 314 if( closedlist_result != m_ClosedList.end() ) 315 { 316 317 // we found this state on closed 318 319 if( (*closedlist_result)->g <= newg ) 320 { 321 // the one on Closed is cheaper than this one 322 FreeNode( (*successor) ); 323 324 continue; 325 } 326 } 327 328 // This node is the best node so far with this particular state 329 // so lets keep it and set up its AStar specific data ... 330 331 (*successor)->parent = n; 332 (*successor)->g = newg; 333 (*successor)->h = (*successor)->m_UserState.GoalDistanceEstimate( m_Goal->m_UserState ); 334 (*successor)->f = (*successor)->g + (*successor)->h; 335 336 // Remove successor from closed if it was on it 337 338 if( closedlist_result != m_ClosedList.end() ) 339 { 340 // remove it from Closed 341 FreeNode( (*closedlist_result) ); 342 m_ClosedList.erase( closedlist_result ); 343 344 // Fix thanks to ... 345 // Greg Douglas <gregdouglasmail@gmail.com> 346 // who noticed that this code path was incorrect 347 // Here we have found a new state which is already CLOSED 348 // anus 349 350 } 351 352 // Update old version of this node 353 if( openlist_result != m_OpenList.end() ) 354 { 355 356 FreeNode( (*openlist_result) ); 357 m_OpenList.erase( openlist_result ); 358 359 // re-make the heap 360 // make_heap rather than sort_heap is an essential bug fix 361 // thanks to Mike Ryynanen for pointing this out and then explaining 362 // it in detail. sort_heap called on an invalid heap does not work 363 make_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() ); 364 365 } 366 367 // heap now unsorted 368 m_OpenList.push_back( (*successor) ); 369 370 // sort back element into heap 371 push_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() ); 372 373 } 374 375 // push n onto Closed, as we have expanded it now 376 377 m_ClosedList.push_back( n ); 378 379 } // end else (not goal so expand) 380 381 return m_State; // Succeeded bool is false at this point. 382 383 } 384 385 // User calls this to add a successor to a list of successors 386 // when expanding the search frontier 387 bool AddSuccessor( UserState &State ) 388 { 389 Node *node = AllocateNode(); 390 391 if( node ) 392 { 393 node->m_UserState = State; 394 395 m_Successors.push_back( node ); 396 397 return true; 398 } 399 400 return false; 401 } 402 403 // Free the solution nodes 404 // This is done to clean up all used Node memory when you are done with the 405 // search 406 void FreeSolutionNodes() 407 { 408 Node *n = m_Start; 409 410 if( m_Start->child ) 411 { 412 do 413 { 414 Node *del = n; 415 n = n->child; 416 FreeNode( del ); 417 418 del = NULL; 419 420 } while( n != m_Goal ); 421 422 FreeNode( n ); // Delete the goal 423 424 } 425 else 426 { 427 // if the start node is the solution we need to just delete the start and goal 428 // nodes 429 FreeNode( m_Start ); 430 FreeNode( m_Goal ); 431 } 432 433 } 434 435 // Functions for traversing the solution 436 437 // Get start node 438 UserState *GetSolutionStart() 439 { 440 m_CurrentSolutionNode = m_Start; 441 if( m_Start ) 442 { 443 return &m_Start->m_UserState; 444 } 445 else 446 { 447 return NULL; 448 } 449 } 450 451 // Get next node 452 UserState *GetSolutionNext() 453 { 454 if( m_CurrentSolutionNode ) 455 { 456 if( m_CurrentSolutionNode->child ) 457 { 458 459 Node *child = m_CurrentSolutionNode->child; 460 461 m_CurrentSolutionNode = m_CurrentSolutionNode->child; 462 463 return &child->m_UserState; 464 } 465 } 466 467 return NULL; 468 } 469 470 // Get end node 471 UserState *GetSolutionEnd() 472 { 473 m_CurrentSolutionNode = m_Goal; 474 if( m_Goal ) 475 { 476 return &m_Goal->m_UserState; 477 } 478 else 479 { 480 return NULL; 481 } 482 } 483 484 // Step solution iterator backwards 485 UserState *GetSolutionPrev() 486 { 487 if( m_CurrentSolutionNode ) 488 { 489 if( m_CurrentSolutionNode->parent ) 490 { 491 492 Node *parent = m_CurrentSolutionNode->parent; 493 494 m_CurrentSolutionNode = m_CurrentSolutionNode->parent; 495 496 return &parent->m_UserState; 497 } 498 } 499 500 return NULL; 501 } 502 503 // For educational use and debugging it is useful to be able to view 504 // the open and closed list at each step, here are two functions to allow that. 505 506 UserState *GetOpenListStart() 507 { 508 float f,g,h; 509 return GetOpenListStart( f,g,h ); 510 } 511 512 UserState *GetOpenListStart( float &f, float &g, float &h ) 513 { 514 iterDbgOpen = m_OpenList.begin(); 515 if( iterDbgOpen != m_OpenList.end() ) 516 { 517 f = (*iterDbgOpen)->f; 518 g = (*iterDbgOpen)->g; 519 h = (*iterDbgOpen)->h; 520 return &(*iterDbgOpen)->m_UserState; 521 } 522 523 return NULL; 524 } 525 526 UserState *GetOpenListNext() 527 { 528 float f,g,h; 529 return GetOpenListNext( f,g,h ); 530 } 531 532 UserState *GetOpenListNext( float &f, float &g, float &h ) 533 { 534 iterDbgOpen++; 535 if( iterDbgOpen != m_OpenList.end() ) 536 { 537 f = (*iterDbgOpen)->f; 538 g = (*iterDbgOpen)->g; 539 h = (*iterDbgOpen)->h; 540 return &(*iterDbgOpen)->m_UserState; 541 } 542 543 return NULL; 544 } 545 546 UserState *GetClosedListStart() 547 { 548 float f,g,h; 549 return GetClosedListStart( f,g,h ); 550 } 551 552 UserState *GetClosedListStart( float &f, float &g, float &h ) 553 { 554 iterDbgClosed = m_ClosedList.begin(); 555 if( iterDbgClosed != m_ClosedList.end() ) 556 { 557 f = (*iterDbgClosed)->f; 558 g = (*iterDbgClosed)->g; 559 h = (*iterDbgClosed)->h; 560 561 return &(*iterDbgClosed)->m_UserState; 562 } 563 564 return NULL; 565 } 566 567 UserState *GetClosedListNext() 568 { 569 float f,g,h; 570 return GetClosedListNext( f,g,h ); 571 } 572 573 UserState *GetClosedListNext( float &f, float &g, float &h ) 574 { 575 iterDbgClosed++; 576 if( iterDbgClosed != m_ClosedList.end() ) 577 { 578 f = (*iterDbgClosed)->f; 579 g = (*iterDbgClosed)->g; 580 h = (*iterDbgClosed)->h; 581 582 return &(*iterDbgClosed)->m_UserState; 583 } 584 585 return NULL; 586 } 587 588 // Get the number of steps 589 590 int GetStepCount() { return m_Steps; } 591 592 void EnsureMemoryFreed() 593 { 594 #if USE_FSA_MEMORY 595 assert(m_AllocateNodeCount == 0); 596 #endif 597 598 } 599 600 private: // methods 601 602 // This is called when a search fails or is cancelled to free all used 603 // memory 604 void FreeAllNodes() 605 { 606 // iterate open list and delete all nodes 607 typename vector< Node * >::iterator iterOpen = m_OpenList.begin(); 608 609 while( iterOpen != m_OpenList.end() ) 610 { 611 Node *n = (*iterOpen); 612 FreeNode( n ); 613 614 iterOpen ++; 615 } 616 617 m_OpenList.clear(); 618 619 // iterate closed list and delete unused nodes 620 typename vector< Node * >::iterator iterClosed; 621 622 for( iterClosed = m_ClosedList.begin(); iterClosed != m_ClosedList.end(); iterClosed ++ ) 623 { 624 Node *n = (*iterClosed); 625 FreeNode( n ); 626 } 627 628 m_ClosedList.clear(); 629 630 // delete the goal 631 632 FreeNode(m_Goal); 633 } 634 635 636 // This call is made by the search class when the search ends. A lot of nodes may be 637 // created that are still present when the search ends. They will be deleted by this 638 // routine once the search ends 639 void FreeUnusedNodes() 640 { 641 // iterate open list and delete unused nodes 642 typename vector< Node * >::iterator iterOpen = m_OpenList.begin(); 643 644 while( iterOpen != m_OpenList.end() ) 645 { 646 Node *n = (*iterOpen); 647 648 if( !n->child ) 649 { 650 FreeNode( n ); 651 652 n = NULL; 653 } 654 655 iterOpen ++; 656 } 657 658 m_OpenList.clear(); 659 660 // iterate closed list and delete unused nodes 661 typename vector< Node * >::iterator iterClosed; 662 663 for( iterClosed = m_ClosedList.begin(); iterClosed != m_ClosedList.end(); iterClosed ++ ) 664 { 665 Node *n = (*iterClosed); 666 667 if( !n->child ) 668 { 669 FreeNode( n ); 670 n = NULL; 671 672 } 673 } 674 675 m_ClosedList.clear(); 676 677 } 678 679 // Node memory management 680 Node *AllocateNode() 681 { 682 683 #if !USE_FSA_MEMORY 684 Node *p = new Node; 685 return p; 686 #else 687 Node *address = m_FixedSizeAllocator.alloc(); 688 689 if( !address ) 690 { 691 return NULL; 692 } 693 m_AllocateNodeCount ++; 694 Node *p = new (address) Node; 695 return p; 696 #endif 697 } 698 699 void FreeNode( Node *node ) 700 { 701 702 m_AllocateNodeCount --; 703 704 #if !USE_FSA_MEMORY 705 delete node; 706 #else 707 m_FixedSizeAllocator.free( node ); 708 #endif 709 } 710 711 private: // data 712 713 // Heap (simple vector but used as a heap, cf. Steve Rabin's game gems article) 714 vector< Node *> m_OpenList; 715 716 // Closed list is a vector. 717 vector< Node * > m_ClosedList; 718 719 // Successors is a vector filled out by the user each type successors to a node 720 // are generated 721 vector< Node * > m_Successors; 722 723 // State 724 unsigned int m_State; 725 726 // Counts steps 727 int m_Steps; 728 729 // Start and goal state pointers 730 Node *m_Start; 731 Node *m_Goal; 732 733 Node *m_CurrentSolutionNode; 734 735 #if USE_FSA_MEMORY 736 // Memory 737 FixedSizeAllocator<Node> m_FixedSizeAllocator; 738 #endif 739 740 //Debug : need to keep these two iterators around 741 // for the user Dbg functions 742 typename vector< Node * >::iterator iterDbgOpen; 743 typename vector< Node * >::iterator iterDbgClosed; 744 745 // debugging : count memory allocation and free's 746 int m_AllocateNodeCount; 747 748 bool m_CancelRequest; 749 750 }; 751 752 753 754