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