1 #ifndef STATIC_RTREE_HPP 2 #define STATIC_RTREE_HPP 3 4 #include "storage/tar_fwd.hpp" 5 6 #include "util/bearing.hpp" 7 #include "util/coordinate_calculation.hpp" 8 #include "util/deallocating_vector.hpp" 9 #include "util/exception.hpp" 10 #include "util/hilbert_value.hpp" 11 #include "util/integer_range.hpp" 12 #include "util/mmap_file.hpp" 13 #include "util/rectangle.hpp" 14 #include "util/typedefs.hpp" 15 #include "util/vector_view.hpp" 16 #include "util/web_mercator.hpp" 17 18 #include "osrm/coordinate.hpp" 19 20 #include "storage/shared_memory_ownership.hpp" 21 22 #include <boost/assert.hpp> 23 #include <boost/filesystem.hpp> 24 #include <boost/format.hpp> 25 #include <boost/iostreams/device/mapped_file.hpp> 26 27 #include <tbb/blocked_range.h> 28 #include <tbb/parallel_for.h> 29 #include <tbb/parallel_sort.h> 30 31 #include <algorithm> 32 #include <array> 33 #include <limits> 34 #include <memory> 35 #include <queue> 36 #include <string> 37 #include <vector> 38 39 namespace osrm 40 { 41 namespace util 42 { 43 template <class EdgeDataT, 44 storage::Ownership Ownership = storage::Ownership::Container, 45 std::uint32_t BRANCHING_FACTOR = 64, 46 std::uint32_t LEAF_PAGE_SIZE = 4096> 47 class StaticRTree; 48 49 namespace serialization 50 { 51 template <class EdgeDataT, 52 storage::Ownership Ownership, 53 std::uint32_t BRANCHING_FACTOR, 54 std::uint32_t LEAF_PAGE_SIZE> 55 inline void read(storage::tar::FileReader &reader, 56 const std::string &name, 57 util::StaticRTree<EdgeDataT, Ownership, BRANCHING_FACTOR, LEAF_PAGE_SIZE> &rtree); 58 59 template <class EdgeDataT, 60 storage::Ownership Ownership, 61 std::uint32_t BRANCHING_FACTOR, 62 std::uint32_t LEAF_PAGE_SIZE> 63 inline void 64 write(storage::tar::FileWriter &writer, 65 const std::string &name, 66 const util::StaticRTree<EdgeDataT, Ownership, BRANCHING_FACTOR, LEAF_PAGE_SIZE> &rtree); 67 } // namespace serialization 68 69 /*** 70 * Static RTree for serving nearest neighbour queries 71 * // All coordinates are pojected first to Web Mercator before the bounding boxes 72 * // are computed, this means the internal distance metric doesn not represent meters! 73 */ 74 75 template <class EdgeDataT, 76 storage::Ownership Ownership, 77 std::uint32_t BRANCHING_FACTOR, 78 std::uint32_t LEAF_PAGE_SIZE> 79 class StaticRTree 80 { 81 /********************************************************** 82 * Example RTree construction: 83 * 84 * 30 elements (EdgeDataT objects) 85 * LEAF_NODE_SIZE = 3 86 * BRANCHING_FACTOR = 2 87 * 88 * 012 345 678 901 234 567 890 123 456 789 <- EdgeDataT objects in .fileIndex data, sorted by 89 * \|/ \|/ \|/ \|/ \|/ \|/ \|/ \|/ \|/ \|/ Hilbert Code of the centroid coordinate 90 * A B C D E F G H I J <- Everything from here down is a Rectangle in 91 * \ / \ / \ / \ / \ / .ramIndex 92 * K L M N O 93 * \ / \ / / 94 * \ / \ / / 95 * \ / \ / / 96 * P Q R 97 * \ / / 98 * \ / / 99 * \ / / 100 * \ / / 101 * \ / / 102 * \ / / 103 * \ / / 104 * U V 105 * \ / 106 * \ / 107 * \ / 108 * W 109 * 110 * Step 1 - objects 01234567... are sorted by Hilbert code (these are the line 111 * segments of the OSM roads) 112 * Step 2 - we grab LEAF_NODE_SIZE of them at a time and create TreeNode A with a 113 * bounding-box that surrounds the first LEAF_NODE_SIZE objects 114 * Step 2a- continue grabbing LEAF_NODE_SIZE objects, creating TreeNodes B,C,D,E...J 115 * until we run out of objects. The last TreeNode J may not have 116 * LEAF_NODE_SIZE entries. Our math later on caters for this. 117 * Step 3 - Now start grabbing nodes from A..J in groups of BRANCHING_FACTOR, 118 * and create K..O with bounding boxes surrounding the groups of 119 * BRANCHING_FACTOR. Again, O, the last entry, may have fewer than 120 * BRANCHING_FACTOR entries. 121 * Step 3a- Repeat this process for each level, until you only create 1 TreeNode 122 * to contain its children (in this case, W). 123 * 124 * As we create TreeNodes, we append them to the m_search_tree vector. 125 * 126 * After this part of the building process, m_search_tree will contain TreeNode 127 * objects in this order: 128 * 129 * ABCDEFGHIJ KLMNO PQR UV W 130 * 10 5 3 2 1 <- number of nodes in the level 131 * 132 * In order to make our math easy later on, we reverse the whole array, 133 * then reverse the nodes within each level: 134 * 135 * Reversed: W VU RQP ONMKL JIHGFEDCBA 136 * Levels reversed: W UV PQR KLMNO ABCDEFGHIJ 137 * 138 * We also now have the following information: 139 * 140 * level sizes = {1,2,3,5,10} 141 * 142 * and we can calculate the array position the nodes for each level 143 * start (based on the sum of the previous level sizes): 144 * 145 * level starts = {0,1,3,6,11} 146 * 147 * Now, some basic math can be used to navigate around the tree. See 148 * the body of the `child_indexes` function for the details. 149 * 150 ***********************************************/ 151 template <typename T> using Vector = ViewOrVector<T, Ownership>; 152 153 public: 154 using Rectangle = RectangleInt2D; 155 using EdgeData = EdgeDataT; 156 using CoordinateList = Vector<util::Coordinate>; 157 158 static_assert(LEAF_PAGE_SIZE >= sizeof(EdgeDataT), "page size is too small"); 159 static_assert(((LEAF_PAGE_SIZE - 1) & LEAF_PAGE_SIZE) == 0, "page size is not a power of 2"); 160 static constexpr std::uint32_t LEAF_NODE_SIZE = (LEAF_PAGE_SIZE / sizeof(EdgeDataT)); 161 162 struct CandidateSegment 163 { 164 Coordinate fixed_projected_coordinate; 165 EdgeDataT data; 166 }; 167 168 /** 169 * Represents a node position somewhere in our tree. This is purely a navigation 170 * class used to find children of each node - the actual data for each node 171 * is in the m_search_tree vector of TreeNode objects. 172 */ 173 struct TreeIndex 174 { TreeIndexosrm::util::StaticRTree::TreeIndex175 TreeIndex() : level(0), offset(0) {} TreeIndexosrm::util::StaticRTree::TreeIndex176 TreeIndex(std::uint32_t level_, std::uint32_t offset_) : level(level_), offset(offset_) {} 177 std::uint32_t level; // Which level of the tree is this node in 178 std::uint32_t offset; // Which node on this level is this (0=leftmost) 179 }; 180 181 /** 182 * An actual node in the tree. It's pretty minimal, we use the TreeIndex 183 * classes to navigate around. The TreeNode is packed into m_search_tree 184 * in a specific order so we can calculate positions of children 185 * (see the children_indexes function) 186 */ 187 struct TreeNode 188 { 189 Rectangle minimum_bounding_rectangle; 190 }; 191 192 private: 193 /** 194 * A lightweight wrapper for the Hilbert Code for each EdgeDataT object 195 * A vector of these is used to sort the EdgeDataT input onto the 196 * Hilbert Curve. 197 * The sorting doesn't modify the original array, so this struct 198 * maintains a pointer to the original index position (m_original_index) 199 * so we can fetch the original data from the sorted position. 200 */ 201 struct WrappedInputElement 202 { WrappedInputElementosrm::util::StaticRTree::WrappedInputElement203 explicit WrappedInputElement(const uint64_t _hilbert_value, 204 const std::uint32_t _original_index) 205 : m_hilbert_value(_hilbert_value), m_original_index(_original_index) 206 { 207 } 208 WrappedInputElementosrm::util::StaticRTree::WrappedInputElement209 WrappedInputElement() : m_hilbert_value(0), m_original_index(UINT_MAX) {} 210 211 uint64_t m_hilbert_value; 212 std::uint32_t m_original_index; 213 operator <osrm::util::StaticRTree::WrappedInputElement214 inline bool operator<(const WrappedInputElement &other) const 215 { 216 return m_hilbert_value < other.m_hilbert_value; 217 } 218 }; 219 220 struct QueryCandidate 221 { QueryCandidateosrm::util::StaticRTree::QueryCandidate222 QueryCandidate(std::uint64_t squared_min_dist, TreeIndex tree_index) 223 : squared_min_dist(squared_min_dist), tree_index(tree_index), 224 segment_index(std::numeric_limits<std::uint32_t>::max()) 225 { 226 } 227 QueryCandidateosrm::util::StaticRTree::QueryCandidate228 QueryCandidate(std::uint64_t squared_min_dist, 229 TreeIndex tree_index, 230 std::uint32_t segment_index, 231 const Coordinate &coordinate) 232 : squared_min_dist(squared_min_dist), tree_index(tree_index), 233 fixed_projected_coordinate(coordinate), segment_index(segment_index) 234 { 235 } 236 is_segmentosrm::util::StaticRTree::QueryCandidate237 inline bool is_segment() const 238 { 239 return segment_index != std::numeric_limits<std::uint32_t>::max(); 240 } 241 operator <osrm::util::StaticRTree::QueryCandidate242 inline bool operator<(const QueryCandidate &other) const 243 { 244 // Attn: this is reversed order. std::priority_queue is a 245 // max pq (biggest item at the front)! 246 return other.squared_min_dist < squared_min_dist; 247 } 248 249 std::uint64_t squared_min_dist; 250 TreeIndex tree_index; 251 Coordinate fixed_projected_coordinate; 252 std::uint32_t segment_index; 253 }; 254 255 // Representation of the in-memory search tree 256 Vector<TreeNode> m_search_tree; 257 // Reference to the actual lon/lat data we need for doing math 258 util::vector_view<const Coordinate> m_coordinate_list; 259 // Holds the start indexes of each level in m_search_tree 260 Vector<std::uint64_t> m_tree_level_starts; 261 // mmap'd .fileIndex file 262 boost::iostreams::mapped_file_source m_objects_region; 263 // This is a view of the EdgeDataT data mmap'd from the .fileIndex file 264 util::vector_view<const EdgeDataT> m_objects; 265 266 public: 267 StaticRTree() = default; 268 StaticRTree(const StaticRTree &) = delete; 269 StaticRTree &operator=(const StaticRTree &) = delete; 270 StaticRTree(StaticRTree &&) = default; 271 StaticRTree &operator=(StaticRTree &&) = default; 272 273 // Construct a packed Hilbert-R-Tree with Kamel-Faloutsos algorithm [1] StaticRTree(const std::vector<EdgeDataT> & input_data_vector,const Vector<Coordinate> & coordinate_list,const boost::filesystem::path & on_disk_file_name)274 explicit StaticRTree(const std::vector<EdgeDataT> &input_data_vector, 275 const Vector<Coordinate> &coordinate_list, 276 const boost::filesystem::path &on_disk_file_name) 277 : m_coordinate_list(coordinate_list.data(), coordinate_list.size()) 278 { 279 const auto element_count = input_data_vector.size(); 280 std::vector<WrappedInputElement> input_wrapper_vector(element_count); 281 282 // Step 1 - create a vector of Hilbert Code/original position pairs 283 tbb::parallel_for( 284 tbb::blocked_range<uint64_t>(0, element_count), 285 [&input_data_vector, &input_wrapper_vector, this]( 286 const tbb::blocked_range<uint64_t> &range) { 287 for (uint64_t element_counter = range.begin(), end = range.end(); 288 element_counter != end; 289 ++element_counter) 290 { 291 WrappedInputElement ¤t_wrapper = input_wrapper_vector[element_counter]; 292 current_wrapper.m_original_index = element_counter; 293 294 EdgeDataT const ¤t_element = input_data_vector[element_counter]; 295 296 // Get Hilbert-Value for centroid in mercartor projection 297 BOOST_ASSERT(current_element.u < m_coordinate_list.size()); 298 BOOST_ASSERT(current_element.v < m_coordinate_list.size()); 299 300 Coordinate current_centroid = coordinate_calculation::centroid( 301 m_coordinate_list[current_element.u], m_coordinate_list[current_element.v]); 302 current_centroid.lat = FixedLatitude{static_cast<std::int32_t>( 303 COORDINATE_PRECISION * 304 web_mercator::latToY(toFloating(current_centroid.lat)))}; 305 306 current_wrapper.m_hilbert_value = GetHilbertCode(current_centroid); 307 } 308 }); 309 310 // sort the hilbert-value representatives 311 tbb::parallel_sort(input_wrapper_vector.begin(), input_wrapper_vector.end()); 312 { 313 boost::iostreams::mapped_file out_objects_region; 314 auto out_objects = mmapFile<EdgeDataT>(on_disk_file_name, 315 out_objects_region, 316 input_data_vector.size() * sizeof(EdgeDataT)); 317 318 // Note, we can't just write everything in one go, because the input_data_vector 319 // is not sorted by hilbert code, only the input_wrapper_vector is in the correct 320 // order. Instead, we iterate over input_wrapper_vector, copy the hilbert-indexed 321 // entries from input_data_vector into a temporary contiguous array, then write 322 // that array to disk. 323 324 // Create the first level of TreeNodes - each bounding LEAF_NODE_COUNT EdgeDataT 325 // objects. 326 std::size_t wrapped_element_index = 0; 327 auto objects_iter = out_objects.begin(); 328 329 while (wrapped_element_index < element_count) 330 { 331 TreeNode current_node; 332 333 // Loop over the next block of EdgeDataT, calculate the bounding box 334 // for the block, and save the data to write to disk in the correct 335 // order. 336 for (std::uint32_t object_index = 0; 337 object_index < LEAF_NODE_SIZE && wrapped_element_index < element_count; 338 ++object_index, ++wrapped_element_index) 339 { 340 const std::uint32_t input_object_index = 341 input_wrapper_vector[wrapped_element_index].m_original_index; 342 const EdgeDataT &object = input_data_vector[input_object_index]; 343 344 *objects_iter++ = object; 345 346 Coordinate projected_u{ 347 web_mercator::fromWGS84(Coordinate{m_coordinate_list[object.u]})}; 348 Coordinate projected_v{ 349 web_mercator::fromWGS84(Coordinate{m_coordinate_list[object.v]})}; 350 351 BOOST_ASSERT(std::abs(toFloating(projected_u.lon).operator double()) <= 180.); 352 BOOST_ASSERT(std::abs(toFloating(projected_u.lat).operator double()) <= 180.); 353 BOOST_ASSERT(std::abs(toFloating(projected_v.lon).operator double()) <= 180.); 354 BOOST_ASSERT(std::abs(toFloating(projected_v.lat).operator double()) <= 180.); 355 356 Rectangle rectangle; 357 rectangle.min_lon = 358 std::min(rectangle.min_lon, std::min(projected_u.lon, projected_v.lon)); 359 rectangle.max_lon = 360 std::max(rectangle.max_lon, std::max(projected_u.lon, projected_v.lon)); 361 362 rectangle.min_lat = 363 std::min(rectangle.min_lat, std::min(projected_u.lat, projected_v.lat)); 364 rectangle.max_lat = 365 std::max(rectangle.max_lat, std::max(projected_u.lat, projected_v.lat)); 366 367 BOOST_ASSERT(rectangle.IsValid()); 368 current_node.minimum_bounding_rectangle.MergeBoundingBoxes(rectangle); 369 } 370 371 m_search_tree.emplace_back(current_node); 372 } 373 } 374 // mmap as read-only now 375 m_objects = mmapFile<EdgeDataT>(on_disk_file_name, m_objects_region); 376 377 // Should hold the number of nodes at the lowest level of the graph (closest 378 // to the data) 379 std::uint32_t nodes_in_previous_level = m_search_tree.size(); 380 381 // Holds the number of TreeNodes in each level. 382 // We always start with the root node, so 383 // m_tree_level_sizes[0] should always be 1 384 std::vector<std::uint64_t> tree_level_sizes; 385 tree_level_sizes.push_back(nodes_in_previous_level); 386 387 // Now, repeatedly create levels of nodes that contain BRANCHING_FACTOR 388 // nodes from the previous level. 389 while (nodes_in_previous_level > 1) 390 { 391 auto previous_level_start_pos = m_search_tree.size() - nodes_in_previous_level; 392 393 // We can calculate how many nodes will be in this level, we divide by 394 // BRANCHING_FACTOR 395 // and round up 396 std::uint32_t nodes_in_current_level = 397 std::ceil(static_cast<double>(nodes_in_previous_level) / BRANCHING_FACTOR); 398 399 for (auto current_node_idx : irange<std::size_t>(0, nodes_in_current_level)) 400 { 401 TreeNode parent_node; 402 auto first_child_index = 403 current_node_idx * BRANCHING_FACTOR + previous_level_start_pos; 404 auto last_child_index = 405 first_child_index + 406 std::min<std::size_t>(BRANCHING_FACTOR, 407 nodes_in_previous_level - 408 current_node_idx * BRANCHING_FACTOR); 409 410 // Calculate the bounding box for BRANCHING_FACTOR nodes in the previous 411 // level, then save that box as a new TreeNode in the new level. 412 for (auto child_node_idx : irange<std::size_t>(first_child_index, last_child_index)) 413 { 414 parent_node.minimum_bounding_rectangle.MergeBoundingBoxes( 415 m_search_tree[child_node_idx].minimum_bounding_rectangle); 416 } 417 m_search_tree.emplace_back(parent_node); 418 } 419 nodes_in_previous_level = nodes_in_current_level; 420 tree_level_sizes.push_back(nodes_in_previous_level); 421 } 422 // At this point, we've got our tree built, but the nodes are in a weird order. 423 // Next thing we'll do is flip it around so that we don't end up with a lot of 424 // `size - n` math later on. 425 426 // Flip the tree so that the root node is at 0. 427 // This just makes our math during search a bit more intuitive 428 std::reverse(m_search_tree.begin(), m_search_tree.end()); 429 430 // Same for the level sizes - root node / base level is at 0 431 std::reverse(tree_level_sizes.begin(), tree_level_sizes.end()); 432 433 // The first level starts at 0 434 m_tree_level_starts = {0}; 435 // The remaining levels start at the partial sum of the preceeding level sizes 436 std::partial_sum(tree_level_sizes.begin(), 437 tree_level_sizes.end(), 438 std::back_inserter(m_tree_level_starts)); 439 BOOST_ASSERT(m_tree_level_starts.size() >= 2); 440 441 // Now we have to flip the coordinates within each level so that math is easier 442 // later on. The workflow here is: 443 // The initial order of tree nodes in the m_search_tree array is roughly: 444 // 6789 345 12 0 (each block here is a level of the tree) 445 // Then we reverse it and get: 446 // 0 21 543 9876 447 // Now the loop below reverses each level to give us the final result 448 // 0 12 345 6789 449 // This ordering keeps the position math easy to understand during later 450 // searches 451 for (auto i : irange<std::size_t>(0, tree_level_sizes.size())) 452 { 453 std::reverse(m_search_tree.begin() + m_tree_level_starts[i], 454 m_search_tree.begin() + m_tree_level_starts[i] + tree_level_sizes[i]); 455 } 456 } 457 458 /** 459 * Constructs an empty RTree for de-serialization. 460 */ 461 template <typename = std::enable_if<Ownership == storage::Ownership::Container>> StaticRTree(const boost::filesystem::path & on_disk_file_name,const Vector<Coordinate> & coordinate_list)462 explicit StaticRTree(const boost::filesystem::path &on_disk_file_name, 463 const Vector<Coordinate> &coordinate_list) 464 : m_coordinate_list(coordinate_list.data(), coordinate_list.size()) 465 { 466 m_objects = mmapFile<EdgeDataT>(on_disk_file_name, m_objects_region); 467 } 468 469 /** 470 * Constructs an r-tree from blocks of memory loaded by someone else 471 * (usually a shared memory block created by osrm-datastore) 472 * These memory blocks basically just contain the files read into RAM, 473 * excep the .fileIndex file always stays on disk, and we mmap() it as usual 474 */ StaticRTree(Vector<TreeNode> search_tree_,Vector<std::uint64_t> tree_level_starts,const boost::filesystem::path & on_disk_file_name,const Vector<Coordinate> & coordinate_list)475 explicit StaticRTree(Vector<TreeNode> search_tree_, 476 Vector<std::uint64_t> tree_level_starts, 477 const boost::filesystem::path &on_disk_file_name, 478 const Vector<Coordinate> &coordinate_list) 479 : m_search_tree(std::move(search_tree_)), 480 m_coordinate_list(coordinate_list.data(), coordinate_list.size()), 481 m_tree_level_starts(std::move(tree_level_starts)) 482 { 483 BOOST_ASSERT(m_tree_level_starts.size() >= 2); 484 m_objects = mmapFile<EdgeDataT>(on_disk_file_name, m_objects_region); 485 } 486 487 /* Returns all features inside the bounding box. 488 Rectangle needs to be projected!*/ SearchInBox(const Rectangle & search_rectangle) const489 std::vector<EdgeDataT> SearchInBox(const Rectangle &search_rectangle) const 490 { 491 const Rectangle projected_rectangle{ 492 search_rectangle.min_lon, 493 search_rectangle.max_lon, 494 toFixed(FloatLatitude{ 495 web_mercator::latToY(toFloating(FixedLatitude(search_rectangle.min_lat)))}), 496 toFixed(FloatLatitude{ 497 web_mercator::latToY(toFloating(FixedLatitude(search_rectangle.max_lat)))})}; 498 std::vector<EdgeDataT> results; 499 500 std::queue<TreeIndex> traversal_queue; 501 traversal_queue.push(TreeIndex{}); 502 503 while (!traversal_queue.empty()) 504 { 505 auto const current_tree_index = traversal_queue.front(); 506 traversal_queue.pop(); 507 508 // If we're at the bottom of the tree, we need to explore the 509 // element array 510 if (is_leaf(current_tree_index)) 511 { 512 513 // Note: irange is [start,finish), so we need to +1 to make sure we visit the 514 // last 515 for (const auto current_child_index : child_indexes(current_tree_index)) 516 { 517 const auto ¤t_edge = m_objects[current_child_index]; 518 519 // we don't need to project the coordinates here, 520 // because we use the unprojected rectangle to test against 521 const Rectangle bbox{std::min(m_coordinate_list[current_edge.u].lon, 522 m_coordinate_list[current_edge.v].lon), 523 std::max(m_coordinate_list[current_edge.u].lon, 524 m_coordinate_list[current_edge.v].lon), 525 std::min(m_coordinate_list[current_edge.u].lat, 526 m_coordinate_list[current_edge.v].lat), 527 std::max(m_coordinate_list[current_edge.u].lat, 528 m_coordinate_list[current_edge.v].lat)}; 529 530 // use the _unprojected_ input rectangle here 531 if (bbox.Intersects(search_rectangle)) 532 { 533 results.push_back(current_edge); 534 } 535 } 536 } 537 else 538 { 539 BOOST_ASSERT(current_tree_index.level + 1 < m_tree_level_starts.size()); 540 541 for (const auto child_index : child_indexes(current_tree_index)) 542 { 543 const auto &child_rectangle = 544 m_search_tree[child_index].minimum_bounding_rectangle; 545 546 if (child_rectangle.Intersects(projected_rectangle)) 547 { 548 traversal_queue.push(TreeIndex( 549 current_tree_index.level + 1, 550 child_index - m_tree_level_starts[current_tree_index.level + 1])); 551 } 552 } 553 } 554 } 555 return results; 556 } 557 558 // Override filter and terminator for the desired behaviour. Nearest(const Coordinate input_coordinate,const std::size_t max_results) const559 std::vector<EdgeDataT> Nearest(const Coordinate input_coordinate, 560 const std::size_t max_results) const 561 { 562 return Nearest( 563 input_coordinate, 564 [](const CandidateSegment &) { return std::make_pair(true, true); }, 565 [max_results](const std::size_t num_results, const CandidateSegment &) { 566 return num_results >= max_results; 567 }); 568 } 569 570 // Override filter and terminator for the desired behaviour. 571 template <typename FilterT, typename TerminationT> Nearest(const Coordinate input_coordinate,const FilterT filter,const TerminationT terminate) const572 std::vector<EdgeDataT> Nearest(const Coordinate input_coordinate, 573 const FilterT filter, 574 const TerminationT terminate) const 575 { 576 std::vector<EdgeDataT> results; 577 auto projected_coordinate = web_mercator::fromWGS84(input_coordinate); 578 Coordinate fixed_projected_coordinate{projected_coordinate}; 579 // initialize queue with root element 580 std::priority_queue<QueryCandidate> traversal_queue; 581 traversal_queue.push(QueryCandidate{0, TreeIndex{}}); 582 583 while (!traversal_queue.empty()) 584 { 585 QueryCandidate current_query_node = traversal_queue.top(); 586 traversal_queue.pop(); 587 588 const TreeIndex ¤t_tree_index = current_query_node.tree_index; 589 if (!current_query_node.is_segment()) 590 { // current object is a tree node 591 if (is_leaf(current_tree_index)) 592 { 593 ExploreLeafNode(current_tree_index, 594 fixed_projected_coordinate, 595 projected_coordinate, 596 traversal_queue); 597 } 598 else 599 { 600 ExploreTreeNode( 601 current_tree_index, fixed_projected_coordinate, traversal_queue); 602 } 603 } 604 else 605 { // current candidate is an actual road segment 606 // We deliberatly make a copy here, we mutate the value below 607 auto edge_data = m_objects[current_query_node.segment_index]; 608 const auto ¤t_candidate = 609 CandidateSegment{current_query_node.fixed_projected_coordinate, edge_data}; 610 611 // to allow returns of no-results if too restrictive filtering, this needs to be 612 // done here even though performance would indicate that we want to stop after 613 // adding the first candidate 614 if (terminate(results.size(), current_candidate)) 615 { 616 break; 617 } 618 619 auto use_segment = filter(current_candidate); 620 if (!use_segment.first && !use_segment.second) 621 { 622 continue; 623 } 624 edge_data.forward_segment_id.enabled &= use_segment.first; 625 edge_data.reverse_segment_id.enabled &= use_segment.second; 626 627 // store phantom node in result vector 628 results.push_back(std::move(edge_data)); 629 } 630 } 631 632 return results; 633 } 634 635 private: 636 /** 637 * Iterates over all the objects in a leaf node and inserts them into our 638 * search priority queue. The speed of this function is very much governed 639 * by the value of LEAF_NODE_SIZE, as we'll calculate the euclidean distance 640 * for every child of each leaf node visited. 641 */ 642 template <typename QueueT> ExploreLeafNode(const TreeIndex & leaf_id,const Coordinate & projected_input_coordinate_fixed,const FloatCoordinate & projected_input_coordinate,QueueT & traversal_queue) const643 void ExploreLeafNode(const TreeIndex &leaf_id, 644 const Coordinate &projected_input_coordinate_fixed, 645 const FloatCoordinate &projected_input_coordinate, 646 QueueT &traversal_queue) const 647 { 648 // Check that we're actually looking at the bottom level of the tree 649 BOOST_ASSERT(is_leaf(leaf_id)); 650 651 for (const auto i : child_indexes(leaf_id)) 652 { 653 const auto ¤t_edge = m_objects[i]; 654 655 const auto projected_u = web_mercator::fromWGS84(m_coordinate_list[current_edge.u]); 656 const auto projected_v = web_mercator::fromWGS84(m_coordinate_list[current_edge.v]); 657 658 FloatCoordinate projected_nearest; 659 std::tie(std::ignore, projected_nearest) = 660 coordinate_calculation::projectPointOnSegment( 661 projected_u, projected_v, projected_input_coordinate); 662 663 const auto squared_distance = coordinate_calculation::squaredEuclideanDistance( 664 projected_input_coordinate_fixed, projected_nearest); 665 // distance must be non-negative 666 BOOST_ASSERT(0. <= squared_distance); 667 BOOST_ASSERT(i < std::numeric_limits<std::uint32_t>::max()); 668 traversal_queue.push(QueryCandidate{squared_distance, 669 leaf_id, 670 static_cast<std::uint32_t>(i), 671 Coordinate{projected_nearest}}); 672 } 673 } 674 675 /** 676 * Iterates over all the children of a TreeNode and inserts them into the search 677 * priority queue using their distance from the search coordinate as the 678 * priority metric. 679 * The closests distance to a box from our point is also the closest distance 680 * to the closest line in that box (assuming the boxes hug their contents). 681 */ 682 template <class QueueT> ExploreTreeNode(const TreeIndex & parent,const Coordinate & fixed_projected_input_coordinate,QueueT & traversal_queue) const683 void ExploreTreeNode(const TreeIndex &parent, 684 const Coordinate &fixed_projected_input_coordinate, 685 QueueT &traversal_queue) const 686 { 687 // Figure out which_id level the parent is on, and it's offset 688 // in that level. 689 // Check that we're actually looking at the bottom level of the tree 690 BOOST_ASSERT(!is_leaf(parent)); 691 692 for (const auto child_index : child_indexes(parent)) 693 { 694 const auto &child = m_search_tree[child_index]; 695 696 const auto squared_lower_bound_to_element = 697 child.minimum_bounding_rectangle.GetMinSquaredDist( 698 fixed_projected_input_coordinate); 699 700 traversal_queue.push(QueryCandidate{ 701 squared_lower_bound_to_element, 702 TreeIndex(parent.level + 1, child_index - m_tree_level_starts[parent.level + 1])}); 703 } 704 } 705 GetLevelSize(const std::size_t level) const706 std::uint64_t GetLevelSize(const std::size_t level) const 707 { 708 BOOST_ASSERT(m_tree_level_starts.size() > level + 1); 709 BOOST_ASSERT(m_tree_level_starts[level + 1] >= m_tree_level_starts[level]); 710 return m_tree_level_starts[level + 1] - m_tree_level_starts[level]; 711 } 712 713 /** 714 * Calculates the absolute position of child data in our packed data 715 * vectors. 716 * 717 * when given a TreeIndex that is a leaf node (i.e. at the bottom of the tree), 718 * this function returns indexes valid for `m_objects` 719 * 720 * otherwise, the indexes are to be used with m_search_tree to iterate over 721 * the children of `parent` 722 * 723 * This function assumes we pack nodes as described in the big comment 724 * at the top of this class. All nodes are fully filled except for the last 725 * one in each level. 726 */ child_indexes(const TreeIndex & parent) const727 range<std::size_t> child_indexes(const TreeIndex &parent) const 728 { 729 // If we're looking at a leaf node, the index is from 0 to m_objects.size(), 730 // there is only 1 level of object data in the m_objects array 731 if (is_leaf(parent)) 732 { 733 const std::uint64_t first_child_index = parent.offset * LEAF_NODE_SIZE; 734 const std::uint64_t end_child_index = std::min( 735 first_child_index + LEAF_NODE_SIZE, static_cast<std::uint64_t>(m_objects.size())); 736 737 BOOST_ASSERT(first_child_index < std::numeric_limits<std::uint32_t>::max()); 738 BOOST_ASSERT(end_child_index < std::numeric_limits<std::uint32_t>::max()); 739 BOOST_ASSERT(end_child_index <= m_objects.size()); 740 741 return irange<std::size_t>(first_child_index, end_child_index); 742 } 743 else 744 { 745 const std::uint64_t first_child_index = 746 m_tree_level_starts[parent.level + 1] + parent.offset * BRANCHING_FACTOR; 747 748 const std::uint64_t end_child_index = 749 std::min(first_child_index + BRANCHING_FACTOR, 750 m_tree_level_starts[parent.level + 1] + GetLevelSize(parent.level + 1)); 751 BOOST_ASSERT(first_child_index < std::numeric_limits<std::uint32_t>::max()); 752 BOOST_ASSERT(end_child_index < std::numeric_limits<std::uint32_t>::max()); 753 BOOST_ASSERT(end_child_index <= m_search_tree.size()); 754 BOOST_ASSERT(end_child_index <= 755 m_tree_level_starts[parent.level + 1] + GetLevelSize(parent.level + 1)); 756 return irange<std::size_t>(first_child_index, end_child_index); 757 } 758 } 759 is_leaf(const TreeIndex & treeindex) const760 bool is_leaf(const TreeIndex &treeindex) const 761 { 762 BOOST_ASSERT(m_tree_level_starts.size() >= 2); 763 return treeindex.level == m_tree_level_starts.size() - 2; 764 } 765 766 friend void serialization::read<EdgeDataT, Ownership, BRANCHING_FACTOR, LEAF_PAGE_SIZE>( 767 storage::tar::FileReader &reader, const std::string &name, StaticRTree &rtree); 768 769 friend void serialization::write<EdgeDataT, Ownership, BRANCHING_FACTOR, LEAF_PAGE_SIZE>( 770 storage::tar::FileWriter &writer, const std::string &name, const StaticRTree &rtree); 771 }; 772 773 //[1] "On Packing R-Trees"; I. Kamel, C. Faloutsos; 1993; DOI: 10.1145/170088.170403 774 //[2] "Nearest Neighbor Queries", N. Roussopulos et al; 1995; DOI: 10.1145/223784.223794 775 //[3] "Distance Browsing in Spatial Databases"; G. Hjaltason, H. Samet; 1999; ACM Trans. DB Sys 776 // Vol.24 No.2, pp.265-318 777 } // namespace util 778 } // namespace osrm 779 780 #endif // STATIC_RTREE_HPP 781