zibbr

Random 3d and p2p stuff that was originally the beginnings of an overly ambitious p2p virtual world thingy
git clone https://code.literati.org/zibbr.git
Log | Files | Refs

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