Optimise R-tree queries in the case if there is no need to limit maximum number of results

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<PhantomNodeWithDistance>

include/engine/geospatial_query.hpp

@@ -47,6 +47,50 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
         return rtree.SearchInBox(bbox);
     }
 
+    std::vector<PhantomNodeWithDistance>
+    NearestPhantomNodes(const util::Coordinate input_coordinate,
+                        const Approach approach,
+                        const double max_distance,
+                        const boost::optional<Bearing> bearing_with_range,
+                        const boost::optional<bool> use_all_edges) const
+    {
+#if 0
+        return NearestPhantomNodes(input_coordinate,
+                                   approach,
+                                   boost::optional<size_t>{},
+                                   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<bool, bool>{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<PhantomNodeWithDistance>
@@ -294,6 +338,11 @@ template <typename RTreeT, typename DataFacadeT> 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;
     }
 

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<std::int32_t>::max()} &&
                max_lat != FixedLatitude{std::numeric_limits<std::int32_t>::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<double>(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
 

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 <typename FilterT>
+    std::vector<CandidateSegment> 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<CandidateSegment> results;
+        for (const auto &current_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 <typename FilterT, typename TerminationT>
     std::vector<CandidateSegment> Nearest(const Coordinate input_coordinate,
@@ -572,8 +623,10 @@ class StaticRTree
                                           const TerminationT terminate) const
     {
         std::vector<CandidateSegment> results;
+
         auto projected_coordinate = web_mercator::fromWGS84(input_coordinate);
         Coordinate fixed_projected_coordinate{projected_coordinate};
+
         // initialize queue with root element
         std::priority_queue<QueryCandidate> traversal_queue;
         traversal_queue.push(QueryCandidate{0, TreeIndex{}});

src/benchmarks/match.cpp

@@ -224,6 +224,7 @@ try
         if (rc != Status::Ok ||
             json_result.values.at("matchings").get<json::Array>().values.size() != 1)
         {
+            std::cerr << "Failure\n";
             return EXIT_FAILURE;
         }
     }