diff --git a/cpp/.clang-format b/cpp/.clang-format new file mode 100644 index 0000000..56f34dc --- /dev/null +++ b/cpp/.clang-format @@ -0,0 +1,7 @@ +# .clang-format +BasedOnStyle: Google +ColumnLimit: 100 +IndentWidth: 4 +AlignAfterOpenBracket: AlwaysBreak +AllowShortFunctionsOnASingleLine: Empty +Standard: Latest diff --git a/cpp/8puzzle.cpp b/cpp/8puzzle.cpp old mode 100755 new mode 100644 index dbb2189..b66eb0f --- a/cpp/8puzzle.cpp +++ b/cpp/8puzzle.cpp @@ -6,10 +6,11 @@ //////////////////////////////////////////////////////////////////////////////////////////////////////////////// -#include -#include #include #include +#include + +#include // Configuration @@ -20,17 +21,17 @@ #define DEBUG_LISTS 0 // AStar search class -#include "stlastar.h" // See header for copyright and usage information +#include "stlastar.h" // See header for copyright and usage information using namespace std; // Global data -#define BOARD_WIDTH (3) -#define BOARD_HEIGHT (3) +#define BOARD_WIDTH (3) +#define BOARD_HEIGHT (3) -#define GM_TILE (-1) -#define GM_SPACE (0) +#define GM_TILE (-1) +#define GM_SPACE (0) #define GM_OFF_BOARD (1) // Definitions @@ -42,11 +43,11 @@ using namespace std; // 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 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 +// (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 // @@ -57,9 +58,9 @@ using namespace std; // 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 +// 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 @@ -68,86 +69,67 @@ using namespace std; // 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; +class PuzzleState { + public: + // defs - // data + typedef enum { + TL_SPACE, + TL_1, + TL_2, + TL_3, + TL_4, + TL_5, + TL_6, + TL_7, + TL_8 - static TILE g_goal[ BOARD_WIDTH*BOARD_HEIGHT]; - static TILE g_start[ BOARD_WIDTH*BOARD_HEIGHT]; + } TILE; - // the tile data for the 8-puzzle - TILE tiles[ BOARD_WIDTH*BOARD_HEIGHT ]; + // data - // member functions + static TILE g_goal[BOARD_WIDTH * BOARD_HEIGHT]; + static TILE g_start[BOARD_WIDTH * BOARD_HEIGHT]; - PuzzleState() { - memcpy( tiles, g_goal, sizeof( TILE ) * BOARD_WIDTH * BOARD_HEIGHT ); - } + // the tile data for the 8-puzzle + TILE tiles[BOARD_WIDTH * BOARD_HEIGHT]; - PuzzleState( TILE *param_tiles ) - { - memcpy( tiles, param_tiles, sizeof( TILE ) * BOARD_WIDTH * BOARD_HEIGHT ); - } + // member functions - float GoalDistanceEstimate( PuzzleState &nodeGoal ); - bool IsGoal( PuzzleState &nodeGoal ); - bool GetSuccessors( AStarSearch *astarsearch, PuzzleState *parent_node ); - float GetCost( PuzzleState &successor ); - bool IsSameState( PuzzleState &rhs ); - size_t Hash(); - - void PrintNodeInfo(); + PuzzleState() { + memcpy(tiles, g_goal, sizeof(TILE) * BOARD_WIDTH * BOARD_HEIGHT); + } -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 ); + 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); + size_t Hash(); + 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, +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[] = -{ +PuzzleState::TILE PuzzleState::g_start[] = { - // Three example start states from Bratko's Prolog Programming for Artificial Intelligence +// Three example start states from Bratko's Prolog Programming for Artificial Intelligence #if 0 // ex a - 4 steps @@ -162,543 +144,393 @@ PuzzleState::TILE PuzzleState::g_start[] = 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 , + + // 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 , + + // 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 , + + // 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 + // sent by email - does work though - TL_1 , TL_2 , TL_3 , - TL_4 , TL_5 , TL_6 , - TL_8 , TL_7 , TL_SPACE , + 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 +// 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 , + // easy 5 + TL_1, TL_3, TL_4, - TL_8 , - TL_6 , - TL_2 , - - TL_7 , - TL_SPACE , - TL_5 , + TL_8, TL_6, TL_2, + TL_7, TL_SPACE, TL_5, #elif 0 - // medium 9 - TL_2 , - TL_8 , - TL_1 , + // medium 9 + TL_2, TL_8, TL_1, - TL_SPACE , - TL_4 , - TL_3 , - - TL_7 , - TL_6 , - TL_5 , + TL_SPACE, TL_4, TL_3, + + TL_7, TL_6, TL_5, #elif 0 - // hard 12 - TL_2 , - TL_8 , - TL_1 , + // hard 12 + TL_2, TL_8, TL_1, - TL_4 , - TL_6 , - TL_3 , - - TL_SPACE , - TL_7 , - TL_5 , + TL_4, TL_6, TL_3, + + TL_SPACE, TL_7, TL_5, #elif 1 - // worst 30 - TL_5 , - TL_6 , - TL_7 , + // worst 30 + TL_5, TL_6, TL_7, - TL_4 , - TL_SPACE , - TL_8 , - - TL_3 , - TL_2 , - TL_1 , + TL_4, TL_SPACE, TL_8, + + TL_3, TL_2, TL_1, #elif 0 - // 123 - // 784 - // 65 + // 123 + // 784 + // 65 - // two move simple board - TL_1 , - TL_2 , - TL_3 , + // two move simple board + TL_1, TL_2, TL_3, - TL_7 , - TL_8 , - TL_4 , - - TL_SPACE , - TL_6 , - TL_5 , + 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 + // a1 b2 c3 d4 e5 f6 g7 h8 + // C3,Blank,H8,A1,G8,F6,E5,D4,B2 - TL_3 , - TL_SPACE , - TL_8 , + TL_3, TL_SPACE, TL_8, - TL_1 , - TL_8 , - TL_6 , - - TL_5 , - TL_4 , - TL_2 , + TL_1, TL_8, TL_6, + TL_5, TL_4, TL_2, -#endif +#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; +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; } // The 9 tiles positions can be encoded as digits -size_t PuzzleState::Hash() -{ - std::size_t hash = 0; - for( size_t i = 0; i < (BOARD_HEIGHT * BOARD_WIDTH); i++ ) - { - hash ^= std::hash()(tiles[i]) + 0x9e3779b9 + (hash << 6) + (hash >> 2); - } - return hash; +size_t PuzzleState::Hash() { + std::size_t hash = 0; + for (size_t i = 0; i < (BOARD_HEIGHT * BOARD_WIDTH); i++) { + hash ^= std::hash()(tiles[i]) + 0x9e3779b9 + (hash << 6) + (hash >> 2); + } + return hash; } -void PuzzleState::PrintNodeInfo() -{ - const int strSize = 100; - char str[strSize]; - snprintf( str, strSize, "%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; +void PuzzleState::PrintNodeInfo() { + const int strSize = 100; + char str[strSize]; + snprintf( + str, strSize, "%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; - +// 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 ); +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; +void PuzzleState::GetSpacePosition(PuzzleState* pn, int* rx, int* ry) { + int x, y; - return; - } - } - } + for (y = 0; y < BOARD_HEIGHT; y++) { + for (x = 0; x < BOARD_WIDTH; x++) { + if (pn->tiles[(y * BOARD_WIDTH) + x] == TL_SPACE) { + *rx = x; + *ry = y; - assert( false && "Something went wrong. There's no space on the board" ); + 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; +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; - } + if (tiles[(y * BOARD_WIDTH) + x] == TL_SPACE) { + return GM_SPACE; + } - return GM_TILE; + 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]; - } - +// 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 - TargetTiles[ (ty*BOARD_WIDTH)+tx ] = StartTiles[ (spy*BOARD_WIDTH)+spx ]; - TargetTiles[ (spy*BOARD_WIDTH)+spx ] = StartTiles[ (ty*BOARD_WIDTH)+tx ]; +bool PuzzleState::LegalMove(TILE* StartTiles, TILE* TargetTiles, int spx, int spy, int tx, int ty) { + int t; - return true; - } - } + 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 false; + 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; +bool PuzzleState::GetSuccessors(AStarSearch* astarsearch, PuzzleState* parent_node) { + PuzzleState NewNode; - int sp_x,sp_y; + int sp_x, sp_y; - GetSpacePosition( this, &sp_x, &sp_y ); + GetSpacePosition(this, &sp_x, &sp_y); - bool ret; + bool ret; - if( LegalMove( tiles, NewNode.tiles, sp_x, sp_y, sp_x, sp_y-1 ) == true ) - { - ret = astarsearch->AddSuccessor( NewNode ); + if (LegalMove(tiles, NewNode.tiles, sp_x, sp_y, sp_x, sp_y - 1) == true) { + ret = astarsearch->AddSuccessor(NewNode); - if( !ret ) return false; - } + 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, sp_y + 1) == true) { + ret = astarsearch->AddSuccessor(NewNode); - if( LegalMove( tiles, NewNode.tiles, sp_x, sp_y, sp_x-1, sp_y ) == true ) - { - ret = astarsearch->AddSuccessor( NewNode ); + if (!ret) return false; + } - if( !ret ) return false; - } + if (LegalMove(tiles, NewNode.tiles, sp_x, sp_y, sp_x - 1, sp_y) == true) { + ret = astarsearch->AddSuccessor(NewNode); - if( LegalMove( tiles, NewNode.tiles, sp_x, sp_y, sp_x+1, sp_y ) == true ) - { - ret = astarsearch->AddSuccessor( NewNode ); - - if( !ret ) return false; - } + if (!ret) return false; + } - return true; + 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 +// 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 - +float PuzzleState::GetCost(PuzzleState& successor) { + return 1.0f; // I love it when life is simple } - // Main -int main( 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'); +int main(int argc, char* argv[]) { + cout << "STL A* 8-puzzle solver implementation\n(C)2001 Justin Heyes-Jones\n"; - PuzzleState::g_start[i] = static_cast(num); - - } - - i++; - } + 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); + } - // Create an instance of the search class... + i++; + } + } - AStarSearch astarsearch; + // Create an instance of the search class... - int NumTimesToSearch = NUM_TIMES_TO_RUN_SEARCH; + AStarSearch astarsearch; - while( NumTimesToSearch-- ) - { + int NumTimesToSearch = NUM_TIMES_TO_RUN_SEARCH; - // Create a start state - PuzzleState nodeStart( PuzzleState::g_start ); + while (NumTimesToSearch--) { + // Create a start state + PuzzleState nodeStart(PuzzleState::g_start); - // Define the goal state - PuzzleState nodeEnd( PuzzleState::g_goal ); + // Define the goal state + PuzzleState nodeEnd(PuzzleState::g_goal); - // Set Start and goal states - astarsearch.SetStartAndGoalStates( nodeStart, nodeEnd ); + // Set Start and goal states + astarsearch.SetStartAndGoalStates(nodeStart, nodeEnd); - unsigned int SearchState; + unsigned int SearchState; #if DEBUG_LISTS - unsigned int SearchSteps = 0; + unsigned int SearchSteps = 0; #endif - do - { - SearchState = astarsearch.SearchStep(); + do { + SearchState = astarsearch.SearchStep(); #if DEBUG_LISTS - float f,g,h; + float f, g, h; - cout << "Search step " << SearchSteps << endl; + 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 << "Open:\n"; + PuzzleState* p = astarsearch.GetOpenListStart(f, g, h); + while (p) { + ((PuzzleState*)p)->PrintNodeInfo(); + cout << "f: " << f << " g: " << g << " h: " << h << "\n\n"; - 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 ); - } + 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 @@ -712,108 +544,92 @@ int main( int argc, char *argv[] ) #endif #if DEBUG_LISTS - SearchSteps++; + SearchSteps++; #endif - } - while( SearchState == AStarSearch::SEARCH_STATE_SEARCHING ); + } while (SearchState == AStarSearch::SEARCH_STATE_SEARCHING); - if( SearchState == AStarSearch::SEARCH_STATE_SUCCEEDED ) - { + if (SearchState == AStarSearch::SEARCH_STATE_SUCCEEDED) { #if DISPLAY_SOLUTION_FORWARDS - cout << "Search found goal state\n"; + cout << "Search found goal state\n"; #endif - PuzzleState *node = astarsearch.GetSolutionStart(); + PuzzleState* node = astarsearch.GetSolutionStart(); #if DISPLAY_SOLUTION_FORWARDS - cout << "Displaying solution\n"; + cout << "Displaying solution\n"; #endif - int steps = 0; + int steps = 0; #if DISPLAY_SOLUTION_FORWARDS - node->PrintNodeInfo(); - cout << endl; + node->PrintNodeInfo(); + cout << endl; #endif - for( ;; ) - { - node = astarsearch.GetSolutionNext(); + for (;;) { + node = astarsearch.GetSolutionNext(); - if( !node ) - { - break; - } + if (!node) { + break; + } #if DISPLAY_SOLUTION_FORWARDS - node->PrintNodeInfo(); - cout << endl; + node->PrintNodeInfo(); + cout << endl; #endif - steps ++; - - }; + steps++; + }; #if DISPLAY_SOLUTION_FORWARDS - // todo move step count into main algorithm - cout << "Solution steps " << steps << endl; + // todo move step count into main algorithm + cout << "Solution steps " << steps << endl; #endif -//////////// + //////////// - node = astarsearch.GetSolutionEnd(); + node = astarsearch.GetSolutionEnd(); #if DISPLAY_SOLUTION_BACKWARDS - cout << "Displaying reverse solution\n"; + cout << "Displaying reverse solution\n"; #endif - steps = 0; + steps = 0; - node->PrintNodeInfo(); - cout << endl; - for( ;; ) - { - node = astarsearch.GetSolutionPrev(); + node->PrintNodeInfo(); + cout << endl; + for (;;) { + node = astarsearch.GetSolutionPrev(); - if( !node ) - { - break; - } + if (!node) { + break; + } #if DISPLAY_SOLUTION_BACKWARDS - node->PrintNodeInfo(); + node->PrintNodeInfo(); cout << endl; #endif - steps ++; - - }; + steps++; + }; #if DISPLAY_SOLUTION_BACKWARDS - cout << "Solution steps " << steps << endl; + 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 ) - { + // 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 ) - { + 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 - } - - + cout << "Search terminated. Out of memory\n"; +#endif + } - // Display the number of loops the search went through + // Display the number of loops the search went through #if DISPLAY_SOLUTION_INFO - cout << "SearchSteps : " << astarsearch.GetStepCount() << endl; + cout << "SearchSteps : " << astarsearch.GetStepCount() << endl; #endif - } + } - return 0; + return 0; } - - diff --git a/cpp/findpath.cpp b/cpp/findpath.cpp old mode 100755 new mode 100644 index cb02ce7..ec82bac --- a/cpp/findpath.cpp +++ b/cpp/findpath.cpp @@ -8,11 +8,12 @@ //////////////////////////////////////////////////////////////////////////////////////////////////////////////// -#include -#include #include +#include + +#include -#include "stlastar.h" // See header for copyright and usage information +#include "stlastar.h" // See header for copyright and usage information using namespace std; @@ -26,328 +27,271 @@ using namespace std; 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 +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]; -} - +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 ); - size_t Hash(); - - void PrintNodeInfo(); - - +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); + size_t Hash(); + + 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; - } - +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; + } } -size_t MapSearchNode::Hash() -{ - size_t h1 = hash{}(x); - size_t h2 = hash{}(y); - return h1 ^ (h2 << 1); +size_t MapSearchNode::Hash() { + size_t h1 = hash{}(x); + size_t h2 = hash{}(y); + return h1 ^ (h2 << 1); } -void MapSearchNode::PrintNodeInfo() -{ - const int strSize = 100; - char str[strSize]; - snprintf( str, strSize, "Node position : (%d,%d)\n", x,y ); +void MapSearchNode::PrintNodeInfo() { + const int strSize = 100; + char str[strSize]; + snprintf(str, strSize, "Node position : (%d,%d)\n", x, y); - cout << str; + cout << str; } // Here's the heuristic function that estimates the distance from a Node -// to the Goal. +// to the Goal. -float MapSearchNode::GoalDistanceEstimate( MapSearchNode &nodeGoal ) -{ - return abs(x - nodeGoal.x) + abs(y - nodeGoal.y); +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; - } +bool MapSearchNode::IsGoal(MapSearchNode& nodeGoal) { + if ((x == nodeGoal.x) && (y == nodeGoal.y)) { + return true; + } - return false; + 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; +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 +// 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 ); - +float MapSearchNode::GetCost(MapSearchNode& successor) { + return (float)GetMap(x, y); } - // Main -int main( int argc, char *argv[] ) -{ +int main(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; - cout << "STL A* Search implementation\n(C)2001 Justin Heyes-Jones\n"; + const unsigned int NumSearches = 1; - // 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. + while (SearchCount < NumSearches) { + // Create a start state + MapSearchNode nodeStart; + nodeStart.x = rand() % MAP_WIDTH; + nodeStart.y = rand() % MAP_HEIGHT; - // Create an instance of the search class... + // Define the goal state + MapSearchNode nodeEnd; + nodeEnd.x = rand() % MAP_WIDTH; + nodeEnd.y = rand() % MAP_HEIGHT; - AStarSearch astarsearch; + // Set Start and goal states - unsigned int SearchCount = 0; + astarsearch.SetStartAndGoalStates(nodeStart, nodeEnd); - const unsigned int NumSearches = 1; + unsigned int SearchState; + unsigned int SearchSteps = 0; - while(SearchCount < NumSearches) - { + do { + SearchState = astarsearch.SearchStep(); - // Create a start state - MapSearchNode nodeStart; - nodeStart.x = rand()%MAP_WIDTH; - nodeStart.y = rand()%MAP_HEIGHT; + SearchSteps++; - // 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 ); +#if DEBUG_LISTS - unsigned int SearchState; - unsigned int SearchSteps = 0; + cout << "Steps:" << SearchSteps << "\n"; - do - { - SearchState = astarsearch.SearchStep(); + int len = 0; - SearchSteps++; + cout << "Open:\n"; + MapSearchNode* p = astarsearch.GetOpenListStart(); + while (p) { + len++; +#if !DEBUG_LIST_LENGTHS_ONLY + ((MapSearchNode*)p)->PrintNodeInfo(); +#endif + p = astarsearch.GetOpenListNext(); + } - #if DEBUG_LISTS + cout << "Open list has " << len << " nodes\n"; - cout << "Steps:" << SearchSteps << "\n"; + len = 0; - int len = 0; + cout << "Closed:\n"; + p = astarsearch.GetClosedListStart(); + while (p) { + len++; +#if !DEBUG_LIST_LENGTHS_ONLY + p->PrintNodeInfo(); +#endif + p = astarsearch.GetClosedListNext(); + } - cout << "Open:\n"; - MapSearchNode *p = astarsearch.GetOpenListStart(); - while( p ) - { - len++; - #if !DEBUG_LIST_LENGTHS_ONLY - ((MapSearchNode *)p)->PrintNodeInfo(); - #endif - p = astarsearch.GetOpenListNext(); - - } + cout << "Closed list has " << len << " nodes\n"; +#endif - cout << "Open list has " << len << " nodes\n"; + } while (SearchState == AStarSearch::SEARCH_STATE_SEARCHING); - len = 0; + if (SearchState == AStarSearch::SEARCH_STATE_SUCCEEDED) { + cout << "Search found goal state\n"; - cout << "Closed:\n"; - p = astarsearch.GetClosedListStart(); - while( p ) - { - len++; - #if !DEBUG_LIST_LENGTHS_ONLY - p->PrintNodeInfo(); - #endif - p = astarsearch.GetClosedListNext(); - } + MapSearchNode* node = astarsearch.GetSolutionStart(); - cout << "Closed list has " << len << " nodes\n"; - #endif +#if DISPLAY_SOLUTION + cout << "Displaying solution\n"; +#endif + int steps = 0; - } - while( SearchState == AStarSearch::SEARCH_STATE_SEARCHING ); + node->PrintNodeInfo(); + for (;;) { + node = astarsearch.GetSolutionNext(); - if( SearchState == AStarSearch::SEARCH_STATE_SUCCEEDED ) - { - cout << "Search found goal state\n"; + if (!node) { + break; + } - MapSearchNode *node = astarsearch.GetSolutionStart(); + node->PrintNodeInfo(); + steps++; + }; - #if DISPLAY_SOLUTION - cout << "Displaying solution\n"; - #endif - int steps = 0; + cout << "Solution steps " << steps << endl; - node->PrintNodeInfo(); - for( ;; ) - { - node = astarsearch.GetSolutionNext(); + // Once you're done with the solution you can free the nodes up + astarsearch.FreeSolutionNodes(); - if( !node ) - { - break; - } + } else if (SearchState == AStarSearch::SEARCH_STATE_FAILED) { + cout << "Search terminated. Did not find goal state\n"; + } - node->PrintNodeInfo(); - steps ++; - - }; + // Display the number of loops the search went through + cout << "SearchSteps : " << SearchSteps << "\n"; - cout << "Solution steps " << steps << endl; + SearchCount++; - // Once you're done with the solution you can free the nodes up - astarsearch.FreeSolutionNodes(); + astarsearch.EnsureMemoryFreed(); + } - - } - 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; + return 0; } //////////////////////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/cpp/fsa.h b/cpp/fsa.h old mode 100755 new mode 100644 index dceaf55..821a982 --- a/cpp/fsa.h +++ b/cpp/fsa.h @@ -1,25 +1,25 @@ -/* +/* 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 +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 + + 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 + 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! @@ -27,17 +27,17 @@ given where due. FixedSizeAllocator class Copyright 2001 Justin Heyes-Jones - This class is a constant time O(1) memory manager for objects of + 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. + 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 + 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. */ @@ -45,208 +45,167 @@ given where due. #ifndef FSA_H #define FSA_H -#include #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; - +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; i < m_MaxElements; i++) { + pElement->pPrev = 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 +#endif // defined FSA_H diff --git a/cpp/stlastar.h b/cpp/stlastar.h old mode 100755 new mode 100644 index 02f0dfb..7b6119f --- a/cpp/stlastar.h +++ b/cpp/stlastar.h @@ -2,23 +2,23 @@ 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 +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 + + 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 + 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! */ @@ -26,16 +26,16 @@ given where due. #ifndef STLASTAR_H #define STLASTAR_H // used for text debugging -#include #include -//#include + +#include #include // stl includes #include +#include #include #include -#include // fast fixed size memory allocator, used for fast node memory management #include "fsa.h" @@ -47,793 +47,687 @@ given where due. // disable warning that debugging information has lines that are truncated // occurs in stl headers #if defined(WIN32) && defined(_WINDOWS) -#pragma warning( disable : 4786 ) +#pragma warning(disable : 4786) #endif -template class AStarState; +template +class AStarState; // The AStar search class. UserState is the users state space type -template class AStarSearch -{ +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 + its 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) {} + + bool operator==(const Node& otherNode) const { + return this->m_UserState.IsSameState(otherNode.m_UserState); + } + + 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) { + } -public: // data + 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) { + } - enum - { - SEARCH_STATE_NOT_INITIALISED, - SEARCH_STATE_SEARCHING, - SEARCH_STATE_SUCCEEDED, - SEARCH_STATE_FAILED, - SEARCH_STATE_OUT_OF_MEMORY, - SEARCH_STATE_INVALID - }; + // 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; - // A node represents a possible state in the search - // The user provided state type is included inside this type + m_Start = AllocateNode(); + m_Goal = AllocateNode(); - public: + assert((m_Start != NULL && m_Goal != NULL)); - class Node - { - public: + m_Start->m_UserState = Start; + m_Goal->m_UserState = Goal; - 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 + its predecessors - float h; // heuristic estimate of distance to goal - float f; // sum of cumulative cost of predecessors and self and heuristic + m_State = SEARCH_STATE_SEARCHING; - Node() : - parent( 0 ), - child( 0 ), - g( 0.0f ), - h( 0.0f ), - f( 0.0f ) - { - } + // Initialise the AStar specific parts of the Start Node + // The user only needs fill out the state information - bool operator==(const Node& otherNode) const - { - return this->m_UserState.IsSameState(otherNode.m_UserState); - } + 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; - UserState m_UserState; - }; + // Push the start node on the Open list - // For sorting the heap the STL needs compare function that lets us compare - // the f value of two nodes + m_OpenList.push_back(m_Start); // heap now unsorted - class HeapCompare_f - { - public: + // Sort back element into heap + push_heap(m_OpenList.begin(), m_OpenList.end(), HeapCompare_f()); - bool operator() ( const Node *x, const Node *y ) const - { - return x->f > y->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)); -public: // methods + // 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; + } - // 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 std::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 std::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 std::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 std::unordered_set::iterator closedlist_result; + // Incremement step count + m_Steps++; - closedlist_result = m_ClosedList.find(*successor); + // 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(); - if( closedlist_result != m_ClosedList.end() ) - { + // 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; - // we found this state on closed + // 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); - if( (*closedlist_result)->g <= newg ) - { - // the one on Closed is cheaper than this one - FreeNode( (*successor) ); + // set the child pointers in each node (except Goal which has no child) + Node* nodeChild = m_Goal; + Node* nodeParent = m_Goal->parent; - continue; - } - } + do { + nodeParent->child = nodeChild; - // This node is the best node so far with this particular state - // so lets keep it and set up its AStar specific data ... + nodeChild = nodeParent; + nodeParent = nodeParent->parent; - (*successor)->parent = n; - (*successor)->g = newg; - (*successor)->h = (*successor)->m_UserState.GoalDistanceEstimate( m_Goal->m_UserState ); - (*successor)->f = (*successor)->g + (*successor)->h; + } while (nodeChild != m_Start); // Start is always the first node by definition + } - // 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 + // delete nodes that aren't needed for the solution + FreeUnusedNodes(); - 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; + m_State = SEARCH_STATE_SUCCEEDED; - // Free successor node - FreeNode( (*successor) ); + 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 ... - // Push closed node into open list - m_OpenList.push_back( (*closedlist_result) ); + m_Successors.clear(); // empty vector of successor nodes to n - // Remove closed node from closed list - m_ClosedList.erase( closedlist_result ); + // 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); - // Sort back element into heap - push_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() ); + if (!ret) { + typename std::vector::iterator successor; - // Fix thanks to ... - // Greg Douglas - // who noticed that this code path was incorrect - // Here we have found a new state which is already CLOSED + // 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 - // Successor in open list - // 1 - Update old version of this node in open list - // 2 - sort heap again in open list + // 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 std::vector::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 std::vector::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)); - 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.insert( 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 + // the one on Open is cheaper than this one + continue; + } + } + typename std::unordered_set::iterator closedlist_result; + + closedlist_result = m_ClosedList.find(*successor); - } + if (closedlist_result != m_ClosedList.end()) { + // we found this state on closed -private: // methods + if ((*closedlist_result)->g <= newg) { + // the one on Closed is cheaper than this one + FreeNode((*successor)); - // 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 std::vector< Node * >::iterator iterOpen = m_OpenList.begin(); + continue; + } + } - while( iterOpen != m_OpenList.end() ) - { - Node *n = (*iterOpen); - FreeNode( n ); + // This node is the best node so far with this particular state + // so lets keep it and set up its AStar specific data ... - iterOpen ++; - } + (*successor)->parent = n; + (*successor)->g = newg; + (*successor)->h = + (*successor)->m_UserState.GoalDistanceEstimate(m_Goal->m_UserState); + (*successor)->f = (*successor)->g + (*successor)->h; - m_OpenList.clear(); + // 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 - // iterate closed list and delete unused nodes - typename std::unordered_set::iterator iterClosed; + 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; - for( iterClosed = m_ClosedList.begin(); iterClosed != m_ClosedList.end(); iterClosed ++ ) - { - Node *n = (*iterClosed); - FreeNode( n ); - } + // Free successor node + FreeNode((*successor)); - m_ClosedList.clear(); + // Push closed node into open list + m_OpenList.push_back((*closedlist_result)); - // delete the goal + // Remove closed node from closed list + m_ClosedList.erase(closedlist_result); - FreeNode(m_Goal); - } + // 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 - // 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 std::vector< Node * >::iterator iterOpen = m_OpenList.begin(); + } - while( iterOpen != m_OpenList.end() ) - { - Node *n = (*iterOpen); + // Successor in open list + // 1 - Update old version of this node in open list + // 2 - sort heap again in open list - if( !n->child ) - { - FreeNode( n ); + 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; - n = NULL; - } + // Free successor node + FreeNode((*successor)); - iterOpen ++; - } + // 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.insert(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 + } - m_OpenList.clear(); + 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 std::vector::iterator iterOpen = m_OpenList.begin(); - // iterate closed list and delete unused nodes - typename std::unordered_set::iterator iterClosed; + while (iterOpen != m_OpenList.end()) { + Node* n = (*iterOpen); + FreeNode(n); - for( iterClosed = m_ClosedList.begin(); iterClosed != m_ClosedList.end(); iterClosed ++ ) - { - Node *n = (*iterClosed); + iterOpen++; + } - if( !n->child ) - { - FreeNode( n ); - n = NULL; + m_OpenList.clear(); - } - } + // iterate closed list and delete unused nodes + typename std::unordered_set::iterator iterClosed; - m_ClosedList.clear(); + for (iterClosed = m_ClosedList.begin(); iterClosed != m_ClosedList.end(); iterClosed++) { + Node* n = (*iterClosed); + FreeNode(n); + } - } + m_ClosedList.clear(); - // Node memory management - Node *AllocateNode() - { + // delete the goal -#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 - } + 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 std::vector::iterator iterOpen = m_OpenList.begin(); + + while (iterOpen != m_OpenList.end()) { + Node* n = (*iterOpen); + + if (!n->child) { + FreeNode(n); + + n = NULL; + } - void FreeNode( Node *node ) - { + iterOpen++; + } - m_AllocateNodeCount --; + m_OpenList.clear(); + // iterate closed list and delete unused nodes + typename std::unordered_set::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 - delete node; + m_AllocateNodeCount++; + Node* p = new Node; + return p; #else - node->~Node(); - m_FixedSizeAllocator.free( node ); + Node* address = m_FixedSizeAllocator.alloc(); + + if (!address) { + return NULL; + } + m_AllocateNodeCount++; + Node* p = new (address) Node; + return p; #endif - } + } -private: // data + void FreeNode(Node* node) { + m_AllocateNodeCount--; - // Heap (simple vector but used as a heap, cf. Steve Rabin's game gems article) - std::vector< Node *> m_OpenList; +#if !USE_FSA_MEMORY + delete node; +#else + node->~Node(); + m_FixedSizeAllocator.free(node); +#endif + } - // Closed is an unordered_set - struct NodeHash { - size_t operator() (Node* const& n) const { - return n->m_UserState.Hash(); - } - }; - struct NodeEqual { - bool operator()(Node* a, Node* b) const { - return a->m_UserState.IsSameState(b->m_UserState); - } - }; - std::unordered_set m_ClosedList; + private: // data + // Heap (simple vector but used as a heap, cf. Steve Rabin's game gems article) + std::vector m_OpenList; + // Closed is an unordered_set + struct NodeHash { + size_t operator()(Node* const& n) const { + return n->m_UserState.Hash(); + } + }; + struct NodeEqual { + bool operator()(Node* a, Node* b) const { + return a->m_UserState.IsSameState(b->m_UserState); + } + }; + std::unordered_set m_ClosedList; - // Successors is a vector filled out by the user each type successors to a node - // are generated - std::vector< Node * > m_Successors; + // Successors is a vector filled out by the user each type successors to a node + // are generated + std::vector m_Successors; - // State - unsigned int m_State; + // State + unsigned int m_State; - // Counts steps - int m_Steps; + // Counts steps + int m_Steps; - // Start and goal state pointers - Node *m_Start; - Node *m_Goal; + // Start and goal state pointers + Node* m_Start; + Node* m_Goal; - Node *m_CurrentSolutionNode; + Node* m_CurrentSolutionNode; #if USE_FSA_MEMORY - // Memory - FixedSizeAllocator m_FixedSizeAllocator; + // Memory + FixedSizeAllocator m_FixedSizeAllocator; #endif - - //Debug : need to keep these two iterators around - // for the user Dbg functions - typename std::vector< Node * >::iterator iterDbgOpen; - typename std::unordered_set::iterator iterDbgClosed; - // debugging : count memory allocation and free's - int m_AllocateNodeCount; - - bool m_CancelRequest; + // Debug : need to keep these two iterators around + // for the user Dbg functions + typename std::vector::iterator iterDbgOpen; + typename std::unordered_set::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 - virtual size_t Hash() = 0; // Returns a hash for the state +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 + virtual size_t Hash() = 0; // Returns a hash for the state }; #endif - - diff --git a/cpp/tests.cpp b/cpp/tests.cpp index 2ddc68b..16f5739 100644 --- a/cpp/tests.cpp +++ b/cpp/tests.cpp @@ -1,8 +1,8 @@ // A unit test suite +#include #include #include -#include #include "stlastar.h" @@ -11,316 +11,259 @@ using namespace std; 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,9,1,9,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 +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, 9, 1, 9, 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]; -} - +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 ); - size_t Hash(); - - void PrintNodeInfo(); - - +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); + size_t Hash(); + + 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; - } - +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() -{ - const int strSize = 100; - char str[strSize]; - snprintf( str, strSize, "Node position : (%d,%d)\n", x,y ); +void MapSearchNode::PrintNodeInfo() { + const int strSize = 100; + char str[strSize]; + snprintf(str, strSize, "Node position : (%d,%d)\n", x, y); - cout << str; + cout << str; } // Here's the heuristic function that estimates the distance from a Node -// to the Goal. +// to the Goal. -float MapSearchNode::GoalDistanceEstimate( MapSearchNode &nodeGoal ) -{ - return abs(x - nodeGoal.x) + abs(y - nodeGoal.y); +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; - } +bool MapSearchNode::IsGoal(MapSearchNode& nodeGoal) { + if ((x == nodeGoal.x) && (y == nodeGoal.y)) { + return true; + } - return false; + 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; +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 +// 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 ); - +float MapSearchNode::GetCost(MapSearchNode& successor) { + return (float)GetMap(x, y); } -size_t MapSearchNode::Hash() -{ - size_t h1 = hash{}(x); - size_t h2 = hash{}(y); - return h1 ^ (h2 << 1); +size_t MapSearchNode::Hash() { + size_t h1 = hash{}(x); + size_t h2 = hash{}(y); + return h1 ^ (h2 << 1); } -int main(int argc, char *argv[]) { +int main(int argc, char* argv[]) { + AStarSearch astarsearch; + + unsigned int SearchCount = 0; + + const unsigned int NumSearches = 1; + + while (SearchCount < NumSearches) { + // Create a start state + MapSearchNode nodeStart; + nodeStart.x = 0; + nodeStart.y = 0; + + // Define the goal state + MapSearchNode nodeEnd; + nodeEnd.x = 3; + nodeEnd.y = 3; - AStarSearch astarsearch; + // Set Start and goal states - unsigned int SearchCount = 0; - - const unsigned int NumSearches = 1; + astarsearch.SetStartAndGoalStates(nodeStart, nodeEnd); - while(SearchCount < NumSearches) - { - - // Create a start state - MapSearchNode nodeStart; - nodeStart.x = 0; - nodeStart.y = 0; + unsigned int SearchState; + unsigned int SearchSteps = 0; - // Define the goal state - MapSearchNode nodeEnd; - nodeEnd.x = 3; - nodeEnd.y = 3; - - // Set Start and goal states - - astarsearch.SetStartAndGoalStates( nodeStart, nodeEnd ); + do { + SearchState = astarsearch.SearchStep(); - unsigned int SearchState; - unsigned int SearchSteps = 0; + SearchSteps++; - do - { - SearchState = astarsearch.SearchStep(); +#if DEBUG_LISTS - SearchSteps++; + cout << "Steps:" << SearchSteps << "\n"; - #if DEBUG_LISTS + int len = 0; - cout << "Steps:" << SearchSteps << "\n"; + cout << "Open:\n"; + MapSearchNode* p = astarsearch.GetOpenListStart(); + while (p) { + len++; +#if !DEBUG_LIST_LENGTHS_ONLY + ((MapSearchNode*)p)->PrintNodeInfo(); +#endif + p = astarsearch.GetOpenListNext(); + } - int len = 0; + cout << "Open list has " << len << " nodes\n"; - cout << "Open:\n"; - MapSearchNode *p = astarsearch.GetOpenListStart(); - while( p ) - { - len++; - #if !DEBUG_LIST_LENGTHS_ONLY - ((MapSearchNode *)p)->PrintNodeInfo(); - #endif - p = astarsearch.GetOpenListNext(); - - } + len = 0; - cout << "Open list has " << len << " nodes\n"; + cout << "Closed:\n"; + p = astarsearch.GetClosedListStart(); + while (p) { + len++; +#if !DEBUG_LIST_LENGTHS_ONLY + p->PrintNodeInfo(); +#endif + p = astarsearch.GetClosedListNext(); + } - len = 0; + cout << "Closed list has " << len << " nodes\n"; +#endif - cout << "Closed:\n"; - p = astarsearch.GetClosedListStart(); - while( p ) - { - len++; - #if !DEBUG_LIST_LENGTHS_ONLY - p->PrintNodeInfo(); - #endif - p = astarsearch.GetClosedListNext(); - } + } while (SearchState == AStarSearch::SEARCH_STATE_SEARCHING); - cout << "Closed list has " << len << " nodes\n"; - #endif + if (SearchState == AStarSearch::SEARCH_STATE_SUCCEEDED) { + cout << "Search found goal state\n"; - } - while( SearchState == AStarSearch::SEARCH_STATE_SEARCHING ); + MapSearchNode* node = astarsearch.GetSolutionStart(); - if( SearchState == AStarSearch::SEARCH_STATE_SUCCEEDED ) - { - cout << "Search found goal state\n"; +#if DISPLAY_SOLUTION + cout << "Displaying solution\n"; +#endif + int steps = 0; - MapSearchNode *node = astarsearch.GetSolutionStart(); + node->PrintNodeInfo(); + for (;;) { + node = astarsearch.GetSolutionNext(); - #if DISPLAY_SOLUTION - cout << "Displaying solution\n"; - #endif - int steps = 0; + if (!node) { + break; + } - node->PrintNodeInfo(); - for( ;; ) - { - node = astarsearch.GetSolutionNext(); + node->PrintNodeInfo(); + steps++; + }; - if( !node ) - { - break; - } + cout << "Solution steps " << steps << endl; - node->PrintNodeInfo(); - steps ++; - - }; + // Once you're done with the solution you can free the nodes up + astarsearch.FreeSolutionNodes(); - cout << "Solution steps " << steps << endl; + } else if (SearchState == AStarSearch::SEARCH_STATE_FAILED) { + cout << "Search terminated. Did not find goal state\n"; + } - // Once you're done with the solution you can free the nodes up - astarsearch.FreeSolutionNodes(); + // Display the number of loops the search went through + cout << "SearchSteps : " << SearchSteps << "\n"; - - } - 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++; - SearchCount ++; - - astarsearch.EnsureMemoryFreed(); - } - - assert(true && "failed to be true"); + astarsearch.EnsureMemoryFreed(); + } - printf("Tests succeeded\n"); + assert(true && "failed to be true"); + printf("Tests succeeded\n"); }