DiagonalDistance.hpp

Go to the documentation of this file.

 
// Copyright (c) 2017 Czech Technical University in Prague | Faculty of Information Technology. All rights reserved.
 
#pragma once
 
#include <cmath>
#include <alib/pair>
#include <functional>
 
namespace graph {
 
namespace heuristic {
 
class DiagonalDistance {
// ---------------------------------------------------------------------------------------------------------------------
 public:
  template<typename TCoordinate>
  static double diagonalDistance(const ext::pair<TCoordinate, TCoordinate> &goal,
                                 const ext::pair<TCoordinate, TCoordinate> &node);
 
// ---------------------------------------------------------------------------------------------------------------------
 
  template<typename TCoordinate>
  static std::function<double(const ext::pair<TCoordinate, TCoordinate> &,
                              const ext::pair<TCoordinate, TCoordinate> &)> diagonalDistanceFunction();
 
// ---------------------------------------------------------------------------------------------------------------------
 
};
 
// =====================================================================================================================
 
template<typename TCoordinate>
double DiagonalDistance::diagonalDistance(const ext::pair<TCoordinate, TCoordinate> &goal,
                                          const ext::pair<TCoordinate, TCoordinate> &node) {
  TCoordinate d_max = std::max(std::abs(node.first - goal.first), std::abs(node.second - goal.second));
  TCoordinate d_min = std::min(std::abs(node.first - goal.first), std::abs(node.second - goal.second));
 
  return M_SQRT2 * d_min + (d_max - d_min);
}
 
// ---------------------------------------------------------------------------------------------------------------------
 
template<typename TCoordinate>
std::function<double(const ext::pair<TCoordinate, TCoordinate> &,
                     const ext::pair<TCoordinate, TCoordinate> &)> DiagonalDistance::diagonalDistanceFunction() {
  return diagonalDistance < TCoordinate >;
}
 
// ---------------------------------------------------------------------------------------------------------------------
 
} // namespace heuristic
 
} // namespace graph