Refactor StaticRTree to remove application dependent code

StaticRTree now acts like a container, just returning the input data
(NodeBasedEdge) and not PhantomNodes.

.travis.yml

@@ -141,5 +141,6 @@ script:
   - make benchmarks
   - ./algorithm-tests
   - ./datastructure-tests
+  - ./util-tests
   - cd ..
   - cucumber -p verify

CMakeLists.txt

@@ -46,7 +46,7 @@ add_custom_target(FingerPrintConfigure ALL ${CMAKE_COMMAND}
   COMMENT "Configuring revision fingerprint"
   VERBATIM)
 
-add_custom_target(tests DEPENDS datastructure-tests algorithm-tests)
+add_custom_target(tests DEPENDS datastructure-tests algorithm-tests util-tests)
 add_custom_target(benchmarks DEPENDS rtree-bench)
 
 set(BOOST_COMPONENTS date_time filesystem iostreams program_options regex system thread unit_test_framework)
@@ -85,6 +85,7 @@ file(GLOB HttpGlob server/http/*.cpp)
 file(GLOB LibOSRMGlob library/*.cpp)
 file(GLOB DataStructureTestsGlob unit_tests/data_structures/*.cpp data_structures/hilbert_value.cpp)
 file(GLOB AlgorithmTestsGlob unit_tests/algorithms/*.cpp algorithms/graph_compressor.cpp)
+file(GLOB UtilTestsGlob unit_tests/util/*.cpp)
 
 set(
   OSRMSources
@@ -109,6 +110,7 @@ add_executable(osrm-datastore datastore.cpp $<TARGET_OBJECTS:COORDINATE> $<TARGE
 # Unit tests
 add_executable(datastructure-tests EXCLUDE_FROM_ALL unit_tests/datastructure_tests.cpp ${DataStructureTestsGlob} $<TARGET_OBJECTS:COORDINATE> $<TARGET_OBJECTS:LOGGER> $<TARGET_OBJECTS:PHANTOMNODE> $<TARGET_OBJECTS:EXCEPTION> $<TARGET_OBJECTS:MERCATOR> $<TARGET_OBJECTS:COMPRESSEDEDGE> $<TARGET_OBJECTS:GRAPHCOMPRESSOR> $<TARGET_OBJECTS:RESTRICTION> $<TARGET_OBJECTS:RASTERSOURCE>)
 add_executable(algorithm-tests EXCLUDE_FROM_ALL unit_tests/algorithm_tests.cpp ${AlgorithmTestsGlob} $<TARGET_OBJECTS:COORDINATE> $<TARGET_OBJECTS:LOGGER> $<TARGET_OBJECTS:PHANTOMNODE> $<TARGET_OBJECTS:EXCEPTION> $<TARGET_OBJECTS:RESTRICTION> $<TARGET_OBJECTS:COMPRESSEDEDGE>)
+add_executable(util-tests EXCLUDE_FROM_ALL unit_tests/util_tests.cpp ${UtilTestsGlob})
 
 # Benchmarks
 add_executable(rtree-bench EXCLUDE_FROM_ALL benchmarks/static_rtree.cpp $<TARGET_OBJECTS:COORDINATE> $<TARGET_OBJECTS:LOGGER> $<TARGET_OBJECTS:PHANTOMNODE> $<TARGET_OBJECTS:EXCEPTION> $<TARGET_OBJECTS:MERCATOR>)
@@ -262,6 +264,7 @@ target_link_libraries(osrm-routed ${Boost_LIBRARIES} ${OPTIONAL_SOCKET_LIBS} OSR
 target_link_libraries(osrm-datastore ${Boost_LIBRARIES})
 target_link_libraries(datastructure-tests ${Boost_LIBRARIES})
 target_link_libraries(algorithm-tests ${Boost_LIBRARIES} ${OPTIONAL_SOCKET_LIBS} OSRM)
+target_link_libraries(util-tests ${Boost_LIBRARIES})
 target_link_libraries(rtree-bench ${Boost_LIBRARIES})
 
 find_package(Threads REQUIRED)

algorithms/geospatial_query.hpp

@@ -0,0 +1,180 @@
+#ifndef GEOSPATIAL_QUERY_HPP
+#define GEOSPATIAL_QUERY_HPP
+
+#include "coordinate_calculation.hpp"
+#include "../typedefs.h"
+#include "../data_structures/phantom_node.hpp"
+#include "../util/bearing.hpp"
+
+#include <osrm/coordinate.hpp>
+
+#include <vector>
+#include <memory>
+#include <algorithm>
+
+// Implements complex queries on top of an RTree and builds PhantomNodes from it.
+//
+// Only holds a weak reference on the RTree!
+template <typename RTreeT> class GeospatialQuery
+{
+    using EdgeData = typename RTreeT::EdgeData;
+    using CoordinateList = typename RTreeT::CoordinateList;
+
+  public:
+    GeospatialQuery(RTreeT &rtree_, std::shared_ptr<CoordinateList> coordinates_)
+        : rtree(rtree_), coordinates(coordinates_)
+    {
+    }
+
+    // Returns nearest PhantomNodes in the given bearing range within max_distance.
+    // Does not filter by small/big component!
+    std::vector<std::pair<double, PhantomNode>>
+    NearestPhantomNodesInRange(const FixedPointCoordinate &input_coordinate,
+                               const float max_distance,
+                               const int bearing = 0,
+                               const int bearing_range = 180)
+    {
+        auto results =
+            rtree.Nearest(input_coordinate,
+                          [this, bearing, bearing_range, max_distance](const EdgeData &data)
+                          {
+                              return checkSegmentBearing(data, bearing, bearing_range);
+                          },
+                          [max_distance](const std::size_t, const float min_dist)
+                          {
+                              return min_dist > max_distance;
+                          });
+
+        return MakePhantomNodes(input_coordinate, results);
+    }
+
+    // Returns max_results nearest PhantomNodes in the given bearing range.
+    // Does not filter by small/big component!
+    std::vector<std::pair<double, PhantomNode>>
+    NearestPhantomNodes(const FixedPointCoordinate &input_coordinate,
+                        const unsigned max_results,
+                        const int bearing = 0,
+                        const int bearing_range = 180)
+    {
+        auto results = rtree.Nearest(input_coordinate,
+                                     [this, bearing, bearing_range](const EdgeData &data)
+                                     {
+                                         return checkSegmentBearing(data, bearing, bearing_range);
+                                     },
+                                     [max_results](const std::size_t num_results, const float)
+                                     {
+                                         return num_results >= max_results;
+                                     });
+
+        return MakePhantomNodes(input_coordinate, results);
+    }
+
+    // Returns the nearest phantom node. If this phantom node is not from a big component
+    // a second phantom node is return that is the nearest coordinate in a big component.
+    std::pair<PhantomNode, PhantomNode>
+    NearestPhantomNodeWithAlternativeFromBigComponent(const FixedPointCoordinate &input_coordinate,
+                                                      const int bearing = 0,
+                                                      const int bearing_range = 180)
+    {
+        bool has_small_component = false;
+        bool has_big_component = false;
+        auto results = rtree.Nearest(
+            input_coordinate,
+            [this, bearing, bearing_range, &has_big_component,
+             &has_small_component](const EdgeData &data)
+            {
+                auto use_segment =
+                    (!has_small_component || (!has_big_component && !data.component.is_tiny)) ;
+                auto use_directions = std::make_pair(use_segment, use_segment);
+
+                if (use_segment)
+                {
+                    use_directions = checkSegmentBearing(data, bearing, bearing_range);
+                    if (use_directions.first || use_directions.second)
+                    {
+                        has_big_component = has_big_component || !data.component.is_tiny;
+                        has_small_component = has_small_component || data.component.is_tiny;
+                    }
+                }
+
+                return use_directions;
+            },
+            [&has_big_component](const std::size_t num_results, const float)
+            {
+                return num_results > 0 && has_big_component;
+            });
+
+
+        if (results.size() == 0)
+        {
+            return std::make_pair(PhantomNode {}, PhantomNode {});
+        }
+
+        BOOST_ASSERT(results.size() > 0);
+        return std::make_pair(MakePhantomNode(input_coordinate, results.front()).second, MakePhantomNode(input_coordinate, results.back()).second);
+    }
+
+  private:
+    inline std::vector<std::pair<double, PhantomNode>>
+    MakePhantomNodes(const FixedPointCoordinate &input_coordinate,
+                     const std::vector<EdgeData> &results) const
+    {
+        std::vector<std::pair<double, PhantomNode>> distance_and_phantoms(results.size());
+        std::transform(results.begin(), results.end(), distance_and_phantoms.begin(),
+                       [this, &input_coordinate](const EdgeData &data)
+                       {
+                           return MakePhantomNode(input_coordinate, data);
+                       });
+        return distance_and_phantoms;
+    }
+
+    inline std::pair<double, PhantomNode>
+    MakePhantomNode(const FixedPointCoordinate &input_coordinate, const EdgeData &data) const
+    {
+        FixedPointCoordinate point_on_segment;
+        float ratio;
+        const auto current_perpendicular_distance = coordinate_calculation::perpendicular_distance(
+            coordinates->at(data.u), coordinates->at(data.v), input_coordinate, point_on_segment,
+            ratio);
+
+        auto transformed =
+            std::make_pair(current_perpendicular_distance, PhantomNode{data, point_on_segment});
+
+        ratio = std::min(1.f, std::max(0.f, ratio));
+
+        if (SPECIAL_NODEID != transformed.second.forward_node_id)
+        {
+            transformed.second.forward_weight *= ratio;
+        }
+        if (SPECIAL_NODEID != transformed.second.reverse_node_id)
+        {
+            transformed.second.reverse_weight *= 1.f - ratio;
+        }
+        return transformed;
+    }
+
+    inline std::pair<bool, bool> checkSegmentBearing(const EdgeData &segment,
+                                                     const float filter_bearing,
+                                                     const float filter_bearing_range)
+    {
+        const float forward_edge_bearing =
+            coordinate_calculation::bearing(coordinates->at(segment.u), coordinates->at(segment.v));
+
+        const float backward_edge_bearing = (forward_edge_bearing + 180) > 360
+                                                ? (forward_edge_bearing - 180)
+                                                : (forward_edge_bearing + 180);
+
+        const bool forward_bearing_valid =
+            bearing::CheckInBounds(forward_edge_bearing, filter_bearing, filter_bearing_range) &&
+            segment.forward_edge_based_node_id != SPECIAL_NODEID;
+        const bool backward_bearing_valid =
+            bearing::CheckInBounds(backward_edge_bearing, filter_bearing, filter_bearing_range) &&
+            segment.reverse_edge_based_node_id != SPECIAL_NODEID;
+        return std::make_pair(forward_bearing_valid, backward_bearing_valid);
+    }
+
+    RTreeT &rtree;
+    const std::shared_ptr<CoordinateList> coordinates;
+};
+
+#endif

data_structures/hidden_markov_model.hpp

@@ -140,7 +140,7 @@ template <class CandidateLists> struct HiddenMarkovModel
             for (const auto s : osrm::irange<std::size_t>(0u, viterbi[initial_timestamp].size()))
             {
                 viterbi[initial_timestamp][s] =
-                    emission_log_probability(candidates_list[initial_timestamp][s].second);
+                    emission_log_probability(candidates_list[initial_timestamp][s].first);
                 parents[initial_timestamp][s] = std::make_pair(initial_timestamp, s);
                 pruned[initial_timestamp][s] =
                     viterbi[initial_timestamp][s] < osrm::matching::MINIMAL_LOG_PROB;

data_structures/phantom_node.hpp

@@ -122,17 +122,7 @@ struct PhantomNode
 static_assert(sizeof(PhantomNode) == 48, "PhantomNode has more padding then expected");
 #endif
 
-using PhantomNodeArray = std::vector<std::vector<PhantomNode>>;
-
-class phantom_node_pair : public std::pair<PhantomNode, PhantomNode>
-{
-};
-
-struct PhantomNodeLists
-{
-    std::vector<PhantomNode> source_phantom_list;
-    std::vector<PhantomNode> target_phantom_list;
-};
+using PhantomNodePair = std::pair<PhantomNode, PhantomNode>;
 
 struct PhantomNodes
 {

data_structures/rectangle.hpp

@@ -170,13 +170,13 @@ struct RectangleInt2D
         const FixedPointCoordinate lower_right(min_lat, max_lon);
         const FixedPointCoordinate lower_left(min_lat, min_lon);
 
-        min_max_dist =
-            std::min(min_max_dist,
+        min_max_dist = std::min(
+            min_max_dist,
             std::max(coordinate_calculation::great_circle_distance(location, upper_left),
                      coordinate_calculation::great_circle_distance(location, upper_right)));
 
-        min_max_dist =
-            std::min(min_max_dist,
+        min_max_dist = std::min(
+            min_max_dist,
             std::max(coordinate_calculation::great_circle_distance(location, upper_right),
                      coordinate_calculation::great_circle_distance(location, lower_right)));
 
@@ -198,14 +198,6 @@ struct RectangleInt2D
         const bool lons_contained = (location.lon >= min_lon) && (location.lon <= max_lon);
         return lats_contained && lons_contained;
     }
-
-    friend std::ostream &operator<<(std::ostream &out, const RectangleInt2D &rect)
-    {
-        out << rect.min_lat / COORDINATE_PRECISION << "," << rect.min_lon / COORDINATE_PRECISION
-            << " " << rect.max_lat / COORDINATE_PRECISION << ","
-            << rect.max_lon / COORDINATE_PRECISION;
-        return out;
-    }
 };
 
 #endif

extractor/extractor.cpp

@@ -575,9 +575,18 @@ void extractor::WriteNodeMapping(const std::vector<QueryNode> & internal_to_exte
 void extractor::BuildRTree(const std::vector<EdgeBasedNode> &node_based_edge_list,
                            const std::vector<QueryNode> &internal_to_external_node_map)
 {
+    SimpleLogger().Write() << "constructing r-tree of " << node_based_edge_list.size()
+                           << " edge elements build on-top of " << internal_to_external_node_map.size()
+                           << " coordinates";
+
+    TIMER_START(construction);
     StaticRTree<EdgeBasedNode>(node_based_edge_list, config.rtree_nodes_output_path.c_str(),
                                config.rtree_leafs_output_path.c_str(),
                                internal_to_external_node_map);
+
+    TIMER_STOP(construction);
+    SimpleLogger().Write() << "finished r-tree construction in " << TIMER_SEC(construction)
+                           << " seconds";
 }
 
 void extractor::WriteEdgeBasedGraph(std::string const &output_file_filename, 

features/locate/locate.feature

@@ -1,197 +0,0 @@
-@locate
-Feature: Locate - return nearest node
-
-    Background:
-        Given the profile "testbot"
-
-    Scenario: Locate - two ways crossing
-        Given the node map
-            |   |  | 0 | c | 1 |  |   |
-            |   |  |   |   |   |  |   |
-            | 7 |  |   | n |   |  | 2 |
-            | a |  | k | x | m |  | b |
-            | 6 |  |   | l |   |  | 3 |
-            |   |  |   |   |   |  |   |
-            |   |  | 5 | d | 4 |  |   |
-
-        And the ways
-            | nodes |
-            | axb   |
-            | cxd   |
-
-        When I request locate I should get
-            | in | out |
-            | 0  | c   |
-            | 1  | c   |
-            | 2  | b   |
-            | 3  | b   |
-            | 4  | d   |
-            | 5  | d   |
-            | 6  | a   |
-            | 7  | a   |
-            | a  | a   |
-            | b  | b   |
-            | c  | c   |
-            | d  | d   |
-            | k  | x   |
-            | l  | x   |
-            | m  | x   |
-            | n  | x   |
-
-    Scenario: Locate - inside a triangle
-        Given the node map
-            |   |  |   |   |   | c |   |   |   |  |   |
-            |   |  |   |   |   | 7 |   |   |   |  |   |
-            |   |  |   | y |   |   |   | z |   |  |   |
-            |   |  | 5 |   | 0 |   | 1 |   | 8 |  |   |
-            | 6 |  |   | 2 |   | 3 |   | 4 |   |  | 9 |
-            | a |  |   | x |   | u |   | w |   |  | b |
-
-        And the ways
-            | nodes |
-            | ab    |
-            | bc    |
-            | ca    |
-
-        When I request locate I should get
-            | in | out |
-            | 0  | c   |
-            | 1  | c   |
-            | 2  | a   |
-            | 3  | c   |
-            | 4  | b   |
-            | 5  | a   |
-            | 6  | a   |
-            | 7  | c   |
-            | 8  | b   |
-            | 9  | b   |
-            | x  | a   |
-            | y  | c   |
-            | z  | c   |
-            | w  | b   |
-
-    Scenario: Nearest - easy-west way
-        Given the node map
-            | 3 | 4 |   | 5 | 6 |
-            | 2 | a | x | b | 7 |
-            | 1 | 0 |   | 9 | 8 |
-
-        And the ways
-            | nodes |
-            | ab    |
-
-        When I request locate I should get
-            | in | out |
-            | 0  | a   |
-            | 1  | a   |
-            | 2  | a   |
-            | 3  | a   |
-            | 4  | a   |
-            | 5  | b   |
-            | 6  | b   |
-            | 7  | b   |
-            | 8  | b   |
-            | 9  | b   |
-
-    Scenario: Nearest - north-south way
-        Given the node map
-            | 1 | 2 | 3 |
-            | 0 | a | 4 |
-            |   | x |   |
-            | 9 | b | 5 |
-            | 8 | 7 | 6 |
-
-        And the ways
-            | nodes |
-            | ab    |
-
-        When I request locate I should get
-            | in | out |
-            | 0  | a   |
-            | 1  | a   |
-            | 2  | a   |
-            | 3  | a   |
-            | 4  | a   |
-            | 5  | b   |
-            | 6  | b   |
-            | 7  | b   |
-            | 8  | b   |
-            | 9  | b   |
-
-    Scenario: Nearest - diagonal 1
-        Given the node map
-            | 2 |   | 3 |   |   |   |
-            |   | a |   | 4 |   |   |
-            | 1 |   | x |   | 5 |   |
-            |   | 0 |   | y |   | 6 |
-            |   |   | 9 |   | b |   |
-            |   |   |   | 8 |   | 7 |
-
-        And the ways
-            | nodes |
-            | axyb  |
-
-        When I request locate I should get
-            | in | out |
-            | 0  | x   |
-            | 1  | a   |
-            | 2  | a   |
-            | 3  | a   |
-            | 4  | x   |
-            | 5  | y   |
-            | 6  | b   |
-            | 7  | b   |
-            | 8  | b   |
-            | 9  | y   |
-            | a  | a   |
-            | b  | b   |
-            | x  | x   |
-            | y  | y   |
-
-    Scenario: Nearest - diagonal 2
-        Given the node map
-            |   |   |   | 6 |   | 7 |
-            |   |   | 5 |   | b |   |
-            |   | 4 |   | y |   | 8 |
-            | 3 |   | x |   | 9 |   |
-            |   | a |   | 0 |   |   |
-            | 2 |   | 1 |   |   |   |
-
-        And the ways
-        | nodes |
-        | ab    |
-
-        When I request nearest I should get
-            | in | out |
-            | 0  | x   |
-            | 1  | a   |
-            | 2  | a   |
-            | 3  | a   |
-            | 4  | x   |
-            | 5  | y   |
-            | 6  | b   |
-            | 7  | b   |
-            | 8  | b   |
-            | 9  | y   |
-            | a  | a   |
-            | b  | b   |
-            | x  | x   |
-            | y  | y   |
-
-        Scenario: Locate - High lat/lon
-           Given the node locations
-            | node | lat | lon  |
-            | a    | -85 | -180 |
-            | b    | 0   | 0    |
-            | c    | 85  | 180  |
-            | x    | -84 | -180 |
-            | y    | 84  | 180  |
-
-           And the ways
-            | nodes |
-            | abc   |
-
-           When I request locate I should get
-            | in | out |
-            | x  | a   |
-            | y  | c   |

features/step_definitions/locate.rb

@@ -1,51 +0,0 @@
-When /^I request locate I should get$/ do |table|
-  reprocess
-  actual = []
-  OSRMLoader.load(self,"#{prepared_file}.osrm") do
-    table.hashes.each_with_index do |row,ri|
-      in_node = find_node_by_name row['in']
-      raise "*** unknown in-node '#{row['in']}" unless in_node
-
-      out_node = find_node_by_name row['out']
-      raise "*** unknown out-node '#{row['out']}" unless out_node
-
-      response = request_locate in_node, @query_params
-      if response.code == "200" && response.body.empty? == false
-        json = JSON.parse response.body
-        if json['status'] == 0
-          coord =  json['mapped_coordinate']
-        end
-      end
-
-      got = {'in' => row['in'], 'out' => coord }
-
-      ok = true
-      row.keys.each do |key|
-        if key=='out'
-          if FuzzyMatch.match_location coord, out_node
-            got[key] = row[key]
-          else
-            row[key] = "#{row[key]} [#{out_node.lat},#{out_node.lon}]"
-            ok = false
-          end
-        end
-      end
-
-      unless ok
-        failed = { :attempt => 'locate', :query => @query, :response => response }
-        log_fail row,got,[failed]
-      end
-
-      actual << got
-    end
-  end
-  table.diff! actual
-end
-
-When /^I request locate (\d+) times I should get$/ do |n,table|
-  ok = true
-  n.to_i.times do
-    ok = false unless step "I request locate I should get", table
-  end
-  ok
-end

features/support/route.rb

@@ -58,14 +58,6 @@ def request_route waypoints, bearings, user_params
   return request_path "viaroute", params
 end
 
-def request_locate node, user_params
-  defaults = [['output', 'json']]
-  params = overwrite_params defaults, user_params
-  params << ["loc", "#{node.lat},#{node.lon}"]
-
-  return request_path "locate", params
-end
-
 def request_nearest node, user_params
   defaults = [['output', 'json']]
   params = overwrite_params defaults, user_params

features/testbot/post.feature

@@ -62,24 +62,6 @@ Feature: POST request
             | d | 200 | 300 | 0   | 300 |
             | e | 300 | 400 | 100 | 0   |
 
-    Scenario: Testbot - locate POST request
-        Given the node locations
-            | node | lat | lon  |
-            | a    | -85 | -180 |
-            | b    | 0   | 0    |
-            | c    | 85  | 180  |
-            | x    | -84 | -180 |
-            | y    | 84  | 180  |
-
-        And the ways
-            | nodes |
-            | abc   |
-
-        When I request locate I should get
-            | in | out |
-            | x  | a   |
-            | y  | c   |
-            
     Scenario: Testbot - nearest POST request
         Given the node locations
             | node | lat     | lon  |

library/osrm_impl.cpp

@@ -38,7 +38,6 @@ class named_mutex;
 
 #include "../plugins/distance_table.hpp"
 #include "../plugins/hello_world.hpp"
-#include "../plugins/locate.hpp"
 #include "../plugins/nearest.hpp"
 #include "../plugins/timestamp.hpp"
 #include "../plugins/trip.hpp"
@@ -81,7 +80,6 @@ OSRM_impl::OSRM_impl(libosrm_config &lib_config)
     RegisterPlugin(new DistanceTablePlugin<BaseDataFacade<QueryEdge::EdgeData>>(
         query_data_facade, lib_config.max_locations_distance_table));
     RegisterPlugin(new HelloWorldPlugin());
-    RegisterPlugin(new LocatePlugin<BaseDataFacade<QueryEdge::EdgeData>>(query_data_facade));
     RegisterPlugin(new NearestPlugin<BaseDataFacade<QueryEdge::EdgeData>>(query_data_facade));
     RegisterPlugin(new MapMatchingPlugin<BaseDataFacade<QueryEdge::EdgeData>>(
         query_data_facade, lib_config.max_locations_map_matching));

plugins/distance_table.hpp

@@ -108,8 +108,8 @@ template <class DataFacadeT> class DistanceTablePlugin final : public BasePlugin
 
         const bool checksum_OK = (route_parameters.check_sum == facade->GetCheckSum());
 
-        PhantomNodeArray phantom_node_source_vector(number_of_sources);
-        PhantomNodeArray phantom_node_target_vector(number_of_destination);
+        std::vector<PhantomNodePair> phantom_node_source_vector(number_of_sources);
+        std::vector<PhantomNodePair> phantom_node_target_vector(number_of_destination);
         auto phantom_node_source_out_iter = phantom_node_source_vector.begin();
         auto phantom_node_target_out_iter = phantom_node_target_vector.begin();
         for (const auto i : osrm::irange<std::size_t>(0u, route_parameters.coordinates.size()))
@@ -123,7 +123,7 @@ template <class DataFacadeT> class DistanceTablePlugin final : public BasePlugin
                 {
                     if (route_parameters.is_source[i])
                     {
-                        phantom_node_source_out_iter->emplace_back(std::move(current_phantom_node));
+                        *phantom_node_source_out_iter = std::make_pair(current_phantom_node, current_phantom_node);
                         if (route_parameters.is_destination[i])
                         {
                             *phantom_node_target_out_iter = *phantom_node_source_out_iter;
@@ -134,7 +134,7 @@ template <class DataFacadeT> class DistanceTablePlugin final : public BasePlugin
                     else
                     {
                         BOOST_ASSERT(route_parameters.is_destination[i] && !route_parameters.is_source[i]);
-                        phantom_node_target_out_iter->emplace_back(std::move(current_phantom_node));
+                        *phantom_node_target_out_iter = std::make_pair(current_phantom_node, current_phantom_node);
                         phantom_node_target_out_iter++;
                     }
                     continue;
@@ -146,11 +146,14 @@ template <class DataFacadeT> class DistanceTablePlugin final : public BasePlugin
                                   : 180;
             if (route_parameters.is_source[i])
             {
-                facade->IncrementalFindPhantomNodeForCoordinate(route_parameters.coordinates[i],
-                                                                *phantom_node_source_out_iter, 1,
-                                                                bearing, range);
-                BOOST_ASSERT(
-                    phantom_node_source_out_iter->front().is_valid(facade->GetNumberOfNodes()));
+                *phantom_node_source_out_iter = facade->NearestPhantomNodeWithAlternativeFromBigComponent(route_parameters.coordinates[i], bearing, range);
+                // we didn't found a fitting node, return error
+                if (!phantom_node_source_out_iter->first.is_valid(facade->GetNumberOfNodes()))
+                {
+                    json_result.values["status_message"] = std::string("Could not find matching road for via ") + std::to_string(i);
+                    return 400;
+                }
+
                 if (route_parameters.is_destination[i])
                 {
                     *phantom_node_target_out_iter = *phantom_node_source_out_iter;
@@ -161,14 +164,16 @@ template <class DataFacadeT> class DistanceTablePlugin final : public BasePlugin
             else
             {
                 BOOST_ASSERT(route_parameters.is_destination[i] && !route_parameters.is_source[i]);
-                facade->IncrementalFindPhantomNodeForCoordinate(route_parameters.coordinates[i],
-                                                                *phantom_node_target_out_iter, 1,
-                                                                bearing, range);
-                BOOST_ASSERT(
-                    phantom_node_target_out_iter->front().is_valid(facade->GetNumberOfNodes()));
+
+                *phantom_node_target_out_iter = facade->NearestPhantomNodeWithAlternativeFromBigComponent(route_parameters.coordinates[i], bearing, range);
+                // we didn't found a fitting node, return error
+                if (!phantom_node_target_out_iter->first.is_valid(facade->GetNumberOfNodes()))
+                {
+                    json_result.values["status_message"] = std::string("Could not find matching road for via ") + std::to_string(i);
+                    return 400;
+                }
                 phantom_node_target_out_iter++;
             }
-
         }
         BOOST_ASSERT((phantom_node_source_out_iter - phantom_node_source_vector.begin()) ==
                      number_of_sources);
@@ -195,20 +200,20 @@ template <class DataFacadeT> class DistanceTablePlugin final : public BasePlugin
         }
         json_result.values["distance_table"] = matrix_json_array;
         osrm::json::Array target_coord_json_array;
-        for (const std::vector<PhantomNode> &phantom_node_vector : phantom_node_target_vector)
+        for (const auto &pair : phantom_node_target_vector)
         {
             osrm::json::Array json_coord;
-            FixedPointCoordinate coord = phantom_node_vector[0].location;
+            FixedPointCoordinate coord = pair.first.location;
             json_coord.values.push_back(coord.lat / COORDINATE_PRECISION);
             json_coord.values.push_back(coord.lon / COORDINATE_PRECISION);
             target_coord_json_array.values.push_back(json_coord);
         }
         json_result.values["destination_coordinates"] = target_coord_json_array;
         osrm::json::Array source_coord_json_array;
-        for (const std::vector<PhantomNode> &phantom_node_vector : phantom_node_source_vector)
+        for (const auto &pair : phantom_node_source_vector)
         {
             osrm::json::Array json_coord;
-            FixedPointCoordinate coord = phantom_node_vector[0].location;
+            FixedPointCoordinate coord = pair.first.location;
             json_coord.values.push_back(coord.lat / COORDINATE_PRECISION);
             json_coord.values.push_back(coord.lon / COORDINATE_PRECISION);
             source_coord_json_array.values.push_back(json_coord);

plugins/locate.hpp

@@ -1,79 +0,0 @@
-/*
-
-Copyright (c) 2015, Project OSRM contributors
-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 LOCATE_HPP
-#define LOCATE_HPP
-
-#include "plugin_base.hpp"
-
-#include "../util/json_renderer.hpp"
-#include "../util/string_util.hpp"
-
-#include <osrm/json_container.hpp>
-
-#include <string>
-
-// locates the nearest node in the road network for a given coordinate.
-template <class DataFacadeT> class LocatePlugin final : public BasePlugin
-{
-  public:
-    explicit LocatePlugin(DataFacadeT *facade) : descriptor_string("locate"), facade(facade) {}
-    const std::string GetDescriptor() const override final { return descriptor_string; }
-
-    int HandleRequest(const RouteParameters &route_parameters,
-                      osrm::json::Object &json_result) override final
-    {
-        // check number of parameters
-        if (route_parameters.coordinates.empty() ||
-            !route_parameters.coordinates.front().is_valid())
-        {
-            return 400;
-        }
-
-        FixedPointCoordinate result;
-        if (!facade->LocateClosestEndPointForCoordinate(route_parameters.coordinates.front(),
-                                                        result))
-        {
-            json_result.values["status"] = 207;
-        }
-        else
-        {
-            json_result.values["status"] = 0;
-            osrm::json::Array json_coordinate;
-            json_coordinate.values.push_back(result.lat / COORDINATE_PRECISION);
-            json_coordinate.values.push_back(result.lon / COORDINATE_PRECISION);
-            json_result.values["mapped_coordinate"] = json_coordinate;
-        }
-        return 200;
-    }
-
-  private:
-    std::string descriptor_string;
-    DataFacadeT *facade;
-};
-
-#endif /* LOCATE_HPP */

plugins/match.hpp

@@ -128,28 +128,25 @@ template <class DataFacadeT> class MapMatchingPlugin : public BasePlugin
                 }
             }
 
-            std::vector<std::pair<PhantomNode, double>> candidates;
             // Use bearing values if supplied, otherwise fallback to 0,180 defaults
             auto bearing = input_bearings.size() > 0 ? input_bearings[current_coordinate].first : 0;
             auto range = input_bearings.size() > 0 ? (input_bearings[current_coordinate].second ? *input_bearings[current_coordinate].second : 10 ) : 180;
-            facade->IncrementalFindPhantomNodeForCoordinateWithMaxDistance(
-                    input_coords[current_coordinate], candidates, query_radius,
-                    bearing, range);
+            auto candidates = facade->NearestPhantomNodesInRange(input_coords[current_coordinate], query_radius, bearing, range);
 
             // sort by foward id, then by reverse id and then by distance
             std::sort(candidates.begin(), candidates.end(),
-                [](const std::pair<PhantomNode, double>& lhs, const std::pair<PhantomNode, double>& rhs) {
-                    return lhs.first.forward_node_id < rhs.first.forward_node_id ||
-                            (lhs.first.forward_node_id == rhs.first.forward_node_id &&
-                             (lhs.first.reverse_node_id < rhs.first.reverse_node_id ||
-                              (lhs.first.reverse_node_id == rhs.first.reverse_node_id &&
-                               lhs.second < rhs.second)));
+                [](const std::pair<double, PhantomNode>& lhs, const std::pair<double, PhantomNode>& rhs) {
+                    return lhs.second.forward_node_id < rhs.second.forward_node_id ||
+                            (lhs.second.forward_node_id == rhs.second.forward_node_id &&
+                             (lhs.second.reverse_node_id < rhs.second.reverse_node_id ||
+                              (lhs.second.reverse_node_id == rhs.second.reverse_node_id &&
+                               lhs.first < rhs.first)));
                 });
 
             auto new_end = std::unique(candidates.begin(), candidates.end(),
-                [](const std::pair<PhantomNode, double>& lhs, const std::pair<PhantomNode, double>& rhs) {
-                    return lhs.first.forward_node_id == rhs.first.forward_node_id &&
-                           lhs.first.reverse_node_id == rhs.first.reverse_node_id;
+                [](const std::pair<double, PhantomNode>& lhs, const std::pair<double, PhantomNode>& rhs) {
+                    return lhs.second.forward_node_id == rhs.second.forward_node_id &&
+                           lhs.second.reverse_node_id == rhs.second.reverse_node_id;
                 });
             candidates.resize(new_end - candidates.begin());
 
@@ -159,22 +156,22 @@ template <class DataFacadeT> class MapMatchingPlugin : public BasePlugin
                 for (const auto i : osrm::irange<std::size_t>(0, compact_size))
                 {
                     // Split edge if it is bidirectional and append reverse direction to end of list
-                    if (candidates[i].first.forward_node_id != SPECIAL_NODEID &&
-                        candidates[i].first.reverse_node_id != SPECIAL_NODEID)
+                    if (candidates[i].second.forward_node_id != SPECIAL_NODEID &&
+                        candidates[i].second.reverse_node_id != SPECIAL_NODEID)
                     {
-                        PhantomNode reverse_node(candidates[i].first);
+                        PhantomNode reverse_node(candidates[i].second);
                         reverse_node.forward_node_id = SPECIAL_NODEID;
-                        candidates.push_back(std::make_pair(reverse_node, candidates[i].second));
+                        candidates.push_back(std::make_pair(candidates[i].first, reverse_node));
 
-                        candidates[i].first.reverse_node_id = SPECIAL_NODEID;
+                        candidates[i].second.reverse_node_id = SPECIAL_NODEID;
                     }
                 }
             }
 
             // sort by distance to make pruning effective
             std::sort(candidates.begin(), candidates.end(),
-                [](const std::pair<PhantomNode, double>& lhs, const std::pair<PhantomNode, double>& rhs) {
-                    return lhs.second < rhs.second;
+                [](const std::pair<double, PhantomNode>& lhs, const std::pair<double, PhantomNode>& rhs) {
+                    return lhs.first < rhs.first;
                 });
 
             candidates_lists.push_back(std::move(candidates));

plugins/nearest.hpp

@@ -67,14 +67,11 @@ template <class DataFacadeT> class NearestPlugin final : public BasePlugin
         }
 
         auto number_of_results = static_cast<std::size_t>(route_parameters.num_results);
-        std::vector<PhantomNode> phantom_node_vector;
         const int bearing = input_bearings.size() > 0 ? input_bearings.front().first : 0;
         const int range = input_bearings.size() > 0 ? (input_bearings.front().second?*input_bearings.front().second:10) : 180;
-        facade->IncrementalFindPhantomNodeForCoordinate(route_parameters.coordinates.front(),
-                                                        phantom_node_vector,
-                                                        static_cast<int>(number_of_results), bearing, range);
+        auto phantom_node_vector = facade->NearestPhantomNodes(route_parameters.coordinates.front(), number_of_results, bearing, range);
 
-        if (phantom_node_vector.empty() || !phantom_node_vector.front().is_valid())
+        if (phantom_node_vector.empty() || !phantom_node_vector.front().second.is_valid())
         {
             json_result.values["status"] = 207;
         }
@@ -91,15 +88,14 @@ template <class DataFacadeT> class NearestPlugin final : public BasePlugin
                 for (const auto i :
                      osrm::irange<std::size_t>(0, std::min(number_of_results, vector_length)))
                 {
+                    const auto& node = phantom_node_vector[i].second;
                     osrm::json::Array json_coordinate;
                     osrm::json::Object result;
-                    json_coordinate.values.push_back(phantom_node_vector.at(i).location.lat /
-                                                     COORDINATE_PRECISION);
-                    json_coordinate.values.push_back(phantom_node_vector.at(i).location.lon /
-                                                     COORDINATE_PRECISION);
+                    json_coordinate.values.push_back(node.location.lat / COORDINATE_PRECISION);
+                    json_coordinate.values.push_back(node.location.lon / COORDINATE_PRECISION);
                     result.values["mapped coordinate"] = json_coordinate;
                     result.values["name"] =
-                        facade->get_name_for_id(phantom_node_vector.at(i).name_id);
+                        facade->get_name_for_id(node.name_id);
                     results.values.push_back(result);
                 }
                 json_result.values["results"] = results;
@@ -107,13 +103,13 @@ template <class DataFacadeT> class NearestPlugin final : public BasePlugin
             else
             {
                 osrm::json::Array json_coordinate;
-                json_coordinate.values.push_back(phantom_node_vector.front().location.lat /
+                json_coordinate.values.push_back(phantom_node_vector.front().second.location.lat /
                                                  COORDINATE_PRECISION);
-                json_coordinate.values.push_back(phantom_node_vector.front().location.lon /
+                json_coordinate.values.push_back(phantom_node_vector.front().second.location.lon /
                                                  COORDINATE_PRECISION);
                 json_result.values["mapped_coordinate"] = json_coordinate;
                 json_result.values["name"] =
-                    facade->get_name_for_id(phantom_node_vector.front().name_id);
+                    facade->get_name_for_id(phantom_node_vector.front().second.name_id);
             }
         }
         return 200;

plugins/trip.hpp

@@ -73,7 +73,7 @@ template <class DataFacadeT> class RoundTripPlugin final : public BasePlugin
     const std::string GetDescriptor() const override final { return descriptor_string; }
 
     void GetPhantomNodes(const RouteParameters &route_parameters,
-                         PhantomNodeArray &phantom_node_vector)
+                         std::vector<PhantomNodePair> &phantom_node_pair_list)
     {
         const bool checksum_OK = (route_parameters.check_sum == facade->GetCheckSum());
         const auto &input_bearings = route_parameters.bearings;
@@ -89,19 +89,16 @@ template <class DataFacadeT> class RoundTripPlugin final : public BasePlugin
                 ObjectEncoder::DecodeFromBase64(route_parameters.hints[i], current_phantom_node);
                 if (current_phantom_node.is_valid(facade->GetNumberOfNodes()))
                 {
-                    phantom_node_vector[i].emplace_back(std::move(current_phantom_node));
+                    phantom_node_pair_list[i] = std::make_pair(current_phantom_node, current_phantom_node);
                     continue;
                 }
             }
             const int bearing = input_bearings.size() > 0 ? input_bearings[i].first : 0;
             const int range = input_bearings.size() > 0 ? (input_bearings[i].second?*input_bearings[i].second:10) : 180;
-            facade->IncrementalFindPhantomNodeForCoordinate(route_parameters.coordinates[i],
-                                                            phantom_node_vector[i], 1, bearing, range);
-            if (phantom_node_vector[i].size() > 1)
-            {
-                phantom_node_vector[i].erase(std::begin(phantom_node_vector[i]));
-            }
-            BOOST_ASSERT(phantom_node_vector[i].front().is_valid(facade->GetNumberOfNodes()));
+            auto phantom_nodes = facade->NearestPhantomNodes(route_parameters.coordinates[i], 1, bearing, range);
+            // FIXME we only use the pair because that is what DistanceTable expects
+            phantom_node_pair_list[i] = std::make_pair(phantom_nodes.front().second, phantom_nodes.front().second);
+            BOOST_ASSERT(phantom_node_pair_list[i].first.is_valid(facade->GetNumberOfNodes()));
         }
     }
 
@@ -206,7 +203,7 @@ template <class DataFacadeT> class RoundTripPlugin final : public BasePlugin
         json_result.values["permutation"] = json_permutation;
     }
 
-    InternalRouteResult ComputeRoute(const PhantomNodeArray &phantom_node_vector,
+    InternalRouteResult ComputeRoute(const std::vector<PhantomNodePair> &phantom_node_pair_list,
                                      const RouteParameters &route_parameters,
                                      const std::vector<NodeID> &trip)
     {
@@ -223,7 +220,7 @@ template <class DataFacadeT> class RoundTripPlugin final : public BasePlugin
             const auto to_node = std::next(it) != end ? *std::next(it) : *start;
 
             viapoint =
-                PhantomNodes{phantom_node_vector[from_node][0], phantom_node_vector[to_node][0]};
+                PhantomNodes{phantom_node_pair_list[from_node].first, phantom_node_pair_list[to_node].first};
             min_route.segment_end_coordinates.emplace_back(viapoint);
         }
         BOOST_ASSERT(min_route.segment_end_coordinates.size() == trip.size());
@@ -257,13 +254,13 @@ template <class DataFacadeT> class RoundTripPlugin final : public BasePlugin
         }
 
         // get phantom nodes
-        PhantomNodeArray phantom_node_vector(route_parameters.coordinates.size());
-        GetPhantomNodes(route_parameters, phantom_node_vector);
-        const auto number_of_locations = phantom_node_vector.size();
+        std::vector<PhantomNodePair> phantom_node_pair_list(route_parameters.coordinates.size());
+        GetPhantomNodes(route_parameters, phantom_node_pair_list);
+        const auto number_of_locations = phantom_node_pair_list.size();
 
         // compute the distance table of all phantom nodes
         const auto result_table = DistTableWrapper<EdgeWeight>(
-            *search_engine_ptr->distance_table(phantom_node_vector, phantom_node_vector), number_of_locations);
+            *search_engine_ptr->distance_table(phantom_node_pair_list, phantom_node_pair_list), number_of_locations);
 
         if (result_table.size() == 0)
         {
@@ -331,7 +328,7 @@ template <class DataFacadeT> class RoundTripPlugin final : public BasePlugin
         comp_route.reserve(route_result.size());
         for (auto &elem : route_result)
         {
-            comp_route.push_back(ComputeRoute(phantom_node_vector, route_parameters, elem));
+            comp_route.push_back(ComputeRoute(phantom_node_pair_list, route_parameters, elem));
         }
 
         TIMER_STOP(TRIP_TIMER);
@@ -360,8 +357,6 @@ template <class DataFacadeT> class RoundTripPlugin final : public BasePlugin
 
         json_result.values["trips"] = std::move(trip);
 
-
-
         return 200;
     }
 };

plugins/viaroute.hpp

@@ -87,7 +87,7 @@ template <class DataFacadeT> class ViaRoutePlugin final : public BasePlugin
             return 400;
         }
 
-        std::vector<phantom_node_pair> phantom_node_pair_list(route_parameters.coordinates.size());
+        std::vector<PhantomNodePair> phantom_node_pair_list(route_parameters.coordinates.size());
         const bool checksum_OK = (route_parameters.check_sum == facade->GetCheckSum());
 
         for (const auto i : osrm::irange<std::size_t>(0, route_parameters.coordinates.size()))
@@ -96,34 +96,26 @@ template <class DataFacadeT> class ViaRoutePlugin final : public BasePlugin
                 !route_parameters.hints[i].empty())
             {
                 ObjectEncoder::DecodeFromBase64(route_parameters.hints[i],
-                                                phantom_node_pair_list[i]);
+                                                phantom_node_pair_list[i].first);
                 if (phantom_node_pair_list[i].first.is_valid(facade->GetNumberOfNodes()))
                 {
                     continue;
                 }
             }
-            std::vector<PhantomNode> phantom_node_vector;
             const int bearing = input_bearings.size() > 0 ? input_bearings[i].first : 0;
             const int range = input_bearings.size() > 0 ? (input_bearings[i].second?*input_bearings[i].second:10) : 180;
-            if (facade->IncrementalFindPhantomNodeForCoordinate(route_parameters.coordinates[i],
-                                                                phantom_node_vector, 1, bearing, range))
-            {
-                BOOST_ASSERT(!phantom_node_vector.empty());
-                phantom_node_pair_list[i].first = phantom_node_vector.front();
-                if (phantom_node_vector.size() > 1)
-                {
-                    phantom_node_pair_list[i].second = phantom_node_vector.back();
-                }
-
-            }
-            else
+            phantom_node_pair_list[i] = facade->NearestPhantomNodeWithAlternativeFromBigComponent(route_parameters.coordinates[i], bearing, range);
+            // we didn't found a fitting node, return error
+            if (!phantom_node_pair_list[i].first.is_valid(facade->GetNumberOfNodes()))
             {
                 json_result.values["status_message"] = std::string("Could not find a matching segment for coordinate ") + std::to_string(i);
                 return 400;
             }
+            BOOST_ASSERT(phantom_node_pair_list[i].first.is_valid(facade->GetNumberOfNodes()));
+            BOOST_ASSERT(phantom_node_pair_list[i].second.is_valid(facade->GetNumberOfNodes()));
         }
 
-        const auto check_component_id_is_tiny = [](const phantom_node_pair &phantom_pair)
+        const auto check_component_id_is_tiny = [](const PhantomNodePair &phantom_pair)
         {
             return phantom_pair.first.component.is_tiny;
         };
@@ -133,18 +125,18 @@ template <class DataFacadeT> class ViaRoutePlugin final : public BasePlugin
                         check_component_id_is_tiny);
 
         // are all phantoms from a tiny cc?
-        const auto check_all_in_same_component = [](const std::vector<phantom_node_pair> &nodes)
+        const auto check_all_in_same_component = [](const std::vector<PhantomNodePair> &nodes)
         {
             const auto component_id = nodes.front().first.component.id;
 
             return std::all_of(std::begin(nodes), std::end(nodes),
-                               [component_id](const phantom_node_pair &phantom_pair)
+                               [component_id](const PhantomNodePair &phantom_pair)
                                {
                                    return component_id == phantom_pair.first.component.id;
                                });
         };
 
-        auto swap_phantom_from_big_cc_into_front = [](phantom_node_pair &phantom_pair)
+        auto swap_phantom_from_big_cc_into_front = [](PhantomNodePair &phantom_pair)
         {
             if (phantom_pair.first.component.is_tiny && phantom_pair.second.is_valid() && !phantom_pair.second.component.is_tiny)
             {
@@ -167,7 +159,7 @@ template <class DataFacadeT> class ViaRoutePlugin final : public BasePlugin
 
         InternalRouteResult raw_route;
         auto build_phantom_pairs =
-            [&raw_route](const phantom_node_pair &first_pair, const phantom_node_pair &second_pair)
+            [&raw_route](const PhantomNodePair &first_pair, const PhantomNodePair &second_pair)
         {
             raw_route.segment_end_coordinates.emplace_back(
                 PhantomNodes{first_pair.first, second_pair.first});

routing_algorithms/many_to_many.hpp

@@ -67,7 +67,7 @@ class ManyToManyRouting final
     ~ManyToManyRouting() {}
 
     std::shared_ptr<std::vector<EdgeWeight>>
-    operator()(const PhantomNodeArray &phantom_sources_array, const PhantomNodeArray &phantom_targets_array) const
+    operator()(const std::vector<PhantomNodePair> &phantom_sources_array, const std::vector<PhantomNodePair> &phantom_targets_array) const
     {
         const auto number_of_sources = phantom_sources_array.size();
         const auto number_of_targets = phantom_targets_array.size();
@@ -83,25 +83,35 @@ class ManyToManyRouting final
         SearchSpaceWithBuckets search_space_with_buckets;
 
         unsigned target_id = 0;
-        for (const std::vector<PhantomNode> &phantom_node_vector : phantom_targets_array)
+        for (const auto &pair : phantom_targets_array)
         {
             query_heap.Clear();
             // insert target(s) at distance 0
 
-            for (const PhantomNode &phantom_node : phantom_node_vector)
+            if (SPECIAL_NODEID != pair.first.forward_node_id)
             {
-                if (SPECIAL_NODEID != phantom_node.forward_node_id)
-                {
-                    query_heap.Insert(phantom_node.forward_node_id,
-                                      phantom_node.GetForwardWeightPlusOffset(),
-                                      phantom_node.forward_node_id);
+                query_heap.Insert(pair.first.forward_node_id,
+                                  pair.first.GetForwardWeightPlusOffset(),
+                                  pair.first.forward_node_id);
             }
-                if (SPECIAL_NODEID != phantom_node.reverse_node_id)
+            if (SPECIAL_NODEID != pair.first.reverse_node_id)
             {
-                    query_heap.Insert(phantom_node.reverse_node_id,
-                                      phantom_node.GetReverseWeightPlusOffset(),
-                                      phantom_node.reverse_node_id);
+                query_heap.Insert(pair.first.reverse_node_id,
+                                  pair.first.GetReverseWeightPlusOffset(),
+                                  pair.first.reverse_node_id);
             }
+
+            if (SPECIAL_NODEID != pair.second.forward_node_id)
+            {
+                query_heap.Insert(pair.second.forward_node_id,
+                                  pair.second.GetForwardWeightPlusOffset(),
+                                  pair.second.forward_node_id);
+            }
+            if (SPECIAL_NODEID != pair.second.reverse_node_id)
+            {
+                query_heap.Insert(pair.second.reverse_node_id,
+                                  pair.second.GetReverseWeightPlusOffset(),
+                                  pair.second.reverse_node_id);
             }
 
             // explore search space
@@ -114,24 +124,35 @@ class ManyToManyRouting final
 
         // for each source do forward search
         unsigned source_id = 0;
-        for (const std::vector<PhantomNode> &phantom_node_vector : phantom_sources_array)
+        for (const auto &pair : phantom_sources_array)
         {
             query_heap.Clear();
-            for (const PhantomNode &phantom_node : phantom_node_vector)
+            // insert target(s) at distance 0
+
+            if (SPECIAL_NODEID != pair.first.forward_node_id)
             {
-                // insert sources at distance 0
-                if (SPECIAL_NODEID != phantom_node.forward_node_id)
-                {
-                    query_heap.Insert(phantom_node.forward_node_id,
-                                      -phantom_node.GetForwardWeightPlusOffset(),
-                                      phantom_node.forward_node_id);
+                query_heap.Insert(pair.first.forward_node_id,
+                                  -pair.first.GetForwardWeightPlusOffset(),
+                                  pair.first.forward_node_id);
             }
-                if (SPECIAL_NODEID != phantom_node.reverse_node_id)
+            if (SPECIAL_NODEID != pair.first.reverse_node_id)
             {
-                    query_heap.Insert(phantom_node.reverse_node_id,
-                                      -phantom_node.GetReverseWeightPlusOffset(),
-                                      phantom_node.reverse_node_id);
+                query_heap.Insert(pair.first.reverse_node_id,
+                                  -pair.first.GetReverseWeightPlusOffset(),
+                                  pair.first.reverse_node_id);
             }
+
+            if (SPECIAL_NODEID != pair.second.forward_node_id)
+            {
+                query_heap.Insert(pair.second.forward_node_id,
+                                  -pair.second.GetForwardWeightPlusOffset(),
+                                  pair.second.forward_node_id);
+            }
+            if (SPECIAL_NODEID != pair.second.reverse_node_id)
+            {
+                query_heap.Insert(pair.second.reverse_node_id,
+                                  -pair.second.GetReverseWeightPlusOffset(),
+                                  pair.second.reverse_node_id);
             }
 
             // explore search space

routing_algorithms/map_matching.hpp

@@ -57,7 +57,7 @@ struct SubMatching
     double confidence;
 };
 
-using CandidateList = std::vector<std::pair<PhantomNode, double>>;
+using CandidateList = std::vector<std::pair<double, PhantomNode>>;
 using CandidateLists = std::vector<CandidateList>;
 using HMM = HiddenMarkovModel<CandidateLists>;
 using SubMatchingList = std::vector<SubMatching>;
@@ -232,7 +232,7 @@ class MapMatching final : public BasicRoutingInterface<DataFacadeT, MapMatching<
                     // how likely is candidate s_prime at time t to be emitted?
                     // FIXME this can be pre-computed
                     const double emission_pr =
-                        emission_log_probability(candidates_list[t][s_prime].second);
+                        emission_log_probability(candidates_list[t][s_prime].first);
                     double new_value = prev_viterbi[s] + emission_pr;
                     if (current_viterbi[s_prime] > new_value)
                     {
@@ -244,8 +244,8 @@ class MapMatching final : public BasicRoutingInterface<DataFacadeT, MapMatching<
 
                     // get distance diff between loc1/2 and locs/s_prime
                     const auto network_distance = super::get_network_distance(
-                        forward_heap, reverse_heap, prev_unbroken_timestamps_list[s].first,
-                        current_timestamps_list[s_prime].first);
+                        forward_heap, reverse_heap, prev_unbroken_timestamps_list[s].second,
+                        current_timestamps_list[s_prime].second);
 
                     const auto d_t = std::abs(network_distance - haversine_distance);
 
@@ -365,7 +365,7 @@ class MapMatching final : public BasicRoutingInterface<DataFacadeT, MapMatching<
                 const auto location_index = reconstructed_indices[i].second;
 
                 matching.indices[i] = timestamp_index;
-                matching.nodes[i] = candidates_list[timestamp_index][location_index].first;
+                matching.nodes[i] = candidates_list[timestamp_index][location_index].second;
                 matching.length += model.path_lengths[timestamp_index][location_index];
 
                 matching_debug.add_chosen(timestamp_index, location_index);

server/data_structures/datafacade_base.hpp

@@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <osrm/coordinate.hpp>
 
 #include <string>
+#include <boost/optional.hpp>
 
 using EdgeRange = osrm::range<EdgeID>;
 
@@ -92,23 +93,22 @@ template <class EdgeDataT> class BaseDataFacade
 
     virtual TravelMode GetTravelModeForEdgeID(const unsigned id) const = 0;
 
-    virtual bool LocateClosestEndPointForCoordinate(const FixedPointCoordinate &input_coordinate,
-                                                    FixedPointCoordinate &result,
-                                                    const unsigned zoom_level = 18) = 0;
+    virtual std::vector<std::pair<double, PhantomNode>>
+    NearestPhantomNodesInRange(const FixedPointCoordinate &input_coordinate,
+                               const float max_distance,
+                               const int bearing = 0,
+                               const int bearing_range = 180) = 0;
 
-    virtual bool
-    IncrementalFindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate,
-                                            std::vector<PhantomNode> &resulting_phantom_node_vector,
-                                            const unsigned number_of_results,
-                                            const int bearing = 0, const int bearing_range = 180) = 0;
+    virtual std::vector<std::pair<double, PhantomNode>>
+    NearestPhantomNodes(const FixedPointCoordinate &input_coordinate,
+                        const unsigned max_results,
+                        const int bearing = 0,
+                        const int bearing_range = 180) = 0;
 
-    virtual bool
-    IncrementalFindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate,
-                                            PhantomNode &resulting_phantom_node) = 0;
-    virtual bool IncrementalFindPhantomNodeForCoordinateWithMaxDistance(
-        const FixedPointCoordinate &input_coordinate,
-        std::vector<std::pair<PhantomNode, double>> &resulting_phantom_node_vector,
-        const double max_distance, const int bearing = 0, const int bearing_range = 180) = 0;
+    virtual std::pair<PhantomNode, PhantomNode>
+    NearestPhantomNodeWithAlternativeFromBigComponent(const FixedPointCoordinate &input_coordinate,
+                                                      const int bearing = 0,
+                                                      const int bearing_range = 180) = 0;
 
     virtual unsigned GetCheckSum() const = 0;
 

server/data_structures/internal_datafacade.hpp

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "datafacade_base.hpp"
 
+#include "../../algorithms/geospatial_query.hpp"
 #include "../../data_structures/original_edge_data.hpp"
 #include "../../data_structures/query_node.hpp"
 #include "../../data_structures/query_edge.hpp"
@@ -45,6 +46,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <osrm/coordinate.hpp>
 #include <osrm/server_paths.hpp>
 
+#include <boost/thread.hpp>
+
 #include <limits>
 
 template <class EdgeDataT> class InternalDataFacade final : public BaseDataFacade<EdgeDataT>
@@ -55,12 +58,14 @@ template <class EdgeDataT> class InternalDataFacade final : public BaseDataFacad
     using QueryGraph = StaticGraph<typename super::EdgeData>;
     using InputEdge = typename QueryGraph::InputEdge;
     using RTreeLeaf = typename super::RTreeLeaf;
+    using InternalRTree = StaticRTree<RTreeLeaf, ShM<FixedPointCoordinate, false>::vector, false>;
+    using InternalGeospatialQuery = GeospatialQuery<InternalRTree>;
 
     InternalDataFacade() {}
 
     unsigned m_check_sum;
     unsigned m_number_of_nodes;
-    QueryGraph *m_query_graph;
+    std::unique_ptr<QueryGraph> m_query_graph;
     std::string m_timestamp;
 
     std::shared_ptr<ShM<FixedPointCoordinate, false>::vector> m_coordinate_list;
@@ -74,8 +79,8 @@ template <class EdgeDataT> class InternalDataFacade final : public BaseDataFacad
     ShM<unsigned, false>::vector m_geometry_list;
     ShM<bool, false>::vector m_is_core_node;
 
-    boost::thread_specific_ptr<
-        StaticRTree<RTreeLeaf, ShM<FixedPointCoordinate, false>::vector, false>> m_static_rtree;
+    boost::thread_specific_ptr<InternalRTree> m_static_rtree;
+    boost::thread_specific_ptr<InternalGeospatialQuery> m_geospatial_query;
     boost::filesystem::path ram_index_path;
     boost::filesystem::path file_index_path;
     RangeTable<16, false> m_name_table;
@@ -116,7 +121,7 @@ template <class EdgeDataT> class InternalDataFacade final : public BaseDataFacad
         // BOOST_ASSERT_MSG(0 != edge_list.size(), "edge list empty");
         SimpleLogger().Write() << "loaded " << node_list.size() << " nodes and " << edge_list.size()
                                << " edges";
-        m_query_graph = new QueryGraph(node_list, edge_list);
+        m_query_graph = std::unique_ptr<QueryGraph>(new QueryGraph(node_list, edge_list));
 
         BOOST_ASSERT_MSG(0 == node_list.size(), "node list not flushed");
         BOOST_ASSERT_MSG(0 == edge_list.size(), "edge list not flushed");
@@ -226,8 +231,8 @@ template <class EdgeDataT> class InternalDataFacade final : public BaseDataFacad
     {
         BOOST_ASSERT_MSG(!m_coordinate_list->empty(), "coordinates must be loaded before r-tree");
 
-        m_static_rtree.reset(
-            new StaticRTree<RTreeLeaf>(ram_index_path, file_index_path, m_coordinate_list));
+        m_static_rtree.reset(new InternalRTree(ram_index_path, file_index_path, m_coordinate_list));
+        m_geospatial_query.reset(new InternalGeospatialQuery(*m_static_rtree, m_coordinate_list));
     }
 
     void LoadStreetNames(const boost::filesystem::path &names_file)
@@ -251,8 +256,8 @@ template <class EdgeDataT> class InternalDataFacade final : public BaseDataFacad
   public:
     virtual ~InternalDataFacade()
     {
-        delete m_query_graph;
         m_static_rtree.reset();
+        m_geospatial_query.reset();
     }
 
     explicit InternalDataFacade(const ServerPaths &server_paths)
@@ -283,9 +288,6 @@ template <class EdgeDataT> class InternalDataFacade final : public BaseDataFacad
         SimpleLogger().Write() << "loading geometries";
         LoadGeometries(file_for("geometries"));
 
-        SimpleLogger().Write() << "loading r-tree";
-        // XXX(daniel-j-h): it says this ^ but doesn't load the rtree here
-
         SimpleLogger().Write() << "loading timestamp";
         LoadTimestamp(file_for("timestamp"));
 
@@ -357,65 +359,52 @@ template <class EdgeDataT> class InternalDataFacade final : public BaseDataFacad
         return m_travel_mode_list.at(id);
     }
 
-    bool LocateClosestEndPointForCoordinate(const FixedPointCoordinate &input_coordinate,
-                                            FixedPointCoordinate &result,
-                                            const unsigned zoom_level = 18) override final
-    {
-        if (!m_static_rtree.get())
-        {
-            LoadRTree();
-        }
 
-        return m_static_rtree->LocateClosestEndPointForCoordinate(input_coordinate, result,
-                                                                  zoom_level);
-    }
-
-    bool IncrementalFindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate,
-                                                 PhantomNode &resulting_phantom_node) override final
-    {
-        std::vector<PhantomNode> resulting_phantom_node_vector;
-        auto result = IncrementalFindPhantomNodeForCoordinate(input_coordinate,
-                                                              resulting_phantom_node_vector, 1);
-        if (result)
-        {
-            BOOST_ASSERT(!resulting_phantom_node_vector.empty());
-            resulting_phantom_node = resulting_phantom_node_vector.front();
-        }
-
-        return result;
-    }
-
-    bool
-    IncrementalFindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate,
-                                            std::vector<PhantomNode> &resulting_phantom_node_vector,
-                                            const unsigned number_of_results,
-                                            const int bearing = 0, const int range = 180) override final
-    {
-        if (!m_static_rtree.get())
-        {
-            LoadRTree();
-        }
-
-        return m_static_rtree->IncrementalFindPhantomNodeForCoordinate(
-            input_coordinate, resulting_phantom_node_vector, number_of_results, bearing, range);
-    }
-
-    bool IncrementalFindPhantomNodeForCoordinateWithMaxDistance(
-        const FixedPointCoordinate &input_coordinate,
-        std::vector<std::pair<PhantomNode, double>> &resulting_phantom_node_vector,
-        const double max_distance,
+    std::vector<std::pair<double, PhantomNode>>
+    NearestPhantomNodesInRange(const FixedPointCoordinate &input_coordinate,
+                               const float max_distance,
                                const int bearing = 0,
                                const int bearing_range = 180) override final
     {
         if (!m_static_rtree.get())
         {
             LoadRTree();
+            BOOST_ASSERT(m_geospatial_query.get());
         }
 
-        return m_static_rtree->IncrementalFindPhantomNodeForCoordinateWithDistance(
-            input_coordinate, resulting_phantom_node_vector, max_distance, bearing, bearing_range);
+        return m_geospatial_query->NearestPhantomNodesInRange(input_coordinate, max_distance, bearing, bearing_range);
     }
 
+    std::vector<std::pair<double, PhantomNode>>
+    NearestPhantomNodes(const FixedPointCoordinate &input_coordinate,
+                        const unsigned max_results,
+                        const int bearing = 0,
+                        const int bearing_range = 180) override final
+    {
+        if (!m_static_rtree.get())
+        {
+            LoadRTree();
+            BOOST_ASSERT(m_geospatial_query.get());
+        }
+
+        return m_geospatial_query->NearestPhantomNodes(input_coordinate, max_results, bearing, bearing_range);
+    }
+
+    std::pair<PhantomNode, PhantomNode>
+    NearestPhantomNodeWithAlternativeFromBigComponent(const FixedPointCoordinate &input_coordinate,
+                                                      const int bearing = 0,
+                                                      const int bearing_range = 180)
+    {
+        if (!m_static_rtree.get())
+        {
+            LoadRTree();
+            BOOST_ASSERT(m_geospatial_query.get());
+        }
+
+        return m_geospatial_query->NearestPhantomNodeWithAlternativeFromBigComponent(input_coordinate, bearing, bearing_range);
+    }
+
+
     unsigned GetCheckSum() const override final { return m_check_sum; }
 
     unsigned GetNameIndexFromEdgeID(const unsigned id) const override final

server/data_structures/shared_datafacade.hpp

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "datafacade_base.hpp"
 #include "shared_datatype.hpp"
 
+#include "../../algorithms/geospatial_query.hpp"
 #include "../../data_structures/range_table.hpp"
 #include "../../data_structures/static_graph.hpp"
 #include "../../data_structures/static_rtree.hpp"
@@ -56,6 +57,7 @@ template <class EdgeDataT> class SharedDataFacade final : public BaseDataFacade<
     using InputEdge = typename QueryGraph::InputEdge;
     using RTreeLeaf = typename super::RTreeLeaf;
     using SharedRTree = StaticRTree<RTreeLeaf, ShM<FixedPointCoordinate, true>::vector, true>;
+    using SharedGeospatialQuery = GeospatialQuery<SharedRTree>;
     using TimeStampedRTreePair = std::pair<unsigned, std::shared_ptr<SharedRTree>>;
     using RTreeNode = typename SharedRTree::TreeNode;
 
@@ -86,6 +88,7 @@ template <class EdgeDataT> class SharedDataFacade final : public BaseDataFacade<
     ShM<bool, true>::vector m_is_core_node;
 
     boost::thread_specific_ptr<std::pair<unsigned, std::shared_ptr<SharedRTree>>> m_static_rtree;
+    boost::thread_specific_ptr<SharedGeospatialQuery> m_geospatial_query;
     boost::filesystem::path file_index_path;
 
     std::shared_ptr<RangeTable<16, true>> m_name_table;
@@ -118,6 +121,7 @@ template <class EdgeDataT> class SharedDataFacade final : public BaseDataFacade<
             osrm::make_unique<SharedRTree>(
                 tree_ptr, data_layout->num_entries[SharedDataLayout::R_SEARCH_TREE],
                 file_index_path, m_coordinate_list)));
+        m_geospatial_query.reset(new SharedGeospatialQuery(*m_static_rtree->second, m_coordinate_list));
     }
 
     void LoadGraph()
@@ -378,63 +382,48 @@ template <class EdgeDataT> class SharedDataFacade final : public BaseDataFacade<
         return m_travel_mode_list.at(id);
     }
 
-    bool LocateClosestEndPointForCoordinate(const FixedPointCoordinate &input_coordinate,
-                                            FixedPointCoordinate &result,
-                                            const unsigned zoom_level = 18) override final
-    {
-        if (!m_static_rtree.get() || CURRENT_TIMESTAMP != m_static_rtree->first)
-        {
-            LoadRTree();
-        }
-
-        return m_static_rtree->second->LocateClosestEndPointForCoordinate(input_coordinate, result,
-                                                                          zoom_level);
-    }
-
-    bool IncrementalFindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate,
-                                                 PhantomNode &resulting_phantom_node) override final
-    {
-        std::vector<PhantomNode> resulting_phantom_node_vector;
-        auto result = IncrementalFindPhantomNodeForCoordinate(input_coordinate,
-                                                              resulting_phantom_node_vector, 1);
-        if (result)
-        {
-            BOOST_ASSERT(!resulting_phantom_node_vector.empty());
-            resulting_phantom_node = resulting_phantom_node_vector.front();
-        }
-
-        return result;
-    }
-
-    bool
-    IncrementalFindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate,
-                                            std::vector<PhantomNode> &resulting_phantom_node_vector,
-                                            const unsigned number_of_results,
-                                            const int bearing = 0, const int range = 180) override final
-    {
-        if (!m_static_rtree.get() || CURRENT_TIMESTAMP != m_static_rtree->first)
-        {
-            LoadRTree();
-        }
-
-        return m_static_rtree->second->IncrementalFindPhantomNodeForCoordinate(
-            input_coordinate, resulting_phantom_node_vector, number_of_results, bearing, range);
-    }
-
-    bool IncrementalFindPhantomNodeForCoordinateWithMaxDistance(
-        const FixedPointCoordinate &input_coordinate,
-        std::vector<std::pair<PhantomNode, double>> &resulting_phantom_node_vector,
-        const double max_distance,
+    std::vector<std::pair<double, PhantomNode>>
+    NearestPhantomNodesInRange(const FixedPointCoordinate &input_coordinate,
+                               const float max_distance,
                                const int bearing = 0,
                                const int bearing_range = 180) override final
     {
         if (!m_static_rtree.get() || CURRENT_TIMESTAMP != m_static_rtree->first)
         {
             LoadRTree();
+            BOOST_ASSERT(m_geospatial_query.get());
         }
 
-        return m_static_rtree->second->IncrementalFindPhantomNodeForCoordinateWithDistance(
-            input_coordinate, resulting_phantom_node_vector, max_distance, bearing, bearing_range);
+        return m_geospatial_query->NearestPhantomNodesInRange(input_coordinate, max_distance, bearing, bearing_range);
+    }
+
+    std::vector<std::pair<double, PhantomNode>>
+    NearestPhantomNodes(const FixedPointCoordinate &input_coordinate,
+                        const unsigned max_results,
+                        const int bearing = 0,
+                        const int bearing_range = 180) override final
+    {
+        if (!m_static_rtree.get() || CURRENT_TIMESTAMP != m_static_rtree->first)
+        {
+            LoadRTree();
+            BOOST_ASSERT(m_geospatial_query.get());
+        }
+
+        return m_geospatial_query->NearestPhantomNodes(input_coordinate, max_results, bearing, bearing_range);
+    }
+
+    std::pair<PhantomNode, PhantomNode>
+    NearestPhantomNodeWithAlternativeFromBigComponent(const FixedPointCoordinate &input_coordinate,
+                                                      const int bearing = 0,
+                                                      const int bearing_range = 180)
+    {
+        if (!m_static_rtree.get() || CURRENT_TIMESTAMP != m_static_rtree->first)
+        {
+            LoadRTree();
+            BOOST_ASSERT(m_geospatial_query.get());
+        }
+
+        return m_geospatial_query->NearestPhantomNodeWithAlternativeFromBigComponent(input_coordinate, bearing, bearing_range);
     }
 
     unsigned GetCheckSum() const override final { return m_check_sum; }

unit_tests/data_structures/static_rtree.cpp

@@ -26,6 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include "../../algorithms/coordinate_calculation.hpp"
+#include "../../algorithms/geospatial_query.hpp"
 #include "../../data_structures/static_rtree.hpp"
 #include "../../data_structures/query_node.hpp"
 #include "../../data_structures/edge_based_node.hpp"
@@ -55,11 +56,12 @@ constexpr uint32_t TEST_BRANCHING_FACTOR = 8;
 constexpr uint32_t TEST_LEAF_NODE_SIZE = 64;
 
 typedef EdgeBasedNode TestData;
-typedef StaticRTree<TestData,
+using TestStaticRTree = StaticRTree<TestData,
                                     std::vector<FixedPointCoordinate>,
                                     false,
                                     TEST_BRANCHING_FACTOR,
-                    TEST_LEAF_NODE_SIZE> TestStaticRTree;
+                                    TEST_LEAF_NODE_SIZE>;
+using MiniStaticRTree = StaticRTree<TestData, std::vector<FixedPointCoordinate>, false, 2, 3>;
 
 // Choosen by a fair W20 dice roll (this value is completely arbitrary)
 constexpr unsigned RANDOM_SEED = 42;
@@ -68,113 +70,35 @@ static const int32_t WORLD_MAX_LAT = 90 * COORDINATE_PRECISION;
 static const int32_t WORLD_MIN_LON = -180 * COORDINATE_PRECISION;
 static const int32_t WORLD_MAX_LON = 180 * COORDINATE_PRECISION;
 
-class LinearSearchNN
+template <typename DataT> class LinearSearchNN
 {
   public:
     LinearSearchNN(const std::shared_ptr<std::vector<FixedPointCoordinate>> &coords,
-                   const std::vector<TestData> &edges)
+                   const std::vector<DataT> &edges)
         : coords(coords), edges(edges)
     {
     }
 
-    bool LocateClosestEndPointForCoordinate(const FixedPointCoordinate &input_coordinate,
-                                            FixedPointCoordinate &result_coordinate)
+    std::vector<DataT> Nearest(const FixedPointCoordinate &input_coordinate,
+                               const unsigned num_results)
     {
-        float min_dist = std::numeric_limits<float>::max();
-        FixedPointCoordinate min_coord;
-        for (const TestData &e : edges)
-        {
-            const FixedPointCoordinate &start = coords->at(e.u);
-            const FixedPointCoordinate &end = coords->at(e.v);
-            float distance = coordinate_calculation::great_circle_distance(
-                input_coordinate.lat, input_coordinate.lon, start.lat, start.lon);
-            if (distance < min_dist)
-            {
-                min_coord = start;
-                min_dist = distance;
-            }
+        std::vector<DataT> local_edges(edges);
 
-            distance = coordinate_calculation::great_circle_distance(
-                input_coordinate.lat, input_coordinate.lon, end.lat, end.lon);
-            if (distance < min_dist)
+        std::nth_element(
+            local_edges.begin(), local_edges.begin() + num_results, local_edges.end(),
+            [this, &input_coordinate](const DataT &lhs, const DataT &rhs)
             {
-                min_coord = end;
-                min_dist = distance;
-            }
-        }
-
-        result_coordinate = min_coord;
-        return result_coordinate.is_valid();
-    }
-
-    bool FindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate,
-                                      PhantomNode &result_phantom_node,
-                                      const unsigned)
-    {
-        float min_dist = std::numeric_limits<float>::max();
-        TestData nearest_edge;
-        for (const TestData &e : edges)
-        {
-            if (e.component.is_tiny)
-                continue;
-
                 float current_ratio = 0.;
                 FixedPointCoordinate nearest;
-            const float current_perpendicular_distance =
-                coordinate_calculation::perpendicular_distance(
-                    coords->at(e.u), coords->at(e.v), input_coordinate, nearest, current_ratio);
+                const float lhs_dist = coordinate_calculation::perpendicular_distance(
+                    coords->at(lhs.u), coords->at(lhs.v), input_coordinate, nearest, current_ratio);
+                const float rhs_dist = coordinate_calculation::perpendicular_distance(
+                    coords->at(rhs.u), coords->at(rhs.v), input_coordinate, nearest, current_ratio);
+                return lhs_dist < rhs_dist;
+            });
+        local_edges.resize(num_results);
 
-            if ((current_perpendicular_distance < min_dist) &&
-                !osrm::epsilon_compare(current_perpendicular_distance, min_dist))
-            { // found a new minimum
-                min_dist = current_perpendicular_distance;
-                result_phantom_node = {e.forward_edge_based_node_id,
-                                       e.reverse_edge_based_node_id,
-                                       e.name_id,
-                                       e.forward_weight,
-                                       e.reverse_weight,
-                                       e.forward_offset,
-                                       e.reverse_offset,
-                                       e.packed_geometry_id,
-                                       e.component.is_tiny,
-                                       e.component.id,
-                                       nearest,
-                                       e.fwd_segment_position,
-                                       e.forward_travel_mode,
-                                       e.backward_travel_mode};
-                nearest_edge = e;
-            }
-        }
-
-        if (result_phantom_node.location.is_valid())
-        {
-            // Hack to fix rounding errors and wandering via nodes.
-            if (1 == std::abs(input_coordinate.lon - result_phantom_node.location.lon))
-            {
-                result_phantom_node.location.lon = input_coordinate.lon;
-            }
-            if (1 == std::abs(input_coordinate.lat - result_phantom_node.location.lat))
-            {
-                result_phantom_node.location.lat = input_coordinate.lat;
-            }
-
-            const float distance_1 = coordinate_calculation::great_circle_distance(
-                coords->at(nearest_edge.u), result_phantom_node.location);
-            const float distance_2 = coordinate_calculation::great_circle_distance(
-                coords->at(nearest_edge.u), coords->at(nearest_edge.v));
-            const float ratio = std::min(1.f, distance_1 / distance_2);
-
-            if (SPECIAL_NODEID != result_phantom_node.forward_node_id)
-            {
-                result_phantom_node.forward_weight *= ratio;
-            }
-            if (SPECIAL_NODEID != result_phantom_node.reverse_node_id)
-            {
-                result_phantom_node.reverse_weight *= (1. - ratio);
-            }
-        }
-
-        return result_phantom_node.location.is_valid();
+        return local_edges;
     }
 
   private:
@@ -295,23 +219,18 @@ void simple_verify_rtree(RTreeT &rtree,
     BOOST_TEST_MESSAGE("Verify end points");
     for (const auto &e : edges)
     {
-        FixedPointCoordinate result_u, result_v;
         const FixedPointCoordinate &pu = coords->at(e.u);
         const FixedPointCoordinate &pv = coords->at(e.v);
-        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 =
-            coordinate_calculation::great_circle_distance(result_u.lat, result_u.lon, pu.lat, pu.lon);
-        BOOST_CHECK_LE(dist_u, std::numeric_limits<float>::epsilon());
-        float dist_v =
-            coordinate_calculation::great_circle_distance(result_v.lat, result_v.lon, pv.lat, pv.lon);
-        BOOST_CHECK_LE(dist_v, std::numeric_limits<float>::epsilon());
+        auto result_u = rtree.Nearest(pu, 1);
+        auto result_v = rtree.Nearest(pv, 1);
+        BOOST_CHECK(result_u.size() == 1 && result_v.size() == 1);
+        BOOST_CHECK(result_u.front().u == e.u || result_u.front().v == e.u);
+        BOOST_CHECK(result_v.front().u == e.v || result_v.front().v == e.v);
     }
 }
 
 template <typename RTreeT>
-void sampling_verify_rtree(RTreeT &rtree, LinearSearchNN &lsnn, unsigned num_samples)
+void sampling_verify_rtree(RTreeT &rtree, LinearSearchNN<TestData> &lsnn, const std::vector<FixedPointCoordinate>& coords, unsigned num_samples)
 {
     std::mt19937 g(RANDOM_SEED);
     std::uniform_int_distribution<> lat_udist(WORLD_MIN_LAT, WORLD_MAX_LAT);
@@ -325,17 +244,22 @@ void sampling_verify_rtree(RTreeT &rtree, LinearSearchNN &lsnn, unsigned num_sam
     BOOST_TEST_MESSAGE("Sampling queries");
     for (const auto &q : queries)
     {
-        FixedPointCoordinate result_rtree;
-        rtree.LocateClosestEndPointForCoordinate(q, result_rtree, 1);
-        FixedPointCoordinate result_ln;
-        lsnn.LocateClosestEndPointForCoordinate(q, result_ln);
-        BOOST_CHECK_EQUAL(result_ln, result_rtree);
+        auto result_rtree = rtree.Nearest(q, 1);
+        auto result_lsnn = lsnn.Nearest(q, 1);
+        BOOST_CHECK(result_rtree.size() == 1);
+        BOOST_CHECK(result_lsnn.size() == 1);
+        auto rtree_u = result_rtree.back().u;
+        auto rtree_v = result_rtree.back().v;
+        auto lsnn_u = result_lsnn.back().u;
+        auto lsnn_v = result_lsnn.back().v;
 
-        PhantomNode phantom_rtree;
-        rtree.FindPhantomNodeForCoordinate(q, phantom_rtree, 1);
-        PhantomNode phantom_ln;
-        lsnn.FindPhantomNodeForCoordinate(q, phantom_ln, 1);
-        BOOST_CHECK_EQUAL(phantom_rtree, phantom_ln);
+        float current_ratio = 0.;
+        FixedPointCoordinate nearest;
+        const float rtree_dist = coordinate_calculation::perpendicular_distance(
+            coords[rtree_u], coords[rtree_v], q, nearest, current_ratio);
+        const float lsnn_dist = coordinate_calculation::perpendicular_distance(
+            coords[lsnn_u], coords[lsnn_v], q, nearest, current_ratio);
+        BOOST_CHECK_LE(std::abs(rtree_dist - lsnn_dist), std::numeric_limits<float>::epsilon());
     }
 }
 
@@ -365,10 +289,10 @@ void construction_test(const std::string &prefix, FixtureT *fixture)
     std::string nodes_path;
     build_rtree<FixtureT, RTreeT>(prefix, fixture, leaves_path, nodes_path);
     RTreeT rtree(nodes_path, leaves_path, fixture->coords);
-    LinearSearchNN lsnn(fixture->coords, fixture->edges);
+    LinearSearchNN<TestData> lsnn(fixture->coords, fixture->edges);
 
     simple_verify_rtree(rtree, fixture->coords, fixture->edges);
-    sampling_verify_rtree(rtree, lsnn, 100);
+    sampling_verify_rtree(rtree, lsnn, *fixture->coords, 100);
 }
 
 BOOST_FIXTURE_TEST_CASE(construct_half_leaf_test, TestRandomGraphFixture_LeafHalfFull)
@@ -396,51 +320,43 @@ BOOST_FIXTURE_TEST_CASE(construct_multiple_levels_test, TestRandomGraphFixture_M
     construction_test("test_5", this);
 }
 
-/*
- * Bug: If you querry a point that lies between two BBs that have a gap,
- * one BB will be pruned, even if it could contain a nearer match.
- */
+// Bug: If you querry a point that lies between two BBs that have a gap,
+// one BB will be pruned, even if it could contain a nearer match.
 BOOST_AUTO_TEST_CASE(regression_test)
 {
-    typedef std::pair<float, float> Coord;
-    typedef std::pair<unsigned, unsigned> Edge;
+    using Coord = std::pair<float, float>;
+    using Edge = std::pair<unsigned, unsigned>;
     GraphFixture fixture(
         {
-         Coord(40.0, 0.0),
-         Coord(35.0, 5.0),
+            Coord(40.0, 0.0), Coord(35.0, 5.0),
 
-         Coord(5.0, 5.0),
-         Coord(0.0, 10.0),
+            Coord(5.0, 5.0), Coord(0.0, 10.0),
 
-         Coord(20.0, 10.0),
-         Coord(20.0, 5.0),
+            Coord(20.0, 10.0), Coord(20.0, 5.0),
 
-         Coord(40.0, 100.0),
-         Coord(35.0, 105.0),
+            Coord(40.0, 100.0), Coord(35.0, 105.0),
 
-         Coord(5.0, 105.0),
-         Coord(0.0, 110.0),
+            Coord(5.0, 105.0), Coord(0.0, 110.0),
         },
         {Edge(0, 1), Edge(2, 3), Edge(4, 5), Edge(6, 7), Edge(8, 9)});
 
-    typedef StaticRTree<TestData, std::vector<FixedPointCoordinate>, false, 2, 3> MiniStaticRTree;
-
     std::string leaves_path;
     std::string nodes_path;
     build_rtree<GraphFixture, MiniStaticRTree>("test_regression", &fixture, leaves_path,
                                                nodes_path);
     MiniStaticRTree rtree(nodes_path, leaves_path, fixture.coords);
+    LinearSearchNN<TestData> lsnn(fixture.coords, fixture.edges);
 
     // query a node just right of the center of the gap
     FixedPointCoordinate input(20.0 * COORDINATE_PRECISION, 55.1 * COORDINATE_PRECISION);
-    FixedPointCoordinate result;
-    rtree.LocateClosestEndPointForCoordinate(input, result, 1);
-    FixedPointCoordinate result_ln;
-    LinearSearchNN lsnn(fixture.coords, fixture.edges);
-    lsnn.LocateClosestEndPointForCoordinate(input, result_ln);
+    auto result_rtree = rtree.Nearest(input, 1);
+    auto result_ls = lsnn.Nearest(input, 1);
 
-    // TODO: reactivate
-    // BOOST_CHECK_EQUAL(result_ln, result);
+    BOOST_CHECK(result_rtree.size() == 1);
+    BOOST_CHECK(result_ls.size() == 1);
+
+    BOOST_CHECK_EQUAL(result_ls.front().u, result_rtree.front().u);
+    BOOST_CHECK_EQUAL(result_ls.front().v, result_rtree.front().v);
 }
 
 void TestRectangle(double width, double height, double center_lat, double center_lon)
@@ -448,7 +364,7 @@ void TestRectangle(double width, double height, double center_lat, double center
     FixedPointCoordinate center(center_lat * COORDINATE_PRECISION,
                                 center_lon * COORDINATE_PRECISION);
 
-    TestStaticRTree::RectangleT rect;
+    TestStaticRTree::Rectangle rect;
     rect.min_lat = center.lat - height / 2.0 * COORDINATE_PRECISION;
     rect.max_lat = center.lat + height / 2.0 * COORDINATE_PRECISION;
     rect.min_lon = center.lon - width / 2.0 * COORDINATE_PRECISION;
@@ -502,100 +418,63 @@ BOOST_AUTO_TEST_CASE(rectangle_test)
     TestRectangle(10, 10, 0, 0);
 }
 
-/* Verify that the bearing-bounds checking function behaves as expected */
-BOOST_AUTO_TEST_CASE(bearing_range_test)
-{
-    // Simple, non-edge-case checks
-    BOOST_CHECK_EQUAL(true, TestStaticRTree::IsBearingWithinBounds(45, 45, 10));
-    BOOST_CHECK_EQUAL(true, TestStaticRTree::IsBearingWithinBounds(35, 45, 10));
-    BOOST_CHECK_EQUAL(true, TestStaticRTree::IsBearingWithinBounds(55, 45, 10));
-
-    BOOST_CHECK_EQUAL(false, TestStaticRTree::IsBearingWithinBounds(34, 45, 10));
-    BOOST_CHECK_EQUAL(false, TestStaticRTree::IsBearingWithinBounds(56, 45, 10));
-
-    BOOST_CHECK_EQUAL(false, TestStaticRTree::IsBearingWithinBounds(34, 45, 10));
-    BOOST_CHECK_EQUAL(false, TestStaticRTree::IsBearingWithinBounds(56, 45, 10));
-
-    // When angle+limit goes > 360
-    BOOST_CHECK_EQUAL(true, TestStaticRTree::IsBearingWithinBounds(359, 355, 10));
-
-    // When angle-limit goes < 0
-    BOOST_CHECK_EQUAL(true, TestStaticRTree::IsBearingWithinBounds(359, 5, 10));
-    BOOST_CHECK_EQUAL(false, TestStaticRTree::IsBearingWithinBounds(354, 5, 10));
-    BOOST_CHECK_EQUAL(false, TestStaticRTree::IsBearingWithinBounds(16, 5, 10));
-
-
-    // Checking other cases of wraparound
-    BOOST_CHECK_EQUAL(true, TestStaticRTree::IsBearingWithinBounds(359, -5, 10));
-    BOOST_CHECK_EQUAL(false, TestStaticRTree::IsBearingWithinBounds(344, -5, 10));
-    BOOST_CHECK_EQUAL(false, TestStaticRTree::IsBearingWithinBounds(6, -5, 10));
-
-    BOOST_CHECK_EQUAL(true, TestStaticRTree::IsBearingWithinBounds(-1, 5, 10));
-    BOOST_CHECK_EQUAL(false, TestStaticRTree::IsBearingWithinBounds(-6, 5, 10));
-
-    BOOST_CHECK_EQUAL(true, TestStaticRTree::IsBearingWithinBounds(-721, 5, 10));
-    BOOST_CHECK_EQUAL(true, TestStaticRTree::IsBearingWithinBounds(719, 5, 10));
-
-}
-
 BOOST_AUTO_TEST_CASE(bearing_tests)
 {
-
-    typedef std::pair<float, float> Coord;
-    typedef std::pair<unsigned, unsigned> Edge;
+    using Coord = std::pair<float, float>;
+    using Edge = std::pair<unsigned, unsigned>;
     GraphFixture fixture(
         {
-         Coord(0.0, 0.0),
-         Coord(10.0, 10.0),
+            Coord(0.0, 0.0), Coord(10.0, 10.0),
         },
         {Edge(0, 1), Edge(1, 0)});
 
-    typedef StaticRTree<TestData, std::vector<FixedPointCoordinate>, false, 2, 3> MiniStaticRTree;
-
     std::string leaves_path;
     std::string nodes_path;
     build_rtree<GraphFixture, MiniStaticRTree>("test_bearing", &fixture, leaves_path, nodes_path);
     MiniStaticRTree rtree(nodes_path, leaves_path, fixture.coords);
+    GeospatialQuery<MiniStaticRTree> query(rtree, fixture.coords);
 
     FixedPointCoordinate input(5.0 * COORDINATE_PRECISION, 5.1 * COORDINATE_PRECISION);
-    std::vector<PhantomNode> results;
 
-    rtree.IncrementalFindPhantomNodeForCoordinate( input, results, 5);
+    {
+        auto results = query.NearestPhantomNodes(input, 5);
         BOOST_CHECK_EQUAL(results.size(), 2);
-    BOOST_CHECK_EQUAL(results.back().forward_node_id, 0);
-    BOOST_CHECK_EQUAL(results.back().reverse_node_id, 1);
-
-    results.clear();
-    rtree.IncrementalFindPhantomNodeForCoordinate( input, results, 5, 270, 10);
-    BOOST_CHECK_EQUAL(results.size(), 0);
-
-    results.clear();
-    rtree.IncrementalFindPhantomNodeForCoordinate( input, results, 5, 45, 10);
-    BOOST_CHECK_EQUAL(results.size(), 2);
-    BOOST_CHECK_EQUAL(results[0].forward_node_id, 1);
-    BOOST_CHECK_EQUAL(results[0].reverse_node_id, SPECIAL_NODEID);
-    BOOST_CHECK_EQUAL(results[1].forward_node_id, SPECIAL_NODEID);
-    BOOST_CHECK_EQUAL(results[1].reverse_node_id, 1);
-
-
-    std::vector<std::pair<PhantomNode, double>> distance_results;
-    rtree.IncrementalFindPhantomNodeForCoordinateWithDistance( input, distance_results, 11000 );
-    BOOST_CHECK_EQUAL(distance_results.size(), 2);
-
-    distance_results.clear();
-    rtree.IncrementalFindPhantomNodeForCoordinateWithDistance( input, distance_results, 11000, 270, 10 );
-    BOOST_CHECK_EQUAL(distance_results.size(), 0);
-
-    distance_results.clear();
-    rtree.IncrementalFindPhantomNodeForCoordinateWithDistance( input, distance_results, 11000, 45, 10 );
-    BOOST_CHECK_EQUAL(distance_results.size(), 2);
-    BOOST_CHECK_EQUAL(distance_results[0].first.forward_node_id, 1);
-    BOOST_CHECK_EQUAL(distance_results[0].first.reverse_node_id, SPECIAL_NODEID);
-    BOOST_CHECK_EQUAL(distance_results[1].first.forward_node_id, SPECIAL_NODEID);
-    BOOST_CHECK_EQUAL(distance_results[1].first.reverse_node_id, 1);
-
+        BOOST_CHECK_EQUAL(results.back().second.forward_node_id, 0);
+        BOOST_CHECK_EQUAL(results.back().second.reverse_node_id, 1);
     }
 
+    {
+        auto results = query.NearestPhantomNodes(input, 5, 270, 10);
+        BOOST_CHECK_EQUAL(results.size(), 0);
+    }
 
+    {
+        auto results = query.NearestPhantomNodes(input, 5, 45, 10);
+        BOOST_CHECK_EQUAL(results.size(), 2);
+        BOOST_CHECK_EQUAL(results[0].second.forward_node_id, 1);
+        BOOST_CHECK_EQUAL(results[0].second.reverse_node_id, SPECIAL_NODEID);
+        BOOST_CHECK_EQUAL(results[1].second.forward_node_id, SPECIAL_NODEID);
+        BOOST_CHECK_EQUAL(results[1].second.reverse_node_id, 1);
+    }
+
+    {
+        auto results = query.NearestPhantomNodesInRange(input, 11000);
+        BOOST_CHECK_EQUAL(results.size(), 2);
+    }
+
+    {
+        auto results = query.NearestPhantomNodesInRange(input, 11000, 270, 10);
+        BOOST_CHECK_EQUAL(results.size(), 0);
+    }
+
+    {
+        auto results = query.NearestPhantomNodesInRange(input, 11000, 45, 10);
+        BOOST_CHECK_EQUAL(results.size(), 2);
+        BOOST_CHECK_EQUAL(results[0].second.forward_node_id, 1);
+        BOOST_CHECK_EQUAL(results[0].second.reverse_node_id, SPECIAL_NODEID);
+        BOOST_CHECK_EQUAL(results[1].second.forward_node_id, SPECIAL_NODEID);
+        BOOST_CHECK_EQUAL(results[1].second.reverse_node_id, 1);
+    }
+}
 
 BOOST_AUTO_TEST_SUITE_END()

unit_tests/util/bearing.cpp

@@ -0,0 +1,72 @@
+/*
+
+Copyright (c) 2015, Project OSRM contributors
+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 "../../util/bearing.hpp"
+#include "../../typedefs.h"
+
+#include <boost/functional/hash.hpp>
+#include <boost/test/unit_test.hpp>
+#include <boost/test/test_case_template.hpp>
+
+BOOST_AUTO_TEST_SUITE(bearing)
+
+// Verify that the bearing-bounds checking function behaves as expected
+BOOST_AUTO_TEST_CASE(bearing_range_test)
+{
+    // Simple, non-edge-case checks
+    BOOST_CHECK_EQUAL(true, bearing::CheckInBounds(45, 45, 10));
+    BOOST_CHECK_EQUAL(true, bearing::CheckInBounds(35, 45, 10));
+    BOOST_CHECK_EQUAL(true, bearing::CheckInBounds(55, 45, 10));
+
+    BOOST_CHECK_EQUAL(false, bearing::CheckInBounds(34, 45, 10));
+    BOOST_CHECK_EQUAL(false, bearing::CheckInBounds(56, 45, 10));
+
+    BOOST_CHECK_EQUAL(false, bearing::CheckInBounds(34, 45, 10));
+    BOOST_CHECK_EQUAL(false, bearing::CheckInBounds(56, 45, 10));
+
+    // When angle+limit goes > 360
+    BOOST_CHECK_EQUAL(true, bearing::CheckInBounds(359, 355, 10));
+
+    // When angle-limit goes < 0
+    BOOST_CHECK_EQUAL(true, bearing::CheckInBounds(359, 5, 10));
+    BOOST_CHECK_EQUAL(false, bearing::CheckInBounds(354, 5, 10));
+    BOOST_CHECK_EQUAL(false, bearing::CheckInBounds(16, 5, 10));
+
+
+    // Checking other cases of wraparound
+    BOOST_CHECK_EQUAL(true, bearing::CheckInBounds(359, -5, 10));
+    BOOST_CHECK_EQUAL(false, bearing::CheckInBounds(344, -5, 10));
+    BOOST_CHECK_EQUAL(false, bearing::CheckInBounds(6, -5, 10));
+
+    BOOST_CHECK_EQUAL(true, bearing::CheckInBounds(-1, 5, 10));
+    BOOST_CHECK_EQUAL(false, bearing::CheckInBounds(-6, 5, 10));
+
+    BOOST_CHECK_EQUAL(true, bearing::CheckInBounds(-721, 5, 10));
+    BOOST_CHECK_EQUAL(true, bearing::CheckInBounds(719, 5, 10));
+}
+
+BOOST_AUTO_TEST_SUITE_END()

unit_tests/util_tests.cpp

@@ -0,0 +1,34 @@
+/*
+
+Copyright (c) 2014, Project OSRM contributors
+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.
+
+*/
+
+#define BOOST_TEST_MODULE util tests
+
+#include <boost/test/unit_test.hpp>
+
+/*
+ * This file will contain an automatically generated main function.
+ */

util/bearing.hpp

@@ -72,6 +72,45 @@ inline std::string get(const double heading)
     }
     return "N";
 }
+
+// Checks whether A is between B-range and B+range, all modulo 360
+// e.g. A = 5, B = 5, range = 10 == true
+//      A = -6, B = 5, range = 10 == false
+//      A = -2, B = 355, range = 10 == true
+//      A = 6, B = 355, range = 10 == false
+//      A = 355, B = -2, range = 10 == true
+//
+// @param A the bearing to check, in degrees, 0-359, 0=north
+// @param B the bearing to check against, in degrees, 0-359, 0=north
+// @param range the number of degrees either side of B that A will still match
+// @return true if B-range <= A <= B+range, modulo 360
+inline bool CheckInBounds(const int A, const int B, const int range)
+{
+
+    if (range >= 180)
+        return true;
+    if (range <= 0)
+        return false;
+
+    // Map both bearings into positive modulo 360 space
+    const int normalized_B = (B < 0) ? (B % 360) + 360 : (B % 360);
+    const int normalized_A = (A < 0) ? (A % 360) + 360 : (A % 360);
+
+    if (normalized_B - range < 0)
+    {
+        return (normalized_B - range + 360 <= normalized_A && normalized_A < 360) ||
+               (0 <= normalized_A && normalized_A <= normalized_B + range);
+    }
+    else if (normalized_B + range > 360)
+    {
+        return (normalized_B - range <= normalized_A && normalized_A < 360) ||
+               (0 <= normalized_A && normalized_A <= normalized_B + range - 360);
+    }
+    else
+    {
+        return normalized_B - range <= normalized_A && normalized_A <= normalized_B + range;
+    }
+}
 }
 
 #endif // BEARING_HPP

util/matching_debug_info.hpp

@@ -62,8 +62,8 @@ struct MatchingDebugInfo
                 osrm::json::Object state;
                 state.values["transitions"] = osrm::json::Array();
                 state.values["coordinate"] =
-                    osrm::json::make_array(elem_s.first.location.lat / COORDINATE_PRECISION,
-                                           elem_s.first.location.lon / COORDINATE_PRECISION);
+                    osrm::json::make_array(elem_s.second.location.lat / COORDINATE_PRECISION,
+                                           elem_s.second.location.lon / COORDINATE_PRECISION);
                 state.values["viterbi"] =
                     osrm::json::clamp_float(osrm::matching::IMPOSSIBLE_LOG_PROB);
                 state.values["pruned"] = 0u;