From 01f323741606346bee2b282f4ee5f7106914ef39 Mon Sep 17 00:00:00 2001 From: Dennis Luxen Date: Tue, 20 Jan 2015 16:24:49 +0100 Subject: [PATCH] speed up nearest neighbor query by pruning, move coordinate calculations away from library interface --- CMakeLists.txt | 2 +- Include/osrm/coordinate.hpp | 47 +-- data_structures/coordinate.cpp | 303 +------------- data_structures/coordinate_calculation.cpp | 422 ++++++++++++++++++++ data_structures/coordinate_calculation.hpp | 97 +++++ data_structures/rectangle.hpp | 36 +- data_structures/static_rtree.hpp | 88 ++-- data_structures/upper_bound.hpp | 75 ++++ descriptors/description_factory.cpp | 3 +- descriptors/descriptor_base.hpp | 1 + descriptors/gpx_descriptor.hpp | 6 +- extractor/extraction_containers.cpp | 3 +- unit_tests/data_structures/coordinate.cpp | 33 +- unit_tests/data_structures/static_rtree.cpp | 33 +- 14 files changed, 733 insertions(+), 416 deletions(-) create mode 100644 data_structures/coordinate_calculation.cpp create mode 100644 data_structures/coordinate_calculation.hpp create mode 100644 data_structures/upper_bound.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 356908071..c00006f33 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -72,7 +72,7 @@ file(GLOB ServerGlob Server/*.cpp) file(GLOB DescriptorGlob descriptors/*.cpp) file(GLOB DatastructureGlob data_structures/search_engine_data.cpp data_structures/route_parameters.cpp Util/bearing.cpp) list(REMOVE_ITEM DatastructureGlob data_structures/Coordinate.cpp) -file(GLOB CoordinateGlob data_structures/coordinate.cpp) +file(GLOB CoordinateGlob data_structures/coordinate*.cpp) file(GLOB AlgorithmGlob algorithms/*.cpp) file(GLOB HttpGlob Server/Http/*.cpp) file(GLOB LibOSRMGlob Library/*.cpp) diff --git a/Include/osrm/coordinate.hpp b/Include/osrm/coordinate.hpp index 3229f256f..b6903930e 100644 --- a/Include/osrm/coordinate.hpp +++ b/Include/osrm/coordinate.hpp @@ -1,6 +1,6 @@ /* -Copyright (c) 2013, Project OSRM, Dennis Luxen, others +Copyright (c) 2015, Project OSRM, Dennis Luxen, others All rights reserved. Redistribution and use in source and binary forms, with or without modification, @@ -36,6 +36,7 @@ namespace { constexpr static const float COORDINATE_PRECISION = 1000000.f; } + struct FixedPointCoordinate { int lat; @@ -56,52 +57,8 @@ struct FixedPointCoordinate bool is_valid() const; bool operator==(const FixedPointCoordinate &other) const; - static double - ApproximateDistance(const int lat1, const int lon1, const int lat2, const int lon2); - - static double ApproximateDistance(const FixedPointCoordinate &first_coordinate, - const FixedPointCoordinate &second_coordinate); - - static float ApproximateEuclideanDistance(const FixedPointCoordinate &first_coordinate, - const FixedPointCoordinate &second_coordinate); - - static float - ApproximateEuclideanDistance(const int lat1, const int lon1, const int lat2, const int lon2); - - static float ApproximateSquaredEuclideanDistance(const FixedPointCoordinate &first_coordinate, - const FixedPointCoordinate &second_coordinate); - - static void convertInternalLatLonToString(const int value, std::string &output); - - static void convertInternalCoordinateToString(const FixedPointCoordinate &coordinate, - std::string &output); - - static void convertInternalReversedCoordinateToString(const FixedPointCoordinate &coordinate, - std::string &output); - - static float ComputePerpendicularDistance(const FixedPointCoordinate &segment_source, - const FixedPointCoordinate &segment_target, - const FixedPointCoordinate &query_location); - - static float ComputePerpendicularDistance(const FixedPointCoordinate &segment_source, - const FixedPointCoordinate &segment_target, - const FixedPointCoordinate &query_location, - FixedPointCoordinate &nearest_location, - float &ratio); - - static int - OrderedPerpendicularDistanceApproximation(const FixedPointCoordinate &segment_source, - const FixedPointCoordinate &segment_target, - const FixedPointCoordinate &query_location); - - static float GetBearing(const FixedPointCoordinate &A, const FixedPointCoordinate &B); - float GetBearing(const FixedPointCoordinate &other) const; - void Output(std::ostream &out) const; - - static float DegreeToRadian(const float degree); - static float RadianToDegree(const float radian); }; inline std::ostream &operator<<(std::ostream &out_stream, FixedPointCoordinate const &coordinate) diff --git a/data_structures/coordinate.cpp b/data_structures/coordinate.cpp index 61fa433da..dbddc4742 100644 --- a/data_structures/coordinate.cpp +++ b/data_structures/coordinate.cpp @@ -25,20 +25,19 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +#include "coordinate_calculation.hpp" + #include "../Util/mercator.hpp" -#ifndef NDEBUG #include "../Util/simple_logger.hpp" -#endif -#include "../Util/string_util.hpp" #include - #include +#include + #ifndef NDEBUG #include #endif -#include #include FixedPointCoordinate::FixedPointCoordinate() @@ -87,235 +86,21 @@ bool FixedPointCoordinate::operator==(const FixedPointCoordinate &other) const return lat == other.lat && lon == other.lon; } -double FixedPointCoordinate::ApproximateDistance(const int lat1, - const int lon1, - const int lat2, - const int lon2) -{ - BOOST_ASSERT(lat1 != std::numeric_limits::min()); - BOOST_ASSERT(lon1 != std::numeric_limits::min()); - BOOST_ASSERT(lat2 != std::numeric_limits::min()); - BOOST_ASSERT(lon2 != std::numeric_limits::min()); - double RAD = 0.017453292519943295769236907684886; - double lt1 = lat1 / COORDINATE_PRECISION; - double ln1 = lon1 / COORDINATE_PRECISION; - double lt2 = lat2 / COORDINATE_PRECISION; - double ln2 = lon2 / COORDINATE_PRECISION; - double dlat1 = lt1 * (RAD); - - double dlong1 = ln1 * (RAD); - double dlat2 = lt2 * (RAD); - double dlong2 = ln2 * (RAD); - - double dLong = dlong1 - dlong2; - double dLat = dlat1 - dlat2; - - double aHarv = pow(sin(dLat / 2.0), 2.0) + cos(dlat1) * cos(dlat2) * pow(sin(dLong / 2.), 2); - double cHarv = 2. * atan2(sqrt(aHarv), sqrt(1.0 - aHarv)); - // 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 double earth = 6372797.560856; - return earth * cHarv; -} - -double FixedPointCoordinate::ApproximateDistance(const FixedPointCoordinate &coordinate_1, - const FixedPointCoordinate &coordinate_2) -{ - return ApproximateDistance( - coordinate_1.lat, coordinate_1.lon, coordinate_2.lat, coordinate_2.lon); -} - -float FixedPointCoordinate::ApproximateEuclideanDistance(const FixedPointCoordinate &coordinate_1, - const FixedPointCoordinate &coordinate_2) -{ - return ApproximateEuclideanDistance( - coordinate_1.lat, coordinate_1.lon, coordinate_2.lat, coordinate_2.lon); -} - -float FixedPointCoordinate::ApproximateEuclideanDistance(const int lat1, - const int lon1, - const int lat2, - const int lon2) -{ - BOOST_ASSERT(lat1 != std::numeric_limits::min()); - BOOST_ASSERT(lon1 != std::numeric_limits::min()); - BOOST_ASSERT(lat2 != std::numeric_limits::min()); - BOOST_ASSERT(lon2 != std::numeric_limits::min()); - - const float RAD = 0.017453292519943295769236907684886f; - const float float_lat1 = (lat1 / COORDINATE_PRECISION) * RAD; - const float float_lon1 = (lon1 / COORDINATE_PRECISION) * RAD; - const float float_lat2 = (lat2 / COORDINATE_PRECISION) * RAD; - const float float_lon2 = (lon2 / COORDINATE_PRECISION) * RAD; - - const float x_value = (float_lon2 - float_lon1) * cos((float_lat1 + float_lat2) / 2.f); - const float y_value = float_lat2 - float_lat1; - const float earth_radius = 6372797.560856f; - return sqrt(x_value * x_value + y_value * y_value) * earth_radius; -} - -float -FixedPointCoordinate::ComputePerpendicularDistance(const FixedPointCoordinate &source_coordinate, - const FixedPointCoordinate &target_coordinate, - const FixedPointCoordinate &query_location) -{ - float ratio; - FixedPointCoordinate nearest_location; - - return ComputePerpendicularDistance(source_coordinate, - target_coordinate, - query_location, - nearest_location, - ratio); -} - -float FixedPointCoordinate::ComputePerpendicularDistance(const FixedPointCoordinate &segment_source, - const FixedPointCoordinate &segment_target, - const FixedPointCoordinate &query_location, - FixedPointCoordinate &nearest_location, - float &ratio) -{ - BOOST_ASSERT(query_location.is_valid()); - - // initialize values - const double x = mercator::lat2y(query_location.lat / COORDINATE_PRECISION); - const double y = query_location.lon / COORDINATE_PRECISION; - const double a = mercator::lat2y(segment_source.lat / COORDINATE_PRECISION); - const double b = segment_source.lon / COORDINATE_PRECISION; - const double c = mercator::lat2y(segment_target.lat / COORDINATE_PRECISION); - const double d = segment_target.lon / COORDINATE_PRECISION; - double p, q /*,mX*/, nY; - if (std::abs(a - c) > std::numeric_limits::epsilon()) - { - const double m = (d - b) / (c - a); // slope - // Projection of (x,y) on line joining (a,b) and (c,d) - p = ((x + (m * y)) + (m * m * a - m * b)) / (1.f + m * m); - q = b + m * (p - a); - } - else - { - p = c; - q = y; - } - nY = (d * p - c * q) / (a * d - b * c); - - // discretize the result to coordinate precision. it's a hack! - if (std::abs(nY) < (1.f / COORDINATE_PRECISION)) - { - nY = 0.f; - } - - // compute ratio - ratio = (p - nY * a) / c; // These values are actually n/m+n and m/m+n , we need - // not calculate the explicit values of m an n as we - // are just interested in the ratio - if (std::isnan(ratio)) - { - ratio = (segment_target == query_location ? 1.f : 0.f); - } - else if (std::abs(ratio) <= std::numeric_limits::epsilon()) - { - ratio = 0.f; - } - else if (std::abs(ratio - 1.f) <= std::numeric_limits::epsilon()) - { - ratio = 1.f; - } - - // compute nearest location - BOOST_ASSERT(!std::isnan(ratio)); - if (ratio <= 0.f) - { - nearest_location = segment_source; - } - else if (ratio >= 1.f) - { - nearest_location = segment_target; - } - else - { - // point lies in between - nearest_location.lat = static_cast(mercator::y2lat(p) * COORDINATE_PRECISION); - nearest_location.lon = static_cast(q * COORDINATE_PRECISION); - } - BOOST_ASSERT(nearest_location.is_valid()); - - const float approximate_distance = - FixedPointCoordinate::ApproximateEuclideanDistance(query_location, nearest_location); - BOOST_ASSERT(0. <= approximate_distance); - return approximate_distance; -} - -void FixedPointCoordinate::convertInternalLatLonToString(const int value, std::string &output) -{ - char buffer[12]; - buffer[11] = 0; // zero termination - output = printInt<11, 6>(buffer, value); -} - -void FixedPointCoordinate::convertInternalCoordinateToString(const FixedPointCoordinate &coord, - std::string &output) -{ - std::string tmp; - tmp.reserve(23); - convertInternalLatLonToString(coord.lon, tmp); - output = tmp; - output += ","; - convertInternalLatLonToString(coord.lat, tmp); - output += tmp; -} - -void -FixedPointCoordinate::convertInternalReversedCoordinateToString(const FixedPointCoordinate &coord, - std::string &output) -{ - std::string tmp; - tmp.reserve(23); - convertInternalLatLonToString(coord.lat, tmp); - output = tmp; - output += ","; - convertInternalLatLonToString(coord.lon, tmp); - output += tmp; -} - void FixedPointCoordinate::Output(std::ostream &out) const { out << "(" << lat / COORDINATE_PRECISION << "," << lon / COORDINATE_PRECISION << ")"; } -float FixedPointCoordinate::GetBearing(const FixedPointCoordinate &first_coordinate, - const FixedPointCoordinate &second_coordinate) -{ - const float lon_diff = - second_coordinate.lon / COORDINATE_PRECISION - first_coordinate.lon / COORDINATE_PRECISION; - const float lon_delta = DegreeToRadian(lon_diff); - const float lat1 = DegreeToRadian(first_coordinate.lat / COORDINATE_PRECISION); - const float lat2 = DegreeToRadian(second_coordinate.lat / COORDINATE_PRECISION); - const float y = sin(lon_delta) * cos(lat2); - const float x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(lon_delta); - float result = RadianToDegree(std::atan2(y, x)); - while (result < 0.f) - { - result += 360.f; - } - - while (result >= 360.f) - { - result -= 360.f; - } - return result; -} - float FixedPointCoordinate::GetBearing(const FixedPointCoordinate &other) const { const float lon_delta = - DegreeToRadian(lon / COORDINATE_PRECISION - other.lon / COORDINATE_PRECISION); - const float lat1 = DegreeToRadian(other.lat / COORDINATE_PRECISION); - const float lat2 = DegreeToRadian(lat / COORDINATE_PRECISION); + coordinate_calculation::deg_to_rad(lon / COORDINATE_PRECISION - other.lon / COORDINATE_PRECISION); + const float lat1 = coordinate_calculation::deg_to_rad(other.lat / COORDINATE_PRECISION); + const float lat2 = coordinate_calculation::deg_to_rad(lat / COORDINATE_PRECISION); const float y_value = std::sin(lon_delta) * std::cos(lat2); const float x_value = std::cos(lat1) * std::sin(lat2) - std::sin(lat1) * std::cos(lat2) * std::cos(lon_delta); - float result = RadianToDegree(std::atan2(y_value, x_value)); + float result = coordinate_calculation::rad_to_deg(std::atan2(y_value, x_value)); while (result < 0.f) { @@ -328,75 +113,3 @@ float FixedPointCoordinate::GetBearing(const FixedPointCoordinate &other) const } return result; } - -float FixedPointCoordinate::DegreeToRadian(const float degree) -{ - return degree * (static_cast(M_PI) / 180.f); -} - -float FixedPointCoordinate::RadianToDegree(const float radian) -{ - return radian * (180.f * static_cast(M_1_PI)); -} - -// This distance computation does integer arithmetic only and is a lot faster than -// the other distance function which are numerically correct('ish). -// It preserves some order among the elements that make it useful for certain purposes -int FixedPointCoordinate::OrderedPerpendicularDistanceApproximation( - const FixedPointCoordinate &input_point, - const FixedPointCoordinate &segment_source, - const FixedPointCoordinate &segment_target) -{ - // initialize values - const float x = static_cast(mercator::lat2y(input_point.lat / COORDINATE_PRECISION)); - const float y = input_point.lon / COORDINATE_PRECISION; - const float a = static_cast(mercator::lat2y(segment_source.lat / COORDINATE_PRECISION)); - const float b = segment_source.lon / COORDINATE_PRECISION; - const float c = static_cast(mercator::lat2y(segment_target.lat / COORDINATE_PRECISION)); - const float d = segment_target.lon / COORDINATE_PRECISION; - - float p, q; - if (a == c) - { - p = c; - q = y; - } - else - { - const float m = (d - b) / (c - a); // slope - // Projection of (x,y) on line joining (a,b) and (c,d) - p = ((x + (m * y)) + (m * m * a - m * b)) / (1.f + m * m); - q = b + m * (p - a); - } - - const float nY = (d * p - c * q) / (a * d - b * c); - float ratio = (p - nY * a) / c; // These values are actually n/m+n and m/m+n , we need - // not calculate the explicit values of m an n as we - // are just interested in the ratio - if (std::isnan(ratio)) - { - ratio = (segment_target == input_point) ? 1.f : 0.f; - } - - // compute target quasi-location - int dx, dy; - if (ratio < 0.f) - { - dx = input_point.lon - segment_source.lon; - dy = input_point.lat - segment_source.lat; - } - else if (ratio > 1.f) - { - dx = input_point.lon - segment_target.lon; - dy = input_point.lat - segment_target.lat; - } - else - { - // point lies in between - dx = input_point.lon - static_cast(q * COORDINATE_PRECISION); - dy = input_point.lat - static_cast(mercator::y2lat(p) * COORDINATE_PRECISION); - } - - // return an approximation in the plane - return static_cast(sqrt(dx * dx + dy * dy)); -} diff --git a/data_structures/coordinate_calculation.cpp b/data_structures/coordinate_calculation.cpp new file mode 100644 index 000000000..cc0474c79 --- /dev/null +++ b/data_structures/coordinate_calculation.cpp @@ -0,0 +1,422 @@ +/* + +Copyright (c) 2015, Project OSRM, Dennis Luxen, others +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +*/ + +#include "coordinate_calculation.hpp" + +#include "../Util/mercator.hpp" +#include "../Util/string_util.hpp" + +#include + +#include + +#include + +namespace +{ +constexpr static const float RAD = 0.017453292519943295769236907684886; +constexpr static const float earth_radius = 6372797.560856f; +} + +double coordinate_calculation::ApproximateDistance(const int lat1, + const int lon1, + const int lat2, + const int lon2) +{ + BOOST_ASSERT(lat1 != std::numeric_limits::min()); + BOOST_ASSERT(lon1 != std::numeric_limits::min()); + BOOST_ASSERT(lat2 != std::numeric_limits::min()); + BOOST_ASSERT(lon2 != std::numeric_limits::min()); + double lt1 = lat1 / COORDINATE_PRECISION; + double ln1 = lon1 / COORDINATE_PRECISION; + double lt2 = lat2 / COORDINATE_PRECISION; + double ln2 = lon2 / COORDINATE_PRECISION; + double dlat1 = lt1 * (RAD); + + double dlong1 = ln1 * (RAD); + double dlat2 = lt2 * (RAD); + double dlong2 = ln2 * (RAD); + + double dLong = dlong1 - dlong2; + double dLat = dlat1 - dlat2; + + double aHarv = pow(sin(dLat / 2.0), 2.0) + cos(dlat1) * cos(dlat2) * pow(sin(dLong / 2.), 2); + double cHarv = 2. * atan2(sqrt(aHarv), sqrt(1.0 - aHarv)); + // 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) + return earth_radius * cHarv; +} + +double coordinate_calculation::ApproximateDistance(const FixedPointCoordinate &coordinate_1, + const FixedPointCoordinate &coordinate_2) +{ + return ApproximateDistance( + coordinate_1.lat, coordinate_1.lon, coordinate_2.lat, coordinate_2.lon); +} + +float coordinate_calculation::approx_euclidean_distance(const FixedPointCoordinate &coordinate_1, + const FixedPointCoordinate &coordinate_2) +{ + return approx_euclidean_distance( + coordinate_1.lat, coordinate_1.lon, coordinate_2.lat, coordinate_2.lon); +} + +float coordinate_calculation::approx_euclidean_distance(const int lat1, + const int lon1, + const int lat2, + const int lon2) +{ + BOOST_ASSERT(lat1 != std::numeric_limits::min()); + BOOST_ASSERT(lon1 != std::numeric_limits::min()); + BOOST_ASSERT(lat2 != std::numeric_limits::min()); + BOOST_ASSERT(lon2 != std::numeric_limits::min()); + + const float float_lat1 = (lat1 / COORDINATE_PRECISION) * RAD; + const float float_lon1 = (lon1 / COORDINATE_PRECISION) * RAD; + const float float_lat2 = (lat2 / COORDINATE_PRECISION) * RAD; + const float float_lon2 = (lon2 / COORDINATE_PRECISION) * RAD; + + const float x_value = (float_lon2 - float_lon1) * cos((float_lat1 + float_lat2) / 2.f); + const float y_value = float_lat2 - float_lat1; + return sqrt(x_value * x_value + y_value * y_value) * earth_radius; +} + +float +coordinate_calculation::ComputePerpendicularDistance(const FixedPointCoordinate &source_coordinate, + const FixedPointCoordinate &target_coordinate, + const FixedPointCoordinate &query_location) +{ + float ratio; + FixedPointCoordinate nearest_location; + + return ComputePerpendicularDistance(source_coordinate, + target_coordinate, + query_location, + nearest_location, + ratio); +} + +float coordinate_calculation::ComputePerpendicularDistance(const FixedPointCoordinate &segment_source, + const FixedPointCoordinate &segment_target, + const FixedPointCoordinate &query_location, + FixedPointCoordinate &nearest_location, + float &ratio) +{ + BOOST_ASSERT(query_location.is_valid()); + + // initialize values + const double x = mercator::lat2y(query_location.lat / COORDINATE_PRECISION); + const double y = query_location.lon / COORDINATE_PRECISION; + const double a = mercator::lat2y(segment_source.lat / COORDINATE_PRECISION); + const double b = segment_source.lon / COORDINATE_PRECISION; + const double c = mercator::lat2y(segment_target.lat / COORDINATE_PRECISION); + const double d = segment_target.lon / COORDINATE_PRECISION; + double p, q /*,mX*/, nY; + if (std::abs(a - c) > std::numeric_limits::epsilon()) + { + const double m = (d - b) / (c - a); // slope + // Projection of (x,y) on line joining (a,b) and (c,d) + p = ((x + (m * y)) + (m * m * a - m * b)) / (1.f + m * m); + q = b + m * (p - a); + } + else + { + p = c; + q = y; + } + nY = (d * p - c * q) / (a * d - b * c); + + // discretize the result to coordinate precision. it's a hack! + if (std::abs(nY) < (1.f / COORDINATE_PRECISION)) + { + nY = 0.f; + } + + // compute ratio + ratio = (p - nY * a) / c; // These values are actually n/m+n and m/m+n , we need + // not calculate the explicit values of m an n as we + // are just interested in the ratio + if (std::isnan(ratio)) + { + ratio = (segment_target == query_location ? 1.f : 0.f); + } + else if (std::abs(ratio) <= std::numeric_limits::epsilon()) + { + ratio = 0.f; + } + else if (std::abs(ratio - 1.f) <= std::numeric_limits::epsilon()) + { + ratio = 1.f; + } + + // compute nearest location + BOOST_ASSERT(!std::isnan(ratio)); + if (ratio <= 0.f) + { + nearest_location = segment_source; + } + else if (ratio >= 1.f) + { + nearest_location = segment_target; + } + else + { + // point lies in between + nearest_location.lat = static_cast(mercator::y2lat(p) * COORDINATE_PRECISION); + nearest_location.lon = static_cast(q * COORDINATE_PRECISION); + } + BOOST_ASSERT(nearest_location.is_valid()); + + const float approximate_distance = + coordinate_calculation::approx_euclidean_distance(query_location, nearest_location); + BOOST_ASSERT(0. <= approximate_distance); + return approximate_distance; +} + +float +coordinate_calculation::perpendicular_distance_from_projected_coordinate( + const FixedPointCoordinate &source_coordinate, + const FixedPointCoordinate &target_coordinate, + const FixedPointCoordinate &query_location, + const std::pair &projected_coordinate) +{ + float ratio; + FixedPointCoordinate nearest_location; + + return perpendicular_distance_from_projected_coordinate(source_coordinate, + target_coordinate, + query_location, + projected_coordinate, + nearest_location, + ratio); +} + +float coordinate_calculation::perpendicular_distance_from_projected_coordinate( + const FixedPointCoordinate &segment_source, + const FixedPointCoordinate &segment_target, + const FixedPointCoordinate &query_location, + const std::pair &projected_coordinate, + FixedPointCoordinate &nearest_location, + float &ratio) +{ + BOOST_ASSERT(query_location.is_valid()); + + // initialize values + const double x = projected_coordinate.first; + const double y = projected_coordinate.second; + const double a = mercator::lat2y(segment_source.lat / COORDINATE_PRECISION); + const double b = segment_source.lon / COORDINATE_PRECISION; + const double c = mercator::lat2y(segment_target.lat / COORDINATE_PRECISION); + const double d = segment_target.lon / COORDINATE_PRECISION; + double p, q /*,mX*/, nY; + if (std::abs(a - c) > std::numeric_limits::epsilon()) + { + const double m = (d - b) / (c - a); // slope + // Projection of (x,y) on line joining (a,b) and (c,d) + p = ((x + (m * y)) + (m * m * a - m * b)) / (1.f + m * m); + q = b + m * (p - a); + } + else + { + p = c; + q = y; + } + nY = (d * p - c * q) / (a * d - b * c); + + // discretize the result to coordinate precision. it's a hack! + if (std::abs(nY) < (1.f / COORDINATE_PRECISION)) + { + nY = 0.f; + } + + // compute ratio + ratio = (p - nY * a) / c; // These values are actually n/m+n and m/m+n , we need + // not calculate the explicit values of m an n as we + // are just interested in the ratio + if (std::isnan(ratio)) + { + ratio = (segment_target == query_location ? 1.f : 0.f); + } + else if (std::abs(ratio) <= std::numeric_limits::epsilon()) + { + ratio = 0.f; + } + else if (std::abs(ratio - 1.f) <= std::numeric_limits::epsilon()) + { + ratio = 1.f; + } + + // compute nearest location + BOOST_ASSERT(!std::isnan(ratio)); + if (ratio <= 0.f) + { + nearest_location = segment_source; + } + else if (ratio >= 1.f) + { + nearest_location = segment_target; + } + else + { + // point lies in between + nearest_location.lat = static_cast(mercator::y2lat(p) * COORDINATE_PRECISION); + nearest_location.lon = static_cast(q * COORDINATE_PRECISION); + } + BOOST_ASSERT(nearest_location.is_valid()); + + const float approximate_distance = + coordinate_calculation::approx_euclidean_distance(query_location, nearest_location); + BOOST_ASSERT(0. <= approximate_distance); + return approximate_distance; +} + +void coordinate_calculation::lat_or_lon_to_string(const int value, std::string &output) +{ + char buffer[12]; + buffer[11] = 0; // zero termination + output = printInt<11, 6>(buffer, value); +} + +void coordinate_calculation::convertInternalCoordinateToString(const FixedPointCoordinate &coord, + std::string &output) +{ + std::string tmp; + tmp.reserve(23); + lat_or_lon_to_string(coord.lon, tmp); + output = tmp; + output += ","; + lat_or_lon_to_string(coord.lat, tmp); + output += tmp; +} + +void +coordinate_calculation::convertInternalReversedCoordinateToString(const FixedPointCoordinate &coord, + std::string &output) +{ + std::string tmp; + tmp.reserve(23); + lat_or_lon_to_string(coord.lat, tmp); + output = tmp; + output += ","; + lat_or_lon_to_string(coord.lon, tmp); + output += tmp; +} + +float coordinate_calculation::GetBearing(const FixedPointCoordinate &first_coordinate, + const FixedPointCoordinate &second_coordinate) +{ + const float lon_diff = + second_coordinate.lon / COORDINATE_PRECISION - first_coordinate.lon / COORDINATE_PRECISION; + const float lon_delta = deg_to_rad(lon_diff); + const float lat1 = deg_to_rad(first_coordinate.lat / COORDINATE_PRECISION); + const float lat2 = deg_to_rad(second_coordinate.lat / COORDINATE_PRECISION); + const float y = sin(lon_delta) * cos(lat2); + const float x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(lon_delta); + float result = rad_to_deg(std::atan2(y, x)); + while (result < 0.f) + { + result += 360.f; + } + + while (result >= 360.f) + { + result -= 360.f; + } + return result; +} + +float coordinate_calculation::deg_to_rad(const float degree) +{ + return degree * (static_cast(M_PI) / 180.f); +} + +float coordinate_calculation::rad_to_deg(const float radian) +{ + return radian * (180.f * static_cast(M_1_PI)); +} + +// This distance computation does integer arithmetic only and is a lot faster than +// the other distance function which are numerically correct('ish). +// It preserves some order among the elements that make it useful for certain purposes +int coordinate_calculation::OrderedPerpendicularDistanceApproximation( + const FixedPointCoordinate &input_point, + const FixedPointCoordinate &segment_source, + const FixedPointCoordinate &segment_target) +{ + // initialize values + const float x = static_cast(mercator::lat2y(input_point.lat / COORDINATE_PRECISION)); + const float y = input_point.lon / COORDINATE_PRECISION; + const float a = static_cast(mercator::lat2y(segment_source.lat / COORDINATE_PRECISION)); + const float b = segment_source.lon / COORDINATE_PRECISION; + const float c = static_cast(mercator::lat2y(segment_target.lat / COORDINATE_PRECISION)); + const float d = segment_target.lon / COORDINATE_PRECISION; + + float p, q; + if (a == c) + { + p = c; + q = y; + } + else + { + const float m = (d - b) / (c - a); // slope + // Projection of (x,y) on line joining (a,b) and (c,d) + p = ((x + (m * y)) + (m * m * a - m * b)) / (1.f + m * m); + q = b + m * (p - a); + } + + const float nY = (d * p - c * q) / (a * d - b * c); + float ratio = (p - nY * a) / c; // These values are actually n/m+n and m/m+n , we need + // not calculate the explicit values of m an n as we + // are just interested in the ratio + if (std::isnan(ratio)) + { + ratio = (segment_target == input_point) ? 1.f : 0.f; + } + + // compute target quasi-location + int dx, dy; + if (ratio < 0.f) + { + dx = input_point.lon - segment_source.lon; + dy = input_point.lat - segment_source.lat; + } + else if (ratio > 1.f) + { + dx = input_point.lon - segment_target.lon; + dy = input_point.lat - segment_target.lat; + } + else + { + // point lies in between + dx = input_point.lon - static_cast(q * COORDINATE_PRECISION); + dy = input_point.lat - static_cast(mercator::y2lat(p) * COORDINATE_PRECISION); + } + + // return an approximation in the plane + return static_cast(sqrt(dx * dx + dy * dy)); +} diff --git a/data_structures/coordinate_calculation.hpp b/data_structures/coordinate_calculation.hpp new file mode 100644 index 000000000..7c76cb446 --- /dev/null +++ b/data_structures/coordinate_calculation.hpp @@ -0,0 +1,97 @@ +/* + +Copyright (c) 2015, Project OSRM, Dennis Luxen, others +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +*/ + +#ifndef COORDINATE_CALCULATION +#define COORDINATE_CALCULATION + +#include + +#include //for std::ostream +#include +#include + +struct coordinate_calculation +{ + static double + ApproximateDistance(const int lat1, const int lon1, const int lat2, const int lon2); + + static double ApproximateDistance(const FixedPointCoordinate &first_coordinate, + const FixedPointCoordinate &second_coordinate); + + static float approx_euclidean_distance(const FixedPointCoordinate &first_coordinate, + const FixedPointCoordinate &second_coordinate); + + static float + approx_euclidean_distance(const int lat1, const int lon1, const int lat2, const int lon2); + + static float ApproximateSquaredEuclideanDistance(const FixedPointCoordinate &first_coordinate, + const FixedPointCoordinate &second_coordinate); + + static void lat_or_lon_to_string(const int value, std::string &output); + + static void convertInternalCoordinateToString(const FixedPointCoordinate &coordinate, + std::string &output); + + static void convertInternalReversedCoordinateToString(const FixedPointCoordinate &coordinate, + std::string &output); + + static float ComputePerpendicularDistance(const FixedPointCoordinate &segment_source, + const FixedPointCoordinate &segment_target, + const FixedPointCoordinate &query_location); + + static float ComputePerpendicularDistance(const FixedPointCoordinate &segment_source, + const FixedPointCoordinate &segment_target, + const FixedPointCoordinate &query_location, + FixedPointCoordinate &nearest_location, + float &ratio); + + static float perpendicular_distance_from_projected_coordinate( + const FixedPointCoordinate &segment_source, + const FixedPointCoordinate &segment_target, + const FixedPointCoordinate &query_location, + const std::pair &projected_coordinate); + + static float perpendicular_distance_from_projected_coordinate( + const FixedPointCoordinate &segment_source, + const FixedPointCoordinate &segment_target, + const FixedPointCoordinate &query_location, + const std::pair &projected_coordinate, + FixedPointCoordinate &nearest_location, + float &ratio); + + static int + OrderedPerpendicularDistanceApproximation(const FixedPointCoordinate &segment_source, + const FixedPointCoordinate &segment_target, + const FixedPointCoordinate &query_location); + + static float GetBearing(const FixedPointCoordinate &A, const FixedPointCoordinate &B); + + static float deg_to_rad(const float degree); + static float rad_to_deg(const float radian); +}; + +#endif //COORDINATE_CALCULATION diff --git a/data_structures/rectangle.hpp b/data_structures/rectangle.hpp index 2f6815cac..43f389365 100644 --- a/data_structures/rectangle.hpp +++ b/data_structures/rectangle.hpp @@ -1,6 +1,6 @@ /* -Copyright (c) 2014, Project OSRM, Dennis Luxen, others +Copyright (c) 2015, Project OSRM, Dennis Luxen, others All rights reserved. Redistribution and use in source and binary forms, with or without modification, @@ -28,6 +28,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef RECTANGLE_HPP #define RECTANGLE_HPP +#include "coordinate_calculation.hpp" + #include #include @@ -115,28 +117,28 @@ struct RectangleInt2D switch (d) { case NORTH: - min_dist = FixedPointCoordinate::ApproximateEuclideanDistance(location, FixedPointCoordinate(max_lat, location.lon)); + min_dist = coordinate_calculation::approx_euclidean_distance(location, FixedPointCoordinate(max_lat, location.lon)); break; case SOUTH: - min_dist = FixedPointCoordinate::ApproximateEuclideanDistance(location, FixedPointCoordinate(min_lat, location.lon)); + min_dist = coordinate_calculation::approx_euclidean_distance(location, FixedPointCoordinate(min_lat, location.lon)); break; case WEST: - min_dist = FixedPointCoordinate::ApproximateEuclideanDistance(location, FixedPointCoordinate(location.lat, min_lon)); + min_dist = coordinate_calculation::approx_euclidean_distance(location, FixedPointCoordinate(location.lat, min_lon)); break; case EAST: - min_dist = FixedPointCoordinate::ApproximateEuclideanDistance(location, FixedPointCoordinate(location.lat, max_lon)); + min_dist = coordinate_calculation::approx_euclidean_distance(location, FixedPointCoordinate(location.lat, max_lon)); break; case NORTH_EAST: - min_dist = FixedPointCoordinate::ApproximateEuclideanDistance(location, FixedPointCoordinate(max_lat, max_lon)); + min_dist = coordinate_calculation::approx_euclidean_distance(location, FixedPointCoordinate(max_lat, max_lon)); break; case NORTH_WEST: - min_dist = FixedPointCoordinate::ApproximateEuclideanDistance(location, FixedPointCoordinate(max_lat, min_lon)); + min_dist = coordinate_calculation::approx_euclidean_distance(location, FixedPointCoordinate(max_lat, min_lon)); break; case SOUTH_EAST: - min_dist = FixedPointCoordinate::ApproximateEuclideanDistance(location, FixedPointCoordinate(min_lat, max_lon)); + min_dist = coordinate_calculation::approx_euclidean_distance(location, FixedPointCoordinate(min_lat, max_lon)); break; case SOUTH_WEST: - min_dist = FixedPointCoordinate::ApproximateEuclideanDistance(location, FixedPointCoordinate(min_lat, min_lon)); + min_dist = coordinate_calculation::approx_euclidean_distance(location, FixedPointCoordinate(min_lat, min_lon)); break; default: break; @@ -159,24 +161,24 @@ struct RectangleInt2D min_max_dist = std::min( min_max_dist, std::max( - FixedPointCoordinate::ApproximateEuclideanDistance(location, upper_left), - FixedPointCoordinate::ApproximateEuclideanDistance(location, upper_right))); + coordinate_calculation::approx_euclidean_distance(location, upper_left), + coordinate_calculation::approx_euclidean_distance(location, upper_right))); min_max_dist = std::min( min_max_dist, std::max( - FixedPointCoordinate::ApproximateEuclideanDistance(location, upper_right), - FixedPointCoordinate::ApproximateEuclideanDistance(location, lower_right))); + coordinate_calculation::approx_euclidean_distance(location, upper_right), + coordinate_calculation::approx_euclidean_distance(location, lower_right))); min_max_dist = std::min( min_max_dist, - std::max(FixedPointCoordinate::ApproximateEuclideanDistance(location, lower_right), - FixedPointCoordinate::ApproximateEuclideanDistance(location, lower_left))); + std::max(coordinate_calculation::approx_euclidean_distance(location, lower_right), + coordinate_calculation::approx_euclidean_distance(location, lower_left))); min_max_dist = std::min( min_max_dist, - std::max(FixedPointCoordinate::ApproximateEuclideanDistance(location, lower_left), - FixedPointCoordinate::ApproximateEuclideanDistance(location, upper_left))); + std::max(coordinate_calculation::approx_euclidean_distance(location, lower_left), + coordinate_calculation::approx_euclidean_distance(location, upper_left))); return min_max_dist; } diff --git a/data_structures/static_rtree.hpp b/data_structures/static_rtree.hpp index a8cccd1bf..fe83c1888 100644 --- a/data_structures/static_rtree.hpp +++ b/data_structures/static_rtree.hpp @@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rectangle.hpp" #include "shared_memory_factory.hpp" #include "shared_memory_vector_wrapper.hpp" +#include "upper_bound.hpp" #include "../Util/floating_point.hpp" #include "../Util/integer_range.hpp" @@ -176,28 +177,28 @@ class StaticRTree switch (d) { case NORTH: - min_dist = FixedPointCoordinate::ApproximateEuclideanDistance(location, FixedPointCoordinate(max_lat, location.lon)); + min_dist = coordinate_calculation::approx_euclidean_distance(location, FixedPointCoordinate(max_lat, location.lon)); break; case SOUTH: - min_dist = FixedPointCoordinate::ApproximateEuclideanDistance(location, FixedPointCoordinate(min_lat, location.lon)); + min_dist = coordinate_calculation::approx_euclidean_distance(location, FixedPointCoordinate(min_lat, location.lon)); break; case WEST: - min_dist = FixedPointCoordinate::ApproximateEuclideanDistance(location, FixedPointCoordinate(location.lat, min_lon)); + min_dist = coordinate_calculation::approx_euclidean_distance(location, FixedPointCoordinate(location.lat, min_lon)); break; case EAST: - min_dist = FixedPointCoordinate::ApproximateEuclideanDistance(location, FixedPointCoordinate(location.lat, max_lon)); + min_dist = coordinate_calculation::approx_euclidean_distance(location, FixedPointCoordinate(location.lat, max_lon)); break; case NORTH_EAST: - min_dist = FixedPointCoordinate::ApproximateEuclideanDistance(location, FixedPointCoordinate(max_lat, max_lon)); + min_dist = coordinate_calculation::approx_euclidean_distance(location, FixedPointCoordinate(max_lat, max_lon)); break; case NORTH_WEST: - min_dist = FixedPointCoordinate::ApproximateEuclideanDistance(location, FixedPointCoordinate(max_lat, min_lon)); + min_dist = coordinate_calculation::approx_euclidean_distance(location, FixedPointCoordinate(max_lat, min_lon)); break; case SOUTH_EAST: - min_dist = FixedPointCoordinate::ApproximateEuclideanDistance(location, FixedPointCoordinate(min_lat, max_lon)); + min_dist = coordinate_calculation::approx_euclidean_distance(location, FixedPointCoordinate(min_lat, max_lon)); break; case SOUTH_WEST: - min_dist = FixedPointCoordinate::ApproximateEuclideanDistance(location, FixedPointCoordinate(min_lat, min_lon)); + min_dist = coordinate_calculation::approx_euclidean_distance(location, FixedPointCoordinate(min_lat, min_lon)); break; default: break; @@ -220,24 +221,24 @@ class StaticRTree min_max_dist = std::min( min_max_dist, std::max( - FixedPointCoordinate::ApproximateEuclideanDistance(location, upper_left), - FixedPointCoordinate::ApproximateEuclideanDistance(location, upper_right))); + coordinate_calculation::approx_euclidean_distance(location, upper_left), + coordinate_calculation::approx_euclidean_distance(location, upper_right))); min_max_dist = std::min( min_max_dist, std::max( - FixedPointCoordinate::ApproximateEuclideanDistance(location, upper_right), - FixedPointCoordinate::ApproximateEuclideanDistance(location, lower_right))); + coordinate_calculation::approx_euclidean_distance(location, upper_right), + coordinate_calculation::approx_euclidean_distance(location, lower_right))); min_max_dist = std::min( min_max_dist, - std::max(FixedPointCoordinate::ApproximateEuclideanDistance(location, lower_right), - FixedPointCoordinate::ApproximateEuclideanDistance(location, lower_left))); + std::max(coordinate_calculation::approx_euclidean_distance(location, lower_right), + coordinate_calculation::approx_euclidean_distance(location, lower_left))); min_max_dist = std::min( min_max_dist, - std::max(FixedPointCoordinate::ApproximateEuclideanDistance(location, lower_left), - FixedPointCoordinate::ApproximateEuclideanDistance(location, upper_left))); + std::max(coordinate_calculation::approx_euclidean_distance(location, lower_left), + coordinate_calculation::approx_euclidean_distance(location, upper_left))); return min_max_dist; } @@ -606,7 +607,7 @@ class StaticRTree } float current_minimum_distance = - FixedPointCoordinate::ApproximateEuclideanDistance( + coordinate_calculation::approx_euclidean_distance( input_coordinate.lat, input_coordinate.lon, m_coordinate_list->at(current_edge.u).lat, @@ -619,7 +620,7 @@ class StaticRTree } current_minimum_distance = - FixedPointCoordinate::ApproximateEuclideanDistance( + coordinate_calculation::approx_euclidean_distance( input_coordinate.lat, input_coordinate.lon, m_coordinate_list->at(current_edge.v).lat, @@ -661,6 +662,15 @@ class StaticRTree unsigned number_of_elements_from_big_cc = 0; unsigned number_of_elements_from_tiny_cc = 0; + unsigned pruned_elements = 0; + + std::pair projected_coordinate = + { mercator::lat2y(input_coordinate.lat / COORDINATE_PRECISION), + input_coordinate.lon / COORDINATE_PRECISION }; + + // upper bound pruning technique + upper_bound pruning_bound(max_number_of_phantom_nodes); + // initialize queue with root element std::priority_queue traversal_queue; traversal_queue.emplace(0.f, m_search_tree[0]); @@ -683,15 +693,22 @@ class StaticRTree { const auto ¤t_edge = current_leaf_node.objects[i]; const float current_perpendicular_distance = - FixedPointCoordinate::ComputePerpendicularDistance( + coordinate_calculation::perpendicular_distance_from_projected_coordinate( m_coordinate_list->at(current_edge.u), m_coordinate_list->at(current_edge.v), - input_coordinate); + input_coordinate, + projected_coordinate); // distance must be non-negative BOOST_ASSERT(0.f <= current_perpendicular_distance); - // put element in queue - traversal_queue.emplace(current_perpendicular_distance, current_edge); + if (pruning_bound.get() >= current_perpendicular_distance || + current_edge.is_in_tiny_cc()) + { + pruning_bound.insert(current_perpendicular_distance); + traversal_queue.emplace(current_perpendicular_distance, current_edge); + } else { + ++pruned_elements; + } } } else @@ -727,10 +744,11 @@ class StaticRTree float current_ratio = 0.f; FixedPointCoordinate foot_point_coordinate_on_segment; // const float current_perpendicular_distance = - FixedPointCoordinate::ComputePerpendicularDistance( + coordinate_calculation::perpendicular_distance_from_projected_coordinate( m_coordinate_list->at(current_segment.u), m_coordinate_list->at(current_segment.v), input_coordinate, + projected_coordinate, foot_point_coordinate_on_segment, current_ratio); @@ -766,13 +784,6 @@ class StaticRTree { // found an element in a big component ++number_of_elements_from_big_cc; } - - // SimpleLogger().Write() << "result_phantom_node_vector.size(): " << result_phantom_node_vector.size(); - // SimpleLogger().Write() << "max_number_of_phantom_nodes: " << max_number_of_phantom_nodes; - // SimpleLogger().Write() << "number_of_elements_from_big_cc: " << number_of_elements_from_big_cc; - // SimpleLogger().Write() << "number_of_elements_from_tiny_cc: " << number_of_elements_from_tiny_cc; - // SimpleLogger().Write() << "inspected_elements: " << inspected_elements; - // SimpleLogger().Write() << "max_checked_elements: " << max_checked_elements; } // stop the search by flushing the queue @@ -782,7 +793,14 @@ class StaticRTree traversal_queue = std::priority_queue{}; } } + // SimpleLogger().Write() << "result_phantom_node_vector.size(): " << result_phantom_node_vector.size(); + // SimpleLogger().Write() << "max_number_of_phantom_nodes: " << max_number_of_phantom_nodes; + // SimpleLogger().Write() << "number_of_elements_from_big_cc: " << number_of_elements_from_big_cc; + // SimpleLogger().Write() << "number_of_elements_from_tiny_cc: " << number_of_elements_from_tiny_cc; // SimpleLogger().Write() << "inspected_elements: " << inspected_elements; + // SimpleLogger().Write() << "max_checked_elements: " << max_checked_elements; + // SimpleLogger().Write() << "pruned_elements: " << pruned_elements; + return !result_phantom_node_vector.empty(); } @@ -828,7 +846,7 @@ class StaticRTree { const auto ¤t_edge = current_leaf_node.objects[i]; const float current_perpendicular_distance = - FixedPointCoordinate::ComputePerpendicularDistance( + coordinate_calculation::ComputePerpendicularDistance( m_coordinate_list->at(current_edge.u), m_coordinate_list->at(current_edge.v), input_coordinate); @@ -885,7 +903,7 @@ class StaticRTree float current_ratio = 0.; FixedPointCoordinate foot_point_coordinate_on_segment; const float current_perpendicular_distance = - FixedPointCoordinate::ComputePerpendicularDistance( + coordinate_calculation::ComputePerpendicularDistance( m_coordinate_list->at(current_segment.u), m_coordinate_list->at(current_segment.v), input_coordinate, @@ -985,7 +1003,7 @@ class StaticRTree float current_ratio = 0.; FixedPointCoordinate nearest; const float current_perpendicular_distance = - FixedPointCoordinate::ComputePerpendicularDistance( + coordinate_calculation::ComputePerpendicularDistance( m_coordinate_list->at(current_edge.u), m_coordinate_list->at(current_edge.v), input_coordinate, @@ -1042,9 +1060,9 @@ class StaticRTree inline void SetForwardAndReverseWeightsOnPhantomNode(const EdgeDataT & nearest_edge, PhantomNode &result_phantom_node) const { - const float distance_1 = FixedPointCoordinate::ApproximateEuclideanDistance( + const float distance_1 = coordinate_calculation::approx_euclidean_distance( m_coordinate_list->at(nearest_edge.u), result_phantom_node.location); - const float distance_2 = FixedPointCoordinate::ApproximateEuclideanDistance( + const float distance_2 = coordinate_calculation::approx_euclidean_distance( m_coordinate_list->at(nearest_edge.u), m_coordinate_list->at(nearest_edge.v)); const float ratio = std::min(1.f, distance_1 / distance_2); diff --git a/data_structures/upper_bound.hpp b/data_structures/upper_bound.hpp new file mode 100644 index 000000000..d39901e8f --- /dev/null +++ b/data_structures/upper_bound.hpp @@ -0,0 +1,75 @@ +/* + +Copyright (c) 2015, Project OSRM, Dennis Luxen, others +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +*/ + +#ifndef LOWER_BOUND_HPP +#define LOWER_BOUND_HPP + +#include +#include +#include + +// max pq holds k elements +// insert if key is smaller than max +// if size > k then remove element +// get() always yields a bound to the k smallest element in the stream + +template +class upper_bound +{ + public: + upper_bound() = delete; + upper_bound(std::size_t size) : size(size) + { + } + + key_type get() const + { + if (queue.size() < size) + { + return std::numeric_limits::max(); + } + return queue.top(); + } + + void insert(const key_type key) + { + if (key < get()) + { + queue.emplace(key); + while (queue.size() > size) + { + queue.pop(); + } + } + } + + private: + std::priority_queue, std::less> queue; + const std::size_t size; +}; + +#endif // LOWER_BOUND_HPP diff --git a/descriptors/description_factory.cpp b/descriptors/description_factory.cpp index 87af445e5..4d41a3e03 100644 --- a/descriptors/description_factory.cpp +++ b/descriptors/description_factory.cpp @@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "description_factory.hpp" #include "../algorithms/polyline_formatter.hpp" +#include "../data_structures/coordinate_calculation.hpp" #include "../data_structures/internal_route_result.hpp" #include "../data_structures/turn_instructions.hpp" #include "../typedefs.h" @@ -134,7 +135,7 @@ void DescriptionFactory::Run(const unsigned zoom_level) { // move down names by one, q&d hack path_description[i - 1].name_id = path_description[i].name_id; - path_description[i].length = FixedPointCoordinate::ApproximateEuclideanDistance( + path_description[i].length = coordinate_calculation::approx_euclidean_distance( path_description[i - 1].location, path_description[i].location); } diff --git a/descriptors/descriptor_base.hpp b/descriptors/descriptor_base.hpp index 393d84440..8166afb7b 100644 --- a/descriptors/descriptor_base.hpp +++ b/descriptors/descriptor_base.hpp @@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef DESCRIPTOR_BASE_HPP #define DESCRIPTOR_BASE_HPP +#include "../data_structures/coordinate_calculation.hpp" #include "../data_structures/internal_route_result.hpp" #include "../data_structures/phantom_node.hpp" #include "../typedefs.h" diff --git a/descriptors/gpx_descriptor.hpp b/descriptors/gpx_descriptor.hpp index 0c7557917..a6af9f8dc 100644 --- a/descriptors/gpx_descriptor.hpp +++ b/descriptors/gpx_descriptor.hpp @@ -1,6 +1,6 @@ /* -Copyright (c) 2014, Project OSRM, Dennis Luxen, others +Copyright (c) 2015, Project OSRM, Dennis Luxen, others All rights reserved. Redistribution and use in source and binary forms, with or without modification, @@ -49,10 +49,10 @@ template class GPXDescriptor final : public BaseDescriptortarget_coordinate.lat = node_iterator->lat; edge_iterator->target_coordinate.lon = node_iterator->lon; - const double distance = FixedPointCoordinate::ApproximateEuclideanDistance( + const double distance = coordinate_calculation::approx_euclidean_distance( edge_iterator->source_coordinate.lat, edge_iterator->source_coordinate.lon, node_iterator->lat, diff --git a/unit_tests/data_structures/coordinate.cpp b/unit_tests/data_structures/coordinate.cpp index 925f44ccc..1acb5e0df 100644 --- a/unit_tests/data_structures/coordinate.cpp +++ b/unit_tests/data_structures/coordinate.cpp @@ -1,5 +1,34 @@ +/* + +Copyright (c) 2015, Project OSRM, Dennis Luxen, others +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +*/ + #include +#include "../../data_structures/coordinate_calculation.hpp" + #include #include @@ -11,11 +40,11 @@ BOOST_AUTO_TEST_CASE(regression_test_1347) FixedPointCoordinate v(10.001 * COORDINATE_PRECISION, -100.002 * COORDINATE_PRECISION); FixedPointCoordinate q(10.002 * COORDINATE_PRECISION, -100.001 * COORDINATE_PRECISION); - float d1 = FixedPointCoordinate::ComputePerpendicularDistance(u, v, q); + float d1 = coordinate_calculation::ComputePerpendicularDistance(u, v, q); float ratio; FixedPointCoordinate nearest_location; - float d2 = FixedPointCoordinate::ComputePerpendicularDistance(u, v, q, nearest_location, ratio); + float d2 = coordinate_calculation::ComputePerpendicularDistance(u, v, q, nearest_location, ratio); BOOST_CHECK_LE(std::abs(d1 - d2), 0.01f); } diff --git a/unit_tests/data_structures/static_rtree.cpp b/unit_tests/data_structures/static_rtree.cpp index 905e58a55..6202ecfa9 100644 --- a/unit_tests/data_structures/static_rtree.cpp +++ b/unit_tests/data_structures/static_rtree.cpp @@ -1,6 +1,6 @@ /* -Copyright (c) 2014, Project OSRM, Dennis Luxen, others +Copyright (c) 2015, Project OSRM, Dennis Luxen, others All rights reserved. Redistribution and use in source and binary forms, with or without modification, @@ -25,6 +25,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +#include "../../data_structures/coordinate_calculation.hpp" #include "../../data_structures/static_rtree.hpp" #include "../../data_structures/query_node.hpp" #include "../../data_structures/edge_based_node.hpp" @@ -77,7 +78,7 @@ class LinearSearchNN { const FixedPointCoordinate &start = coords->at(e.u); const FixedPointCoordinate &end = coords->at(e.v); - float distance = FixedPointCoordinate::ApproximateEuclideanDistance( + float distance = coordinate_calculation::approx_euclidean_distance( input_coordinate.lat, input_coordinate.lon, start.lat, start.lon); if (distance < min_dist) { @@ -85,7 +86,7 @@ class LinearSearchNN min_dist = distance; } - distance = FixedPointCoordinate::ApproximateEuclideanDistance( + distance = coordinate_calculation::approx_euclidean_distance( input_coordinate.lat, input_coordinate.lon, end.lat, end.lon); if (distance < min_dist) { @@ -112,7 +113,7 @@ class LinearSearchNN float current_ratio = 0.; FixedPointCoordinate nearest; const float current_perpendicular_distance = - FixedPointCoordinate::ComputePerpendicularDistance( + coordinate_calculation::ComputePerpendicularDistance( coords->at(e.u), coords->at(e.v), input_coordinate, nearest, current_ratio); if ((current_perpendicular_distance < min_dist) && @@ -148,9 +149,9 @@ class LinearSearchNN result_phantom_node.location.lat = input_coordinate.lat; } - const float distance_1 = FixedPointCoordinate::ApproximateEuclideanDistance( + const float distance_1 = coordinate_calculation::approx_euclidean_distance( coords->at(nearest_edge.u), result_phantom_node.location); - const float distance_2 = FixedPointCoordinate::ApproximateEuclideanDistance( + const float distance_2 = coordinate_calculation::approx_euclidean_distance( coords->at(nearest_edge.u), coords->at(nearest_edge.v)); const float ratio = std::min(1.f, distance_1 / distance_2); @@ -285,10 +286,10 @@ void simple_verify_rtree(RTreeT &rtree, bool found_u = rtree.LocateClosestEndPointForCoordinate(pu, result_u, 1); bool found_v = rtree.LocateClosestEndPointForCoordinate(pv, result_v, 1); BOOST_CHECK(found_u && found_v); - float dist_u = FixedPointCoordinate::ApproximateEuclideanDistance( + float dist_u = coordinate_calculation::approx_euclidean_distance( result_u.lat, result_u.lon, pu.lat, pu.lon); BOOST_CHECK_LE(dist_u, std::numeric_limits::epsilon()); - float dist_v = FixedPointCoordinate::ApproximateEuclideanDistance( + float dist_v = coordinate_calculation::approx_euclidean_distance( result_v.lat, result_v.lon, pv.lat, pv.lon); BOOST_CHECK_LE(dist_v, std::numeric_limits::epsilon()); } @@ -449,30 +450,30 @@ void TestRectangle(double width, double height, double center_lat, double center /* Distance to line segments of rectangle */ BOOST_CHECK_EQUAL(rect.GetMinDist(north), - FixedPointCoordinate::ApproximateEuclideanDistance( + coordinate_calculation::approx_euclidean_distance( north, FixedPointCoordinate(rect.max_lat, north.lon))); BOOST_CHECK_EQUAL(rect.GetMinDist(south), - FixedPointCoordinate::ApproximateEuclideanDistance( + coordinate_calculation::approx_euclidean_distance( south, FixedPointCoordinate(rect.min_lat, south.lon))); BOOST_CHECK_EQUAL(rect.GetMinDist(west), - FixedPointCoordinate::ApproximateEuclideanDistance( + coordinate_calculation::approx_euclidean_distance( west, FixedPointCoordinate(west.lat, rect.min_lon))); BOOST_CHECK_EQUAL(rect.GetMinDist(east), - FixedPointCoordinate::ApproximateEuclideanDistance( + coordinate_calculation::approx_euclidean_distance( east, FixedPointCoordinate(east.lat, rect.max_lon))); /* Distance to corner points */ BOOST_CHECK_EQUAL(rect.GetMinDist(north_east), - FixedPointCoordinate::ApproximateEuclideanDistance( + coordinate_calculation::approx_euclidean_distance( north_east, FixedPointCoordinate(rect.max_lat, rect.max_lon))); BOOST_CHECK_EQUAL(rect.GetMinDist(north_west), - FixedPointCoordinate::ApproximateEuclideanDistance( + coordinate_calculation::approx_euclidean_distance( north_west, FixedPointCoordinate(rect.max_lat, rect.min_lon))); BOOST_CHECK_EQUAL(rect.GetMinDist(south_east), - FixedPointCoordinate::ApproximateEuclideanDistance( + coordinate_calculation::approx_euclidean_distance( south_east, FixedPointCoordinate(rect.min_lat, rect.max_lon))); BOOST_CHECK_EQUAL(rect.GetMinDist(south_west), - FixedPointCoordinate::ApproximateEuclideanDistance( + coordinate_calculation::approx_euclidean_distance( south_west, FixedPointCoordinate(rect.min_lat, rect.min_lon))); }