move common code into Util header

DataStructures/StaticRTree.h

@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "SharedMemoryVectorWrapper.h"
 
 #include "../Util/MercatorUtil.h"
+#include "../Util/NumericUtil.h"
 #include "../Util/OSRMException.h"
 #include "../Util/SimpleLogger.h"
 #include "../Util/TimingUtil.h"
@@ -869,6 +870,168 @@ class StaticRTree
         return !result_phantom_node_vector.empty();
     }
 
+    // implementation of the Hjaltason/Samet query [3], a BFS traversal of the tree
+    bool
+    IncrementalFindPhantomNodeForCoordinateWithDistance(const FixedPointCoordinate &input_coordinate,
+                                                        std::vector<std::pair<PhantomNode, double>> &result_phantom_node_vector,
+                                                        const unsigned zoom_level,
+                                                        const unsigned number_of_results,
+                                                        const unsigned max_checked_segments = 4*LEAF_NODE_SIZE)
+    {
+        std::vector<float> min_found_distances(number_of_results, std::numeric_limits<float>::max());
+
+        unsigned number_of_results_found_in_big_cc = 0;
+        unsigned number_of_results_found_in_tiny_cc = 0;
+
+        unsigned inspected_segments = 0;
+
+        // initialize queue with root element
+        std::priority_queue<IncrementalQueryCandidate> traversal_queue;
+        traversal_queue.emplace(0.f, m_search_tree[0]);
+
+        while (!traversal_queue.empty())
+        {
+            const IncrementalQueryCandidate current_query_node = traversal_queue.top();
+            traversal_queue.pop();
+
+            const float current_min_dist = min_found_distances[number_of_results-1];
+
+            if (current_query_node.min_dist > current_min_dist)
+            {
+                continue;
+            }
+
+            if (current_query_node.RepresentsTreeNode())
+            {
+                const TreeNode & current_tree_node = boost::get<TreeNode>(current_query_node.node);
+                if (current_tree_node.child_is_on_disk)
+                {
+                    LeafNode current_leaf_node;
+                    LoadLeafFromDisk(current_tree_node.children[0], current_leaf_node);
+                    // Add all objects from leaf into queue
+                    for (uint32_t i = 0; i < current_leaf_node.object_count; ++i)
+                    {
+                        const auto &current_edge = current_leaf_node.objects[i];
+                        const float current_perpendicular_distance =
+                            FixedPointCoordinate::ComputePerpendicularDistance(
+                                m_coordinate_list->at(current_edge.u),
+                                m_coordinate_list->at(current_edge.v),
+                                input_coordinate);
+                        // distance must be non-negative
+                        BOOST_ASSERT(0. <= current_perpendicular_distance);
+
+                        if (current_perpendicular_distance < current_min_dist)
+                        {
+                            traversal_queue.emplace(current_perpendicular_distance, current_edge);
+                        }
+                    }
+                }
+                else
+                {
+                    // for each child mbr
+                    for (uint32_t i = 0; i < current_tree_node.child_count; ++i)
+                    {
+                        const int32_t child_id = current_tree_node.children[i];
+                        const TreeNode &child_tree_node = m_search_tree[child_id];
+                        const RectangleT &child_rectangle = child_tree_node.minimum_bounding_rectangle;
+                        const float lower_bound_to_element = child_rectangle.GetMinDist(input_coordinate);
+
+                        // TODO - enough elements found, i.e. nearest distance > maximum distance?
+                        //        ie. some measure of 'confidence of accuracy'
+
+                        // check if it needs to be explored by mindist
+                        if (lower_bound_to_element < current_min_dist)
+                        {
+                            traversal_queue.emplace(lower_bound_to_element, child_tree_node);
+                        }
+                    }
+                    // SimpleLogger().Write(logDEBUG) << "added " << current_tree_node.child_count << " mbrs into queue of " << traversal_queue.size();
+                }
+            }
+            else
+            {
+                ++inspected_segments;
+                // inspecting an actual road segment
+                const EdgeDataT & current_segment = boost::get<EdgeDataT>(current_query_node.node);
+
+                // don't collect too many results from small components
+                if (number_of_results_found_in_big_cc == number_of_results && !current_segment.is_in_tiny_cc)
+                {
+                    continue;
+                }
+
+                // don't collect too many results from big components
+                if (number_of_results_found_in_tiny_cc == number_of_results && current_segment.is_in_tiny_cc)
+                {
+                    continue;
+                }
+
+                // check if it is smaller than what we had before
+                float current_ratio = 0.;
+                FixedPointCoordinate foot_point_coordinate_on_segment;
+                const float current_perpendicular_distance =
+                    FixedPointCoordinate::ComputePerpendicularDistance(
+                        m_coordinate_list->at(current_segment.u),
+                        m_coordinate_list->at(current_segment.v),
+                        input_coordinate,
+                        foot_point_coordinate_on_segment,
+                        current_ratio);
+
+                BOOST_ASSERT(0. <= current_perpendicular_distance);
+
+                if ((current_perpendicular_distance < current_min_dist) &&
+                    !EpsilonCompare(current_perpendicular_distance, current_min_dist))
+                {
+                    // store phantom node in result vector
+                    result_phantom_node_vector.emplace_back(
+                        current_segment.forward_edge_based_node_id,
+                        current_segment.reverse_edge_based_node_id,
+                        current_segment.name_id,
+                        current_segment.forward_weight,
+                        current_segment.reverse_weight,
+                        current_segment.forward_offset,
+                        current_segment.reverse_offset,
+                        current_segment.packed_geometry_id,
+                        foot_point_coordinate_on_segment,
+                        current_segment.fwd_segment_position,
+                        current_perpendicular_distance);
+
+                    // Hack to fix rounding errors and wandering via nodes.
+                    FixUpRoundingIssue(input_coordinate, result_phantom_node_vector.back());
+
+                    // set forward and reverse weights on the phantom node
+                    SetForwardAndReverseWeightsOnPhantomNode(current_segment,
+                                                             result_phantom_node_vector.back());
+
+                    // do we have results only in a small scc
+                    if (current_segment.is_in_tiny_cc)
+                    {
+                        ++number_of_results_found_in_tiny_cc;
+                    }
+                    else
+                    {
+                        // found an element in a large component
+                        min_found_distances[number_of_results_found_in_big_cc] = current_perpendicular_distance;
+                        ++number_of_results_found_in_big_cc;
+                        // SimpleLogger().Write(logDEBUG) << std::setprecision(8) << foot_point_coordinate_on_segment << " at " << current_perpendicular_distance;
+                    }
+                }
+            }
+
+            // TODO add indicator to prune if maxdist > threshold
+            if (number_of_results == number_of_results_found_in_big_cc || inspected_segments >= max_checked_segments)
+            {
+                // SimpleLogger().Write(logDEBUG) << "flushing queue of " << traversal_queue.size() << " elements";
+                // work-around for traversal_queue.clear();
+                traversal_queue = std::priority_queue<IncrementalQueryCandidate>{};
+            }
+        }
+
+        return !result_phantom_node_vector.empty();
+    }
+
+
+
     bool FindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate,
                                       PhantomNode &result_phantom_node,
                                       const unsigned zoom_level)
@@ -1047,11 +1210,6 @@ class StaticRTree
     {
         return (a == b && c == d) || (a == c && b == d) || (a == d && b == c);
     }
-
-    template <typename FloatT> inline bool EpsilonCompare(const FloatT d1, const FloatT d2) const
-    {
-        return (std::abs(d1 - d2) < std::numeric_limits<FloatT>::epsilon());
-    }
 };
 
 //[1] "On Packing R-Trees"; I. Kamel, C. Faloutsos; 1993; DOI: 10.1145/170088.170403

UnitTests/DataStructures/StaticRTreeTest.cpp

@@ -1,9 +1,11 @@
 #include "../../DataStructures/StaticRTree.h"
 #include "../../DataStructures/QueryNode.h"
 #include "../../DataStructures/EdgeBasedNode.h"
-#include "../../Include/osrm/Coordinate.h"
+#include "../../Util/NumericUtil.h"
 #include "../../typedefs.h"
 
+#include <osrm/Coordinate.h>
+
 #include <boost/test/unit_test.hpp>
 #include <boost/test/test_case_template.hpp>
 #include <boost/mpl/list.hpp>
@@ -156,12 +158,6 @@ public:
         return result_phantom_node.location.isValid();
     }
 
-    template<typename FloatT>
-    inline bool EpsilonCompare(const FloatT d1, const FloatT d2) const
-    {
-        return (std::abs(d1 - d2) < std::numeric_limits<FloatT>::epsilon());
-    }
-
 private:
     const std::shared_ptr<std::vector<FixedPointCoordinate>>& coords;
     const std::vector<TestData>& edges;

Util/NumericUtil.h

@@ -0,0 +1,39 @@
+/*
+Copyright (c) 2013, Project OSRM, Dennis Luxen, others
+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 __NUMERIC_UTIL_H__
+#define __NUMERIC_UTIL_H__
+
+#include <cstdlib>
+
+#include <limits>
+
+template <typename FloatT> inline bool EpsilonCompare(const FloatT d1, const FloatT d2)
+{
+    return (std::abs(d1 - d2) < std::numeric_limits<FloatT>::epsilon());
+}
+
+#endif