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

findpath.cpp (8109B)


      1 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
      2 
      3 // STL A* Search implementation
      4 // (C)2001 Justin Heyes-Jones
      5 //
      6 // Finding a path on a simple grid maze
      7 // This shows how to do shortest path finding using A*
      8 
      9 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
     10 
     11 #include "stlastar.h" // See header for copyright and usage information
     12 
     13 #include <iostream>
     14 #include <math.h>
     15 
     16 #define DEBUG_LISTS 0
     17 #define DEBUG_LIST_LENGTHS_ONLY 0
     18 
     19 using namespace std;
     20 
     21 // Global data
     22 
     23 // The world map
     24 
     25 const int MAP_WIDTH = 20;
     26 const int MAP_HEIGHT = 20;
     27 
     28 int map[ MAP_WIDTH * MAP_HEIGHT ] = 
     29 {
     30 
     31 // 0001020304050607080910111213141516171819
     32 	1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,   // 00
     33 	1,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,1,   // 01
     34 	1,9,9,1,1,9,9,9,1,9,1,9,1,9,1,9,9,9,1,1,   // 02
     35 	1,9,9,1,1,9,9,9,1,9,1,9,1,9,1,9,9,9,1,1,   // 03
     36 	1,9,1,1,1,1,9,9,1,9,1,9,1,1,1,1,9,9,1,1,   // 04
     37 	1,9,1,1,9,1,1,1,1,9,1,1,1,1,9,1,1,1,1,1,   // 05
     38 	1,9,9,9,9,1,1,1,1,1,1,9,9,9,9,1,1,1,1,1,   // 06
     39 	1,9,9,9,9,9,9,9,9,1,1,1,9,9,9,9,9,9,9,1,   // 07
     40 	1,9,1,1,1,1,1,1,1,1,1,9,1,1,1,1,1,1,1,1,   // 08
     41 	1,9,1,9,9,9,9,9,9,9,1,1,9,9,9,9,9,9,9,1,   // 09
     42 	1,9,1,1,1,1,9,1,1,9,1,1,1,1,1,1,1,1,1,1,   // 10
     43 	1,9,9,9,9,9,1,9,1,9,1,9,9,9,9,9,1,1,1,1,   // 11
     44 	1,9,1,9,1,9,9,9,1,9,1,9,1,9,1,9,9,9,1,1,   // 12
     45 	1,9,1,9,1,9,9,9,1,9,1,9,1,9,1,9,9,9,1,1,   // 13
     46 	1,9,1,1,1,1,9,9,1,9,1,9,1,1,1,1,9,9,1,1,   // 14
     47 	1,9,1,1,9,1,1,1,1,9,1,1,1,1,9,1,1,1,1,1,   // 15
     48 	1,9,9,9,9,1,1,1,1,1,1,9,9,9,9,1,1,1,1,1,   // 16
     49 	1,1,9,9,9,9,9,9,9,1,1,1,9,9,9,1,9,9,9,9,   // 17
     50 	1,9,1,1,1,1,1,1,1,1,1,9,1,1,1,1,1,1,1,1,   // 18
     51 	1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,   // 19
     52 
     53 };
     54 
     55 // map helper functions
     56 
     57 int GetMap( int x, int y )
     58 {
     59 
     60 	if( x < 0 ||
     61 	    x >= MAP_WIDTH ||
     62 		 y < 0 ||
     63 		 y >= MAP_HEIGHT
     64 	  )
     65 	{
     66 		return 9;	 
     67 	}
     68 
     69 	return map[(y*MAP_WIDTH)+x];
     70 }
     71 
     72 
     73 
     74 // Definitions
     75 
     76 class MapSearchNode
     77 {
     78 public:
     79 	unsigned int x;	 // the (x,y) positions of the node
     80 	unsigned int y;	
     81 	
     82 	MapSearchNode() { x = y = 0; }
     83 	MapSearchNode( unsigned int px, unsigned int py ) { x=px; y=py; }
     84 
     85 	float GoalDistanceEstimate( MapSearchNode &nodeGoal );
     86 	bool IsGoal( MapSearchNode &nodeGoal );
     87 	bool GetSuccessors( AStarSearch<MapSearchNode> *astarsearch, MapSearchNode *parent_node );
     88 	float GetCost( MapSearchNode &successor );
     89 	bool IsSameState( MapSearchNode &rhs );
     90 
     91 	void PrintNodeInfo(); 
     92 
     93 
     94 };
     95 
     96 bool MapSearchNode::IsSameState( MapSearchNode &rhs )
     97 {
     98 
     99 	// same state in a maze search is simply when (x,y) are the same
    100 	if( (x == rhs.x) &&
    101 		(y == rhs.y) )
    102 	{
    103 		return true;
    104 	}
    105 	else
    106 	{
    107 		return false;
    108 	}
    109 
    110 }
    111 
    112 void MapSearchNode::PrintNodeInfo()
    113 {
    114 	cout << "Node position : (" << x << ", " << y << ")" << endl;
    115 }
    116 
    117 // Here's the heuristic function that estimates the distance from a Node
    118 // to the Goal. 
    119 
    120 float MapSearchNode::GoalDistanceEstimate( MapSearchNode &nodeGoal )
    121 {
    122 	float xd = fabs(float(((float)x - (float)nodeGoal.x)));
    123 	float yd = fabs(float(((float)y - (float)nodeGoal.y)));
    124 
    125 	return xd + yd;
    126 }
    127 
    128 bool MapSearchNode::IsGoal( MapSearchNode &nodeGoal )
    129 {
    130 
    131 	if( (x == nodeGoal.x) &&
    132 		(y == nodeGoal.y) )
    133 	{
    134 		return true;
    135 	}
    136 
    137 	return false;
    138 }
    139 
    140 // This generates the successors to the given Node. It uses a helper function called
    141 // AddSuccessor to give the successors to the AStar class. The A* specific initialisation
    142 // is done for each node internally, so here you just set the state information that
    143 // is specific to the application
    144 bool MapSearchNode::GetSuccessors( AStarSearch<MapSearchNode> *astarsearch, MapSearchNode *parent_node )
    145 {
    146 
    147 	int parent_x = -1; 
    148 	int parent_y = -1; 
    149 
    150 	if( parent_node )
    151 	{
    152 		parent_x = parent_node->x;
    153 		parent_y = parent_node->y;
    154 	}
    155 	
    156 
    157 	MapSearchNode NewNode;
    158 
    159 	// push each possible move except allowing the search to go backwards
    160 
    161 	if( (GetMap( x-1, y ) < 9) 
    162 		&& !((parent_x == x-1) && (parent_y == y))
    163 	  ) 
    164 	{
    165 		NewNode = MapSearchNode( x-1, y );
    166 		astarsearch->AddSuccessor( NewNode );
    167 	}	
    168 
    169 	if( (GetMap( x, y-1 ) < 9) 
    170 		&& !((parent_x == x) && (parent_y == y-1))
    171 	  ) 
    172 	{
    173 		NewNode = MapSearchNode( x, y-1 );
    174 		astarsearch->AddSuccessor( NewNode );
    175 	}	
    176 
    177 	if( (GetMap( x+1, y ) < 9)
    178 		&& !((parent_x == x+1) && (parent_y == y))
    179 	  ) 
    180 	{
    181 		NewNode = MapSearchNode( x+1, y );
    182 		astarsearch->AddSuccessor( NewNode );
    183 	}	
    184 
    185 		
    186 	if( (GetMap( x, y+1 ) < 9) 
    187 		&& !((parent_x == x) && (parent_y == y+1))
    188 		)
    189 	{
    190 		NewNode = MapSearchNode( x, y+1 );
    191 		astarsearch->AddSuccessor( NewNode );
    192 	}	
    193 
    194 	return true;
    195 }
    196 
    197 // given this node, what does it cost to move to successor. In the case
    198 // of our map the answer is the map terrain value at this node since that is 
    199 // conceptually where we're moving
    200 
    201 float MapSearchNode::GetCost( MapSearchNode &successor )
    202 {
    203 	return (float) GetMap( x, y );
    204 
    205 }
    206 
    207 
    208 // Main
    209 
    210 int main( int argc, char *argv[] )
    211 {
    212 
    213 	cout << "STL A* Search implementation\n(C)2001 Justin Heyes-Jones\n";
    214 
    215 	// Our sample problem defines the world as a 2d array representing a terrain
    216 	// Each element contains an integer from 0 to 5 which indicates the cost 
    217 	// of travel across the terrain. Zero means the least possible difficulty 
    218 	// in travelling (think ice rink if you can skate) whilst 5 represents the 
    219 	// most difficult. 9 indicates that we cannot pass.
    220 
    221 	// Create an instance of the search class...
    222 
    223 	AStarSearch<MapSearchNode> astarsearch;
    224 
    225 	unsigned int SearchCount = 0;
    226 
    227 	const unsigned int NumSearches = 1;
    228 
    229 	while(SearchCount < NumSearches)
    230 	{
    231 
    232 		// Create a start state
    233 		MapSearchNode nodeStart;
    234 		nodeStart.x = rand()%MAP_WIDTH;
    235 		nodeStart.y = rand()%MAP_HEIGHT; 
    236 
    237 		// Define the goal state
    238 		MapSearchNode nodeEnd;
    239 		nodeEnd.x = rand()%MAP_WIDTH;						
    240 		nodeEnd.y = rand()%MAP_HEIGHT; 
    241 		
    242 		// Set Start and goal states
    243 		
    244 		astarsearch.SetStartAndGoalStates( nodeStart, nodeEnd );
    245 
    246 		unsigned int SearchState;
    247 		unsigned int SearchSteps = 0;
    248 
    249 		do
    250 		{
    251 			SearchState = astarsearch.SearchStep();
    252 
    253 			SearchSteps++;
    254 
    255 	#if DEBUG_LISTS
    256 
    257 			cout << "Steps:" << SearchSteps << "\n";
    258 
    259 			int len = 0;
    260 
    261 			cout << "Open:\n";
    262 			MapSearchNode *p = astarsearch.GetOpenListStart();
    263 			while( p )
    264 			{
    265 				len++;
    266 	#if !DEBUG_LIST_LENGTHS_ONLY			
    267 				((MapSearchNode *)p)->PrintNodeInfo();
    268 	#endif
    269 				p = astarsearch.GetOpenListNext();
    270 				
    271 			}
    272 
    273 			cout << "Open list has " << len << " nodes\n";
    274 
    275 			len = 0;
    276 
    277 			cout << "Closed:\n";
    278 			p = astarsearch.GetClosedListStart();
    279 			while( p )
    280 			{
    281 				len++;
    282 	#if !DEBUG_LIST_LENGTHS_ONLY			
    283 				p->PrintNodeInfo();
    284 	#endif			
    285 				p = astarsearch.GetClosedListNext();
    286 			}
    287 
    288 			cout << "Closed list has " << len << " nodes\n";
    289 	#endif
    290 
    291 		}
    292 		while( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_SEARCHING );
    293 
    294 		if( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_SUCCEEDED )
    295 		{
    296 			cout << "Search found goal state\n";
    297 
    298 				MapSearchNode *node = astarsearch.GetSolutionStart();
    299 
    300 	#if DISPLAY_SOLUTION
    301 				cout << "Displaying solution\n";
    302 	#endif
    303 				int steps = 0;
    304 
    305 				node->PrintNodeInfo();
    306 				for( ;; )
    307 				{
    308 					node = astarsearch.GetSolutionNext();
    309 
    310 					if( !node )
    311 					{
    312 						break;
    313 					}
    314 
    315 					node->PrintNodeInfo();
    316 					steps ++;
    317 				
    318 				};
    319 
    320 				cout << "Solution steps " << steps << endl;
    321 
    322 				// Once you're done with the solution you can free the nodes up
    323 				astarsearch.FreeSolutionNodes();
    324 
    325 	
    326 		}
    327 		else if( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_FAILED ) 
    328 		{
    329 			cout << "Search terminated. Did not find goal state\n";
    330 		
    331 		}
    332 
    333 		// Display the number of loops the search went through
    334 		cout << "SearchSteps : " << SearchSteps << "\n";
    335 
    336 		SearchCount ++;
    337 
    338 		astarsearch.EnsureMemoryFreed();
    339 	}
    340 	
    341 	return 0;
    342 }
    343 
    344 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////