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