Open-Harbor/osrm-backend
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 3 Wiki Activity

Use FCC algorithm for map matching distance calculation

Browse Source
This commit is contained in:
Patrick Niklaus 2018-04-26 22:26:01 +00:00 committed by Patrick Niklaus
parent f928956584
commit a649a8a5cf
3 changed files with 12 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
include/engine/routing_algorithms/routing_base.hpp
View File

@ -323,53 +323,19 @@ void annotatePath(const FacadeT &facade,
template <typename Algorithm> template <typename Algorithm>
double getPathDistance(const DataFacade<Algorithm> &facade, double getPathDistance(const DataFacade<Algorithm> &facade,
const std::vector<PathData> unpacked_path, const std::vector<PathData> &unpacked_path,
const PhantomNode &source_phantom, const PhantomNode &source_phantom,
const PhantomNode &target_phantom) const PhantomNode &target_phantom)
{ {
using util::coordinate_calculation::detail::DEGREE_TO_RAD;
using util::coordinate_calculation::detail::EARTH_RADIUS;
double distance = 0; double distance = 0;
double prev_lat = auto prev_coordinate = source_phantom.location;
static_cast<double>(util::toFloating(source_phantom.location.lat)) * DEGREE_TO_RAD;
double prev_lon =
static_cast<double>(util::toFloating(source_phantom.location.lon)) * DEGREE_TO_RAD;
double prev_cos = std::cos(prev_lat);
for (const auto &p : unpacked_path) for (const auto &p : unpacked_path)
{ {
const auto current_coordinate = facade.GetCoordinateOfNode(p.turn_via_node); const auto current_coordinate = facade.GetCoordinateOfNode(p.turn_via_node);
distance += util::coordinate_calculation::fccApproximateDistance(prev_coordinate, current_coordinate);
const double current_lat = prev_coordinate = current_coordinate;
static_cast<double>(util::toFloating(current_coordinate.lat)) * DEGREE_TO_RAD;
const double current_lon =
static_cast<double>(util::toFloating(current_coordinate.lon)) * DEGREE_TO_RAD;
const double current_cos = std::cos(current_lat);
const double sin_dlon = std::sin((prev_lon - current_lon) / 2.0);
const double sin_dlat = std::sin((prev_lat - current_lat) / 2.0);
const double aharv = sin_dlat * sin_dlat + prev_cos * current_cos * sin_dlon * sin_dlon;
const double charv = 2. * std::atan2(std::sqrt(aharv), std::sqrt(1.0 - aharv));
distance += EARTH_RADIUS * charv;
prev_lat = current_lat;
prev_lon = current_lon;
prev_cos = current_cos;
} }
distance += util::coordinate_calculation::fccApproximateDistance(prev_coordinate, target_phantom.location);
const double current_lat =
static_cast<double>(util::toFloating(target_phantom.location.lat)) * DEGREE_TO_RAD;
const double current_lon =
static_cast<double>(util::toFloating(target_phantom.location.lon)) * DEGREE_TO_RAD;
const double current_cos = std::cos(current_lat);
const double sin_dlon = std::sin((prev_lon - current_lon) / 2.0);
const double sin_dlat = std::sin((prev_lat - current_lat) / 2.0);
const double aharv = sin_dlat * sin_dlat + prev_cos * current_cos * sin_dlon * sin_dlon;
const double charv = 2. * std::atan2(std::sqrt(aharv), std::sqrt(1.0 - aharv));
distance += EARTH_RADIUS * charv;
return distance; return distance;
} }

3
include/util/coordinate_calculation.hpp
View File

@ -23,9 +23,6 @@ namespace detail
{ {
const constexpr double DEGREE_TO_RAD = 0.017453292519943295769236907684886; const constexpr double DEGREE_TO_RAD = 0.017453292519943295769236907684886;
const constexpr double RAD_TO_DEGREE = 1. / DEGREE_TO_RAD; const constexpr double RAD_TO_DEGREE = 1. / DEGREE_TO_RAD;
// earth radius varies between 6,356.750-6,378.135 km (3,949.901-3,963.189mi)
// The IUGG value for the equatorial radius is 6378.137 km (3963.19 miles)
const constexpr long double EARTH_RADIUS = 6372797.560856;
inline double degToRad(const double degree) inline double degToRad(const double degree)
{ {

9
src/util/coordinate_calculation.cpp
View File

@ -22,6 +22,11 @@ namespace coordinate_calculation
namespace namespace
{ {
// earth radius varies between 6,356.750-6,378.135 km (3,949.901-3,963.189mi)
// The IUGG value for the equatorial radius is 6378.137 km (3963.19 miles)
const constexpr long double EARTH_RADIUS = 6372797.560856;
class CheapRulerContainer class CheapRulerContainer
{ {
public: public:
@ -112,7 +117,7 @@ double haversineDistance(const Coordinate coordinate_1, const Coordinate coordin
const double aharv = std::pow(std::sin(dlat / 2.0), 2.0) + const double aharv = std::pow(std::sin(dlat / 2.0), 2.0) +
std::cos(dlat1) * std::cos(dlat2) * std::pow(std::sin(dlong / 2.), 2); std::cos(dlat1) * std::cos(dlat2) * std::pow(std::sin(dlong / 2.), 2);
const double charv = 2. * std::atan2(std::sqrt(aharv), std::sqrt(1.0 - aharv)); const double charv = 2. * std::atan2(std::sqrt(aharv), std::sqrt(1.0 - aharv));
return detail::EARTH_RADIUS * charv; return EARTH_RADIUS * charv;
} }
double greatCircleDistance(const Coordinate coordinate_1, const Coordinate coordinate_2) double greatCircleDistance(const Coordinate coordinate_1, const Coordinate coordinate_2)
@ -133,7 +138,7 @@ double greatCircleDistance(const Coordinate coordinate_1, const Coordinate coord
const double x_value = (float_lon2 - float_lon1) * std::cos((float_lat1 + float_lat2) / 2.0); const double x_value = (float_lon2 - float_lon1) * std::cos((float_lat1 + float_lat2) / 2.0);
const double y_value = float_lat2 - float_lat1; const double y_value = float_lat2 - float_lat1;
return std::hypot(x_value, y_value) * detail::EARTH_RADIUS; return std::hypot(x_value, y_value) * EARTH_RADIUS;
} }
double perpendicularDistance(const Coordinate segment_source, double perpendicularDistance(const Coordinate segment_source,