NavHeuDistances.hpp

Go to the documentation of this file.

#ifndef LGL_NAVIGATION_DISTANCES_HEURISTICS_HPP
#define LGL_NAVIGATION_DISTANCES_HEURISTICS_HPP

#include <math.h>

namespace jafar { 
  namespace lgl { 

    // Heuristics for grid maps
    // 
    // On a grid, there are well-known heuristic functions to use.
    // 
    // 1 - Manhattan distance
    // 
    // h(n) = (abs(n.x-goal.x) + abs(n.y-goal.y))
    // 
    // 2 - Diagonal distance
    // 
    // h(n) = max(abs(n.x-goal.x), abs(n.y-goal.y))
    // 
    // 3 - Euclidean Distance 
    // 
    // h(n) = sqrt((n.x-goal.x)^2 + (n.y-goal.y)^2)
    // 

    // Amit P Stanford Distance : http://theory.stanford.edu/~amitp/GameProgramming/
    template <class Point, class DistType> 
    DistType mahattanDist(const Point& v1, const Point& v2, const int& d1, const int& d2) {

      DistType a1 = d1*v1.x();
      DistType a2 =  d1*v1.y() + v1.x()%d1 - v1.x();
      DistType a3 = -d1*v1.y() - v1.x()%d1 - v1.x(); // == -a1-a2

      DistType b1 = d1*v2.x();
      DistType b2 =  d1*v2.y() + v2.x()%d1 - v2.x();
      DistType b3 = -d1*v2.y() - v2.x()%d1 - v2.x(); // == -b1-b2

      /* One step on the map is 10 in this function */
      return d2*max(abs(a1-b1), max(abs(a2-b2), abs(a3-b3)));   
    }

    template <class Point, class DistType> 
    DistType Mahattan2dDist(const Point& v1, const Point& v2) {
      return (fabs(v1.x()-v2.x()) + fabs(v1.y()-v2.y()));   
    }

    template <class Point, class DistType> 
    DistType Mahattan3dDist(const Point& v1, const Point& v2) {
      return fabs(v1.x()-v2.x()) + fabs(v1.y()-v2.y()) + fabs(v1.z()-v2.z());   
    }

    template <class Point, class DistType> 
    DistType Euclidean2DDist(const Point& v1, const Point& v2) {
      DistType dx = v2.x() - v1.x();
      DistType dy = v2.y() - v1.y();
      return sqrt(dx * dx + dy * dy);
    }
  }
}

#endif /* LGL_NAVIGATION_DISTANCES_HEURISTICS_HPP */