NavAStar.hpp

Go to the documentation of this file.

#ifndef LGL_GRAPH_ASTAR_HPP
#define LGL_GRAPH_ASTAR_HPP

#include <vector>

#include <boost/graph/astar_search.hpp>
#include <boost/function.hpp>

#include <lgl/NavGNode.hpp>
#include <lgl/GraphPath.hpp>
#include <lgl/NavHeuristics.hpp>

using namespace boost;

namespace jafar {
  namespace lgl {
    
    // a star exception for termination
    struct found_goal {}; 
    
    template <class Vertex>
    class astar_goal_visitor : public boost::default_astar_visitor
    {
      public:
        astar_goal_visitor(Vertex goal) : m_goal(goal) 
        { //{{{
          this->eventWatcher =  NULL;
        } //}}}

        astar_goal_visitor(Vertex goal, boost::function<void(const NavGNode&, int)> _eventWatcher ) : m_goal(goal) 
        { //{{{
          this->eventWatcher =  _eventWatcher;
        } //}}}

        template <class Graph>
        void examine_vertex(Vertex u, const Graph& g) {
          if (this->eventWatcher != NULL)
          {
            this->eventWatcher((g)[u], 2); // should be defined in LGLViewer
          }

          if(u == m_goal)
          {
            throw found_goal();
          }
        }

        template <class Graph>
        void discover_vertex(Vertex u, const Graph& g) {
          if (eventWatcher != NULL)
          {
            eventWatcher((g)[u], 1); // should be defined in LGLViewer
          }
        }

      private:
        Vertex m_goal;
        boost::function<void(const NavGNode&, int)> eventWatcher;

    };


    template<
      class Graph, 
      class Vertex, 
      typename CostType
      >
    class NavAStar {
        
      public: 
        
        NavAStar() {}
        ~NavAStar() {}
        
        template <class Heuristic,class WeightMap>
        GraphPath<Vertex> AStarSearch(const Heuristic& heuristic, const Graph& graph,const  WeightMap& weightmap, const Vertex& start, const Vertex& goal, 
        //std::map<Vertex, CostType>& resultingFullCost, 
        std::vector<CostType>& resultingFullCost,
        //std::map<Vertex, Vertex>& predecessorMap,
        std::vector<Vertex>& predecessorMap,
        std::vector<default_color_type>& colorMap,
        boost::function<void(const NavGNode&, int)> _eventWatcher)
        {//{{{
      
          GraphPath<Vertex> shortest_path;
      
          // Used as the distance map: shortest path weight
          std::vector<CostType> dist(num_vertices(graph));

          //-- visitor to catch the events and the end of the
          //   algorithm while the goal is the first in the OPEN list.
          astar_goal_visitor<Vertex> visCatcher(goal, boost::ref(_eventWatcher));
      
          try {
            // call astar named parameter interface
            astar_search
              (graph, start,
               heuristic,
               boost::predecessor_map(&predecessorMap[0]).
               distance_map(&dist[0]).
               weight_map(weightmap).
               rank_map(&resultingFullCost[0]).
               color_map(&colorMap[0]).
               visitor(visCatcher)
               );
            
            
          } catch(found_goal fg) { // found a path to the goal
            for(Vertex v = goal;; v = predecessorMap[v]) {
              shortest_path.push_front(v);
              if (predecessorMap[v] == v) { break; }
            }
          }
      
          return shortest_path; 
        } //}}}

    };
  }
} 
#endif  /* LGL_GRAPH_ASTAR_HPP */