scaffolding for distances in matrix

logging format change

logging format change

computeEdgeDistance outputs values! that are probably wrong but still!

notes

offset functions but packing and padding errors

offset functions work now

include/engine/datafacade/contiguous_internalmem_datafacade.hpp

@@ -246,7 +246,13 @@ class ContiguousInternalMemoryDataFacadeBase : public BaseDataFacade
     // node and edge information access
     util::Coordinate GetCoordinateOfNode(const NodeID id) const override final
     {
-        return m_coordinate_list[id];
+        std::cout << "m_coordinate_list: ";
+        for (auto it = m_coordinate_list.begin(); it != m_coordinate_list.end(); ++it)
+            std::cout << (*it).lon << ";" << (*it).lat << ", ";
+        std::cout << std::endl;
+        return m_coordinate_list.at(id); // get default
+        // return m_coordinate_list[id]; causes socket to hang up when we pass in garbage node_is
+        // (turn_id)
     }
 
     OSMNodeID GetOSMNodeIDOfNode(const NodeID id) const override final

include/engine/geospatial_query.hpp

@@ -448,6 +448,9 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
         const auto forward_duration_range = datafacade.GetUncompressedForwardDurations(geometry_id);
         const auto reverse_duration_range = datafacade.GetUncompressedReverseDurations(geometry_id);
 
+        const auto forward_geometry_range = datafacade.GetUncompressedForwardGeometry(geometry_id);
+        const auto reverse_geometry_range = datafacade.GetUncompressedReverseGeometry(geometry_id);
+
         const auto forward_weight_offset =
             std::accumulate(forward_weight_range.begin(),
                             forward_weight_range.begin() + data.fwd_segment_position,
@@ -458,8 +461,27 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
                             forward_duration_range.begin() + data.fwd_segment_position,
                             EdgeDuration{0});
 
+        // const auto forward_geometries_offset =
+        //     std::accumulate(forward_geometry_range.begin(),
+        //                     forward_geometry_range.begin() + data.fwd_segment_position,
+        //                     EdgeDistance{0});
+
+        const auto forward_distance_offset = 0;
+        for (auto current = forward_geometry_range.begin() + 1;
+             current != forward_geometry_range.begin() + data.fwd_segment_position + 1;
+             ++current)
+        {
+            auto prev = current - 1;
+
+            forward_distance_offset += util::coordinate_calculation::haversineDistance(
+                facade.GetCoordinateOfNode(*prev), facade.GetCoordinateOfNode(*current));
+        }
+
         EdgeWeight forward_weight = forward_weight_range[data.fwd_segment_position];
         EdgeDuration forward_duration = forward_duration_range[data.fwd_segment_position];
+        EdgeDistance forward_geometry = HaversineDistance(
+            facade.GetCoordinateOfNode(forward_geometry_range[data.fwd_segment_position]),
+            point_on_segment);
 
         BOOST_ASSERT(data.fwd_segment_position <
                      std::distance(forward_duration_range.begin(), forward_duration_range.end()));
@@ -474,10 +496,30 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
                             reverse_duration_range.end() - data.fwd_segment_position - 1,
                             EdgeDuration{0});
 
+        // const auto reverse_geometries_offset =
+        //     std::accumulate(reverse_geometry_range.begin(),
+        //                     reverse_geometry_range.begin() + data.fwd_segment_position,
+        //                     EdgeDistance{0});
+
+        const auto reverse_distance_offset = 0;
+        for (auto current = reverse_geometry_range.begin() + 1;
+             current != reverse_geometry_range.begin() + data.fwd_segment_position + 1;
+             ++current)
+        {
+            auto prev = current - 1;
+
+            reverse_distance_offset += util::coordinate_calculation::haversineDistance(
+                facade.GetCoordinateOfNode(*prev), facade.GetCoordinateOfNode(*current));
+        }
+
         EdgeWeight reverse_weight =
             reverse_weight_range[reverse_weight_range.size() - data.fwd_segment_position - 1];
         EdgeDuration reverse_duration =
             reverse_duration_range[reverse_duration_range.size() - data.fwd_segment_position - 1];
+        EdgeDistance reverse_distance = HaversineDistance(
+            facade.GetCoordinateOfNode(reverse_geometry_range[reverse_geometry_range.size() -
+                                                              data.fwd_segment_position - 1]),
+            point_on_segment);
 
         ratio = std::min(1.0, std::max(0.0, ratio));
         if (data.forward_segment_id.id != SPECIAL_SEGMENTID)
@@ -516,6 +558,10 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
                         reverse_duration,
                         forward_duration_offset,
                         reverse_duration_offset,
+                        forward_distance,
+                        reverse_distance,
+                        forward_distance_offset,
+                        reverse_distance_offset,
                         is_forward_valid_source,
                         is_forward_valid_target,
                         is_reverse_valid_source,

include/engine/hint.hpp

@@ -63,8 +63,8 @@ struct Hint
     friend std::ostream &operator<<(std::ostream &, const Hint &);
 };
 
-static_assert(sizeof(Hint) == 64 + 4, "Hint is bigger than expected");
-constexpr std::size_t ENCODED_HINT_SIZE = 92;
+static_assert(sizeof(Hint) == 80 + 4, "Hint is bigger than expected");
+constexpr std::size_t ENCODED_HINT_SIZE = 112;
 static_assert(ENCODED_HINT_SIZE / 4 * 3 >= sizeof(Hint),
               "ENCODED_HINT_SIZE does not match size of Hint");
 }

include/engine/phantom_node.hpp

@@ -48,9 +48,12 @@ struct PhantomNode
           reverse_segment_id{SPECIAL_SEGMENTID, false}, forward_weight(INVALID_EDGE_WEIGHT),
           reverse_weight(INVALID_EDGE_WEIGHT), forward_weight_offset(0), reverse_weight_offset(0),
           forward_duration(MAXIMAL_EDGE_DURATION), reverse_duration(MAXIMAL_EDGE_DURATION),
-          forward_duration_offset(0), reverse_duration_offset(0), fwd_segment_position(0),
-          is_valid_forward_source{false}, is_valid_forward_target{false},
-          is_valid_reverse_source{false}, is_valid_reverse_target{false}, bearing(0)
+          forward_distance(MAXIMAL_EDGE_DISTANCE), reverse_distance(MAXIMAL_EDGE_DISTANCE),
+          forward_distance_offset(0), reverse_distance_offset(0), forward_duration_offset(0),
+          reverse_duration_offset(0), fwd_segment_position(0), is_valid_forward_source{false},
+          is_valid_forward_target{false}, is_valid_reverse_source{false},
+          is_valid_reverse_target{false}, bearing(0)
+
     {
     }
 
@@ -78,6 +81,20 @@ struct PhantomNode
         return reverse_duration + reverse_duration_offset;
     }
 
+    // DO THIS FOR DISTANCE
+
+    EdgeWeight GetForwardDistance() const
+    {
+        BOOST_ASSERT(forward_segment_id.enabled);
+        return forward_distance + forward_distance_offset;
+    }
+
+    EdgeWeight GetReverseDistance() const
+    {
+        BOOST_ASSERT(reverse_segment_id.enabled);
+        return reverse_distance + reverse_distance_offset;
+    }
+
     bool IsBidirected() const { return forward_segment_id.enabled && reverse_segment_id.enabled; }
 
     bool IsValid(const unsigned number_of_nodes) const
@@ -88,6 +105,8 @@ struct PhantomNode
                 (reverse_weight != INVALID_EDGE_WEIGHT)) &&
                ((forward_duration != MAXIMAL_EDGE_DURATION) ||
                 (reverse_duration != MAXIMAL_EDGE_DURATION)) &&
+               ((forward_distance != MAXIMAL_EDGE_DISTANCE) ||
+                (reverse_distance != MAXIMAL_EDGE_DISTANCE)) &&
                (component.id != INVALID_COMPONENTID);
     }
 
@@ -134,6 +153,10 @@ struct PhantomNode
                          EdgeWeight reverse_duration,
                          EdgeWeight forward_duration_offset,
                          EdgeWeight reverse_duration_offset,
+                         EdgeDistance forward_distance,
+                         EdgeDistance reverse_distance,
+                         EdgeDistance forward_distance_offset,
+                         EdgeDistance reverse_distance_offset,
                          bool is_valid_forward_source,
                          bool is_valid_forward_target,
                          bool is_valid_reverse_source,
@@ -146,7 +169,9 @@ struct PhantomNode
           reverse_weight{reverse_weight}, forward_weight_offset{forward_weight_offset},
           reverse_weight_offset{reverse_weight_offset}, forward_duration{forward_duration},
           reverse_duration{reverse_duration}, forward_duration_offset{forward_duration_offset},
-          reverse_duration_offset{reverse_duration_offset},
+          reverse_duration_offset{reverse_duration_offset}, forward_distance{forward_distance},
+          reverse_distance{reverse_distance}, forward_distance_offset{forward_distance_offset},
+          reverse_distance_offset{reverse_distance_offset},
           component{component.id, component.is_tiny}, location{location},
           input_location{input_location}, fwd_segment_position{other.fwd_segment_position},
           is_valid_forward_source{is_valid_forward_source},
@@ -162,13 +187,17 @@ struct PhantomNode
     EdgeWeight reverse_weight;
     EdgeWeight forward_weight_offset; // TODO: try to remove -> requires path unpacking changes
     EdgeWeight reverse_weight_offset; // TODO: try to remove -> requires path unpacking changes
+    EdgeDistance forward_distance;
+    EdgeDistance reverse_distance;
+    EdgeDistance forward_distance_offset;
+    EdgeDistance reverse_distance_offset;
     EdgeWeight forward_duration;
     EdgeWeight reverse_duration;
     EdgeWeight forward_duration_offset; // TODO: try to remove -> requires path unpacking changes
     EdgeWeight reverse_duration_offset; // TODO: try to remove -> requires path unpacking changes
     ComponentID component;
 
-    util::Coordinate location;
+    util::Coordinate location; // this is the coordinate of x
     util::Coordinate input_location;
     unsigned short fwd_segment_position;
     // is phantom node valid to be used as source or target
@@ -180,7 +209,7 @@ struct PhantomNode
     unsigned short bearing : 12;
 };
 
-static_assert(sizeof(PhantomNode) == 64, "PhantomNode has more padding then expected");
+static_assert(sizeof(PhantomNode) == 80, "PhantomNode has more padding then expected");
 
 using PhantomNodePair = std::pair<PhantomNode, PhantomNode>;
 

include/engine/routing_algorithms/routing_base.hpp

@@ -424,6 +424,47 @@ EdgeDuration computeEdgeDuration(const FacadeT &facade, NodeID node_id, NodeID t
     return total_duration;
 }
 
+template <typename FacadeT>
+EdgeDistance computeEdgeDistance(const FacadeT &facade, NodeID node_id_1, NodeID node_id_2)
+{
+    (void)node_id_2;
+    const auto geometry_index = facade.GetGeometryIndex(node_id_1);
+
+    // datastructures to hold extracted data from geometry
+    EdgeDistance total_distance = 0.0;
+
+    if (geometry_index.forward)
+    {
+        auto geometry_range = facade.GetUncompressedForwardGeometry(geometry_index.id);
+        for (auto current = geometry_range.begin() + 1; current != geometry_range.end(); ++current)
+        {
+            auto prev = current - 1;
+
+            const auto coordinate_1 = facade.GetCoordinateOfNode(*prev);
+            const auto coordinate_2 = facade.GetCoordinateOfNode(*current);
+
+            total_distance +=
+                util::coordinate_calculation::haversineDistance(coordinate_1, coordinate_2);
+        }
+    }
+    else
+    {
+        auto geometry_range = facade.GetUncompressedReverseDurations(geometry_index.id);
+        for (auto current = geometry_range.begin() + 1; current != geometry_range.end(); ++current)
+        {
+            auto prev = current - 1;
+
+            const auto coordinate_1 = facade.GetCoordinateOfNode(*prev);
+            const auto coordinate_2 = facade.GetCoordinateOfNode(*current);
+
+            total_distance +=
+                util::coordinate_calculation::haversineDistance(coordinate_1, coordinate_2);
+        }
+    }
+
+    return total_distance;
+}
+
 } // namespace routing_algorithms
 } // namespace engine
 } // namespace osrm

include/engine/routing_algorithms/routing_base_ch.hpp

@@ -289,19 +289,21 @@ void unpackPath(const DataFacade<Algorithm> &facade,
     }
 }
 
+using PathAnnotation = std::pair<EdgeDuration, EdgeDistance>;
 template <typename BidirectionalIterator>
-EdgeDuration calculateEBGNodeAnnotations(const DataFacade<Algorithm> &facade,
-                                         BidirectionalIterator packed_path_begin,
-                                         BidirectionalIterator packed_path_end,
-                                         UnpackingCache &unpacking_cache)
+PathAnnotation calculateEBGNodeAnnotations(const DataFacade<Algorithm> &facade,
+                                           BidirectionalIterator packed_path_begin,
+                                           BidirectionalIterator packed_path_end,
+                                           UnpackingCache &unpacking_cache)
 {
     // make sure we have at least something to unpack
     if (packed_path_begin == packed_path_end ||
         std::distance(packed_path_begin, packed_path_end) <= 1)
-        return 0;
+        return std::make_pair(0, 0);
 
     std::stack<std::tuple<NodeID, NodeID, bool>> recursion_stack;
     std::stack<EdgeDuration> duration_stack;
+    std::stack<EdgeDistance> distance_stack;
 
     // We have to push the path in reverse order onto the stack because it's LIFO.
     for (auto current = std::prev(packed_path_end); current != packed_path_begin;
@@ -326,9 +328,10 @@ EdgeDuration calculateEBGNodeAnnotations(const DataFacade<Algorithm> &facade,
             if (unpacking_cache.IsEdgeInCache(std::make_tuple(
                     std::get<0>(edge), std::get<1>(edge), facade.GetExcludeIndex())))
             {
-                EdgeDuration duration = unpacking_cache.GetDuration(std::make_tuple(
+                PathAnnotation annotation = unpacking_cache.GetAnnotation(std::make_tuple(
                     std::get<0>(edge), std::get<1>(edge), facade.GetExcludeIndex()));
-                duration_stack.emplace(duration);
+                duration_stack.emplace(annotation.first);
+                distance_stack.emplace(annotation.second);
             }
             else
             {
@@ -369,14 +372,17 @@ EdgeDuration calculateEBGNodeAnnotations(const DataFacade<Algorithm> &facade,
                 }
                 else
                 {
-                    auto temp = std::make_tuple(
+                    auto new_edge = std::make_tuple(
                         std::get<0>(edge), std::get<1>(edge), facade.GetExcludeIndex());
                     // compute the duration here and put it onto the duration stack using method
                     // similar to annotatePath but smaller
                     EdgeDuration duration =
                         computeEdgeDuration(facade, std::get<0>(edge), data.turn_id);
+                    EdgeDistance distance =
+                        computeEdgeDistance(facade, std::get<0>(edge), std::get<1>(edge));
                     duration_stack.emplace(duration);
-                    unpacking_cache.AddEdge(temp, duration);
+                    distance_stack.emplace(distance);
+                    unpacking_cache.AddEdge(new_edge, std::make_pair(duration, distance));
                 }
             }
         }
@@ -391,9 +397,19 @@ EdgeDuration calculateEBGNodeAnnotations(const DataFacade<Algorithm> &facade,
             duration_stack.pop();
             EdgeDuration duration = edge1 + edge2;
             duration_stack.emplace(duration);
+
+            BOOST_ASSERT_MSG(distance_stack.size() >= 2,
+                             "There are not enough (at least 2) values on the distance stack");
+            EdgeDistance distance1 = distance_stack.top();
+            distance_stack.pop();
+            EdgeDistance distance2 = distance_stack.top();
+            distance_stack.pop();
+            EdgeDistance distance = distance1 + distance2;
+            distance_stack.emplace(distance);
+
             unpacking_cache.AddEdge(
                 std::make_tuple(std::get<0>(edge), std::get<1>(edge), facade.GetExcludeIndex()),
-                duration);
+                std::make_pair(duration, distance));
         }
     }
 
@@ -403,7 +419,15 @@ EdgeDuration calculateEBGNodeAnnotations(const DataFacade<Algorithm> &facade,
         total_duration += duration_stack.top();
         duration_stack.pop();
     }
-    return total_duration;
+
+    EdgeDistance total_distance = 0;
+    while (!distance_stack.empty())
+    {
+        total_distance += distance_stack.top();
+        distance_stack.pop();
+    }
+
+    return std::make_pair(total_duration, total_distance);
 }
 
 template <typename RandomIter, typename FacadeT>

include/engine/unpacking_cache.hpp

@@ -10,14 +10,16 @@ namespace osrm
 {
 namespace engine
 {
+using PathAnnotation = std::pair<EdgeDuration, EdgeDistance>;
 class UnpackingCache
 {
   private:
-    boost::compute::detail::lru_cache<std::tuple<NodeID, NodeID, std::size_t>, EdgeDuration> cache;
+    boost::compute::detail::lru_cache<std::tuple<NodeID, NodeID, std::size_t>, PathAnnotation>
+        cache;
     unsigned current_data_timestamp = 0;
 
   public:
-    UnpackingCache(unsigned timestamp) : cache(200), current_data_timestamp(timestamp){};
+    UnpackingCache(unsigned timestamp) : cache(16000000), current_data_timestamp(timestamp){};
 
     void Clear(unsigned new_data_timestamp)
     {
@@ -33,15 +35,16 @@ class UnpackingCache
         return cache.contains(edge);
     }
 
-    void AddEdge(std::tuple<NodeID, NodeID, std::size_t> edge, EdgeDuration duration)
+    void AddEdge(std::tuple<NodeID, NodeID, std::size_t> edge, PathAnnotation annotation)
     {
-        cache.insert(edge, duration);
+        cache.insert(edge, annotation);
     }
 
-    EdgeDuration GetDuration(std::tuple<NodeID, NodeID, std::size_t> edge)
+    PathAnnotation GetAnnotation(std::tuple<NodeID, NodeID, std::size_t> edge)
     {
-        boost::optional<EdgeDuration> duration = cache.get(edge);
-        return *duration ? *duration : MAXIMAL_EDGE_DURATION;
+        boost::optional<PathAnnotation> annotation = cache.get(edge);
+        return annotation ? *annotation
+                          : std::make_pair(MAXIMAL_EDGE_DURATION, MAXIMAL_EDGE_DISTANCE);
     }
 };
 } // engine