diff --git a/include/engine/datafacade/contiguous_internalmem_datafacade.hpp b/include/engine/datafacade/contiguous_internalmem_datafacade.hpp index 9b35989e1..ac9497eb0 100644 --- a/include/engine/datafacade/contiguous_internalmem_datafacade.hpp +++ b/include/engine/datafacade/contiguous_internalmem_datafacade.hpp @@ -375,7 +375,7 @@ class ContiguousInternalMemoryDataFacadeBase : public BaseDataFacade BOOST_ASSERT(m_geospatial_query.get()); return m_geospatial_query->NearestPhantomNodes( - input_coordinate, approach, boost::none, max_distance, bearing, use_all_edges); + input_coordinate, approach, max_distance, bearing, use_all_edges); } std::vector diff --git a/include/engine/geospatial_query.hpp b/include/engine/geospatial_query.hpp index b8e43b637..86e5c9e87 100644 --- a/include/engine/geospatial_query.hpp +++ b/include/engine/geospatial_query.hpp @@ -47,6 +47,50 @@ template class GeospatialQuery return rtree.SearchInBox(bbox); } + std::vector + NearestPhantomNodes(const util::Coordinate input_coordinate, + const Approach approach, + const double max_distance, + const boost::optional bearing_with_range, + const boost::optional use_all_edges) const + { +#if 0 + return NearestPhantomNodes(input_coordinate, + approach, + boost::optional{}, + max_distance, + bearing_with_range, + use_all_edges); +#else + auto results = rtree.SearchInRange( + input_coordinate, + max_distance, + [this, approach, &input_coordinate, &bearing_with_range, &use_all_edges, max_distance]( + const CandidateSegment &segment) + { + auto invalidDistance = + CheckSegmentDistance(input_coordinate, segment, max_distance); + if (invalidDistance) + { + return std::pair{false, false}; + } + (void)approach; + (void)use_all_edges; + (void)bearing_with_range; + (void)use_all_edges; + + auto valid = CheckSegmentExclude(segment) && + CheckApproach(input_coordinate, segment, approach) && + (use_all_edges ? HasValidEdge(segment, *use_all_edges) + : HasValidEdge(segment)) && + (bearing_with_range ? CheckSegmentBearing(segment, *bearing_with_range) + : std::make_pair(true, true)); + return valid; + }); + return MakePhantomNodes(input_coordinate, results); +#endif + } + // Returns max_results nearest PhantomNodes that are valid within the provided parameters. // Does not filter by small/big component! std::vector @@ -294,6 +338,11 @@ template class GeospatialQuery distance_and_phantoms.begin(), [this, &input_coordinate](const CandidateSegment &segment) { return MakePhantomNode(input_coordinate, segment.data); }); + std::sort(distance_and_phantoms.begin(), distance_and_phantoms.end(), [](const auto &lhs, + const auto &rhs) + { + return lhs.distance < rhs.distance; + }); return distance_and_phantoms; } diff --git a/include/util/rectangle.hpp b/include/util/rectangle.hpp index 65050f200..ff4c2c6bc 100644 --- a/include/util/rectangle.hpp +++ b/include/util/rectangle.hpp @@ -1,6 +1,7 @@ #ifndef OSRM_UTIL_RECTANGLE_HPP #define OSRM_UTIL_RECTANGLE_HPP +#include "coordinate.hpp" #include "util/coordinate.hpp" #include "util/coordinate_calculation.hpp" @@ -168,6 +169,26 @@ struct RectangleInt2D min_lat != FixedLatitude{std::numeric_limits::max()} && max_lat != FixedLatitude{std::numeric_limits::min()}; } + + static double MetersPerLngDegree(const FixedLatitude lat) + { + constexpr static double kMetersPerDegreeLat = 110567.0f; + + constexpr float kRadPerDeg = (3.14 /* TODO */ / 180.0f); + + return std::cos(kRadPerDeg * static_cast(toFloating(lat))) * kMetersPerDegreeLat; + } + static RectangleInt2D ExpandMeters(const Coordinate &coordinate, const double meters) + { + constexpr static double kMetersPerDegreeLat = 110567.0f; + const double lat_offset = meters / kMetersPerDegreeLat; + const double lon_offset = meters / MetersPerLngDegree(coordinate.lat); + + return RectangleInt2D{coordinate.lon - toFixed(FloatLongitude{lon_offset}), + coordinate.lon + toFixed(FloatLongitude{lon_offset}), + coordinate.lat - toFixed(FloatLatitude{lat_offset}), + coordinate.lat + toFixed(FloatLatitude{lat_offset})}; + } }; } // namespace osrm::util diff --git a/include/util/static_rtree.hpp b/include/util/static_rtree.hpp index d39d8454d..0b562afe1 100644 --- a/include/util/static_rtree.hpp +++ b/include/util/static_rtree.hpp @@ -2,7 +2,6 @@ #define STATIC_RTREE_HPP #include "storage/tar_fwd.hpp" - #include "util/bearing.hpp" #include "util/coordinate_calculation.hpp" #include "util/deallocating_vector.hpp" @@ -11,6 +10,7 @@ #include "util/integer_range.hpp" #include "util/mmap_file.hpp" #include "util/rectangle.hpp" +#include "util/timing_util.hpp" #include "util/typedefs.hpp" #include "util/vector_view.hpp" #include "util/web_mercator.hpp" @@ -565,6 +565,57 @@ class StaticRTree { return num_results >= max_results; }); } + inline double GetSegmentDistance(const Coordinate input_coordinate, + const CandidateSegment &segment) const + { + BOOST_ASSERT(segment.data.forward_segment_id.id != SPECIAL_SEGMENTID || + !segment.data.forward_segment_id.enabled); + BOOST_ASSERT(segment.data.reverse_segment_id.id != SPECIAL_SEGMENTID || + !segment.data.reverse_segment_id.enabled); + + Coordinate wsg84_coordinate = + util::web_mercator::toWGS84(segment.fixed_projected_coordinate); + + return util::coordinate_calculation::greatCircleDistance(input_coordinate, + wsg84_coordinate); + } + + template + std::vector SearchInRange(const Coordinate input_coordinate, + double maxDistanceMeters, + const FilterT filter) const + { + (void)filter; + auto projected_coordinate = web_mercator::fromWGS84(input_coordinate); + Coordinate fixed_projected_coordinate{projected_coordinate}; + + auto bbox = Rectangle::ExpandMeters(input_coordinate, maxDistanceMeters); + auto results_in_bbox = SearchInBox(bbox); + std::vector results; + for (const auto ¤t_edge : results_in_bbox) + { + const auto projected_u = web_mercator::fromWGS84(m_coordinate_list[current_edge.u]); + const auto projected_v = web_mercator::fromWGS84(m_coordinate_list[current_edge.v]); + + FloatCoordinate projected_nearest; + std::tie(std::ignore, projected_nearest) = + coordinate_calculation::projectPointOnSegment( + projected_u, projected_v, fixed_projected_coordinate); + + CandidateSegment current_candidate{projected_nearest, current_edge}; + auto use_segment = filter(current_candidate); + if (!use_segment.first && !use_segment.second) + { + continue; + } + current_candidate.data.forward_segment_id.enabled &= use_segment.first; + current_candidate.data.reverse_segment_id.enabled &= use_segment.second; + + results.push_back(std::move(current_candidate)); + } + return results; + } + // Return edges in distance order with the coordinate of the closest point on the edge. template std::vector Nearest(const Coordinate input_coordinate, @@ -572,8 +623,10 @@ class StaticRTree const TerminationT terminate) const { std::vector results; + auto projected_coordinate = web_mercator::fromWGS84(input_coordinate); Coordinate fixed_projected_coordinate{projected_coordinate}; + // initialize queue with root element std::priority_queue traversal_queue; traversal_queue.push(QueryCandidate{0, TreeIndex{}}); diff --git a/src/benchmarks/match.cpp b/src/benchmarks/match.cpp index caed1a00b..cd57b22e4 100644 --- a/src/benchmarks/match.cpp +++ b/src/benchmarks/match.cpp @@ -224,6 +224,7 @@ try if (rc != Status::Ok || json_result.values.at("matchings").get().values.size() != 1) { + std::cerr << "Failure\n"; return EXIT_FAILURE; } }