Hi,
Some weeks ago, I wrote on this mailing list, that I succeed achieving
an A* search using an implicit graph.
A Boost user emailed me to send him an example of my implementation.
As the web is lacking of such example, I think the best place to send
such example would be here as it might help others in the future.
My example might not be the best, the most optimized or most well-coded.
I am open to suggestions, corrections and comments.
Let me know if something unclear.
The code comes below.
Few words to quickly explain the code:
The goal is to A* search a graph (named m_SearchGraph) that grows (i.e.
m_SearchGraph is implicit) based on another graph (named MyGraph).
The search starts from 2D point (_startX, _startZ) and must get enough
close (less than error "_epsilon") to 2D point (_goalX, _goalZ).
The example is dumb and simply increment current position whatever the
node in MyGraph (i.e. currX++, currZ++, currWeight += sqrt(2)).
User need to adapt this dummy behavior depending on its needs (search
for "//to change").
Most adaptations have to be done in "examine_vertex" (and also by
providing ref/ptr to needed objects to the visitor constructor)
PS: The implementation assumes both graphs are adjacency_list with
"vecs" for both edges and vertices (in which a vertex/edge descriptor is
equivalent to an "int", for my understanding)
PS2: Id == descriptor (e.g. edgeId == edge_descriptor)
====================================================================================
====================================================================================
astarSearch.h
====================================================================================
#ifndef ASTARSEARCH_H
#define ASTARSEARCH_H
#include
#include "myGraph.h"
class AStarSearch
{
public:
AStarSearch(MyGraph * _myGraph);
virtual ~AStarSearch();
bool findPath(MyGraph::VertexId _startMyGraphVertextId, double
_startX, double _startZ, double _goalX, double _goalZ, double _epsilon);
private:
//search graph
struct m_Vertex
{
m_Vertex()
{
currX = 0.0;
currZ = 0.0;
refMyGraphVertexId = -1;
prevVertexId = -1;
double inf = numeric_limits<double>::max
BOOST_PREVENT_MACRO_SUBSTITUTION ();
distance = inf;
cost = inf;
color = white_color;
}
//properties for heuristic / visitor
double currX;
double currZ;
int refMyGraphVertexId; //reference to correspondant vertex in
myGraph
//properties required by astar_search_no_init(...)
int prevVertexId; // == predecessor
double distance;
double cost;
default_color_type color;
};
struct m_Edge
{
m_Edge()
{
weight = 0.0;
}
double weight;
};
//graph property = save the vertex id when goal is reached
//saving goal vertex could be also saved by a reference to an int
passed to the visitor class
typedef property m_GraphProperty;
typedef adjacency_list m_SearchGraph;
typedef graph_traits::vertex_descriptor m_VertexId;
typedef graph_traits::edge_descriptor m_EdgeId;
typedef graph_traits::vertex_iterator m_VertexIt;
typedef graph_traits::adjacency_iterator
m_AdjacencyIt;
typedef graph_traits::edge_iterator m_EdgeIt;
typedef graph_traits::out_edge_iterator m_OutEdgeIt;
//heuristic
class m_Heuristic : public astar_heuristic
{
public:
m_Heuristic(m_SearchGraph & _searchGraph, double _goalX, double
_goalZ);
virtual ~m_Heuristic();
double operator()(m_VertexId _currVertexId);
private:
m_SearchGraph & m_searchGraph;
double m_goalX;
double m_goalZ;
};
//exception to end search
struct m_endSearch{};
//visitor
class m_Visitor : public default_astar_visitor
{
public:
m_Visitor(MyGraph * _myGraph,
m_SearchGraph & _searchGraph,
double _goalX, double _goalZ,
double _epsilon);
virtual ~m_Visitor();
void examine_vertex(m_VertexId _currVertexId, const
m_SearchGraph & _graph);
private:
MyGraph * m_myGraph;
m_SearchGraph & m_searchGraph;
double m_goalX;
double m_goalZ;
double m_epsilon;
};
void m_printSearchGraph(m_SearchGraph & _searchGraph);
list<int> m_getPath(m_SearchGraph & _searchGraph, m_VertexId _vertexId);
vector< list<int> > m_getAllPaths(m_SearchGraph & _searchGraph);
void m_printPath(list<int> _path);
void m_printShortestPath(m_SearchGraph & _searchGraph);
void m_printAllPaths(m_SearchGraph & _searchGraph);
MyGraph * m_myGraph;
};
#endif
====================================================================================
====================================================================================
====================================================================================
aStarSearch.cpp
====================================================================================
====================================================================================
#include "aStarSearch.h"
AStarSearch::AStarSearch(MyGraph * _myGraph):
m_myGraph(_myGraph)
{
}
AStarSearch::~AStarSearch()
{
}
bool AStarSearch::findPath(MyGraph::VertexId _startMyGraphVertextId,
double _startX, double _startZ, double _goalX, double _goalZ, double
_epsilon)
{
m_SearchGraph searchGraph;
m_Heuristic heuristic(searchGraph, _goalX, _goalZ);
m_Visitor visitor(m_myGraph, searchGraph, _goalX, _goalZ, _epsilon);
//typedef property maps
typedef property_map::type IndexMap;
typedef property_map::type WeightMap;
typedef property_map::type
PredecessorMap;
typedef property_map::type
DistanceMap;
typedef property_map::type CostMap;
typedef property_map::type ColorMap;
//create property maps
IndexMap indexMap = get(vertex_index, searchGraph);
WeightMap weightMap = get(&m_Edge::weight, searchGraph);
PredecessorMap predecessorMap = get(&m_Vertex::prevVertexId,
searchGraph);
DistanceMap distanceMap = get(&m_Vertex::distance, searchGraph);
CostMap costMap = get(&m_Vertex::cost, searchGraph);
ColorMap colorMap = get(&m_Vertex::color, searchGraph);
double inf = numeric_limits<double>::max
BOOST_PREVENT_MACRO_SUBSTITUTION ();
double zero = double();
//add & init first vertex
m_VertexId startVertexId = add_vertex(searchGraph);
m_Vertex & startVertex = searchGraph[startVertexId];
startVertex.currX = _startX;
startVertex.currZ = _startZ;
startVertex.refMyGraphVertexId = _startMyGraphVertextId;
startVertex.prevVertexId = startVertexId;
startVertex.distance = zero;
startVertex.cost = heuristic(startVertexId);
try
{
astar_search_no_init(searchGraph,
startVertexId,
heuristic,
visitor,
predecessorMap,
costMap,
distanceMap,
weightMap,
colorMap,
indexMap,
std::less<double>(),
std::plus<double>(),
inf,
zero);
}
catch(m_endSearch end)
{
m_printSearchGraph(searchGraph);
m_printShortestPath(searchGraph);
m_printAllPaths(searchGraph);
return true;
}
return false;
}
AStarSearch::m_Heuristic::m_Heuristic(m_SearchGraph & _searchGraph,
double _goalX, double _goalZ):
m_searchGraph(_searchGraph),
m_goalX(_goalX),
m_goalZ(_goalZ)
{
}
AStarSearch::m_Heuristic::~m_Heuristic()
{
}
double AStarSearch::m_Heuristic::operator()(m_VertexId _currVertexId)
{
m_Vertex currVertex = m_searchGraph[_currVertexId];
return sqrt(pow(m_goalX - currVertex.currX, 2) + pow(m_goalZ -
currVertex.currZ, 2)); //for A* search
//return 0; //for BFS search
}
AStarSearch::m_Visitor::m_Visitor(MyGraph * _myGraph, m_SearchGraph &
_searchGraph, double _goalX, double _goalZ, double _epsilon):
m_myGraph(_myGraph),
m_searchGraph(_searchGraph),
m_goalX(_goalX),
m_goalZ(_goalZ),
m_epsilon(_epsilon)
{
}
AStarSearch::m_Visitor::~m_Visitor()
{
}
void AStarSearch::m_Visitor::examine_vertex(m_VertexId _currVertexId,
const m_SearchGraph & _graph)
{
//get currVertex / currX / currZ
m_Vertex currVertex = m_searchGraph[_currVertexId];
double currX = currVertex.currX;
double currZ = currVertex.currZ;
//check if goal is reached
if(sqrt(pow(m_goalX - currX, 2) + pow(m_goalZ - currZ, 2)) < m_epsilon)
{
set_property(m_searchGraph, graph_name, _currVertexId);
//_currVertexId == goal vertex id
throw m_endSearch();
}
//declare currMyGraphVertexId / nextMyGraphVertexId /
currMyGraphEdgeId / currMyGraphEdge
MyGraph::VertexId currMyGraphVertexId = currVertex.refMyGraphVertexId;
MyGraph::VertexId nextMyGraphVertexId;
MyGraph::EdgeId currMyGraphEdgeId;
bool currMyGraphEdgeExists;
MyGraph::Edge currMyGraphEdge;
//declare newVertexId / newEdgeId / newEdge
m_VertexId newVertexId;
m_EdgeId newEdgeId;
bool newEdgeCreated;
//generate neighbors to currVertex
MyGraph::OutEdgeIt outMyGraph_i, outMyGraph_end;
for(tie(outMyGraph_i, outMyGraph_end) =
out_edges(currMyGraphVertexId, *m_myGraph); outMyGraph_i !=
outMyGraph_end ; ++outMyGraph_i)
{
//get nexMyGraphVertexId / currMyGraphEdgeId / currMyGraphEdge
nextMyGraphVertexId = target(*outMyGraph_i, *m_myGraph);
tie(currMyGraphEdgeId, currMyGraphEdgeExists) =
edge(currMyGraphVertexId, nextMyGraphVertexId, *m_myGraph);
if(currMyGraphEdgeExists) currMyGraphEdge =
(*m_myGraph)[currMyGraphEdgeId];
else exit(1);
//add/set newVertex
newVertexId = add_vertex(m_searchGraph);
m_Vertex & newVertex = m_searchGraph[newVertexId];
newVertex.currX = currX + 1; //to change
newVertex.currZ = currZ + 1; //to change
newVertex.refMyGraphVertexId = nextMyGraphVertexId;
newVertex.prevVertexId = newVertexId;
//add/set newEdge
tie(newEdgeId, newEdgeCreated) = add_edge(_currVertexId,
newVertexId, m_searchGraph);
if(newEdgeCreated)
{
m_Edge & newEdge = m_searchGraph[newEdgeId];
newEdge.weight = sqrt(2); //to change
}
else exit(1);
}
}
void AStarSearch::m_printSearchGraph(m_SearchGraph & _searchGraph)
{
m_Vertex currVertex;
m_VertexIt v_i, v_end;
printf("id\t");
printf("id prev\t");
printf("id ref\t");
printf("dist\t");
printf("cost\t\t");
printf("color\t");
printf("\n");
for(tie(v_i, v_end) = vertices(_searchGraph); v_i != v_end; ++v_i)
{
currVertex = _searchGraph[*v_i];
printf("%i\t", *v_i);
printf("%i\t", currVertex.prevVertexId);
printf("%i\t", currVertex.refMyGraphVertexId);
printf("%f\t", currVertex.distance);
printf("%f\t", currVertex.cost);
printf("%i\t", currVertex.color);
printf("\n");
}
printf("\n");
}
list<int> AStarSearch::m_getPath(m_SearchGraph & _searchGraph,
m_VertexId _vertexId)
{
list<int> path;
for(m_VertexId currVertexId = _vertexId;
; //cf if()... break; below for stopping condition
currVertexId = _searchGraph[currVertexId].prevVertexId)
{
path.push_front(currVertexId);
if(_searchGraph[currVertexId].prevVertexId == int(currVertexId))
break;
}
return path;
}
vector< list<int> > AStarSearch::m_getAllPaths(m_SearchGraph & _searchGraph)
{
m_VertexIt v_i, v_begin, v_end;
for(tie(v_i, v_end) = vertices(_searchGraph); v_i != v_end; ++v_i)
{
_searchGraph[*v_i].color = white_color;
}
vector< list<int> > allPaths;
tie(v_begin, v_i) = vertices(_searchGraph);
for(--v_i;; --v_i)
{
//printf("curr vertex %i\tcolor:%i\n", *v_i,
_searchGraph[*v_i].color);
if(_searchGraph[*v_i].color == white_color)
{
list<int> newPath = m_getPath(_searchGraph, *v_i);
list<int>::iterator currVertex;
for(currVertex = newPath.begin(); currVertex !=
newPath.end(); ++currVertex)
{
_searchGraph[*currVertex].color = black_color;
}
allPaths.push_back(newPath);
}
if(v_i == v_begin)
break;
}
return allPaths;
}
void AStarSearch::m_printPath(list<int> _path)
{
list<int>::iterator pathIt;
for(pathIt = _path.begin(); pathIt != _path.end(); ++pathIt)
{
printf("%i\t", *pathIt);
}
printf("\n");
}
void AStarSearch::m_printShortestPath(m_SearchGraph & _searchGraph)
{
printf("shortest path:\n");
m_printPath(m_getPath(_searchGraph, get_property(_searchGraph,
graph_name)));
}
void AStarSearch::m_printAllPaths(m_SearchGraph & _searchGraph)
{
vector< list<int> > allPaths = m_getAllPaths(_searchGraph);
printf("all paths:\n");
vector< list<int> >::iterator currPath;
for(currPath = allPaths.begin(); currPath != allPaths.end(); ++currPath)
{
m_printPath(*currPath);
}
}
====================================================================================
Cheers,
Damien
