From 1c1221f89d226c51932594bd0f82518c53ad5d25 Mon Sep 17 00:00:00 2001 From: Thinner123 <285509681@qq.com> Date: Sun, 3 Jul 2022 17:24:43 +0800 Subject: [PATCH] Astar --- src/Astar/8puzzle.cpp | 803 +++++++++++++++++++++++++++ src/Astar/findpath.cpp | 343 ++++++++++++ src/Astar/fsa.h | 252 +++++++++ src/Astar/min_path_to_Bucharest.cpp | 315 +++++++++++ src/Astar/stlastar.h | 833 ++++++++++++++++++++++++++++ 5 files changed, 2546 insertions(+) create mode 100644 src/Astar/8puzzle.cpp create mode 100644 src/Astar/findpath.cpp create mode 100644 src/Astar/fsa.h create mode 100644 src/Astar/min_path_to_Bucharest.cpp create mode 100644 src/Astar/stlastar.h diff --git a/src/Astar/8puzzle.cpp b/src/Astar/8puzzle.cpp new file mode 100644 index 0000000..48842ba --- /dev/null +++ b/src/Astar/8puzzle.cpp @@ -0,0 +1,803 @@ +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// STL A* Search implementation +// (C)2001 Justin Heyes-Jones +// +// This uses my A* code to solve the 8-puzzle + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +#include +#include +#include +#include + +#include + +using namespace std; + +// Configuration + +#define NUM_TIMES_TO_RUN_SEARCH 1 +#define DISPLAY_SOLUTION_FORWARDS 1 +#define DISPLAY_SOLUTION_BACKWARDS 0 +#define DISPLAY_SOLUTION_INFO 1 +#define DEBUG_LISTS 0 + +// AStar search class +#include "stlastar.h" // See header for copyright and usage information + +// Global data + +#define BOARD_WIDTH (3) +#define BOARD_HEIGHT (3) + +#define GM_TILE (-1) +#define GM_SPACE (0) +#define GM_OFF_BOARD (1) + +// Definitions + +// To use the search class you must define the following calls... + +// Data +// Your own state space information +// Functions +// (Optional) Constructor. +// Nodes are created by the user, so whether you use a +// constructor with parameters as below, or just set the object up after the +// constructor, is up to you. +// +// (Optional) Destructor. +// The destructor will be called if you create one. You +// can rely on the default constructor unless you dynamically allocate something in +// your data +// +// float GoalDistanceEstimate( PuzzleState &nodeGoal ); +// Return the estimated cost to goal from this node (pass reference to goal node) +// +// bool IsGoal( PuzzleState &nodeGoal ); +// Return true if this node is the goal. +// +// bool GetSuccessors( AStarSearch *astarsearch ); +// For each successor to this state call the AStarSearch's AddSuccessor call to +// add each one to the current search - return false if you are out of memory and the search +// will fail +// +// float GetCost( PuzzleState *successor ); +// Return the cost moving from this state to the state of successor +// +// bool IsSameState( PuzzleState &rhs ); +// Return true if the provided state is the same as this state + +// Here the example is the 8-puzzle state ... +class PuzzleState +{ + +public: + + // defs + + typedef enum + { + TL_SPACE, + TL_1, + TL_2, + TL_3, + TL_4, + TL_5, + TL_6, + TL_7, + TL_8 + + } TILE; + + // data + + static TILE g_goal[ BOARD_WIDTH*BOARD_HEIGHT]; + static TILE g_start[ BOARD_WIDTH*BOARD_HEIGHT]; + + // the tile data for the 8-puzzle + TILE tiles[ BOARD_WIDTH*BOARD_HEIGHT ]; + + // member functions + + PuzzleState() { + memcpy( tiles, g_goal, sizeof( TILE ) * BOARD_WIDTH * BOARD_HEIGHT ); + } + + PuzzleState( TILE *param_tiles ) + { + memcpy( tiles, param_tiles, sizeof( TILE ) * BOARD_WIDTH * BOARD_HEIGHT ); + } + + float GoalDistanceEstimate( PuzzleState &nodeGoal ); + bool IsGoal( PuzzleState &nodeGoal ); + bool GetSuccessors( AStarSearch *astarsearch, PuzzleState *parent_node ); + float GetCost( PuzzleState &successor ); + bool IsSameState( PuzzleState &rhs ); + + void PrintNodeInfo(); + +private: + // User stuff - Just add what you need to help you write the above functions... + + void GetSpacePosition( PuzzleState *pn, int *rx, int *ry ); + bool LegalMove( TILE *StartTiles, TILE *TargetTiles, int spx, int spy, int tx, int ty ); + int GetMap( int x, int y, TILE *tiles ); + + + + +}; + +// Goal state +PuzzleState::TILE PuzzleState::g_goal[] = +{ + TL_1, + TL_2, + TL_3, + TL_8, + TL_SPACE, + TL_4, + TL_7, + TL_6, + TL_5, +}; + +// Some nice Start states +PuzzleState::TILE PuzzleState::g_start[] = +{ + + // Three example start states from Bratko's Prolog Programming for Artificial Intelligence + +#if 1 + // ex a - 4 steps + TL_1 , + TL_3 , + TL_4 , + TL_8 , + TL_SPACE , + TL_2 , + TL_7 , + TL_6 , + TL_5 , + +#elif 0 + + // ex b - 5 steps + TL_2 , + TL_8 , + TL_3 , + TL_1 , + TL_6 , + TL_4 , + TL_7 , + TL_SPACE , + TL_5 , + +#elif 0 + + // ex c - 18 steps + TL_2 , + TL_1 , + TL_6 , + TL_4 , + TL_SPACE , + TL_8 , + TL_7 , + TL_5 , + TL_3 , + +#elif 0 + + // nasty one - doesn't solve + TL_6 , + TL_3 , + TL_SPACE , + TL_4 , + TL_8 , + TL_5 , + TL_7 , + TL_2 , + TL_1 , + +#elif 0 + + // sent by email - does work though + + TL_1 , TL_2 , TL_3 , + TL_4 , TL_5 , TL_6 , + TL_8 , TL_7 , TL_SPACE , + + +// from http://www.cs.utexas.edu/users/novak/asg-8p.html + +//Goal: Easy: Medium: Hard: Worst: + +//1 2 3 1 3 4 2 8 1 2 8 1 5 6 7 +//8 4 8 6 2 4 3 4 6 3 4 8 +//7 6 5 7 5 7 6 5 7 5 3 2 1 + + +#elif 0 + + // easy 5 + TL_1 , + TL_3 , + TL_4 , + + TL_8 , + TL_6 , + TL_2 , + + TL_7 , + TL_SPACE , + TL_5 , + + +#elif 0 + + // medium 9 + TL_2 , + TL_8 , + TL_1 , + + TL_SPACE , + TL_4 , + TL_3 , + + TL_7 , + TL_6 , + TL_5 , + +#elif 0 + + // hard 12 + TL_2 , + TL_8 , + TL_1 , + + TL_4 , + TL_6 , + TL_3 , + + TL_SPACE , + TL_7 , + TL_5 , + +#elif 0 + + // worst 30 + TL_5 , + TL_6 , + TL_7 , + + TL_4 , + TL_SPACE , + TL_8 , + + TL_3 , + TL_2 , + TL_1 , + +#elif 0 + + // 123 + // 784 + // 65 + + // two move simple board + TL_1 , + TL_2 , + TL_3 , + + TL_7 , + TL_8 , + TL_4 , + + TL_SPACE , + TL_6 , + TL_5 , + +#elif 0 + // a1 b2 c3 d4 e5 f6 g7 h8 + //C3,Blank,H8,A1,G8,F6,E5,D4,B2 + + TL_3 , + TL_SPACE , + TL_8 , + + TL_1 , + TL_8 , + TL_6 , + + TL_5 , + TL_4 , + TL_2 , + + +#endif + +}; + +bool PuzzleState::IsSameState( PuzzleState &rhs ) +{ + + for( int i=0; i<(BOARD_HEIGHT*BOARD_WIDTH); i++ ) + { + if( tiles[i] != rhs.tiles[i] ) + { + return false; + } + } + + return true; + +} + +void PuzzleState::PrintNodeInfo() +{ + char str[100]; + sprintf( str, "%c %c %c\n%c %c %c\n%c %c %c\n", + tiles[0] + '0', + tiles[1] + '0', + tiles[2] + '0', + tiles[3] + '0', + tiles[4] + '0', + tiles[5] + '0', + tiles[6] + '0', + tiles[7] + '0', + tiles[8] + '0' + ); + + cout << str; +} + +// Here's the heuristic function that estimates the distance from a PuzzleState +// to the Goal. + +float PuzzleState::GoalDistanceEstimate( PuzzleState &nodeGoal ) +{ + + // Nilsson's sequence score + + int i, cx, cy, ax, ay, h = 0, s, t; + + // given a tile this returns the tile that should be clockwise + TILE correct_follower_to[ BOARD_WIDTH * BOARD_HEIGHT ] = + { + TL_SPACE, // always wrong + TL_2, + TL_3, + TL_4, + TL_5, + TL_6, + TL_7, + TL_8, + TL_1, + }; + + // given a table index returns the index of the tile that is clockwise to it 3*3 only + int clockwise_tile_of[ BOARD_WIDTH * BOARD_HEIGHT ] = + { + 1, + 2, // 012 + 5, // 345 + 0, // 678 + -1, // never called with center square + 8, + 3, + 6, + 7 + }; + + int tile_x[ BOARD_WIDTH * BOARD_HEIGHT ] = + { + /* TL_SPACE */ 1, + /* TL_1 */ 0, + /* TL_2 */ 1, + /* TL_3 */ 2, + /* TL_4 */ 2, + /* TL_5 */ 2, + /* TL_6 */ 1, + /* TL_7 */ 0, + /* TL_8 */ 0, + }; + + int tile_y[ BOARD_WIDTH * BOARD_HEIGHT ] = + { + /* TL_SPACE */ 1, + /* TL_1 */ 0, + /* TL_2 */ 0, + /* TL_3 */ 0, + /* TL_4 */ 1, + /* TL_5 */ 2, + /* TL_6 */ 2, + /* TL_7 */ 2, + /* TL_8 */ 1, + }; + + s=0; + + // score 1 point if centre is not correct + if( tiles[(BOARD_HEIGHT*BOARD_WIDTH)/2] != nodeGoal.tiles[(BOARD_HEIGHT*BOARD_WIDTH)/2] ) + { + s = 1; + } + + for( i=0; i<(BOARD_HEIGHT*BOARD_WIDTH); i++ ) + { + // this loop adds up the totaldist element in h and + // the sequence score in s + + // the space does not count + if( tiles[i] == TL_SPACE ) + { + continue; + } + + // get correct x and y of this tile + cx = tile_x[tiles[i]]; + cy = tile_y[tiles[i]]; + + // get actual + ax = i % BOARD_WIDTH; + ay = i / BOARD_WIDTH; + + // add manhatten distance to h + h += abs( cx-ax ); + h += abs( cy-ay ); + + // no s score for center tile + if( (ax == (BOARD_WIDTH/2)) && (ay == (BOARD_HEIGHT/2)) ) + { + continue; + } + + // score 2 points if not followed by successor + if( correct_follower_to[ tiles[i] ] != tiles[ clockwise_tile_of[ i ] ] ) + { + s += 2; + } + + + } + + // mult by 3 and add to h + t = h + (3*s); + + return (float) t; + +} + +bool PuzzleState::IsGoal( PuzzleState &nodeGoal ) +{ + return IsSameState( nodeGoal ); +} + +// Helper +// Return the x and y position of the space tile +void PuzzleState::GetSpacePosition( PuzzleState *pn, int *rx, int *ry ) +{ + int x,y; + + for( y=0; ytiles[(y*BOARD_WIDTH)+x] == TL_SPACE ) + { + *rx = x; + *ry = y; + + return; + } + } + } + + assert( false && "Something went wrong. There's no space on the board" ); + +} + +int PuzzleState::GetMap( int x, int y, TILE *tiles ) +{ + + if( x < 0 || + x >= BOARD_WIDTH || + y < 0 || + y >= BOARD_HEIGHT + ) + return GM_OFF_BOARD; + + if( tiles[(y*BOARD_WIDTH)+x] == TL_SPACE ) + { + return GM_SPACE; + } + + return GM_TILE; +} + +// Given a node set of tiles and a set of tiles to move them into, do the move as if it was on a tile board +// note : returns false if the board wasn't changed, and simply returns the tiles as they were in the target +// spx and spy is the space position while tx and ty is the target move from position + +bool PuzzleState::LegalMove( TILE *StartTiles, TILE *TargetTiles, int spx, int spy, int tx, int ty ) +{ + + int t; + + if( GetMap( spx, spy, StartTiles ) == GM_SPACE ) + { + if( GetMap( tx, ty, StartTiles ) == GM_TILE ) + { + + // copy tiles + for( t=0; t<(BOARD_HEIGHT*BOARD_WIDTH); t++ ) + { + TargetTiles[t] = StartTiles[t]; + } + + + TargetTiles[ (ty*BOARD_WIDTH)+tx ] = StartTiles[ (spy*BOARD_WIDTH)+spx ]; + TargetTiles[ (spy*BOARD_WIDTH)+spx ] = StartTiles[ (ty*BOARD_WIDTH)+tx ]; + + return true; + } + } + + + return false; + +} + +// This generates the successors to the given PuzzleState. It uses a helper function called +// AddSuccessor to give the successors to the AStar class. The A* specific initialisation +// is done for each node internally, so here you just set the state information that +// is specific to the application +bool PuzzleState::GetSuccessors( AStarSearch *astarsearch, PuzzleState *parent_node ) +{ + PuzzleState NewNode; + + int sp_x,sp_y; + + GetSpacePosition( this, &sp_x, &sp_y ); + + bool ret; + + if( LegalMove( tiles, NewNode.tiles, sp_x, sp_y, sp_x, sp_y-1 ) == true ) + { + ret = astarsearch->AddSuccessor( NewNode ); + + if( !ret ) return false; + } + + if( LegalMove( tiles, NewNode.tiles, sp_x, sp_y, sp_x, sp_y+1 ) == true ) + { + ret = astarsearch->AddSuccessor( NewNode ); + + if( !ret ) return false; + } + + if( LegalMove( tiles, NewNode.tiles, sp_x, sp_y, sp_x-1, sp_y ) == true ) + { + ret = astarsearch->AddSuccessor( NewNode ); + + if( !ret ) return false; + } + + if( LegalMove( tiles, NewNode.tiles, sp_x, sp_y, sp_x+1, sp_y ) == true ) + { + ret = astarsearch->AddSuccessor( NewNode ); + + if( !ret ) return false; + } + + return true; +} + +// given this node, what does it cost to move to successor. In the case +// of our map the answer is the map terrain value at this node since that is +// conceptually where we're moving + +float PuzzleState::GetCost( PuzzleState &successor ) +{ + return 1.0f; // I love it when life is simple + +} + + +// Main + +int puzzle( int argc, char *argv[] ) +{ + + cout << "STL A* 8-puzzle solver implementation\n(C)2001 Justin Heyes-Jones\n"; + + if( argc > 1 ) + { + int i = 0; + int c; + + while( (c = argv[1][i]) ) + { + if( isdigit( c ) ) + { + int num = (c - '0'); + + PuzzleState::g_start[i] = static_cast(num); + + } + + i++; + } + + + } + + // Create an instance of the search class... + + AStarSearch astarsearch; + + int NumTimesToSearch = NUM_TIMES_TO_RUN_SEARCH; + + while( NumTimesToSearch-- ) + { + + // Create a start state + PuzzleState nodeStart( PuzzleState::g_start ); + + // Define the goal state + PuzzleState nodeEnd( PuzzleState::g_goal ); + + // Set Start and goal states + astarsearch.SetStartAndGoalStates( nodeStart, nodeEnd ); + + unsigned int SearchState; + + unsigned int SearchSteps = 0; + + do + { + SearchState = astarsearch.SearchStep(); + +#if DEBUG_LISTS + + float f,g,h; + + cout << "Search step " << SearchSteps << endl; + + cout << "Open:\n"; + PuzzleState *p = astarsearch.GetOpenListStart( f,g,h ); + while( p ) + { + ((PuzzleState *)p)->PrintNodeInfo(); + cout << "f: " << f << " g: " << g << " h: " << h << "\n\n"; + + p = astarsearch.GetOpenListNext( f,g,h ); + + } + + cout << "Closed:\n"; + p = astarsearch.GetClosedListStart( f,g,h ); + while( p ) + { + p->PrintNodeInfo(); + cout << "f: " << f << " g: " << g << " h: " << h << "\n\n"; + + p = astarsearch.GetClosedListNext( f,g,h ); + } + +#endif + +// Test cancel search +#if 0 + int StepCount = astarsearch.GetStepCount(); + if( StepCount == 10 ) + { + astarsearch.CancelSearch(); + } +#endif + SearchSteps++; + } + while( SearchState == AStarSearch::SEARCH_STATE_SEARCHING ); + + if( SearchState == AStarSearch::SEARCH_STATE_SUCCEEDED ) + { +#if DISPLAY_SOLUTION_FORWARDS + cout << "Search found goal state\n"; +#endif + PuzzleState *node = astarsearch.GetSolutionStart(); + +#if DISPLAY_SOLUTION_FORWARDS + cout << "Displaying solution\n"; +#endif + int steps = 0; + +#if DISPLAY_SOLUTION_FORWARDS + node->PrintNodeInfo(); + cout << endl; +#endif + for( ;; ) + { + node = astarsearch.GetSolutionNext(); + + if( !node ) + { + break; + } + +#if DISPLAY_SOLUTION_FORWARDS + node->PrintNodeInfo(); + cout << endl; +#endif + steps ++; + + }; + +#if DISPLAY_SOLUTION_FORWARDS + // todo move step count into main algorithm + cout << "Solution steps " << steps << endl; +#endif + +//////////// + + node = astarsearch.GetSolutionEnd(); + +#if DISPLAY_SOLUTION_BACKWARDS + cout << "Displaying reverse solution\n"; +#endif + steps = 0; + + node->PrintNodeInfo(); + cout << endl; + for( ;; ) + { + node = astarsearch.GetSolutionPrev(); + + if( !node ) + { + break; + } +#if DISPLAY_SOLUTION_BACKWARDS + node->PrintNodeInfo(); + cout << endl; +#endif + steps ++; + + }; + +#if DISPLAY_SOLUTION_BACKWARDS + cout << "Solution steps " << steps << endl; +#endif + +////////////// + + // Once you're done with the solution you can free the nodes up + astarsearch.FreeSolutionNodes(); + + } + else if( SearchState == AStarSearch::SEARCH_STATE_FAILED ) + { +#if DISPLAY_SOLUTION_INFO + cout << "Search terminated. Did not find goal state\n"; +#endif + } + else if( SearchState == AStarSearch::SEARCH_STATE_OUT_OF_MEMORY ) + { +#if DISPLAY_SOLUTION_INFO + cout << "Search terminated. Out of memory\n"; +#endif + } + + + + // Display the number of loops the search went through +#if DISPLAY_SOLUTION_INFO + cout << "SearchSteps : " << astarsearch.GetStepCount() << endl; +#endif + } + + return 0; +} + + diff --git a/src/Astar/findpath.cpp b/src/Astar/findpath.cpp new file mode 100644 index 0000000..01c7390 --- /dev/null +++ b/src/Astar/findpath.cpp @@ -0,0 +1,343 @@ +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +// STL A* Search implementation +// (C)2001 Justin Heyes-Jones +// +// Finding a path on a simple grid maze +// This shows how to do shortest path finding using A* + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +#include "stlastar.h" // See header for copyright and usage information + +#include +#include +#include + +#define DEBUG_LISTS 0 +#define DEBUG_LIST_LENGTHS_ONLY 0 + +using namespace std; + +// Global data + +// The world map + +const int MAP_WIDTH = 20; +const int MAP_HEIGHT = 20; + +int world_map[ MAP_WIDTH * MAP_HEIGHT ] = +{ + +// 0001020304050607080910111213141516171819 + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, // 00 + 1,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,1, // 01 + 1,9,9,1,1,9,9,9,1,9,1,9,1,9,1,9,9,9,1,1, // 02 + 1,9,9,1,1,9,9,9,1,9,1,9,1,9,1,9,9,9,1,1, // 03 + 1,9,1,1,1,1,9,9,1,9,1,9,1,1,1,1,9,9,1,1, // 04 + 1,9,1,1,9,1,1,1,1,9,1,1,1,1,9,1,1,1,1,1, // 05 + 1,9,9,9,9,1,1,1,1,1,1,9,9,9,9,1,1,1,1,1, // 06 + 1,9,9,9,9,9,9,9,9,1,1,1,9,9,9,9,9,9,9,1, // 07 + 1,9,1,1,1,1,1,1,1,1,1,9,1,1,1,1,1,1,1,1, // 08 + 1,9,1,9,9,9,9,9,9,9,1,1,9,9,9,9,9,9,9,1, // 09 + 1,9,1,1,1,1,9,1,1,9,1,1,1,1,1,1,1,1,1,1, // 10 + 1,9,9,9,9,9,1,9,1,9,1,9,9,9,9,9,1,1,1,1, // 11 + 1,9,1,9,1,9,9,9,1,9,1,9,1,9,1,9,9,9,1,1, // 12 + 1,9,1,9,1,9,9,9,1,9,1,9,1,9,1,9,9,9,1,1, // 13 + 1,9,1,1,1,1,9,9,1,9,1,9,1,1,1,1,9,9,1,1, // 14 + 1,9,1,1,9,1,1,1,1,9,1,1,1,1,9,1,1,1,1,1, // 15 + 1,9,9,9,9,1,1,1,1,1,1,9,9,9,9,1,1,1,1,1, // 16 + 1,1,9,9,9,9,9,9,9,1,1,1,9,9,9,1,9,9,9,9, // 17 + 1,9,1,1,1,1,1,1,1,1,1,9,1,1,1,1,1,1,1,1, // 18 + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, // 19 + +}; + +// map helper functions + +int GetMap( int x, int y ) +{ + if( x < 0 || + x >= MAP_WIDTH || + y < 0 || + y >= MAP_HEIGHT + ) + { + return 9; + } + + return world_map[(y*MAP_WIDTH)+x]; +} + + + +// Definitions + +class MapSearchNode +{ +public: + int x; // the (x,y) positions of the node + int y; + + MapSearchNode() { x = y = 0; } + MapSearchNode( int px, int py ) { x=px; y=py; } + + float GoalDistanceEstimate( MapSearchNode &nodeGoal ); + bool IsGoal( MapSearchNode &nodeGoal ); + bool GetSuccessors( AStarSearch *astarsearch, MapSearchNode *parent_node ); + float GetCost( MapSearchNode &successor ); + bool IsSameState( MapSearchNode &rhs ); + + void PrintNodeInfo(); + + +}; + +bool MapSearchNode::IsSameState( MapSearchNode &rhs ) +{ + + // same state in a maze search is simply when (x,y) are the same + if( (x == rhs.x) && + (y == rhs.y) ) + { + return true; + } + else + { + return false; + } + +} + +void MapSearchNode::PrintNodeInfo() +{ + char str[100]; + sprintf( str, "Node position : (%d,%d)\n", x,y ); + + cout << str; +} + +// Here's the heuristic function that estimates the distance from a Node +// to the Goal. + +float MapSearchNode::GoalDistanceEstimate( MapSearchNode &nodeGoal ) +{ + return abs(x - nodeGoal.x) + abs(y - nodeGoal.y); +} + +bool MapSearchNode::IsGoal( MapSearchNode &nodeGoal ) +{ + + if( (x == nodeGoal.x) && + (y == nodeGoal.y) ) + { + return true; + } + + return false; +} + +// This generates the successors to the given Node. It uses a helper function called +// AddSuccessor to give the successors to the AStar class. The A* specific initialisation +// is done for each node internally, so here you just set the state information that +// is specific to the application +bool MapSearchNode::GetSuccessors( AStarSearch *astarsearch, MapSearchNode *parent_node ) +{ + + int parent_x = -1; + int parent_y = -1; + + if( parent_node ) + { + parent_x = parent_node->x; + parent_y = parent_node->y; + } + + + MapSearchNode NewNode; + + // push each possible move except allowing the search to go backwards + + if( (GetMap( x-1, y ) < 9) + && !((parent_x == x-1) && (parent_y == y)) + ) + { + NewNode = MapSearchNode( x-1, y ); + astarsearch->AddSuccessor( NewNode ); + } + + if( (GetMap( x, y-1 ) < 9) + && !((parent_x == x) && (parent_y == y-1)) + ) + { + NewNode = MapSearchNode( x, y-1 ); + astarsearch->AddSuccessor( NewNode ); + } + + if( (GetMap( x+1, y ) < 9) + && !((parent_x == x+1) && (parent_y == y)) + ) + { + NewNode = MapSearchNode( x+1, y ); + astarsearch->AddSuccessor( NewNode ); + } + + + if( (GetMap( x, y+1 ) < 9) + && !((parent_x == x) && (parent_y == y+1)) + ) + { + NewNode = MapSearchNode( x, y+1 ); + astarsearch->AddSuccessor( NewNode ); + } + + return true; +} + +// given this node, what does it cost to move to successor. In the case +// of our map the answer is the map terrain value at this node since that is +// conceptually where we're moving + +float MapSearchNode::GetCost( MapSearchNode &successor ) +{ + return (float) GetMap( x, y ); +} + + +// Main + +int findpath( int argc, char *argv[] ) +{ + + cout << "STL A* Search implementation\n(C)2001 Justin Heyes-Jones\n"; + + // Our sample problem defines the world as a 2d array representing a terrain + // Each element contains an integer from 0 to 5 which indicates the cost + // of travel across the terrain. Zero means the least possible difficulty + // in travelling (think ice rink if you can skate) whilst 5 represents the + // most difficult. 9 indicates that we cannot pass. + + // Create an instance of the search class... + + AStarSearch astarsearch; + + unsigned int SearchCount = 0; + + const unsigned int NumSearches = 1; + + while(SearchCount < NumSearches) + { + + // Create a start state + MapSearchNode nodeStart; + nodeStart.x = rand()%MAP_WIDTH; + nodeStart.y = rand()%MAP_HEIGHT; + + // Define the goal state + MapSearchNode nodeEnd; + nodeEnd.x = rand()%MAP_WIDTH; + nodeEnd.y = rand()%MAP_HEIGHT; + + // Set Start and goal states + + astarsearch.SetStartAndGoalStates( nodeStart, nodeEnd ); + + unsigned int SearchState; + unsigned int SearchSteps = 0; + + do + { + SearchState = astarsearch.SearchStep(); + + SearchSteps++; + + #if DEBUG_LISTS + + cout << "Steps:" << SearchSteps << "\n"; + + int len = 0; + + cout << "Open:\n"; + MapSearchNode *p = astarsearch.GetOpenListStart(); + while( p ) + { + len++; + #if !DEBUG_LIST_LENGTHS_ONLY + ((MapSearchNode *)p)->PrintNodeInfo(); + #endif + p = astarsearch.GetOpenListNext(); + + } + + cout << "Open list has " << len << " nodes\n"; + + len = 0; + + cout << "Closed:\n"; + p = astarsearch.GetClosedListStart(); + while( p ) + { + len++; + #if !DEBUG_LIST_LENGTHS_ONLY + p->PrintNodeInfo(); + #endif + p = astarsearch.GetClosedListNext(); + } + + cout << "Closed list has " << len << " nodes\n"; + #endif + + } + while( SearchState == AStarSearch::SEARCH_STATE_SEARCHING ); + + if( SearchState == AStarSearch::SEARCH_STATE_SUCCEEDED ) + { + cout << "Search found goal state\n"; + + MapSearchNode *node = astarsearch.GetSolutionStart(); + + #if DISPLAY_SOLUTION + cout << "Displaying solution\n"; + #endif + int steps = 0; + + node->PrintNodeInfo(); + for( ;; ) + { + node = astarsearch.GetSolutionNext(); + + if( !node ) + { + break; + } + + node->PrintNodeInfo(); + steps ++; + + }; + + cout << "Solution steps " << steps << endl; + + // Once you're done with the solution you can free the nodes up + astarsearch.FreeSolutionNodes(); + + + } + else if( SearchState == AStarSearch::SEARCH_STATE_FAILED ) + { + cout << "Search terminated. Did not find goal state\n"; + + } + + // Display the number of loops the search went through + cout << "SearchSteps : " << SearchSteps << "\n"; + + SearchCount ++; + + astarsearch.EnsureMemoryFreed(); + } + + return 0; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/src/Astar/fsa.h b/src/Astar/fsa.h new file mode 100644 index 0000000..2efca15 --- /dev/null +++ b/src/Astar/fsa.h @@ -0,0 +1,252 @@ +/* + +A* Algorithm Implementation using STL is +Copyright (C)2001-2005 Justin Heyes-Jones + +Permission is given by the author to freely redistribute and +include this code in any program as long as this credit is +given where due. + + COVERED CODE IS PROVIDED UNDER THIS LICENSE ON AN "AS IS" BASIS, + WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, + INCLUDING, WITHOUT LIMITATION, WARRANTIES THAT THE COVERED CODE + IS FREE OF DEFECTS, MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE + OR NON-INFRINGING. THE ENTIRE RISK AS TO THE QUALITY AND + PERFORMANCE OF THE COVERED CODE IS WITH YOU. SHOULD ANY COVERED + CODE PROVE DEFECTIVE IN ANY RESPECT, YOU (NOT THE INITIAL + DEVELOPER OR ANY OTHER CONTRIBUTOR) ASSUME THE COST OF ANY + NECESSARY SERVICING, REPAIR OR CORRECTION. THIS DISCLAIMER OF + WARRANTY CONSTITUTES AN ESSENTIAL PART OF THIS LICENSE. NO USE + OF ANY COVERED CODE IS AUTHORIZED HEREUNDER EXCEPT UNDER + THIS DISCLAIMER. + + Use at your own risk! + + + + FixedSizeAllocator class + Copyright 2001 Justin Heyes-Jones + + This class is a constant time O(1) memory manager for objects of + a specified type. The type is specified using a template class. + + Memory is allocated from a fixed size buffer which you can specify in the + class constructor or use the default. + + Using GetFirst and GetNext it is possible to iterate through the elements + one by one, and this would be the most common use for the class. + + I would suggest using this class when you want O(1) add and delete + and you don't do much searching, which would be O(n). Structures such as binary + trees can be used instead to get O(logn) access time. + +*/ + +#ifndef FSA_H +#define FSA_H + +#include +#include + +template class FixedSizeAllocator +{ + +public: + // Constants + enum + { + FSA_DEFAULT_SIZE = 100 + }; + + // This class enables us to transparently manage the extra data + // needed to enable the user class to form part of the double-linked + // list class + struct FSA_ELEMENT + { + USER_TYPE UserType; + + FSA_ELEMENT *pPrev; + FSA_ELEMENT *pNext; + }; + +public: // methods + FixedSizeAllocator( unsigned int MaxElements = FSA_DEFAULT_SIZE ) : + m_pFirstUsed( NULL ), + m_MaxElements( MaxElements ) + { + // Allocate enough memory for the maximum number of elements + + char *pMem = new char[ m_MaxElements * sizeof(FSA_ELEMENT) ]; + + m_pMemory = (FSA_ELEMENT *) pMem; + + // Set the free list first pointer + m_pFirstFree = m_pMemory; + + // Clear the memory + memset( m_pMemory, 0, sizeof( FSA_ELEMENT ) * m_MaxElements ); + + // Point at first element + FSA_ELEMENT *pElement = m_pFirstFree; + + // Set the double linked free list + for( unsigned int i=0; ipPrev = pElement-1; + pElement->pNext = pElement+1; + + pElement++; + } + + // first element should have a null prev + m_pFirstFree->pPrev = NULL; + // last element should have a null next + (pElement-1)->pNext = NULL; + + } + + + ~FixedSizeAllocator() + { + // Free up the memory + delete [] (char *) m_pMemory; + } + + // Allocate a new USER_TYPE and return a pointer to it + USER_TYPE *alloc() + { + + FSA_ELEMENT *pNewNode = NULL; + + if( !m_pFirstFree ) + { + return NULL; + } + else + { + pNewNode = m_pFirstFree; + m_pFirstFree = pNewNode->pNext; + + // if the new node points to another free node then + // change that nodes prev free pointer... + if( pNewNode->pNext ) + { + pNewNode->pNext->pPrev = NULL; + } + + // node is now on the used list + + pNewNode->pPrev = NULL; // the allocated node is always first in the list + + if( m_pFirstUsed == NULL ) + { + pNewNode->pNext = NULL; // no other nodes + } + else + { + m_pFirstUsed->pPrev = pNewNode; // insert this at the head of the used list + pNewNode->pNext = m_pFirstUsed; + } + + m_pFirstUsed = pNewNode; + } + + return reinterpret_cast(pNewNode); + } + + // Free the given user type + // For efficiency I don't check whether the user_data is a valid + // pointer that was allocated. I may add some debug only checking + // (To add the debug check you'd need to make sure the pointer is in + // the m_pMemory area and is pointing at the start of a node) + void free( USER_TYPE *user_data ) + { + FSA_ELEMENT *pNode = reinterpret_cast(user_data); + + // manage used list, remove this node from it + if( pNode->pPrev ) + { + pNode->pPrev->pNext = pNode->pNext; + } + else + { + // this handles the case that we delete the first node in the used list + m_pFirstUsed = pNode->pNext; + } + + if( pNode->pNext ) + { + pNode->pNext->pPrev = pNode->pPrev; + } + + // add to free list + if( m_pFirstFree == NULL ) + { + // free list was empty + m_pFirstFree = pNode; + pNode->pPrev = NULL; + pNode->pNext = NULL; + } + else + { + // Add this node at the start of the free list + m_pFirstFree->pPrev = pNode; + pNode->pNext = m_pFirstFree; + m_pFirstFree = pNode; + } + + } + + // For debugging this displays both lists (using the prev/next list pointers) + void Debug() + { + printf( "free list " ); + + FSA_ELEMENT *p = m_pFirstFree; + while( p ) + { + printf( "%x!%x ", p->pPrev, p->pNext ); + p = p->pNext; + } + printf( "\n" ); + + printf( "used list " ); + + p = m_pFirstUsed; + while( p ) + { + printf( "%x!%x ", p->pPrev, p->pNext ); + p = p->pNext; + } + printf( "\n" ); + } + + // Iterators + + USER_TYPE *GetFirst() + { + return reinterpret_cast(m_pFirstUsed); + } + + USER_TYPE *GetNext( USER_TYPE *node ) + { + return reinterpret_cast + ( + (reinterpret_cast(node))->pNext + ); + } + +public: // data + +private: // methods + +private: // data + + FSA_ELEMENT *m_pFirstFree; + FSA_ELEMENT *m_pFirstUsed; + unsigned int m_MaxElements; + FSA_ELEMENT *m_pMemory; + +}; + +#endif // defined FSA_H diff --git a/src/Astar/min_path_to_Bucharest.cpp b/src/Astar/min_path_to_Bucharest.cpp new file mode 100644 index 0000000..6423a9b --- /dev/null +++ b/src/Astar/min_path_to_Bucharest.cpp @@ -0,0 +1,315 @@ +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// This example code illustrate how to use STL A* Search implementation to find the minimum path between two +// cities given a map. The example is taken from the book AI: A Modern Approach, 3rd Ed., by Russel, where a map +// of Romania is given. The target node is Bucharest, and the user can specify the initial city from which the +// search algorithm will start looking for the minimum path to Bucharest. +// +// Usage: min_path_to_Bucharest +// Example: +// min_path_to_Bucharest Arad +// +// Thanks to Rasoul Mojtahedzadeh for this contribution +// Mojtahedzadeh _atsign_ gmail com +// +// Please note that this exercise is academic in nature and that the distances between the cities may not be +// correct compared to the latitude and longnitude differences in real life. Thanks to parthi2929 for noticing +// this issue. In a real application you would use some kind of exact x,y position of each city in order to get +// accurate heuristics. In fact, that is part of the point of this example, in the book you will see Norvig +// mention that the algorithm does some backtracking because the heuristic is not accurate (yet still admissable). +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// +#include "stlastar.h" +#include +#include +#include +#include + +#define DEBUG_LISTS 0 +#define DEBUG_LIST_LENGTHS_ONLY 0 + +using namespace std; + +const int MAX_CITIES = 20; + +enum ENUM_CITIES{Arad=0, Bucharest, Craiova, Drobeta, Eforie, Fagaras, Giurgiu, Hirsova, Iasi, Lugoj, Mehadia, Neamt, Oradea, Pitesti, RimnicuVilcea, Sibiu, Timisoara, Urziceni, Vaslui, Zerind}; +vector CityNames(MAX_CITIES); +float RomaniaMap[MAX_CITIES][MAX_CITIES]; + +class PathSearchNode +{ +public: + + ENUM_CITIES city; + + PathSearchNode() { city = Arad; } + PathSearchNode( ENUM_CITIES in ) { city = in; } + + float GoalDistanceEstimate( PathSearchNode &nodeGoal ); + bool IsGoal( PathSearchNode &nodeGoal ); + bool GetSuccessors( AStarSearch *astarsearch, PathSearchNode *parent_node ); + float GetCost( PathSearchNode &successor ); + bool IsSameState( PathSearchNode &rhs ); + + void PrintNodeInfo(); +}; + +// check if "this" node is the same as "rhs" node +bool PathSearchNode::IsSameState( PathSearchNode &rhs ) +{ + if(city == rhs.city) return(true); + return(false); +} + +// Euclidean distance between "this" node city and Bucharest +// Note: Numbers are taken from the book +float PathSearchNode::GoalDistanceEstimate( PathSearchNode &nodeGoal ) +{ + // goal is always Bucharest! + switch(city) + { + case Arad: return 366; break; + case Bucharest: return 0; break; + case Craiova: return 160; break; + case Drobeta: return 242; break; + case Eforie: return 161; break; + case Fagaras: return 176; break; + case Giurgiu: return 77; break; + case Hirsova: return 151; break; + case Iasi: return 226; break; + case Lugoj: return 244; break; + case Mehadia: return 241; break; + case Neamt: return 234; break; + case Oradea: return 380; break; + case Pitesti: return 100; break; + case RimnicuVilcea: return 193; break; + case Sibiu: return 253; break; + case Timisoara: return 329; break; + case Urziceni: return 80; break; + case Vaslui: return 199; break; + case Zerind: return 374; break; + } + cerr << "ASSERT: city = " << CityNames[city] << endl; + return 0; +} + +// check if "this" node is the goal node +bool PathSearchNode::IsGoal( PathSearchNode &nodeGoal ) +{ + if( city == Bucharest ) return(true); + return(false); +} + +// generates the successor nodes of "this" node +bool PathSearchNode::GetSuccessors( AStarSearch *astarsearch, PathSearchNode *parent_node ) +{ + PathSearchNode NewNode; + for(int c=0; cAddSuccessor( NewNode ); + } + return true; +} + +// the cost of going from "this" node to the "successor" node +float PathSearchNode::GetCost( PathSearchNode &successor ) +{ + return RomaniaMap[city][successor.city]; +} + +// prints out information about the node +void PathSearchNode::PrintNodeInfo() +{ + cout << " " << CityNames[city] << "\n"; +} + +// Main +int min_path_to_Bucharest( int argc, char *argv[] ) +{ + // creating map of Romania + for(int i=0; i astarsearch; + + unsigned int SearchCount = 0; + const unsigned int NumSearches = 1; + + while(SearchCount < NumSearches) + { + // Create a start state + PathSearchNode nodeStart; + nodeStart.city = initCity; + + // Define the goal state, always Bucharest! + PathSearchNode nodeEnd; + nodeEnd.city = Bucharest; + + // Set Start and goal states + astarsearch.SetStartAndGoalStates( nodeStart, nodeEnd ); + + unsigned int SearchState; + unsigned int SearchSteps = 0; + + do + { + SearchState = astarsearch.SearchStep(); + SearchSteps++; + + #if DEBUG_LISTS + + cout << "Steps:" << SearchSteps << "\n"; + + int len = 0; + + cout << "Open:\n"; + PathSearchNode *p = astarsearch.GetOpenListStart(); + while( p ) + { + len++; + #if !DEBUG_LIST_LENGTHS_ONLY + ((PathSearchNode *)p)->PrintNodeInfo(); + #endif + p = astarsearch.GetOpenListNext(); + + } + cout << "Open list has " << len << " nodes\n"; + + len = 0; + + cout << "Closed:\n"; + p = astarsearch.GetClosedListStart(); + while( p ) + { + len++; + #if !DEBUG_LIST_LENGTHS_ONLY + p->PrintNodeInfo(); + #endif + p = astarsearch.GetClosedListNext(); + } + + cout << "Closed list has " << len << " nodes\n"; + #endif + + } + while( SearchState == AStarSearch::SEARCH_STATE_SEARCHING ); + + if( SearchState == AStarSearch::SEARCH_STATE_SUCCEEDED ) + { + cout << "Search found the goal state\n"; + PathSearchNode *node = astarsearch.GetSolutionStart(); + cout << "Displaying solution\n"; + int steps = 0; + node->PrintNodeInfo(); + for( ;; ) + { + node = astarsearch.GetSolutionNext(); + if( !node ) break; + node->PrintNodeInfo(); + steps ++; + }; + cout << "Solution steps " << steps << endl; + // Once you're done with the solution you can free the nodes up + astarsearch.FreeSolutionNodes(); + } + else if( SearchState == AStarSearch::SEARCH_STATE_FAILED ) + { + cout << "Search terminated. Did not find goal state\n"; + } + // Display the number of loops the search went through + cout << "SearchSteps : " << SearchSteps << "\n"; + SearchCount ++; + astarsearch.EnsureMemoryFreed(); + } + + return 0; +} diff --git a/src/Astar/stlastar.h b/src/Astar/stlastar.h new file mode 100644 index 0000000..4fe9d77 --- /dev/null +++ b/src/Astar/stlastar.h @@ -0,0 +1,833 @@ +/* +A* Algorithm Implementation using STL is +Copyright (C)2001-2005 Justin Heyes-Jones + +Permission is given by the author to freely redistribute and +include this code in any program as long as this credit is +given where due. + + COVERED CODE IS PROVIDED UNDER THIS LICENSE ON AN "AS IS" BASIS, + WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, + INCLUDING, WITHOUT LIMITATION, WARRANTIES THAT THE COVERED CODE + IS FREE OF DEFECTS, MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE + OR NON-INFRINGING. THE ENTIRE RISK AS TO THE QUALITY AND + PERFORMANCE OF THE COVERED CODE IS WITH YOU. SHOULD ANY COVERED + CODE PROVE DEFECTIVE IN ANY RESPECT, YOU (NOT THE INITIAL + DEVELOPER OR ANY OTHER CONTRIBUTOR) ASSUME THE COST OF ANY + NECESSARY SERVICING, REPAIR OR CORRECTION. THIS DISCLAIMER OF + WARRANTY CONSTITUTES AN ESSENTIAL PART OF THIS LICENSE. NO USE + OF ANY COVERED CODE IS AUTHORIZED HEREUNDER EXCEPT UNDER + THIS DISCLAIMER. + + Use at your own risk! + +*/ + +#ifndef STLASTAR_H +#define STLASTAR_H +// used for text debugging +#include +#include +//#include +#include + +// stl includes +#include +#include +#include +#include + +using namespace std; + +// fast fixed size memory allocator, used for fast node memory management +#include "fsa.h" + +// Fixed size memory allocator can be disabled to compare performance +// Uses std new and delete instead if you turn it off +#define USE_FSA_MEMORY 1 + +// disable warning that debugging information has lines that are truncated +// occurs in stl headers +#if defined(WIN32) && defined(_WINDOWS) +#pragma warning( disable : 4786 ) +#endif + +template class AStarState; + +// The AStar search class. UserState is the users state space type +template class AStarSearch +{ + +public: // data + + enum + { + SEARCH_STATE_NOT_INITIALISED, + SEARCH_STATE_SEARCHING, + SEARCH_STATE_SUCCEEDED, + SEARCH_STATE_FAILED, + SEARCH_STATE_OUT_OF_MEMORY, + SEARCH_STATE_INVALID + }; + + + // A node represents a possible state in the search + // The user provided state type is included inside this type + + public: + + class Node + { + public: + + Node *parent; // used during the search to record the parent of successor nodes + Node *child; // used after the search for the application to view the search in reverse + + float g; // cost of this node + it's predecessors + float h; // heuristic estimate of distance to goal + float f; // sum of cumulative cost of predecessors and self and heuristic + + Node() : + parent( 0 ), + child( 0 ), + g( 0.0f ), + h( 0.0f ), + f( 0.0f ) + { + } + + UserState m_UserState; + }; + + + // For sorting the heap the STL needs compare function that lets us compare + // the f value of two nodes + + class HeapCompare_f + { + public: + + bool operator() ( const Node *x, const Node *y ) const + { + return x->f > y->f; + } + }; + + +public: // methods + + + // constructor just initialises private data + AStarSearch() : + m_State( SEARCH_STATE_NOT_INITIALISED ), + m_CurrentSolutionNode( NULL ), +#if USE_FSA_MEMORY + m_FixedSizeAllocator( 1000 ), +#endif + m_AllocateNodeCount(0), + m_CancelRequest( false ) + { + } + + AStarSearch( int MaxNodes ) : + m_State( SEARCH_STATE_NOT_INITIALISED ), + m_CurrentSolutionNode( NULL ), +#if USE_FSA_MEMORY + m_FixedSizeAllocator( MaxNodes ), +#endif + m_AllocateNodeCount(0), + m_CancelRequest( false ) + { + } + + // call at any time to cancel the search and free up all the memory + void CancelSearch() + { + m_CancelRequest = true; + } + + // Set Start and goal states + void SetStartAndGoalStates( UserState &Start, UserState &Goal ) + { + m_CancelRequest = false; + + m_Start = AllocateNode(); + m_Goal = AllocateNode(); + + assert((m_Start != NULL && m_Goal != NULL)); + + m_Start->m_UserState = Start; + m_Goal->m_UserState = Goal; + + m_State = SEARCH_STATE_SEARCHING; + + // Initialise the AStar specific parts of the Start Node + // The user only needs fill out the state information + + m_Start->g = 0; + m_Start->h = m_Start->m_UserState.GoalDistanceEstimate( m_Goal->m_UserState ); + m_Start->f = m_Start->g + m_Start->h; + m_Start->parent = 0; + + // Push the start node on the Open list + + m_OpenList.push_back( m_Start ); // heap now unsorted + + // Sort back element into heap + push_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() ); + + // Initialise counter for search steps + m_Steps = 0; + } + + // Advances search one step + unsigned int SearchStep() + { + // Firstly break if the user has not initialised the search + assert( (m_State > SEARCH_STATE_NOT_INITIALISED) && + (m_State < SEARCH_STATE_INVALID) ); + + // Next I want it to be safe to do a searchstep once the search has succeeded... + if( (m_State == SEARCH_STATE_SUCCEEDED) || + (m_State == SEARCH_STATE_FAILED) + ) + { + return m_State; + } + + // Failure is defined as emptying the open list as there is nothing left to + // search... + // New: Allow user abort + if( m_OpenList.empty() || m_CancelRequest ) + { + FreeAllNodes(); + m_State = SEARCH_STATE_FAILED; + return m_State; + } + + // Incremement step count + m_Steps ++; + + // Pop the best node (the one with the lowest f) + Node *n = m_OpenList.front(); // get pointer to the node + pop_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() ); + m_OpenList.pop_back(); + + // Check for the goal, once we pop that we're done + if( n->m_UserState.IsGoal( m_Goal->m_UserState ) ) + { + // The user is going to use the Goal Node he passed in + // so copy the parent pointer of n + m_Goal->parent = n->parent; + m_Goal->g = n->g; + + // A special case is that the goal was passed in as the start state + // so handle that here + if( false == n->m_UserState.IsSameState( m_Start->m_UserState ) ) + { + FreeNode( n ); + + // set the child pointers in each node (except Goal which has no child) + Node *nodeChild = m_Goal; + Node *nodeParent = m_Goal->parent; + + do + { + nodeParent->child = nodeChild; + + nodeChild = nodeParent; + nodeParent = nodeParent->parent; + + } + while( nodeChild != m_Start ); // Start is always the first node by definition + + } + + // delete nodes that aren't needed for the solution + FreeUnusedNodes(); + + m_State = SEARCH_STATE_SUCCEEDED; + + return m_State; + } + else // not goal + { + + // We now need to generate the successors of this node + // The user helps us to do this, and we keep the new nodes in + // m_Successors ... + + m_Successors.clear(); // empty vector of successor nodes to n + + // User provides this functions and uses AddSuccessor to add each successor of + // node 'n' to m_Successors + bool ret = n->m_UserState.GetSuccessors( this, n->parent ? &n->parent->m_UserState : NULL ); + + if( !ret ) + { + + typename vector< Node * >::iterator successor; + + // free the nodes that may previously have been added + for( successor = m_Successors.begin(); successor != m_Successors.end(); successor ++ ) + { + FreeNode( (*successor) ); + } + + m_Successors.clear(); // empty vector of successor nodes to n + + // free up everything else we allocated + FreeNode( (n) ); + FreeAllNodes(); + + m_State = SEARCH_STATE_OUT_OF_MEMORY; + return m_State; + } + + // Now handle each successor to the current node ... + for( typename vector< Node * >::iterator successor = m_Successors.begin(); successor != m_Successors.end(); successor ++ ) + { + + // The g value for this successor ... + float newg = n->g + n->m_UserState.GetCost( (*successor)->m_UserState ); + + // Now we need to find whether the node is on the open or closed lists + // If it is but the node that is already on them is better (lower g) + // then we can forget about this successor + + // First linear search of open list to find node + + typename vector< Node * >::iterator openlist_result; + + for( openlist_result = m_OpenList.begin(); openlist_result != m_OpenList.end(); openlist_result ++ ) + { + if( (*openlist_result)->m_UserState.IsSameState( (*successor)->m_UserState ) ) + { + break; + } + } + + if( openlist_result != m_OpenList.end() ) + { + + // we found this state on open + + if( (*openlist_result)->g <= newg ) + { + FreeNode( (*successor) ); + + // the one on Open is cheaper than this one + continue; + } + } + + typename vector< Node * >::iterator closedlist_result; + + for( closedlist_result = m_ClosedList.begin(); closedlist_result != m_ClosedList.end(); closedlist_result ++ ) + { + if( (*closedlist_result)->m_UserState.IsSameState( (*successor)->m_UserState ) ) + { + break; + } + } + + if( closedlist_result != m_ClosedList.end() ) + { + + // we found this state on closed + + if( (*closedlist_result)->g <= newg ) + { + // the one on Closed is cheaper than this one + FreeNode( (*successor) ); + + continue; + } + } + + // This node is the best node so far with this particular state + // so lets keep it and set up its AStar specific data ... + + (*successor)->parent = n; + (*successor)->g = newg; + (*successor)->h = (*successor)->m_UserState.GoalDistanceEstimate( m_Goal->m_UserState ); + (*successor)->f = (*successor)->g + (*successor)->h; + + // Successor in closed list + // 1 - Update old version of this node in closed list + // 2 - Move it from closed to open list + // 3 - Sort heap again in open list + + if( closedlist_result != m_ClosedList.end() ) + { + // Update closed node with successor node AStar data + //*(*closedlist_result) = *(*successor); + (*closedlist_result)->parent = (*successor)->parent; + (*closedlist_result)->g = (*successor)->g; + (*closedlist_result)->h = (*successor)->h; + (*closedlist_result)->f = (*successor)->f; + + // Free successor node + FreeNode( (*successor) ); + + // Push closed node into open list + m_OpenList.push_back( (*closedlist_result) ); + + // Remove closed node from closed list + m_ClosedList.erase( closedlist_result ); + + // Sort back element into heap + push_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() ); + + // Fix thanks to ... + // Greg Douglas + // who noticed that this code path was incorrect + // Here we have found a new state which is already CLOSED + + } + + // Successor in open list + // 1 - Update old version of this node in open list + // 2 - sort heap again in open list + + else if( openlist_result != m_OpenList.end() ) + { + // Update open node with successor node AStar data + //*(*openlist_result) = *(*successor); + (*openlist_result)->parent = (*successor)->parent; + (*openlist_result)->g = (*successor)->g; + (*openlist_result)->h = (*successor)->h; + (*openlist_result)->f = (*successor)->f; + + // Free successor node + FreeNode( (*successor) ); + + // re-make the heap + // make_heap rather than sort_heap is an essential bug fix + // thanks to Mike Ryynanen for pointing this out and then explaining + // it in detail. sort_heap called on an invalid heap does not work + make_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() ); + } + + // New successor + // 1 - Move it from successors to open list + // 2 - sort heap again in open list + + else + { + // Push successor node into open list + m_OpenList.push_back( (*successor) ); + + // Sort back element into heap + push_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() ); + } + + } + + // push n onto Closed, as we have expanded it now + + m_ClosedList.push_back( n ); + + } // end else (not goal so expand) + + return m_State; // Succeeded bool is false at this point. + + } + + // User calls this to add a successor to a list of successors + // when expanding the search frontier + bool AddSuccessor( UserState &State ) + { + Node *node = AllocateNode(); + + if( node ) + { + node->m_UserState = State; + + m_Successors.push_back( node ); + + return true; + } + + return false; + } + + // Free the solution nodes + // This is done to clean up all used Node memory when you are done with the + // search + void FreeSolutionNodes() + { + Node *n = m_Start; + + if( m_Start->child ) + { + do + { + Node *del = n; + n = n->child; + FreeNode( del ); + + del = NULL; + + } while( n != m_Goal ); + + FreeNode( n ); // Delete the goal + + } + else + { + // if the start node is the solution we need to just delete the start and goal + // nodes + FreeNode( m_Start ); + FreeNode( m_Goal ); + } + + } + + // Functions for traversing the solution + + // Get start node + UserState *GetSolutionStart() + { + m_CurrentSolutionNode = m_Start; + if( m_Start ) + { + return &m_Start->m_UserState; + } + else + { + return NULL; + } + } + + // Get next node + UserState *GetSolutionNext() + { + if( m_CurrentSolutionNode ) + { + if( m_CurrentSolutionNode->child ) + { + + Node *child = m_CurrentSolutionNode->child; + + m_CurrentSolutionNode = m_CurrentSolutionNode->child; + + return &child->m_UserState; + } + } + + return NULL; + } + + // Get end node + UserState *GetSolutionEnd() + { + m_CurrentSolutionNode = m_Goal; + if( m_Goal ) + { + return &m_Goal->m_UserState; + } + else + { + return NULL; + } + } + + // Step solution iterator backwards + UserState *GetSolutionPrev() + { + if( m_CurrentSolutionNode ) + { + if( m_CurrentSolutionNode->parent ) + { + + Node *parent = m_CurrentSolutionNode->parent; + + m_CurrentSolutionNode = m_CurrentSolutionNode->parent; + + return &parent->m_UserState; + } + } + + return NULL; + } + + // Get final cost of solution + // Returns FLT_MAX if goal is not defined or there is no solution + float GetSolutionCost() + { + if( m_Goal && m_State == SEARCH_STATE_SUCCEEDED ) + { + return m_Goal->g; + } + else + { + return FLT_MAX; + } + } + + // For educational use and debugging it is useful to be able to view + // the open and closed list at each step, here are two functions to allow that. + + UserState *GetOpenListStart() + { + float f,g,h; + return GetOpenListStart( f,g,h ); + } + + UserState *GetOpenListStart( float &f, float &g, float &h ) + { + iterDbgOpen = m_OpenList.begin(); + if( iterDbgOpen != m_OpenList.end() ) + { + f = (*iterDbgOpen)->f; + g = (*iterDbgOpen)->g; + h = (*iterDbgOpen)->h; + return &(*iterDbgOpen)->m_UserState; + } + + return NULL; + } + + UserState *GetOpenListNext() + { + float f,g,h; + return GetOpenListNext( f,g,h ); + } + + UserState *GetOpenListNext( float &f, float &g, float &h ) + { + iterDbgOpen++; + if( iterDbgOpen != m_OpenList.end() ) + { + f = (*iterDbgOpen)->f; + g = (*iterDbgOpen)->g; + h = (*iterDbgOpen)->h; + return &(*iterDbgOpen)->m_UserState; + } + + return NULL; + } + + UserState *GetClosedListStart() + { + float f,g,h; + return GetClosedListStart( f,g,h ); + } + + UserState *GetClosedListStart( float &f, float &g, float &h ) + { + iterDbgClosed = m_ClosedList.begin(); + if( iterDbgClosed != m_ClosedList.end() ) + { + f = (*iterDbgClosed)->f; + g = (*iterDbgClosed)->g; + h = (*iterDbgClosed)->h; + + return &(*iterDbgClosed)->m_UserState; + } + + return NULL; + } + + UserState *GetClosedListNext() + { + float f,g,h; + return GetClosedListNext( f,g,h ); + } + + UserState *GetClosedListNext( float &f, float &g, float &h ) + { + iterDbgClosed++; + if( iterDbgClosed != m_ClosedList.end() ) + { + f = (*iterDbgClosed)->f; + g = (*iterDbgClosed)->g; + h = (*iterDbgClosed)->h; + + return &(*iterDbgClosed)->m_UserState; + } + + return NULL; + } + + // Get the number of steps + + int GetStepCount() { return m_Steps; } + + void EnsureMemoryFreed() + { +#if USE_FSA_MEMORY + assert(m_AllocateNodeCount == 0); +#endif + + } + +private: // methods + + // This is called when a search fails or is cancelled to free all used + // memory + void FreeAllNodes() + { + // iterate open list and delete all nodes + typename vector< Node * >::iterator iterOpen = m_OpenList.begin(); + + while( iterOpen != m_OpenList.end() ) + { + Node *n = (*iterOpen); + FreeNode( n ); + + iterOpen ++; + } + + m_OpenList.clear(); + + // iterate closed list and delete unused nodes + typename vector< Node * >::iterator iterClosed; + + for( iterClosed = m_ClosedList.begin(); iterClosed != m_ClosedList.end(); iterClosed ++ ) + { + Node *n = (*iterClosed); + FreeNode( n ); + } + + m_ClosedList.clear(); + + // delete the goal + + FreeNode(m_Goal); + } + + + // This call is made by the search class when the search ends. A lot of nodes may be + // created that are still present when the search ends. They will be deleted by this + // routine once the search ends + void FreeUnusedNodes() + { + // iterate open list and delete unused nodes + typename vector< Node * >::iterator iterOpen = m_OpenList.begin(); + + while( iterOpen != m_OpenList.end() ) + { + Node *n = (*iterOpen); + + if( !n->child ) + { + FreeNode( n ); + + n = NULL; + } + + iterOpen ++; + } + + m_OpenList.clear(); + + // iterate closed list and delete unused nodes + typename vector< Node * >::iterator iterClosed; + + for( iterClosed = m_ClosedList.begin(); iterClosed != m_ClosedList.end(); iterClosed ++ ) + { + Node *n = (*iterClosed); + + if( !n->child ) + { + FreeNode( n ); + n = NULL; + + } + } + + m_ClosedList.clear(); + + } + + // Node memory management + Node *AllocateNode() + { + +#if !USE_FSA_MEMORY + m_AllocateNodeCount ++; + Node *p = new Node; + return p; +#else + Node *address = m_FixedSizeAllocator.alloc(); + + if( !address ) + { + return NULL; + } + m_AllocateNodeCount ++; + Node *p = new (address) Node; + return p; +#endif + } + + void FreeNode( Node *node ) + { + + m_AllocateNodeCount --; + +#if !USE_FSA_MEMORY + delete node; +#else + node->~Node(); + m_FixedSizeAllocator.free( node ); +#endif + } + +private: // data + + // Heap (simple vector but used as a heap, cf. Steve Rabin's game gems article) + vector< Node *> m_OpenList; + + // Closed list is a vector. + vector< Node * > m_ClosedList; + + // Successors is a vector filled out by the user each type successors to a node + // are generated + vector< Node * > m_Successors; + + // State + unsigned int m_State; + + // Counts steps + int m_Steps; + + // Start and goal state pointers + Node *m_Start; + Node *m_Goal; + + Node *m_CurrentSolutionNode; + +#if USE_FSA_MEMORY + // Memory + FixedSizeAllocator m_FixedSizeAllocator; +#endif + + //Debug : need to keep these two iterators around + // for the user Dbg functions + typename vector< Node * >::iterator iterDbgOpen; + typename vector< Node * >::iterator iterDbgClosed; + + // debugging : count memory allocation and free's + int m_AllocateNodeCount; + + bool m_CancelRequest; + +}; + +template class AStarState +{ +public: + virtual ~AStarState() {} + virtual float GoalDistanceEstimate( T &nodeGoal ) = 0; // Heuristic function which computes the estimated cost to the goal node + virtual bool IsGoal( T &nodeGoal ) = 0; // Returns true if this node is the goal node + virtual bool GetSuccessors( AStarSearch *astarsearch, T *parent_node ) = 0; // Retrieves all successors to this node and adds them via astarsearch.addSuccessor() + virtual float GetCost( T &successor ) = 0; // Computes the cost of travelling from this node to the successor node + virtual bool IsSameState( T &rhs ) = 0; // Returns true if this node is the same as the rhs node +}; + +#endif + +