diff --git a/include/engine/routing_algorithms/routing_base.hpp b/include/engine/routing_algorithms/routing_base.hpp index 0c0808a3f..f1a89f195 100644 --- a/include/engine/routing_algorithms/routing_base.hpp +++ b/include/engine/routing_algorithms/routing_base.hpp @@ -323,53 +323,19 @@ 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; - 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); + auto prev_coordinate = source_phantom.location; for (const auto &p : unpacked_path) { const auto current_coordinate = facade.GetCoordinateOfNode(p.turn_via_node); - - 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, current_coordinate); + prev_coordinate = current_coordinate; } - - 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; + distance += util::coordinate_calculation::fccApproximateDistance(prev_coordinate, target_phantom.location); return distance; } diff --git a/include/util/coordinate_calculation.hpp b/include/util/coordinate_calculation.hpp index c946842a2..884169e4c 100644 --- a/include/util/coordinate_calculation.hpp +++ b/include/util/coordinate_calculation.hpp @@ -23,9 +23,6 @@ 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 cb9adb10a..4303375ff 100644 --- a/src/util/coordinate_calculation.cpp +++ b/src/util/coordinate_calculation.cpp @@ -22,6 +22,11 @@ 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: @@ -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) + 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 detail::EARTH_RADIUS * charv; + return EARTH_RADIUS * charv; } 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 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,