stlastar.h

00001 /*
00002 A* Algorithm Implementation using STL is
00003 Copyright (C)2001-2005 Justin Heyes-Jones
00004 
00005 Permission is given by the author to freely redistribute and 
00006 include this code in any program as long as this credit is 
00007 given where due.
00008  
00009   COVERED CODE IS PROVIDED UNDER THIS LICENSE ON AN "AS IS" BASIS, 
00010   WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, 
00011   INCLUDING, WITHOUT LIMITATION, WARRANTIES THAT THE COVERED CODE 
00012   IS FREE OF DEFECTS, MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE
00013   OR NON-INFRINGING. THE ENTIRE RISK AS TO THE QUALITY AND 
00014   PERFORMANCE OF THE COVERED CODE IS WITH YOU. SHOULD ANY COVERED 
00015   CODE PROVE DEFECTIVE IN ANY RESPECT, YOU (NOT THE INITIAL 
00016   DEVELOPER OR ANY OTHER CONTRIBUTOR) ASSUME THE COST OF ANY 
00017   NECESSARY SERVICING, REPAIR OR CORRECTION. THIS DISCLAIMER OF 
00018   WARRANTY CONSTITUTES AN ESSENTIAL PART OF THIS LICENSE. NO USE 
00019   OF ANY COVERED CODE IS AUTHORIZED HEREUNDER EXCEPT UNDER
00020   THIS DISCLAIMER.
00021  
00022   Use at your own risk!
00023 
00024 */
00025     
00026 #ifndef STL_ASTAR_H
00027 #define STL_ASTAR_H
00028 
00029 // used for text debugging
00030 #include <iostream>
00031 #include <stdio.h>
00032 //#include <conio.h>
00033 #include <assert.h>
00034 
00035 // stl includes
00036 #include <algorithm>
00037 #include <set>
00038 #include <vector>
00039 
00040 using namespace std;
00041 
00042 // fast fixed size memory allocator, used for fast node memory management
00043 #include "fsa.h"
00044 
00045 // Fixed size memory allocator can be disabled to compare performance
00046 // Uses std new and delete instead if you turn it off
00047 #define USE_FSA_MEMORY 1
00048 
00049 // disable warning that debugging information has lines that are truncated
00050 // occurs in stl headers
00051 #pragma warning( disable : 4786 )
00052              
00053              
00057 const int MAX_SEARCH_NODES = 1000;
00058 
00059 // The AStar search class. UserState is the users state space type
00060 template <class UserState> class AStarSearch
00061 {
00062 
00063 public: // data
00064 
00065         enum
00066         {
00067                 SEARCH_STATE_NOT_INITIALISED,
00068                 SEARCH_STATE_SEARCHING,
00069                 SEARCH_STATE_SUCCEEDED,
00070                 SEARCH_STATE_FAILED,
00071                 SEARCH_STATE_OUT_OF_MEMORY,
00072                 SEARCH_STATE_INVALID
00073         };
00074 
00075 
00076         // A node represents a possible state in the search
00077         // The user provided state type is included inside this type
00078 
00079         public:
00080 
00081         class Node
00082         {
00083                 public:
00084 
00085                         Node *parent; // used during the search to record the parent of successor nodes
00086                         Node *child; // used after the search for the application to view the search in reverse
00087                         
00088                         float g; // cost of this node + it's predecessors
00089                         float h; // heuristic estimate of distance to goal
00090                         float f; // sum of cumulative cost of predecessors and self and heuristic
00091 
00092                         Node() :
00093                                 parent( 0 ),
00094                                 child( 0 ),
00095                                 g( 0.0f ),
00096                                 h( 0.0f ),
00097                                 f( 0.0f )
00098                         {                       
00099                         }
00100 
00101                         UserState m_UserState;
00102         };
00103 
00104 
00105         // For sorting the heap the STL needs compare function that lets us compare
00106         // the f value of two nodes
00107 
00108         class HeapCompare_f 
00109         {
00110                 public:
00111 
00112                         bool operator() ( const Node *x, const Node *y ) const
00113                         {
00114                                 return x->f > y->f;
00115                         }
00116         };
00117 
00118 
00119 public: // methods
00120 
00121 
00122         // constructor just initialises private data
00123         AStarSearch( int MaxNodes = MAX_SEARCH_NODES ) :
00124                 m_AllocateNodeCount(0),
00125 #if USE_FSA_MEMORY
00126                 m_FixedSizeAllocator( MaxNodes ),
00127 #endif
00128                 m_State( SEARCH_STATE_NOT_INITIALISED ),
00129                 m_CurrentSolutionNode( NULL ),
00130                 m_CancelRequest( false )
00131         {
00132         }
00133 
00134         // call at any time to cancel the search and free up all the memory
00135         void CancelSearch()
00136         {
00137                 m_CancelRequest = true;
00138         }
00139 
00140         // Set Start and goal states
00141         void SetStartAndGoalStates( UserState &Start, UserState &Goal )
00142         {
00143                 m_CancelRequest = false;
00144 
00145                 m_Start = AllocateNode();
00146                 m_Goal = AllocateNode();
00147 
00148                 assert((m_Start != NULL && m_Goal != NULL));
00149                 
00150                 m_Start->m_UserState = Start;
00151                 m_Goal->m_UserState = Goal;
00152 
00153                 m_State = SEARCH_STATE_SEARCHING;
00154                 
00155                 // Initialise the AStar specific parts of the Start Node
00156                 // The user only needs fill out the state information
00157 
00158                 m_Start->g = 0; 
00159                 m_Start->h = m_Start->m_UserState.GoalDistanceEstimate( m_Goal->m_UserState );
00160                 m_Start->f = m_Start->g + m_Start->h;
00161                 m_Start->parent = 0;
00162 
00163                 // Push the start node on the Open list
00164 
00165                 m_OpenList.push_back( m_Start ); // heap now unsorted
00166 
00167                 // Sort back element into heap
00168                 push_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
00169 
00170                 // Initialise counter for search steps
00171                 m_Steps = 0;
00172         }
00173 
00174         // Advances search one step 
00175         unsigned int SearchStep()
00176         {
00177                 // Firstly break if the user has not initialised the search
00178                 assert( (m_State > SEARCH_STATE_NOT_INITIALISED) &&
00179                                 (m_State < SEARCH_STATE_INVALID) );
00180 
00181                 // Next I want it to be safe to do a searchstep once the search has succeeded...
00182                 if( (m_State == SEARCH_STATE_SUCCEEDED) ||
00183                         (m_State == SEARCH_STATE_FAILED) 
00184                   )
00185                 {
00186                         return m_State; 
00187                 }
00188 
00189                 // Failure is defined as emptying the open list as there is nothing left to 
00190                 // search...
00191                 // New: Allow user abort
00192                 if( m_OpenList.empty() || m_CancelRequest )
00193                 {
00194                         FreeAllNodes();
00195                         m_State = SEARCH_STATE_FAILED;
00196                         return m_State;
00197                 }
00198                 
00199                 // Incremement step count
00200                 m_Steps ++;
00201 
00202                 // Pop the best node (the one with the lowest f) 
00203                 Node *n = m_OpenList.front(); // get pointer to the node
00204                 pop_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
00205                 m_OpenList.pop_back();
00206 
00207                 // Check for the goal, once we pop that we're done
00208                 if( n->m_UserState.IsGoal( m_Goal->m_UserState ) )
00209                 {
00210                         // The user is going to use the Goal Node he passed in 
00211                         // so copy the parent pointer of n 
00212                         m_Goal->parent = n->parent;
00213 
00214                         // A special case is that the goal was passed in as the start state
00215                         // so handle that here
00216                         if( false == n->m_UserState.IsSameState( m_Start->m_UserState ) )
00217                         {
00218                                 FreeNode( n );
00219 
00220                                 // set the child pointers in each node (except Goal which has no child)
00221                                 Node *nodeChild = m_Goal;
00222                                 Node *nodeParent = m_Goal->parent;
00223 
00224                                 do 
00225                                 {
00226                                         nodeParent->child = nodeChild;
00227 
00228                                         nodeChild = nodeParent;
00229                                         nodeParent = nodeParent->parent;
00230                                 
00231                                 } 
00232                                 while( nodeChild != m_Start ); // Start is always the first node by definition
00233 
00234                         }
00235 
00236                         // delete nodes that aren't needed for the solution
00237                         FreeUnusedNodes();
00238 
00239                         m_State = SEARCH_STATE_SUCCEEDED;
00240 
00241                         return m_State;
00242                 }
00243                 else // not goal
00244                 {
00245 
00246                         // We now need to generate the successors of this node
00247                         // The user helps us to do this, and we keep the new nodes in
00248                         // m_Successors ...
00249 
00250                         m_Successors.clear(); // empty vector of successor nodes to n
00251 
00252                         // User provides this functions and uses AddSuccessor to add each successor of
00253                         // node 'n' to m_Successors
00254                         bool ret = n->m_UserState.GetSuccessors( this, n->parent ? &n->parent->m_UserState : NULL ); 
00255 
00256                         if( !ret )
00257                         {
00258 
00259                             typename vector< Node * >::iterator successor;
00260 
00261                                 // free the nodes that may previously have been added 
00262                                 for( successor = m_Successors.begin(); successor != m_Successors.end(); successor ++ )
00263                                 {
00264                                         FreeNode( (*successor) );
00265                                 }
00266 
00267                                 m_Successors.clear(); // empty vector of successor nodes to n
00268 
00269                                 // free up everything else we allocated
00270                                 FreeAllNodes();
00271 
00272                                 m_State = SEARCH_STATE_OUT_OF_MEMORY;
00273                                 return m_State;
00274                         }
00275                         
00276                         // Now handle each successor to the current node ...
00277                         for( typename vector< Node * >::iterator successor = m_Successors.begin(); successor != m_Successors.end(); successor ++ )
00278                         {
00279 
00280                                 //      The g value for this successor ...
00281                                 float newg = n->g + n->m_UserState.GetCost( (*successor)->m_UserState );
00282 
00283                                 // Now we need to find whether the node is on the open or closed lists
00284                                 // If it is but the node that is already on them is better (lower g)
00285                                 // then we can forget about this successor
00286 
00287                                 // First linear search of open list to find node
00288 
00289                                 typename vector< Node * >::iterator openlist_result;
00290 
00291                                 for( openlist_result = m_OpenList.begin(); openlist_result != m_OpenList.end(); openlist_result ++ )
00292                                 {
00293                                         if( (*openlist_result)->m_UserState.IsSameState( (*successor)->m_UserState ) )
00294                                         {
00295                                                 break;                                  
00296                                         }
00297                                 }
00298 
00299                                 if( openlist_result != m_OpenList.end() )
00300                                 {
00301 
00302                                         // we found this state on open
00303 
00304                                         if( (*openlist_result)->g <= newg )
00305                                         {
00306                                                 FreeNode( (*successor) );
00307 
00308                                                 // the one on Open is cheaper than this one
00309                                                 continue;
00310                                         }
00311                                 }
00312 
00313                                 typename vector< Node * >::iterator closedlist_result;
00314 
00315                                 for( closedlist_result = m_ClosedList.begin(); closedlist_result != m_ClosedList.end(); closedlist_result ++ )
00316                                 {
00317                                         if( (*closedlist_result)->m_UserState.IsSameState( (*successor)->m_UserState ) )
00318                                         {
00319                                                 break;                                  
00320                                         }
00321                                 }
00322 
00323                                 if( closedlist_result != m_ClosedList.end() )
00324                                 {
00325 
00326                                         // we found this state on closed
00327 
00328                                         if( (*closedlist_result)->g <= newg )
00329                                         {
00330                                                 // the one on Closed is cheaper than this one
00331                                                 FreeNode( (*successor) );
00332 
00333                                                 continue;
00334                                         }
00335                                 }
00336 
00337                                 // This node is the best node so far with this particular state
00338                                 // so lets keep it and set up its AStar specific data ...
00339 
00340                                 (*successor)->parent = n;
00341                                 (*successor)->g = newg;
00342                                 (*successor)->h = (*successor)->m_UserState.GoalDistanceEstimate( m_Goal->m_UserState );
00343                                 (*successor)->f = (*successor)->g + (*successor)->h;
00344 
00345                                 // Remove successor from closed if it was on it
00346 
00347                                 if( closedlist_result != m_ClosedList.end() )
00348                                 {
00349                                         // remove it from Closed
00350                                         FreeNode(  (*closedlist_result) ); 
00351                                         m_ClosedList.erase( closedlist_result );
00352 
00353                                         // Fix thanks to ...
00354                                         // Greg Douglas <gregdouglasmail@gmail.com>
00355                                         // who noticed that this code path was incorrect
00356                                         // Here we have found a new state which is already CLOSED
00357                                         // anus
00358                                         
00359                                 }
00360 
00361                                 // Update old version of this node
00362                                 if( openlist_result != m_OpenList.end() )
00363                                 {          
00364 
00365                                         FreeNode( (*openlist_result) ); 
00366                                         m_OpenList.erase( openlist_result );
00367 
00368                                         // re-make the heap 
00369                                         // make_heap rather than sort_heap is an essential bug fix
00370                                         // thanks to Mike Ryynanen for pointing this out and then explaining
00371                                         // it in detail. sort_heap called on an invalid heap does not work
00372                                         make_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
00373                         
00374                                 }
00375 
00376                                 // heap now unsorted
00377                                 m_OpenList.push_back( (*successor) );
00378 
00379                                 // sort back element into heap
00380                                 push_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
00381 
00382                         }
00383 
00384                         // push n onto Closed, as we have expanded it now
00385 
00386                         m_ClosedList.push_back( n );
00387 
00388                 } // end else (not goal so expand)
00389 
00390                 return m_State; // Succeeded bool is false at this point. 
00391 
00392         }
00393 
00394         // User calls this to add a successor to a list of successors
00395         // when expanding the search frontier
00396         bool AddSuccessor( UserState &State )
00397         {
00398                 Node *node = AllocateNode();
00399 
00400                 if( node )
00401                 {
00402                         node->m_UserState = State;
00403 
00404                         m_Successors.push_back( node );
00405 
00406                         return true;
00407                 }
00408 
00409                 return false;
00410         }
00411 
00412         // Free the solution nodes
00413         // This is done to clean up all used Node memory when you are done with the
00414         // search
00415         void FreeSolutionNodes()
00416         {
00417                 Node *n = m_Start;
00418 
00419                 if( m_Start->child )
00420                 {
00421                         do
00422                         {
00423                                 Node *del = n;
00424                                 n = n->child;
00425                                 FreeNode( del );
00426 
00427                                 del = NULL;
00428 
00429                         } while( n != m_Goal );
00430 
00431                         FreeNode( n ); // Delete the goal
00432 
00433                 }
00434                 else
00435                 {
00436                         // if the start node is the solution we need to just delete the start and goal
00437                         // nodes
00438                         FreeNode( m_Start );
00439                         FreeNode( m_Goal );
00440                 }
00441 
00442         }
00443 
00444         // Functions for traversing the solution
00445 
00446         // Get start node
00447         UserState *GetSolutionStart()
00448         {
00449                 m_CurrentSolutionNode = m_Start;
00450                 if( m_Start )
00451                 {
00452                         return &m_Start->m_UserState;
00453                 }
00454                 else
00455                 {
00456                         return NULL;
00457                 }
00458         }
00459         
00460         // Get next node
00461         UserState *GetSolutionNext()
00462         {
00463                 if( m_CurrentSolutionNode )
00464                 {
00465                         if( m_CurrentSolutionNode->child )
00466                         {
00467 
00468                                 Node *child = m_CurrentSolutionNode->child;
00469 
00470                                 m_CurrentSolutionNode = m_CurrentSolutionNode->child;
00471 
00472                                 return &child->m_UserState;
00473                         }
00474                 }
00475 
00476                 return NULL;
00477         }
00478         
00479         // Get end node
00480         UserState *GetSolutionEnd()
00481         {
00482                 m_CurrentSolutionNode = m_Goal;
00483                 if( m_Goal )
00484                 {
00485                         return &m_Goal->m_UserState;
00486                 }
00487                 else
00488                 {
00489                         return NULL;
00490                 }
00491         }
00492         
00493         // Step solution iterator backwards
00494         UserState *GetSolutionPrev()
00495         {
00496                 if( m_CurrentSolutionNode )
00497                 {
00498                         if( m_CurrentSolutionNode->parent )
00499                         {
00500 
00501                                 Node *parent = m_CurrentSolutionNode->parent;
00502 
00503                                 m_CurrentSolutionNode = m_CurrentSolutionNode->parent;
00504 
00505                                 return &parent->m_UserState;
00506                         }
00507                 }
00508 
00509                 return NULL;
00510         }
00511 
00512         // For educational use and debugging it is useful to be able to view
00513         // the open and closed list at each step, here are two functions to allow that.
00514 
00515         UserState *GetOpenListStart()
00516         {
00517                 float f,g,h;
00518                 return GetOpenListStart( f,g,h );
00519         }
00520 
00521         UserState *GetOpenListStart( float &f, float &g, float &h )
00522         {
00523                 iterDbgOpen = m_OpenList.begin();
00524                 if( iterDbgOpen != m_OpenList.end() )
00525                 {
00526                         f = (*iterDbgOpen)->f;
00527                         g = (*iterDbgOpen)->g;
00528                         h = (*iterDbgOpen)->h;
00529                         return &(*iterDbgOpen)->m_UserState;
00530                 }
00531 
00532                 return NULL;
00533         }
00534 
00535         UserState *GetOpenListNext()
00536         {
00537                 float f,g,h;
00538                 return GetOpenListNext( f,g,h );
00539         }
00540 
00541         UserState *GetOpenListNext( float &f, float &g, float &h )
00542         {
00543                 iterDbgOpen++;
00544                 if( iterDbgOpen != m_OpenList.end() )
00545                 {
00546                         f = (*iterDbgOpen)->f;
00547                         g = (*iterDbgOpen)->g;
00548                         h = (*iterDbgOpen)->h;
00549                         return &(*iterDbgOpen)->m_UserState;
00550                 }
00551 
00552                 return NULL;
00553         }
00554 
00555         UserState *GetClosedListStart()
00556         {
00557                 float f,g,h;
00558                 return GetClosedListStart( f,g,h );
00559         }
00560 
00561         UserState *GetClosedListStart( float &f, float &g, float &h )
00562         {
00563                 iterDbgClosed = m_ClosedList.begin();
00564                 if( iterDbgClosed != m_ClosedList.end() )
00565                 {
00566                         f = (*iterDbgClosed)->f;
00567                         g = (*iterDbgClosed)->g;
00568                         h = (*iterDbgClosed)->h;
00569 
00570                         return &(*iterDbgClosed)->m_UserState;
00571                 }
00572 
00573                 return NULL;
00574         }
00575 
00576         UserState *GetClosedListNext()
00577         {
00578                 float f,g,h;
00579                 return GetClosedListNext( f,g,h );
00580         }
00581 
00582         UserState *GetClosedListNext( float &f, float &g, float &h )
00583         {
00584                 iterDbgClosed++;
00585                 if( iterDbgClosed != m_ClosedList.end() )
00586                 {
00587                         f = (*iterDbgClosed)->f;
00588                         g = (*iterDbgClosed)->g;
00589                         h = (*iterDbgClosed)->h;
00590 
00591                         return &(*iterDbgClosed)->m_UserState;
00592                 }
00593 
00594                 return NULL;
00595         }
00596 
00597         // Get the number of steps
00598 
00599         int GetStepCount() { return m_Steps; }
00600 
00601         void EnsureMemoryFreed()
00602         {
00603 #if USE_FSA_MEMORY
00604                 assert(m_AllocateNodeCount == 0);
00605 #endif
00606 
00607         }
00608 
00609 private: // methods
00610 
00611         // This is called when a search fails or is cancelled to free all used
00612         // memory 
00613         void FreeAllNodes()
00614         {
00615                 // iterate open list and delete all nodes
00616                 typename vector< Node * >::iterator iterOpen = m_OpenList.begin();
00617 
00618                 while( iterOpen != m_OpenList.end() )
00619                 {
00620                         Node *n = (*iterOpen);
00621                         FreeNode( n );
00622 
00623                         iterOpen ++;
00624                 }
00625 
00626                 m_OpenList.clear();
00627 
00628                 // iterate closed list and delete unused nodes
00629                 typename vector< Node * >::iterator iterClosed;
00630 
00631                 for( iterClosed = m_ClosedList.begin(); iterClosed != m_ClosedList.end(); iterClosed ++ )
00632                 {
00633                         Node *n = (*iterClosed);
00634                         FreeNode( n );
00635                 }
00636 
00637                 m_ClosedList.clear();
00638 
00639                 // delete the goal
00640 
00641                 FreeNode(m_Goal);
00642         }
00643 
00644 
00645         // This call is made by the search class when the search ends. A lot of nodes may be
00646         // created that are still present when the search ends. They will be deleted by this 
00647         // routine once the search ends
00648         void FreeUnusedNodes()
00649         {
00650                 // iterate open list and delete unused nodes
00651                 typename vector< Node * >::iterator iterOpen = m_OpenList.begin();
00652 
00653                 while( iterOpen != m_OpenList.end() )
00654                 {
00655                         Node *n = (*iterOpen);
00656 
00657                         if( !n->child )
00658                         {
00659                                 FreeNode( n );
00660 
00661                                 n = NULL;
00662                         }
00663 
00664                         iterOpen ++;
00665                 }
00666 
00667                 m_OpenList.clear();
00668 
00669                 // iterate closed list and delete unused nodes
00670                 typename vector< Node * >::iterator iterClosed;
00671 
00672                 for( iterClosed = m_ClosedList.begin(); iterClosed != m_ClosedList.end(); iterClosed ++ )
00673                 {
00674                         Node *n = (*iterClosed);
00675 
00676                         if( !n->child )
00677                         {
00678                                 FreeNode( n );
00679                                 n = NULL;
00680 
00681                         }
00682                 }
00683 
00684                 m_ClosedList.clear();
00685 
00686         }
00687 
00688         // Node memory management
00689         Node *AllocateNode()
00690         {
00691 
00692 #if !USE_FSA_MEMORY
00693                 Node *p = new Node;
00694                 return p;
00695 #else
00696                 Node *address = m_FixedSizeAllocator.alloc();
00697 
00698                 if( !address )
00699                 {
00700                         return NULL;
00701                 }
00702                 m_AllocateNodeCount ++;
00703                 Node *p = new (address) Node;
00704                 return p;
00705 #endif
00706         }
00707 
00708         void FreeNode( Node *node )
00709         {
00710 
00711                 m_AllocateNodeCount --;
00712 
00713 #if !USE_FSA_MEMORY
00714                 delete node;
00715 #else
00716                 m_FixedSizeAllocator.free( node );
00717 #endif
00718         }
00719 
00720 private: // data
00721 
00722         // Heap (simple vector but used as a heap, cf. Steve Rabin's game gems article)
00723         vector< Node *> m_OpenList;
00724 
00725         // Closed list is a vector.
00726         vector< Node * > m_ClosedList; 
00727 
00728         // Successors is a vector filled out by the user each type successors to a node
00729         // are generated
00730         vector< Node * > m_Successors;
00731 
00732         // State
00733         unsigned int m_State;
00734 
00735         // Counts steps
00736         int m_Steps;
00737 
00738         // Start and goal state pointers
00739         Node *m_Start;
00740         Node *m_Goal;
00741 
00742         Node *m_CurrentSolutionNode;
00743 
00744 #if USE_FSA_MEMORY
00745         // Memory
00746         FixedSizeAllocator<Node> m_FixedSizeAllocator;
00747 #endif
00748         
00749         //Debug : need to keep these two iterators around
00750         // for the user Dbg functions
00751         typename vector< Node * >::iterator iterDbgOpen;
00752         typename vector< Node * >::iterator iterDbgClosed;
00753 
00754         // debugging : count memory allocation and free's
00755         int m_AllocateNodeCount;
00756         
00757         bool m_CancelRequest;
00758 
00759 };
00760 
00761 
00762 #endif
00763 

Generated on Tue Jun 19 11:15:43 2007 for mainplansys.kdevelop by  doxygen 1.4.7