NavHeuristics.hpp

Go to the documentation of this file.

#ifndef LGL_NAVIGATION_HEURISTICS_HPP
#define LGL_NAVIGATION_HEURISTICS_HPP

#include <math.h>

#include <lgl/Location.hpp>
#include <lgl/NavHeuDistances.hpp>

namespace jafar { 
  namespace lgl { 

    // HEURISTIC 1 : Euclidean Distance
    template <
      class Graph,    // the graph in question 
      class CostType, // the CostType (int, double or any other class)
      typename Vertex    //the vertex template
      >
    class nav_heuristic : public boost::astar_heuristic<Graph, CostType>
    {
      public:
    
        // constructor of the heuristic : take the goal
        nav_heuristic(const Graph& _g, const Vertex& _goal)
          : g(_g), goal(_goal) {}
        
        CostType operator()(const Vertex& u)
        {
          return Euclidean2DDist<NavLocation,CostType>(g[u].pos, g[goal].pos);      
        }
    
      private:
        Graph g;
        Vertex goal;
    };

    // HEURISTIC 2 : Euclidean Distance + Path Length
    template <
      class Graph,    // the graph in question 
      class CostType, // the CostType (int, double or any other class)
      typename Vertex    //the vertex template
      >
    class nav_heuristic_rdv: public boost::astar_heuristic<Graph, CostType>
    {
      public:
        nav_heuristic_rdv(const Graph& g_, const Vertex& goal_, CostType authDist_, std::vector<Vertex> &predMap_)
          : g(g_), goal(goal_), authDist(authDist_), predMap(predMap_) {}
        
        CostType operator()(const Vertex& u)
        {
          CostType eucDist = Euclidean2DDist<NavLocation,CostType>(g[u].pos, g[goal].pos);      
          // Calculating path length at this vertex greedily.
          // Could be optimized.
          CostType pathLength = 0.0;
          for(Vertex ui = u;; ui = predMap[ui]) {
            if(predMap[ui] == ui)
            {
              break;
            }
            pathLength += Euclidean2DDist<NavLocation,CostType>(
              g[predMap[ui]].pos,
              g[ui].pos);
          }

          // Adapting pathLength for cost: using squared differences
          // Cost grows fast when pathLength is not close to authDist.
          pathLength = fabs(authDist - pathLength);
          //pathLength = pathLength * pathLength;

          return (eucDist + pathLength);

        }
    
      private:
        Graph g;
        Vertex goal;
        Vertex start;
        CostType authDist;
        std::vector<Vertex> predMap;
    };
  }
}
#endif /* LGL_NAVIGATION_HEURISTICS_HPP */