Adds support for a new b= parameter on the viaroute and match

plugins, allowing for better nearest neighbor matching when a heading
is known.

data_structures/route_parameters.cpp

@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <boost/fusion/container/vector.hpp>
 #include <boost/fusion/sequence/intrinsic.hpp>
 #include <boost/fusion/include/at_c.hpp>
+#include <boost/spirit/include/qi.hpp>
 
 #include <osrm/route_parameters.hpp>
 
@@ -117,6 +118,18 @@ void RouteParameters::addTimestamp(const unsigned timestamp)
     }
 }
 
+void RouteParameters::addBearing(const int bearing, boost::spirit::qi::unused_type unused, bool& pass)
+{
+    bearings.resize(coordinates.size());
+    pass = false;
+    if (bearing < 0 || bearing > 359) return;
+    if (!bearings.empty())
+    {
+        bearings.back() = bearing;
+        pass = true;
+    }
+}
+
 void RouteParameters::setLanguage(const std::string &language_string)
 {
     language = language_string;

data_structures/static_rtree.hpp

@@ -642,10 +642,55 @@ class StaticRTree
         return result_coordinate.is_valid();
     }
 
+    /**
+     * 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
+     * */
+    static bool IsBearingWithinBounds(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;
+        }
+    }
+
+
+
     bool IncrementalFindPhantomNodeForCoordinate(
         const FixedPointCoordinate &input_coordinate,
         std::vector<PhantomNode> &result_phantom_node_vector,
         const unsigned max_number_of_phantom_nodes,
+        const int filter_bearing = 0,
+        const int filter_bearing_range = 180,
         const float max_distance = 1100,
         const unsigned max_checked_elements = 4 * LEAF_NODE_SIZE)
     {
@@ -737,10 +782,35 @@ class StaticRTree
                     m_coordinate_list->at(current_segment.v), input_coordinate,
                     projected_coordinate, foot_point_coordinate_on_segment, current_ratio);
 
+                const float forward_edge_bearing = coordinate_calculation::bearing(
+                                m_coordinate_list->at(current_segment.u),
+                                m_coordinate_list->at(current_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 = IsBearingWithinBounds(forward_edge_bearing, filter_bearing, filter_bearing_range);
+                const bool backward_bearing_valid = IsBearingWithinBounds(backward_edge_bearing, filter_bearing, filter_bearing_range);
+
+                if (!forward_bearing_valid && !backward_bearing_valid)
+                {
+                    continue;
+                }
+
                 // store phantom node in result vector
                 result_phantom_node_vector.emplace_back(current_segment,
                                                         foot_point_coordinate_on_segment);
 
+                if (!forward_bearing_valid)
+                {
+                    result_phantom_node_vector.back().forward_node_id = SPECIAL_NODEID;
+                } 
+                else if (!backward_bearing_valid)
+                {
+                    result_phantom_node_vector.back().reverse_node_id = SPECIAL_NODEID;
+                }
+
                 // Hack to fix rounding errors and wandering via nodes.
                 FixUpRoundingIssue(input_coordinate, result_phantom_node_vector.back());
 
@@ -788,6 +858,8 @@ class StaticRTree
         const FixedPointCoordinate &input_coordinate,
         std::vector<std::pair<PhantomNode, double>> &result_phantom_node_vector,
         const double max_distance,
+        const int filter_bearing = 0,
+        const int filter_bearing_range = 180,
         const unsigned max_checked_elements = 4 * LEAF_NODE_SIZE)
     {
         unsigned inspected_elements = 0;
@@ -881,6 +953,23 @@ class StaticRTree
                     continue;
                 }
 
+                const float forward_edge_bearing = coordinate_calculation::bearing(
+                                m_coordinate_list->at(current_segment.u),
+                                m_coordinate_list->at(current_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 = IsBearingWithinBounds(forward_edge_bearing, filter_bearing, filter_bearing_range);
+                const bool backward_bearing_valid = IsBearingWithinBounds(backward_edge_bearing, filter_bearing, filter_bearing_range);
+
+                if (!forward_bearing_valid && !backward_bearing_valid)
+                {
+                    // This edge doesn't fall within our bearing filter
+                    continue;
+                }
+
                 // store phantom node in result vector
                 result_phantom_node_vector.emplace_back(
                     PhantomNode(
@@ -893,6 +982,15 @@ class StaticRTree
                         current_segment.forward_travel_mode, current_segment.backward_travel_mode),
                     current_perpendicular_distance);
 
+                if (!forward_bearing_valid)
+                {
+                    result_phantom_node_vector.back().first.forward_node_id = SPECIAL_NODEID;
+                }
+                if (!backward_bearing_valid)
+                {
+                    result_phantom_node_vector.back().first.reverse_node_id = SPECIAL_NODEID;
+                }
+
                 // Hack to fix rounding errors and wandering via nodes.
                 FixUpRoundingIssue(input_coordinate, result_phantom_node_vector.back().first);
 

include/osrm/route_parameters.hpp

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <osrm/coordinate.hpp>
 
 #include <boost/fusion/container/vector/vector_fwd.hpp>
+#include <boost/spirit/include/qi.hpp>
 
 #include <string>
 #include <vector>
@@ -71,6 +72,8 @@ struct RouteParameters
 
     void addTimestamp(const unsigned timestamp);
 
+    void addBearing(const int timestamp, boost::spirit::qi::unused_type unused, bool& pass);
+
     void setLanguage(const std::string &language);
 
     void setGeometryFlag(const bool flag);
@@ -99,6 +102,7 @@ struct RouteParameters
     std::string language;
     std::vector<std::string> hints;
     std::vector<unsigned> timestamps;
+    std::vector<int> bearings;
     std::vector<bool> uturns;
     std::vector<FixedPointCoordinate> coordinates;
 };

plugins/match.hpp

@@ -93,7 +93,8 @@ template <class DataFacadeT> class MapMatchingPlugin : public BasePlugin
         return label_with_confidence;
     }
 
-    bool getCandiates(const std::vector<FixedPointCoordinate> &input_coords,
+    bool getCandidates(const std::vector<FixedPointCoordinate> &input_coords,
+                      const std::vector<int> &input_bearings,
                       const double gps_precision,
                       std::vector<double> &sub_trace_lengths,
                       osrm::matching::CandidateLists &candidates_lists)
@@ -128,8 +129,12 @@ 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] : 0;
+            auto range = input_bearings.size() > 0 ? 10 : 180;
             facade->IncrementalFindPhantomNodeForCoordinateWithMaxDistance(
-                    input_coords[current_coordinate], candidates, query_radius);
+                    input_coords[current_coordinate], candidates, query_radius,
+                    bearing, range);
 
             // sort by foward id, then by reverse id and then by distance
             std::sort(candidates.begin(), candidates.end(),
@@ -270,12 +275,19 @@ template <class DataFacadeT> class MapMatchingPlugin : public BasePlugin
         osrm::matching::CandidateLists candidates_lists;
         const auto &input_coords = route_parameters.coordinates;
         const auto &input_timestamps = route_parameters.timestamps;
+        const auto &input_bearings = route_parameters.bearings;
         if (input_timestamps.size() > 0 && input_coords.size() != input_timestamps.size())
         {
             json_result.values["status"] = "Number of timestamps does not match number of coordinates .";
             return 400;
         }
 
+        if (input_bearings.size() > 0 && input_coords.size() != input_bearings.size())
+        {
+            json_result.values["status"] = "Number of bearings does not match number of coordinates .";
+            return 400;
+        }
+
         // enforce maximum number of locations for performance reasons
         if (max_locations_map_matching > 0 &&
             static_cast<int>(input_coords.size()) < max_locations_map_matching)
@@ -292,7 +304,7 @@ template <class DataFacadeT> class MapMatchingPlugin : public BasePlugin
         }
 
         const bool found_candidates =
-            getCandiates(input_coords, route_parameters.gps_precision, sub_trace_lengths, candidates_lists);
+            getCandidates(input_coords, input_bearings, route_parameters.gps_precision, sub_trace_lengths, candidates_lists);
         if (!found_candidates)
         {
             json_result.values["status"] = "No suitable matching candidates found.";

plugins/viaroute.hpp

@@ -80,6 +80,13 @@ template <class DataFacadeT> class ViaRoutePlugin final : public BasePlugin
             return 400;
         }
 
+        const auto &input_bearings = route_parameters.bearings;
+        if (input_bearings.size() > 0 && route_parameters.coordinates.size() != input_bearings.size())
+        {
+            json_result.values["status"] = "Number of bearings does not match number of coordinates .";
+            return 400;
+        }
+
         std::vector<phantom_node_pair> phantom_node_pair_list(route_parameters.coordinates.size());
         const bool checksum_OK = (route_parameters.check_sum == facade->GetCheckSum());
 
@@ -96,8 +103,10 @@ template <class DataFacadeT> class ViaRoutePlugin final : public BasePlugin
                 }
             }
             std::vector<PhantomNode> phantom_node_vector;
+            int bearing = input_bearings.size() > 0 ? input_bearings[i] : 0;
+            int range = input_bearings.size() > 0 ? 8 : 180;
             if (facade->IncrementalFindPhantomNodeForCoordinate(route_parameters.coordinates[i],
-                                                                phantom_node_vector, 1))
+                                                                phantom_node_vector, 1, bearing, range))
             {
                 BOOST_ASSERT(!phantom_node_vector.empty());
                 phantom_node_pair_list[i].first = phantom_node_vector.front();

server/api_grammar.hpp

@@ -40,7 +40,7 @@ template <typename Iterator, class HandlerT> struct APIGrammar : qi::grammar<Ite
     {
         api_call = qi::lit('/') >> string[boost::bind(&HandlerT::setService, handler, ::_1)] >>
                    *(query) >> -(uturns);
-        query = ('?') >> (+(zoom | output | jsonp | checksum | location | hint | timestamp | u | cmp |
+        query = ('?') >> (+(zoom | output | jsonp | checksum | location | hint | timestamp | bearing | u | cmp |
                             language | instruction | geometry | alt_route | old_API | num_results |
                             matching_beta | gps_precision | classify | locs));
 
@@ -65,6 +65,8 @@ template <typename Iterator, class HandlerT> struct APIGrammar : qi::grammar<Ite
                stringwithDot[boost::bind(&HandlerT::addHint, handler, ::_1)];
         timestamp = (-qi::lit('&')) >> qi::lit("t") >> '=' >>
                qi::uint_[boost::bind(&HandlerT::addTimestamp, handler, ::_1)];
+        bearing = (-qi::lit('&')) >> qi::lit("b") >> '=' >>
+               qi::int_[boost::bind(&HandlerT::addBearing, handler, ::_1, ::_2, ::_3)];
         u = (-qi::lit('&')) >> qi::lit("u") >> '=' >>
             qi::bool_[boost::bind(&HandlerT::setUTurn, handler, ::_1)];
         uturns = (-qi::lit('&')) >> qi::lit("uturns") >> '=' >>
@@ -95,7 +97,7 @@ template <typename Iterator, class HandlerT> struct APIGrammar : qi::grammar<Ite
 
     qi::rule<Iterator> api_call, query;
     qi::rule<Iterator, std::string()> service, zoom, output, string, jsonp, checksum, location,
-        hint, timestamp, stringwithDot, stringwithPercent, language, instruction, geometry, cmp, alt_route, u,
+        hint, timestamp, bearing, stringwithDot, stringwithPercent, language, instruction, geometry, cmp, alt_route, u,
         uturns, old_API, num_results, matching_beta, gps_precision, classify, locs, stringforPolyline;
 
     HandlerT *handler;

server/data_structures/datafacade_base.hpp

@@ -99,7 +99,8 @@ template <class EdgeDataT> class BaseDataFacade
     virtual bool
     IncrementalFindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate,
                                             std::vector<PhantomNode> &resulting_phantom_node_vector,
-                                            const unsigned number_of_results) = 0;
+                                            const unsigned number_of_results,
+                                            const int bearing = 0, const int bearing_range = 180) = 0;
 
     virtual bool
     IncrementalFindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate,
@@ -107,7 +108,7 @@ template <class EdgeDataT> class BaseDataFacade
     virtual bool IncrementalFindPhantomNodeForCoordinateWithMaxDistance(
         const FixedPointCoordinate &input_coordinate,
         std::vector<std::pair<PhantomNode, double>> &resulting_phantom_node_vector,
-        const double max_distance) = 0;
+        const double max_distance, const int bearing = 0, const int bearing_range = 180) = 0;
 
     virtual unsigned GetCheckSum() const = 0;
 

server/data_structures/internal_datafacade.hpp

@@ -388,7 +388,8 @@ template <class EdgeDataT> class InternalDataFacade final : public BaseDataFacad
     bool
     IncrementalFindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate,
                                             std::vector<PhantomNode> &resulting_phantom_node_vector,
-                                            const unsigned number_of_results) override final
+                                            const unsigned number_of_results,
+                                            const int bearing = 0, const int range = 180) override final
     {
         if (!m_static_rtree.get())
         {
@@ -396,13 +397,15 @@ template <class EdgeDataT> class InternalDataFacade final : public BaseDataFacad
         }
 
         return m_static_rtree->IncrementalFindPhantomNodeForCoordinate(
-            input_coordinate, resulting_phantom_node_vector, number_of_results);
+            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) override final
+        const double max_distance,
+        const int bearing = 0,
+        const int bearing_range = 180) override final
     {
         if (!m_static_rtree.get())
         {
@@ -410,7 +413,7 @@ template <class EdgeDataT> class InternalDataFacade final : public BaseDataFacad
         }
 
         return m_static_rtree->IncrementalFindPhantomNodeForCoordinateWithDistance(
-            input_coordinate, resulting_phantom_node_vector, max_distance);
+            input_coordinate, resulting_phantom_node_vector, max_distance, bearing, bearing_range);
     }
 
     unsigned GetCheckSum() const override final { return m_check_sum; }

server/data_structures/shared_datafacade.hpp

@@ -409,7 +409,8 @@ template <class EdgeDataT> class SharedDataFacade final : public BaseDataFacade<
     bool
     IncrementalFindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate,
                                             std::vector<PhantomNode> &resulting_phantom_node_vector,
-                                            const unsigned number_of_results) override final
+                                            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)
         {
@@ -417,13 +418,15 @@ template <class EdgeDataT> class SharedDataFacade final : public BaseDataFacade<
         }
 
         return m_static_rtree->second->IncrementalFindPhantomNodeForCoordinate(
-            input_coordinate, resulting_phantom_node_vector, number_of_results);
+            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) override final
+        const double max_distance,
+        const int bearing = 0,
+        const int bearing_range = 180) override final
     {
         if (!m_static_rtree.get() || CURRENT_TIMESTAMP != m_static_rtree->first)
         {
@@ -431,7 +434,7 @@ template <class EdgeDataT> class SharedDataFacade final : public BaseDataFacade<
         }
 
         return m_static_rtree->second->IncrementalFindPhantomNodeForCoordinateWithDistance(
-            input_coordinate, resulting_phantom_node_vector, max_distance);
+            input_coordinate, resulting_phantom_node_vector, max_distance, bearing, bearing_range);
     }
 
     unsigned GetCheckSum() const override final { return m_check_sum; }

unit_tests/data_structures/static_rtree.cpp

@@ -258,6 +258,12 @@ struct GraphFixture
             TestData d;
             d.u = pair.first;
             d.v = pair.second;
+            // We set the forward nodes to the target node-based-node IDs, just
+            // so we have something to test against.  Because this isn't a real
+            // graph, the actual values aren't important, we just need something
+            // to examine during tests.
+            d.forward_edge_based_node_id = pair.second;
+            d.reverse_edge_based_node_id = pair.first;
             edges.emplace_back(d);
         }
     }
@@ -495,4 +501,100 @@ 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;
+    GraphFixture fixture(
+        {
+         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);
+
+    FixedPointCoordinate input(5.0 * COORDINATE_PRECISION, 5.1 * COORDINATE_PRECISION);
+    std::vector<PhantomNode> results;
+
+    rtree.IncrementalFindPhantomNodeForCoordinate( input, results, 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_AUTO_TEST_SUITE_END()