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 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////