From edd97839abfef6281e4bfffa98287a3dfa829c83 Mon Sep 17 00:00:00 2001 From: Kajari Ghosh Date: Wed, 5 Sep 2018 13:58:45 -0400 Subject: [PATCH] Revert "Use FCC algorithm for map matching distance calculation" This reverts commit a649a8a5cfb0b00d02f2bf50bf6c1db387cb9f2e. --- .../routing_algorithms/routing_base.hpp | 44 ++++++++++++++++--- include/util/coordinate_calculation.hpp | 3 ++ src/util/coordinate_calculation.cpp | 9 +--- 3 files changed, 44 insertions(+), 12 deletions(-) diff --git a/include/engine/routing_algorithms/routing_base.hpp b/include/engine/routing_algorithms/routing_base.hpp index f1a89f195..0c0808a3f 100644 --- a/include/engine/routing_algorithms/routing_base.hpp +++ b/include/engine/routing_algorithms/routing_base.hpp @@ -323,19 +323,53 @@ void annotatePath(const FacadeT &facade, template double getPathDistance(const DataFacade &facade, - const std::vector &unpacked_path, + const std::vector unpacked_path, const PhantomNode &source_phantom, const PhantomNode &target_phantom) { + using util::coordinate_calculation::detail::DEGREE_TO_RAD; + using util::coordinate_calculation::detail::EARTH_RADIUS; + double distance = 0; - auto prev_coordinate = source_phantom.location; + double prev_lat = + static_cast(util::toFloating(source_phantom.location.lat)) * DEGREE_TO_RAD; + double prev_lon = + static_cast(util::toFloating(source_phantom.location.lon)) * DEGREE_TO_RAD; + double prev_cos = std::cos(prev_lat); for (const auto &p : unpacked_path) { const auto current_coordinate = facade.GetCoordinateOfNode(p.turn_via_node); - distance += util::coordinate_calculation::fccApproximateDistance(prev_coordinate, current_coordinate); - prev_coordinate = current_coordinate; + + const double current_lat = + static_cast(util::toFloating(current_coordinate.lat)) * DEGREE_TO_RAD; + const double current_lon = + static_cast(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(util::toFloating(target_phantom.location.lat)) * DEGREE_TO_RAD; + const double current_lon = + static_cast(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; } diff --git a/include/util/coordinate_calculation.hpp b/include/util/coordinate_calculation.hpp index 884169e4c..c946842a2 100644 --- a/include/util/coordinate_calculation.hpp +++ b/include/util/coordinate_calculation.hpp @@ -23,6 +23,9 @@ namespace detail { const constexpr double DEGREE_TO_RAD = 0.017453292519943295769236907684886; 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) { diff --git a/src/util/coordinate_calculation.cpp b/src/util/coordinate_calculation.cpp index 4303375ff..cb9adb10a 100644 --- a/src/util/coordinate_calculation.cpp +++ b/src/util/coordinate_calculation.cpp @@ -22,11 +22,6 @@ namespace coordinate_calculation 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 { public: @@ -117,7 +112,7 @@ double haversineDistance(const Coordinate coordinate_1, const Coordinate coordin 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); const double charv = 2. * std::atan2(std::sqrt(aharv), std::sqrt(1.0 - aharv)); - return EARTH_RADIUS * charv; + return detail::EARTH_RADIUS * charv; } double greatCircleDistance(const Coordinate coordinate_1, const Coordinate coordinate_2) @@ -138,7 +133,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 y_value = float_lat2 - float_lat1; - return std::hypot(x_value, y_value) * EARTH_RADIUS; + return std::hypot(x_value, y_value) * detail::EARTH_RADIUS; } double perpendicularDistance(const Coordinate segment_source,