[Boost-users] BGL: A* search & implicit graph

Subject: [Boost-users] BGL: A* search & implicit graph
From: Damien Maupu (damien.maupu_at_[hidden])
Date: 2010-03-01 08:32:18

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 <boost/graph/astar_search.hpp>

#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<graph_name_t, int> m_GraphProperty;

    typedef adjacency_list<vecS, vecS, directedS, m_Vertex, m_Edge, 
m_GraphProperty> m_SearchGraph;

    typedef graph_traits<m_SearchGraph>::vertex_descriptor    m_VertexId;
    typedef graph_traits<m_SearchGraph>::edge_descriptor    m_EdgeId;

    typedef graph_traits<m_SearchGraph>::vertex_iterator    m_VertexIt;
    typedef graph_traits<m_SearchGraph>::adjacency_iterator    
m_AdjacencyIt;
    typedef graph_traits<m_SearchGraph>::edge_iterator        m_EdgeIt;
    typedef graph_traits<m_SearchGraph>::out_edge_iterator    m_OutEdgeIt;

    //heuristic
    class m_Heuristic : public astar_heuristic<m_SearchGraph, double>
    {
    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<m_SearchGraph, vertex_index_t>::type IndexMap;
    typedef property_map<m_SearchGraph, double m_Edge::*>::type WeightMap;
    typedef property_map<m_SearchGraph, int m_Vertex::*>::type 
PredecessorMap;
    typedef property_map<m_SearchGraph, double m_Vertex::*>::type 
DistanceMap;
    typedef property_map<m_SearchGraph, double m_Vertex::*>::type CostMap;
    typedef property_map<m_SearchGraph, default_color_type 
m_Vertex::*>::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