From 206bdff9e744d4921f57f7b41010f00ccbb09b35 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Sun, 1 May 2016 00:41:33 +0200 Subject: [PATCH] Inline and vectorize haversine for GetPathDistance --- .../routing_algorithms/routing_base.hpp | 48 +++++++++++++++---- src/util/coordinate_calculation.cpp | 2 +- 2 files changed, 41 insertions(+), 9 deletions(-) diff --git a/include/engine/routing_algorithms/routing_base.hpp b/include/engine/routing_algorithms/routing_base.hpp index 2215c24cb..deeec646a 100644 --- a/include/engine/routing_algorithms/routing_base.hpp +++ b/include/engine/routing_algorithms/routing_base.hpp @@ -779,18 +779,50 @@ template class BasicRoutingInterface nodes.target_phantom = target_phantom; UnpackPath(packed_path.begin(), packed_path.end(), nodes, unpacked_path); - util::Coordinate previous_coordinate = source_phantom.location; - util::Coordinate current_coordinate; + using util::coordinate_calculation::detail::DEGREE_TO_RAD; + using util::coordinate_calculation::detail::EARTH_RADIUS; + double distance = 0; + double prev_lat = + static_cast(toFloating(source_phantom.location.lat)) * DEGREE_TO_RAD; + double prev_lon = + static_cast(toFloating(source_phantom.location.lon)) * DEGREE_TO_RAD; + double prev_cos = std::cos(prev_lat); for (const auto &p : unpacked_path) { - current_coordinate = facade->GetCoordinateOfNode(p.turn_via_node); - distance += util::coordinate_calculation::haversineDistance(previous_coordinate, - current_coordinate); - previous_coordinate = current_coordinate; + const auto current_coordinate = facade->GetCoordinateOfNode(p.turn_via_node); + + const double current_lat = + static_cast(toFloating(current_coordinate.lat)) * DEGREE_TO_RAD; + const double current_lon = + static_cast(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::haversineDistance(previous_coordinate, - target_phantom.location); + + const double current_lat = + static_cast(toFloating(target_phantom.location.lat)) * DEGREE_TO_RAD; + const double current_lon = + static_cast(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/src/util/coordinate_calculation.cpp b/src/util/coordinate_calculation.cpp index 334815353..4e3492db8 100644 --- a/src/util/coordinate_calculation.cpp +++ b/src/util/coordinate_calculation.cpp @@ -41,8 +41,8 @@ double haversineDistance(const Coordinate coordinate_1, const Coordinate coordin const double ln1 = lon1 / COORDINATE_PRECISION; const double lt2 = lat2 / COORDINATE_PRECISION; const double ln2 = lon2 / COORDINATE_PRECISION; - const double dlat1 = lt1 * detail::DEGREE_TO_RAD; + const double dlat1 = lt1 * detail::DEGREE_TO_RAD; const double dlong1 = ln1 * detail::DEGREE_TO_RAD; const double dlat2 = lt2 * detail::DEGREE_TO_RAD; const double dlong2 = ln2 * detail::DEGREE_TO_RAD;