diff --git a/DataStructures/StaticRTree.h b/DataStructures/StaticRTree.h index ab711f51b..1e7eba625 100644 --- a/DataStructures/StaticRTree.h +++ b/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> &result_phantom_node_vector, + const unsigned zoom_level, + const unsigned number_of_results, + const unsigned max_checked_segments = 4*LEAF_NODE_SIZE) + { + std::vector min_found_distances(number_of_results, std::numeric_limits::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 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(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 ¤t_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(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{}; + } + } + + 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 inline bool EpsilonCompare(const FloatT d1, const FloatT d2) const - { - return (std::abs(d1 - d2) < std::numeric_limits::epsilon()); - } }; //[1] "On Packing R-Trees"; I. Kamel, C. Faloutsos; 1993; DOI: 10.1145/170088.170403 diff --git a/UnitTests/DataStructures/StaticRTreeTest.cpp b/UnitTests/DataStructures/StaticRTreeTest.cpp index 55aeb9e34..df0f0e8c0 100644 --- a/UnitTests/DataStructures/StaticRTreeTest.cpp +++ b/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 + #include #include #include @@ -156,12 +158,6 @@ public: return result_phantom_node.location.isValid(); } - template - inline bool EpsilonCompare(const FloatT d1, const FloatT d2) const - { - return (std::abs(d1 - d2) < std::numeric_limits::epsilon()); - } - private: const std::shared_ptr>& coords; const std::vector& edges; diff --git a/Util/NumericUtil.h b/Util/NumericUtil.h new file mode 100644 index 000000000..5853f8186 --- /dev/null +++ b/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 + +#include + +template inline bool EpsilonCompare(const FloatT d1, const FloatT d2) +{ + return (std::abs(d1 - d2) < std::numeric_limits::epsilon()); +} + +#endif