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