Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
tree: aadd1f610c
Fetching contributors…

Cannot retrieve contributors at this time

349 lines (251 sloc) 7.67 kb
////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// 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 <math.h>
#define DEBUG_LISTS 0
#define DEBUG_LIST_LENGTHS_ONLY 0
using namespace std;
//using namespace astar;
// map helper functions
static uint8_t* _map = NULL;
static unsigned int _map_width=0, _map_height=0;
uint8_t GetMap( unsigned int x, unsigned int y )
{
assert(_map);
if(x >= _map_width || y >= _map_height)
return 9;
return _map[(y*_map_width)+x];
}
// Definitions
class MapSearchNode
{
public:
unsigned int x; // the (x,y) positions of the node
unsigned int y;
MapSearchNode() { x = y = 0; }
MapSearchNode( unsigned int px, unsigned 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()
{
//cout << x << "\t" << y << endl;
cout << "Node position : (" << x << ", " << y << ")" << endl;
}
// Here's the heuristic function that estimates the distance from a Node
// to the Goal.
float MapSearchNode::GoalDistanceEstimate( MapSearchNode &nodeGoal )
{
float xd = fabs(float(((float)x - (float)nodeGoal.x)));
float yd = fabs(float(((float)y - (float)nodeGoal.y)));
return xd + yd;
//return 1.001 * (xd + yd );
}
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;
int ix = (int)x;
int iy = (int)y;
// push each possible move except allowing the search to go backwards
if( (GetMap( ix-1, iy ) < 9u)
&& !((parent_x == ix-1) && (parent_y == iy))
)
{
NewNode = MapSearchNode( ix-1, iy );
astarsearch->AddSuccessor( NewNode );
}
if( (GetMap( ix, iy-1 ) < 9u)
&& !((parent_x == ix) && (parent_y == iy-1))
)
{
NewNode = MapSearchNode( ix, iy-1 );
astarsearch->AddSuccessor( NewNode );
}
if( (GetMap( ix+1, iy ) < 9u)
&& !((parent_x == ix+1) && (parent_y == iy))
)
{
NewNode = MapSearchNode( ix+1, iy );
astarsearch->AddSuccessor( NewNode );
}
if( (GetMap( ix, iy+1 ) < 9u)
&& !((parent_x == ix) && (parent_y == iy+1))
)
{
NewNode = MapSearchNode( ix, iy+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
#include "astar.h"
using namespace ast;
bool ast::astar( uint8_t* map,
uint32_t width,
uint32_t height,
const point_t start,
const point_t goal,
std::vector<point_t>& path )
{
//cout << "STL A* Search implementation\n(C)2001 Justin Heyes-Jones\n";
// set the static vars
_map = map;
_map_width = width;
_map_height = height;
// 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;
bool path_found = false;
while(SearchCount < NumSearches)
{
// Create a start state
MapSearchNode nodeStart;
nodeStart.x = start.x;
nodeStart.y = start.y;
// Define the goal state
MapSearchNode nodeEnd;
nodeEnd.x = goal.x;
nodeEnd.y = goal.y;
// 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();
path.push_back( point_t( node->x, node->y ) );
for( ;; )
{
node = astarsearch.GetSolutionNext();
if( !node )
{
break;
}
//node->PrintNodeInfo();
path.push_back( point_t( node->x, node->y ) );
steps ++;
};
//cout << "Solution steps " << steps << endl;
// Once you're done with the solution you can free the nodes up
astarsearch.FreeSolutionNodes();
path_found = true;
}
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 path_found;
}
// // STL container iterator macros
// #define VAR(V,init) __typeof(init) V=(init)
// #define FOR_EACH(I,C) for(VAR(I,(C).begin());I!=(C).end();I++)
// int main( int argc, char *argv[] )
// {
// std::vector<point_t> path;
// bool result = astar( map, 30, 30, point_t( 1,1 ), point_t( 25,20 ), path );
// printf( "#%s:\n", result ? "PATH" : "NOPATH" );
// FOR_EACH( it, path )
// printf( "%d, %d\n", it->x, it->y );
// puts("");
// }
Jump to Line
Something went wrong with that request. Please try again.