Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2001-2025 German Aerospace Center (DLR) and others.
4 : // This program and the accompanying materials are made available under the
5 : // terms of the Eclipse Public License 2.0 which is available at
6 : // https://www.eclipse.org/legal/epl-2.0/
7 : // This Source Code may also be made available under the following Secondary
8 : // Licenses when the conditions for such availability set forth in the Eclipse
9 : // Public License 2.0 are satisfied: GNU General Public License, version 2
10 : // or later which is available at
11 : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 : /****************************************************************************/
14 : /// @file KDTreePartition.h
15 : /// @author Ruediger Ebendt
16 : /// @date 01.11.2023
17 : ///
18 : // Class for partitioning the router's network's nodes wrt a k-d tree subdivision scheme.
19 : // All nodes are inserted at once at creation. Offers an O(log n) method to search a
20 : // node within the bottom / leaf cells (i.e., the cell containing the respective node
21 : // is returned). Here, n denotes the number of cells.
22 : /****************************************************************************/
23 : #pragma once
24 : #include <config.h>
25 : #include <vector>
26 : #include <unordered_set>
27 : #include <math.h>
28 : #include <cmath>
29 : #include <limits>
30 : #include <stdexcept>
31 : #include <string>
32 : #include <cinttypes>
33 : #include <utility>
34 :
35 : #define KDTP_EXCLUDE_INTERNAL_EDGES
36 : // Use KDTP_KEEP_OUTGOING_BOUNDARY_EDGES and KDTP_KEEP_BOUNDARY_FROM_NODES on a reversed graph to build arc infos
37 : // for the arc flag shortest path routing algorithm
38 : #define KDTP_KEEP_OUTGOING_BOUNDARY_EDGES
39 : // Use KDTP_KEEP_INCOMING_BOUNDARY_EDGES on a forward graph to apply arc flag shortest path routing algorithm
40 : #define KDTP_KEEP_INCOMING_BOUNDARY_EDGES
41 : //#define KDTP_KEEP_BOUNDARY_EDGES
42 : //#define KDTP_KEEP_BOUNDARY_NODES
43 : // Use KDTP_KEEP_BOUNDARY_FROM_NODES on a reversed graph to build arc infos
44 : // for the arc flag shortest path routing algorithm
45 : #define KDTP_KEEP_BOUNDARY_FROM_NODES
46 : //#define KDTP_KEEP_BOUNDARY_TO_NODES
47 : //#define KDTP_FOR_SYNTHETIC_NETWORKS
48 : // Use KDTP_WRITE_QGIS_FILTERS to filter boundary nodes of cells in QGIS operating on a (e.g. PostGreSQL) database of
49 : // the (e.g. OSM) network
50 : //#define KDTP_WRITE_QGIS_FILTERS
51 : #ifdef KDTP_WRITE_QGIS_FILTERS
52 : #include <fstream>
53 : #include <sstream>
54 : #endif
55 :
56 : //#define KDTP_DEBUG_LEVEL_0
57 : //#define KDTP_DEBUG_LEVEL_1
58 : //#define KDTP_DEBUG_LEVEL_2
59 :
60 : #ifdef KDTP_DEBUG_LEVEL_2
61 : #define KDTP_DEBUG_LEVEL_1
62 : #endif
63 :
64 : #ifdef KDTP_DEBUG_LEVEL_1
65 : #define KDTP_DEBUG_LEVEL_0
66 : #endif
67 :
68 : // uncomment to disable assert()
69 : // #define NDEBUG
70 : #include <cassert>
71 :
72 : // ===========================================================================
73 : // class definitions
74 : // ===========================================================================
75 :
76 : /**
77 : * @class KDTreePartition
78 : * @brief Partitions the router's network wrt a k-d tree subdivision scheme
79 : *
80 : * The template parameters are:
81 : * @param E The edge class to use (MSEdge/ROEdge)
82 : * @param N The node class to use (MSJunction/RONode)
83 : * @param V The vehicle class to use (MSVehicle/ROVehicle)
84 : */
85 : template<class E, class N, class V>
86 : class KDTreePartition {
87 : public:
88 : /// @brief Enum type for cell axis
89 : enum class Axis {
90 : X, Y
91 : };
92 :
93 : /**
94 : * @class NodeXComparator
95 : * Class to compare (and so sort) nodes by x-coordinate value
96 : */
97 : class NodeXComparator {
98 : public:
99 : /** @brief Comparing method for X-axis
100 : * @param[in] firstNode The first node
101 : * @param[in] secondNode The second node
102 : * @return true iff first node's x-coordinate is smaller than that of the second
103 : * @note In case of ties: true iff integer interpretation of first node's id is smaller than that of the second
104 : */
105 0 : bool operator()(const N* firstNode, const N* secondNode) const {
106 0 : if (firstNode->getPosition().x() == secondNode->getPosition().x()) { // tie
107 : std::string str = firstNode->getID();
108 0 : std::intmax_t firstValue = std::strtoimax(str.c_str(), nullptr, 10);
109 : str = secondNode->getID();
110 0 : std::intmax_t secondValue = std::strtoimax(str.c_str(), nullptr, 10);
111 0 : return firstValue < secondValue;
112 : }
113 0 : return firstNode->getPosition().x() < secondNode->getPosition().x();
114 : }
115 : };
116 :
117 : /**
118 : * @class NodeYComparator
119 : * Class to compare (and so sort) nodes by y-coordinate value
120 : */
121 : class NodeYComparator {
122 : public:
123 : /** @brief Comparing method for Y-axis
124 : * @param[in] firstNode The first node
125 : * @param[in] secondNode The second node
126 : * @return true iff first node's y-coordinate is smaller than that of the second
127 : * @note In case of ties: true iff integer interpretation of first node's id is smaller than that of the second
128 : */
129 0 : bool operator()(const N* firstNode, const N* secondNode) const {
130 0 : if (firstNode->getPosition().y() == secondNode->getPosition().y()) { // tie
131 : std::string str = firstNode->getID();
132 0 : std::intmax_t firstValue = std::strtoimax(str.c_str(), nullptr, 10);
133 : str = secondNode->getID();
134 0 : std::intmax_t secondValue = std::strtoimax(str.c_str(), nullptr, 10);
135 0 : return firstValue < secondValue;
136 : }
137 0 : return firstNode->getPosition().y() < secondNode->getPosition().y();
138 : }
139 : };
140 :
141 : /**
142 : * @class Cell
143 : * @brief Represents an element of the node partition (i.e. a node set)
144 : */
145 : class Cell {
146 : public:
147 : #ifdef KDTP_FOR_SYNTHETIC_NETWORKS
148 : /** @brief Constructor
149 : * @note Works recursively, builds the median-based k-d tree
150 : * @param[in] cells The vector of all cells
151 : * @param[in] sortedNodes The vector of nodes
152 : * @note Initially unsorted, gets sorted wrt to the divisional scheme of the k-dtree after instantiation of the k-d tree
153 : * @param[in] numberOfLevels The number of levels
154 : * @param[in] level The level
155 : * @param[in] levelCells The vector of all level cell vectors
156 : * @param[in] axis The axis (X or X)
157 : * @param[in] fromInclusive The from-index (inclusive)
158 : * @param[in] toExclusive The to-index (exclusive)
159 : * @param[in] supercell The supercell
160 : * @param[in] minX The minimum X-value of nodes in the cell
161 : * @param[in] maxX The maximum X-value of nodes in the cell
162 : * @param[in] minY The minimum Y-value of nodes in the cell
163 : * @param[in] maxY The maximum Y-value of nodes in the cell
164 : * @param[in] northernConflictNodes The northern spatial conflict nodes
165 : * @param[in] easternConflictNodes The eastern spatial conflict nodes
166 : * @param[in] southernConflictNodes The southern spatial conflict nodes
167 : * @param[in] westernConflictNodes The western spatial conflict nodes
168 : * @param[in] isLeftOrLowerCell Boolean flag indicating whether this cell is a left or lower cell or not
169 : * @param[in] vehicle The vehicle.
170 : * @param[in] havePermissions The boolean flag indicating whether edge permissions need to be considered.
171 : * @param[in] haveRestrictions The boolean flag indicating whether edge restrictions need to be considered.
172 : */
173 : Cell(std::vector<const Cell*>* cells, std::vector<std::vector<const Cell*>>* levelCells, std::vector<const N*>* sortedNodes, int numberOfLevels,
174 : int level, Axis axis, size_t fromInclusive, size_t toExclusive, Cell* supercell, double minX, double maxX, double minY, double maxY,
175 : std::unordered_set<const N*>* northernConflictNodes, std::unordered_set<const N*>* easternConflictNodes,
176 : std::unordered_set<const N*>* southernConflictNodes, std::unordered_set<const N*>* westernConflictNodes,
177 : bool isLeftOrLowerCell, const V* const vehicle, const bool havePermissions, const bool haveRestrictions);
178 : #else
179 : /** @brief Constructor
180 : * @note Works recursively, builds the median-based k-d tree
181 : * @param[in] cells The vector of all cells
182 : * @param[in] levelCells The vector of all level cell vectors
183 : * @param[in] sortedNodes The vector of nodes
184 : * @note Initially unsorted, after instantiation gets sorted wrt to the k-d tree subdivision scheme
185 : * @param[in] numberOfLevels The number of levels
186 : * @param[in] level The level
187 : * @param[in] axis The axis (X or X)
188 : * @param[in] fromInclusive The from-index (inclusive)
189 : * @param[in] toExclusive The to-index (exclusive)
190 : * @param[in] supercell The supercell
191 : * @param[in] minX The minimum X-value of nodes in the cell
192 : * @param[in] maxX The maximum X-value of nodes in the cell
193 : * @param[in] minY The minimum Y-value of nodes in the cell
194 : * @param[in] maxY The maximum Y-value of nodes in the cell
195 : * @param[in] isLeftOrLowerCell Boolean flag indicating whether this cell is a left or lower cell or not
196 : * @param[in] vehicle The vehicle
197 : * @param[in] havePermissions The boolean flag indicating whether edge permissions need to be considered or not
198 : * @param[in] haveRestrictions The boolean flag indicating whether edge restrictions need to be considered or not
199 : */
200 : Cell(std::vector<const Cell*>* cells, std::vector<std::vector<const Cell*>>* levelCells, std::vector<const N*>* sortedNodes,
201 : int numberOfLevels, int level, Axis axis, size_t fromInclusive, size_t toExclusive, Cell* supercell, double minX, double maxX,
202 : double minY, double maxY, bool isLeftOrLowerCell, const V* const vehicle, const bool havePermissions, const bool haveRestrictions);
203 : #endif
204 :
205 : /// @brief Destructor
206 0 : virtual ~Cell() {
207 0 : delete myLeftOrLowerSubcell;
208 0 : delete myRightOrUpperSubcell;
209 0 : }
210 :
211 : /// @brief Returns the axis of the cell's spatial extension (x or y)
212 : Axis getAxis() const {
213 0 : return myAxis;
214 : };
215 : /**
216 : * @brief Returns all edges situated inside the cell
217 : * @param[in] vehicle The vehicle
218 : */
219 : std::unordered_set<const E*>* edgeSet(const V* const vehicle) const;
220 : /**
221 : * @brief Returns the number of edges ending in the cell
222 : * @param[in] vehicle The vehicle.
223 : */
224 : size_t numberOfEdgesEndingInCell(const V* const vehicle) const;
225 : /**
226 : * @brief Returns the number of edges starting in the cell
227 : * @param[in] vehicle The vehicle.
228 : */
229 : size_t numberOfEdgesStartingInCell(const V* const vehicle) const;
230 : /// @brief Returns the cell's number
231 : int getNumber() const {
232 : return myNumber;
233 : }
234 : /// @brief Returns the cell's level
235 : // @note Level 0: root
236 : int getLevel() const {
237 : return myLevel;
238 : }
239 : /** @brief Returns the number of cells
240 : * @return The number of the cells
241 : */
242 : int getNumberOfCells() const {
243 : return cellCounter();
244 : }
245 : /** @brief Returns a pair of iterators (first, last) to iterate over the nodes of the cell
246 : * @return A pair of iterators (first, last) to iterate over the nodes of the cell
247 : */
248 : std::pair<typename std::vector<const N*>::const_iterator,
249 : typename std::vector<const N*>::const_iterator> nodeIterators() const;
250 : /** @brief Returns a new vector of nodes in the cell
251 : * @return A new vector of nodes in the cell
252 : */
253 : std::vector<const N*>* nodes() const;
254 : #ifdef KDTP_KEEP_BOUNDARY_NODES
255 : /// @brief Returns the vector of boundary nodes
256 : const std::vector<const N*>& getBoundaryNodes() const {
257 : return myBoundaryNodes;
258 : }
259 : #endif
260 : #ifdef KDTP_KEEP_BOUNDARY_FROM_NODES
261 : /// @brief Returns the vector of boundary nodes which are from-nodes of outgoing boundary edges
262 : const std::vector<const N*>& getBoundaryFromNodes() const {
263 : return myBoundaryFromNodes;
264 : }
265 : #endif
266 : #ifdef KDTP_KEEP_BOUNDARY_TO_NODES
267 : /// @brief Returns the vector of boundary nodes which are to-nodes of incoming boundary edges
268 : const std::vector<const N*>& getBoundaryToNodes() const {
269 : return myBoundaryToNodes;
270 : }
271 : #endif
272 : #ifdef KDTP_KEEP_BOUNDARY_EDGES
273 : /// @brief Returns the set of boundary edges
274 : const std::unordered_set<const E*>& getBoundaryEdges() const {
275 : return myBoundaryEdges;
276 : }
277 : #endif
278 : #ifdef KDTP_KEEP_INCOMING_BOUNDARY_EDGES
279 : /// @brief Returns the set of incoming boundary edges
280 : const std::unordered_set<const E*>& getIncomingBoundaryEdges() const {
281 : return myIncomingBoundaryEdges;
282 : }
283 : #endif
284 : #ifdef KDTP_KEEP_OUTGOING_BOUNDARY_EDGES
285 : /// @brief Returns the set of outgoing boundary edges
286 : const std::unordered_set<const E*>& getOutgoingBoundaryEdges() const {
287 0 : return myOutgoingBoundaryEdges;
288 : }
289 : #endif
290 : /// @brief Returns the left (cell's axis is X) or lower (cell's axis is Y) subcell
291 : const Cell* getLeftOrLowerSubcell() const {
292 0 : return myLeftOrLowerSubcell;
293 : }
294 : /// @brief Returns the right (cell's axis is X) or upper (cell's axis is Y) subcell
295 : const Cell* getRightOrUpperSubcell() const {
296 0 : return myRightOrUpperSubcell;
297 : }
298 : /// @brief Returns the supercell
299 : const Cell* getSupercell() const {
300 0 : return mySupercell;
301 : }
302 : /// @brief Returns the minimum coordinate of a cell node in X-direction (aka left border coordinate)
303 : double getMinX() const {
304 : return myMinX;
305 : }
306 : /// @brief Returns the maximum coordinate of a cell node in X-direction (aka right border coordinate)
307 : double getMaxX() const {
308 : return myMaxX;
309 : }
310 : /// @brief Returns the minimum coordinate of a cell node in Y-direction (aka lower border coordinate)
311 : double getMinY() const {
312 : return myMinY;
313 : }
314 : /// @brief Returns the maximum coordinate of a cell node in Y-direction (aka upper border coordinate)
315 : double getMaxY() const {
316 : return myMaxY;
317 : }
318 : /** @brief Tests whether the given node belongs to the cell
319 : * @param node The node
320 : * @return true iff the given node belongs to the cell
321 : */
322 : bool contains(const N* node) const;
323 : /// @brief Returns the boolean flag indicating whether this cell is a left or lower cell or not
324 : bool isLeftOrLowerCell() const {
325 0 : return myIsLeftOrLowerCell;
326 : }
327 : /// @brief Returns the median coordinate
328 : double getMedianCoordinate() const {
329 0 : return myMedianCoordinate;
330 : }
331 :
332 : private:
333 : /** @brief Performs one partition step on the set of nodes sorted wrt to the k-d tree subdivision scheme
334 : * @returns The median index
335 : */
336 : size_t partition();
337 : /** @brief Returns the global cell counter
338 : * @return The global cell counter
339 : */
340 :
341 : static int& cellCounter() {
342 : static int cellCounter{ 0 };
343 : return cellCounter;
344 : }
345 :
346 : /** @brief Returns true iff driving the given vehicle on the given edge is prohibited
347 : * @param[in] edge The edge
348 : * @param[in] vehicle The vehicle
349 : * @return true iff driving the given vehicle on the given edge is prohibited
350 : */
351 0 : bool isProhibited(const E* const edge, const V* const vehicle) const {
352 0 : return (myHavePermissions && edge->prohibits(vehicle)) || (myHaveRestrictions && edge->restricts(vehicle));
353 : }
354 : #ifdef KDTP_FOR_SYNTHETIC_NETWORKS
355 : /** @brief Returns a pair with sets of spatial conflict nodes for a) the left / lower subcell and b) the right / upper subcell
356 : * @param medianIndex The index into mySortedNodes dividing the cell nodes into two halves of equal size
357 : * @return A pair with sets of spatial conflict nodes for a) the left / lower subcell and b) the right / upper subcell
358 : */
359 : std::pair<std::unordered_set<const N*>*, std::unordered_set<const N*>*> spatialConflictNodes(size_t medianIndex) const;
360 : #endif
361 : /** @brief Tests whether a given node is situated within the spatial bounds of the cell
362 : * @param node The node
363 : * @return true iff a given node is situated within the spatial bounds of the cell
364 : */
365 : bool isInBounds(const N* node) const;
366 : /// @brief Completes the information about the spatial extent
367 : void completeSpatialInfo();
368 : /** @brief Returns the minimum coordinate of the cell nodes' positions, in the direction of the given axis (X or Y)
369 : * @param axis The axis
370 : * @return The minimum coordinate of the cell nodes' positions, in the direction of the given axis (X or Y)
371 : */
372 : double minAxisValue(Axis axis) const;
373 : /** @brief Returns the minimum coordinate of the cell nodes' positions, in the direction of the cell's axis (X or Y)
374 : * @return The minimum coordinate of the cell nodes' positions, in the direction of the cell's axis(X or Y)
375 : */
376 : double minAxisValue() const;
377 : /** @brief Returns the maximum coordinate of the cell nodes' positions, in the direction of the given axis (X or Y)
378 : * @param axis The axis
379 : * @return The maximum coordinate of the cell nodes' positions, in the direction of the given axis (X or Y)
380 : */
381 : double maxAxisValue(Axis axis) const;
382 : /** @brief Returns the maximum coordinate of the cell nodes' positions, in the direction of the cell's axis (X or Y)
383 : * @return The minimum coordinate of the cell nodes' positions, in the direction of the cell's axis(X or Y)
384 : */
385 : double maxAxisValue() const;
386 : /// @brief The cells
387 : std::vector<const Cell*>* myCells;
388 : /// @brief The cells of all partitions at all levels of the k-d tree subdivisional scheme
389 : std::vector<std::vector<const Cell*>>* myLevelCells;
390 : /// @brief The container with all nodes, sorted wrt to the k-d tree subdivisional scheme
391 : std::vector<const N*>* mySortedNodes;
392 : /// @brief The total number of levels of the k-d tree
393 : const int myNumberOfLevels;
394 : /// @brief The level
395 : const int myLevel;
396 : /// @brief The number
397 : const int myNumber;
398 : /// @brief The axis of the spatial extension
399 : const Axis myAxis;
400 : /// @brief The from-index (inclusive)
401 : const size_t myFromInclusive;
402 : /// @brief The to-index (exclusive)
403 : const size_t myToExclusive;
404 : #if defined(KDTP_KEEP_BOUNDARY_NODES) || defined(KDTP_WRITE_QGIS_FILTERS)
405 : /// @brief The nodes on the cell boundary
406 : std::vector<const N*> myBoundaryNodes;
407 : #endif
408 : #ifdef KDTP_KEEP_BOUNDARY_FROM_NODES
409 : /// @brief Those nodes on the cell boundary, which are from-nodes of outgoing boundary edges
410 : std::vector<const N*> myBoundaryFromNodes;
411 : #endif
412 : #ifdef KDTP_KEEP_BOUNDARY_TO_NODES
413 : /// @brief Those nodes on the cell boundary, which are to-nodes of incoming boundary edges
414 : std::vector<const N*> myBoundaryToNodes;
415 : #endif
416 : #ifdef KDTP_KEEP_INCOMING_BOUNDARY_EDGES
417 : /// @brief The incoming edges on the cell boundary
418 : std::unordered_set<const E*> myIncomingBoundaryEdges;
419 :
420 : #endif
421 : #ifdef KDTP_KEEP_OUTGOING_BOUNDARY_EDGES
422 : /// @brief The outgoing edges on the cell boundary
423 : // @note Used for arc flag setter's shortest path tree algorithm, working on a reversed graph
424 : std::unordered_set<const E*> myOutgoingBoundaryEdges;
425 : #endif
426 : #ifdef KDTP_KEEP_BOUNDARY_EDGES
427 : /// @brief The edges on the cell boundary
428 : std::unordered_set<const E*> myBoundaryEdges;
429 : #endif
430 : /// @brief The super cell
431 : Cell* mySupercell;
432 : /// @brief The left (cell's axis is X) or lower (cell's axis is Y) subcell
433 : Cell* myLeftOrLowerSubcell;
434 : /// @brief The right (cell's axis is X) or upper (cell's axis is Y) subcell
435 : Cell* myRightOrUpperSubcell;
436 : /// @brief The minimum x-value of a node in the cell
437 : double myMinX;
438 : /// @brief The maximum x-value of a node in the cell
439 : double myMaxX;
440 : /// @brief The minimum y-value of a node in the cell
441 : double myMinY;
442 : /// @brief The maximum y-value of a node in the cell
443 : double myMaxY;
444 : /// @brief The boolean flag indicating whether the information about the cell's spatial extent is complete or not
445 : bool myHasCompleteSpatialInfo;
446 : /// @brief The boolean flag indicating whether the cell's nodes are sorted wrt to the cell's axis or not
447 : bool myHasNodesSortedWrtToMyAxis;
448 : /// @brief The boolean flag indicating whether this cell is a left/lower cell or not
449 : bool myIsLeftOrLowerCell;
450 : #ifdef KDTP_FOR_SYNTHETIC_NETWORKS
451 : /// @brief The northern spatial conflict nodes
452 : std::unordered_set<const N*>* myNorthernConflictNodes;
453 : /// @brief The eastern spatial conflict nodes
454 : std::unordered_set<const N*>* myEasternConflictNodes;
455 : /// @brief The southern spatial conflict nodes
456 : std::unordered_set<const N*>* mySouthernConflictNodes;
457 : /// @brief The western spatial conflict nodes
458 : std::unordered_set<const N*>* myWesternConflictNodes;
459 : #endif
460 : /// @brief The boolean flag indicating whether edge permissions need to be considered or not
461 : const bool myHavePermissions;
462 : /// @brief The boolean flag indicating whether edge restrictions need to be considered or not
463 : const bool myHaveRestrictions;
464 : /// @brief The coordinate in the axis' direction of the node at the median index
465 : double myMedianCoordinate;
466 : }; // end of class Cell declaration
467 :
468 : /** @brief Constructor
469 : * @param[in] numberOfLevels The number of levels
470 : * @param[in] edges The container with all edges of the network
471 : * @param[in] havePermissions The boolean flag indicating whether edge permissions need to be considered or not
472 : * @param[in] haveRestrictions The boolean flag indicating whether edge restrictions need to be considered or not
473 : */
474 : KDTreePartition(int numberOfLevels, const std::vector<E*>& edges,
475 : const bool havePermissions, const bool haveRestrictions);
476 :
477 : /// @brief Destructor
478 0 : ~KDTreePartition() {
479 0 : std::vector<const N*>().swap(mySortedNodes);
480 0 : std::vector<const Cell*>().swap(myCells);
481 0 : std::vector<std::vector<const Cell*>>().swap(myLevelCells);
482 0 : delete myRoot;
483 0 : };
484 :
485 : /** @brief Initialize the k-d tree wrt to the given vehicle's permissions
486 : * @param[in] vehicle The vehicle
487 : */
488 : void init(const V* const vehicle);
489 : /** @brief Delete the k-d tree, and create a new one
490 : * param[in] vehicle The vehicle
491 : * @note Recreated wrt the network given at construction and the given edge
492 : */
493 0 : void reset(const V* const vehicle) {
494 0 : delete myRoot;
495 0 : init(vehicle);
496 0 : }
497 : /// @brief Returns the root of the k-d tree
498 : const Cell* getRoot() const {
499 : return myRoot;
500 : }
501 : /// @brief Returns all cells
502 : const std::vector<const Cell*>& getCells() {
503 : return myCells;
504 : }
505 : /** @brief Returns the cell with the given number
506 : * @note Returns nullptr, if no such cell exists
507 : * @param[in] number The cell number
508 : * @return The cell with the given number
509 : */
510 : const Cell* cell(int number) const;
511 : /// @brief Returns all cells of a level
512 : const std::vector<const Cell*>& getCellsAtLevel(int level) const {
513 0 : return myLevelCells[level];
514 : }
515 : /** @brief Returns the numbers of the cells which the given node is situated in
516 : * @param[in] node The node
517 : * @return The numbers of the cells which the given node is situated in
518 : */
519 : std::vector<int>* cellNumbers(const N* node) const;
520 : /** @brief Returns the conjugated 2D axis
521 : * @note X->Y / Y->X
522 : * @param[in] axis The axis
523 : * @return The conjugated 2D axis
524 : */
525 : static Axis flip(Axis axis) {
526 0 : return axis == Axis::X ? Axis::Y : Axis::X;
527 : }
528 : /** @brief Returns the number of arc flags required in a multi-level approach
529 : * @note E.g.: number of levels := 3 -> 2 + 2 = 4 bits are required
530 : * @note No flag(s) for root level 0; each non-leaf cell has two subcells
531 : * @return The number of arc flags required in a multi-level approach
532 : */
533 : int numberOfArcFlags() const {
534 0 : return 2 * (myNumberOfLevels - 1);
535 : }
536 : /// @brief Returns the number of levels L incl. the zeroth level
537 : // @note Levels go from 0 to L-1
538 : int getNumberOfLevels() const {
539 0 : return myNumberOfLevels;
540 : }
541 : /** @brief Returns the number of cells at all levels
542 : * @return The number of cells at all levels
543 : */
544 0 : size_t numberOfCells() const {
545 0 : return static_cast<size_t>(std::lround(std::pow(2, myNumberOfLevels)) - 1);
546 : }
547 : /** @brief Returns the number of regions, i.e. the number of cells at the shallowest (bottom/leaf) level
548 : * @return The number of regions, i.e. the number of cells at the shallowest (bottom/leaf) level
549 : */
550 : size_t numberOfRegions() const {
551 : return static_cast<size_t>(std::lround(std::pow(2, myNumberOfLevels - 1)));
552 : }
553 : /// @brief Returns true iff the k-d tree is uninitialized
554 : bool isClean() {
555 0 : return myAmClean;
556 : }
557 : /** @brief Search a node in the bottom cells (i.e., return the respective cell)
558 : * @note Uses the position of the node and the divisional structure of the k-d tree for the search.
559 : * @note O(log n) where n = number of cells
560 : * @param[in] node The node
561 : * @return The bottom cell containing the node, or nullptr if the node could not be found
562 : */
563 : const Cell* searchNode(const N* node) const;
564 :
565 : private:
566 : /** @brief Helper method for {@link #cellNumbers()}.
567 : * @param node The node
568 : * @param cell The cell
569 : * @param level The level
570 : * @param nodeCellNumbers The vector of cell numbers for the passed node
571 : */
572 : void cellNumbersAux(const N* node, const Cell* cell, int level, std::vector<int>* nodeCellNumbers) const;
573 : /// @brief The root of the k-d tree
574 : const Cell* myRoot;
575 : /// @brief The number of levels
576 : const int myNumberOfLevels;
577 : /// @brief The reference to a constant container with pointers to edges
578 : const std::vector<E*>& myEdges;
579 : /// @brief The container with all nodes, sorted wrt to the k-d tree subdivision scheme
580 : std::vector<const N*> mySortedNodes;
581 : /// @brief The cells
582 : std::vector<const Cell*> myCells;
583 : /// @brief The cells of all partitions at all levels of the k-d tree subdivision scheme
584 : std::vector<std::vector<const Cell*>> myLevelCells;
585 : /// @brief The boolean flag indicating whether edge permissions need to be considered or not
586 : const bool myHavePermissions;
587 : /// @brief The boolean flag indicating whether edge restrictions need to be considered or not
588 : const bool myHaveRestrictions;
589 : /// @brief The boolean flag indicating whether the k-d tree is a clean (empty, uninitialized) instance or not
590 : bool myAmClean;
591 : }; // end of class KDTreePartition declaration
592 :
593 : // ===========================================================================
594 : // method definitions
595 : // ===========================================================================
596 :
597 : template<class E, class N, class V>
598 0 : KDTreePartition<E, N, V>::KDTreePartition(int numberOfLevels, const std::vector<E*>& edges,
599 : const bool havePermissions, const bool haveRestrictions) :
600 0 : myNumberOfLevels(numberOfLevels),
601 0 : myEdges(edges),
602 0 : myHavePermissions(havePermissions),
603 0 : myHaveRestrictions(haveRestrictions),
604 0 : myAmClean(true) { /* still uninitialized */
605 0 : if (numberOfLevels <= 0) {
606 0 : throw std::invalid_argument("KDTreePartition::KDTreePartition: zero or negative number of levels has been passed!");
607 : }
608 0 : }
609 :
610 : template<class E, class N, class V>
611 0 : void KDTreePartition<E, N, V>::init(const V* const vehicle) {
612 : size_t edgeCounter = 0;
613 : std::unordered_set<const N*> nodeSet;
614 : // extract nodes from edges
615 0 : mySortedNodes.resize(myEdges.size() * 2);
616 : #ifdef KDTP_DEBUG_LEVEL_1
617 : std::cout << "Extracting nodes from edges..." << std::endl;
618 : #endif
619 0 : for (E* edge : myEdges) {
620 0 : if ((myHavePermissions && edge->prohibits(vehicle))
621 0 : || (myHaveRestrictions && edge->restricts(vehicle))) {
622 0 : continue;
623 : }
624 0 : const N* node = edge->getFromJunction();
625 : typename std::unordered_set<const N*>::const_iterator it = nodeSet.find(node);
626 0 : if (it == nodeSet.end()) {
627 : nodeSet.insert(node);
628 : assert(edgeCounter < mySortedNodes.size());
629 0 : mySortedNodes[edgeCounter++] = node;
630 : }
631 0 : node = edge->getToJunction();
632 : it = nodeSet.find(node);
633 0 : if (it == nodeSet.end()) {
634 : nodeSet.insert(node);
635 : assert(edgeCounter < mySortedNodes.size());
636 0 : mySortedNodes[edgeCounter++] = node;
637 : }
638 : }
639 : mySortedNodes.shrink_to_fit();
640 : #ifdef KDTP_DEBUG_LEVEL_1
641 : std::cout << "Nodes extracted from edges." << std::endl;
642 : #endif
643 0 : mySortedNodes.resize(edgeCounter);
644 0 : myCells.resize(numberOfCells());
645 0 : myLevelCells.resize(myNumberOfLevels);
646 : // call the recursive cell constructor at the root (instantiates the whole k-d tree of cells)
647 : #ifdef KDTP_DEBUG_LEVEL_1
648 : std::cout << "Calling root cell constructor..." << std::endl;
649 : #endif
650 : #ifdef KDTP_FOR_SYNTHETIC_NETWORKS
651 : myRoot = new Cell(&myCells, &myLevelCells, &mySortedNodes, myNumberOfLevels, 0, Axis::Y, 0, mySortedNodes.size(), nullptr, -1, -1, -1, -1,
652 : nullptr, nullptr, nullptr, nullptr, false, vehicle, havePermissions, haveRestrictions);
653 : #else
654 0 : myRoot = new Cell(&myCells, &myLevelCells, &mySortedNodes, myNumberOfLevels, 0, Axis::Y, 0, mySortedNodes.size(), nullptr, -1, -1, -1, -1,
655 0 : false, vehicle, myHavePermissions, myHaveRestrictions);
656 : #endif
657 : #ifdef KDTP_DEBUG_LEVEL_1
658 : std::cout << "Done." << std::endl;
659 : #endif
660 : assert(myCells[0] == myRoot);
661 : assert(myRoot->getNumber() == 0);
662 : // nodes are now sorted wrt to the k-d tree's subdivisional scheme
663 : #ifdef KDTP_DEBUG_LEVEL_0
664 : const N* node = mySortedNodes[0];
665 : std::vector<int>* numbers = cellNumbers(node);
666 : int i;
667 : for (i = 0; i < myNumberOfLevels; ++i) {
668 : std::cout << "Cell numbers of test node: " << (*numbers)[i] << std::endl;
669 : }
670 : delete numbers;
671 : for (i = 0; i < myNumberOfLevels; ++i) {
672 : const std::vector<const Cell*>& levelCells = getCellsAtLevel(i);
673 : size_t size = levelCells.size();
674 : std::cout << "Level " << i << " has " << size << " cells." << std::endl;
675 : std::cout << "The numbers of the cells are: " << std::endl;
676 : size_t k = 0;
677 : for (const Cell* cell : levelCells) {
678 : std::cout << cell->getNumber() << (k++ < size ? ", " : "") << std::endl;
679 : }
680 : }
681 : #endif
682 0 : myAmClean = false;
683 0 : }
684 :
685 : template<class E, class N, class V>
686 : const typename KDTreePartition<E, N, V>::Cell* KDTreePartition<E, N, V>::cell(int number) const {
687 : // for now fast enough since the number of cells is usually small
688 : for (const KDTreePartition<E, N, V>::Cell* cell : myCells) {
689 : if (cell->getNumber() == number) {
690 : return cell;
691 : }
692 : }
693 : return nullptr;
694 : }
695 :
696 : template<class E, class N, class V>
697 : std::vector<int>* KDTreePartition<E, N, V>::cellNumbers(const N* node) const {
698 : std::vector<int>* nodeCellNumbers = new std::vector<int>(myNumberOfLevels);
699 : int i;
700 : for (i = 0; i < myNumberOfLevels; ++i) {
701 : (*nodeCellNumbers)[i] = -1;
702 : }
703 : cellNumbersAux(node, myCells[0], 0, nodeCellNumbers);
704 : #ifdef KDTP_DEBUG_LEVEL_1
705 : std::cout << "The following entries in nodeCellNumbers should all be set (!=-1): " << std::endl;
706 : for (i = 0; i < myNumberOfLevels; ++i) {
707 : assert((*nodeCellNumbers)[i] != -1);
708 : std::cout << "nodeCellNumbers[" << i << "]:" << (*nodeCellNumbers)[i] << std::endl;
709 : }
710 : #endif
711 : return nodeCellNumbers;
712 : }
713 :
714 : template<class E, class N, class V>
715 : void KDTreePartition<E, N, V>::cellNumbersAux(const N* node, const Cell* cell, int level, std::vector<int>* nodeCellNumbers) const {
716 : #ifdef KDTP_DEBUG_LEVEL_1
717 : std::cout << "Call ...aux, level: " << level << ", cell->getAxis(): " << (cell->getAxis() == Axis::X ? "X" : "Y") << std::endl;
718 : #endif
719 : assert(level < myNumberOfLevels);
720 : #ifdef KDTP_DEBUG_LEVEL_1
721 : Axis flippedCellAxis = flip(cell->getAxis());
722 : double nodeAxisValue = flippedCellAxis == Axis::Y ? node->getPosition().y() : node->getPosition().x();
723 : std::cout << "Flipped axis of cell (=parent cell axis): " << (flippedCellAxis == Axis::X ? "X" : "Y")
724 : << ", cell min axis value in flipped axis: " << (flippedCellAxis == Axis::X ? cell->getMinX() : cell->getMinY())
725 : << ", node axis value in flipped axis: " << nodeAxisValue << ", cell max axis value in flipped axis: "
726 : << (flippedCellAxis == Axis::X ? cell->getMaxX() : cell->getMaxY()) << std::endl;
727 : #endif
728 : if (cell->contains(node)) {
729 : // node is inside the cell, hence cell is one of the cells of node
730 : (*nodeCellNumbers)[level] = cell->getNumber();
731 : #ifdef KDTP_DEBUG_LEVEL_1
732 : std::cout << "Just filled nodeCellNumbers[" << level << "]:" << (*nodeCellNumbers)[level] << std::endl;
733 : #endif
734 : // only one of the below two calls may actually alter array nodeCellNumbers
735 : const Cell* leftOrLowerSubcell = cell->getLeftOrLowerSubcell();
736 : if (leftOrLowerSubcell) { // no more calls at shallowest (i.e. leaf) level
737 : cellNumbersAux(node, leftOrLowerSubcell, level + 1, nodeCellNumbers);
738 : }
739 : const Cell* rightOrUpperSubcell = cell->getRightOrUpperSubcell();
740 : if (rightOrUpperSubcell) {
741 : cellNumbersAux(node, rightOrUpperSubcell, level + 1, nodeCellNumbers);
742 : }
743 : }
744 : #ifdef KDTP_DEBUG_LEVEL_1
745 : else { // node is not inside the cell, no need to look for node in subcells (consequently, do nothing more, return)
746 : std::cout << "Not inside cell, do nothing." << std::endl;
747 : }
748 : #endif
749 : }
750 :
751 : template<class E, class N, class V>
752 : #ifdef KDTP_FOR_SYNTHETIC_NETWORKS
753 : KDTreePartition<E, N, V>::Cell::Cell(std::vector<const Cell*>* cells, std::vector<std::vector<const Cell*>>* levelCells, std::vector<const N*>* sortedNodes, int numberOfLevels, int level,
754 : Axis axis, size_t fromInclusive, size_t toExclusive, Cell* supercell, double minX, double maxX, double minY, double maxY, std::unordered_set<const N*>* northernConflictNodes,
755 : std::unordered_set<const N*>* easternConflictNodes, std::unordered_set<const N*>* southernConflictNodes, std::unordered_set<const N*>* westernConflictNodes, const V* const vehicle,
756 : const bool havePermissions, const bool haveRestrictions) :
757 : myCells(cells),
758 : myLevelCells(levelCells),
759 : mySortedNodes(sortedNodes),
760 : myNumberOfLevels(numberOfLevels),
761 : myLevel(level), myNumber(cellCounter()++),
762 : myAxis(axis),
763 : myFromInclusive(fromInclusive),
764 : myToExclusive(toExclusive),
765 : mySupercell(supercell),
766 : myMinX(minX),
767 : myMaxX(maxX),
768 : myMinY(minY),
769 : myMaxY(maxY),
770 : myNorthernConflictNodes(northernConflictNodes),
771 : myEasternConflictNodes(easternConflictNodes),
772 : mySouthernConflictNodes(southernConflictNodes),
773 : myWesternConflictNodes(westernConflictNodes),
774 : myIsLeftOrLowerCell(isLeftOrLowerCell),
775 : myHavePermissions(havePermissions),
776 : myHaveRestrictions(haveRestrictions),
777 : myMedianCoordinate(-1.) {
778 : #else
779 0 : KDTreePartition<E, N, V>::Cell::Cell(std::vector<const Cell*>* cells, std::vector<std::vector<const Cell*>>* levelCells, std::vector<const N*>* sortedNodes, int numberOfLevels, int level,
780 : Axis axis, size_t fromInclusive, size_t toExclusive, Cell* supercell, double minX, double maxX, double minY, double maxY, bool isLeftOrLowerCell, const V* const vehicle,
781 : const bool havePermissions, const bool haveRestrictions) :
782 0 : myCells(cells),
783 0 : myLevelCells(levelCells),
784 0 : mySortedNodes(sortedNodes),
785 0 : myNumberOfLevels(numberOfLevels),
786 0 : myLevel(level),
787 0 : myNumber(cellCounter()++),
788 0 : myAxis(axis),
789 0 : myFromInclusive(fromInclusive),
790 0 : myToExclusive(toExclusive),
791 0 : mySupercell(supercell),
792 0 : myMinX(minX),
793 0 : myMaxX(maxX),
794 0 : myMinY(minY),
795 0 : myMaxY(maxY),
796 0 : myIsLeftOrLowerCell(isLeftOrLowerCell),
797 0 : myHavePermissions(havePermissions),
798 0 : myHaveRestrictions(haveRestrictions),
799 0 : myMedianCoordinate(-1.) {
800 : #endif
801 : // throw invalid argument exception if fromInclusive is greater than or equal to toExclusive
802 0 : if (myFromInclusive >= myToExclusive) {
803 0 : throw std::invalid_argument("Cell::Cell: myFromInclusive greater than or equal to myToExclusive!");
804 : }
805 0 : myHasCompleteSpatialInfo = false;
806 0 : myHasNodesSortedWrtToMyAxis = false;
807 0 : (*myCells)[myNumber] = this;
808 0 : (*myLevelCells)[myLevel].push_back(this);
809 : #ifdef KDTP_DEBUG_LEVEL_1
810 : std::cout << "Cell number: " << myNumber << ", cell's extent: minX=" << myMinX << ", maxX="
811 : << myMaxX << ", minY=" << myMinY << ", maxY=" << myMaxY << std::endl;
812 : std::vector<const N*>* cellNodes = nodes();
813 : int cnt = 0;
814 : for (const N* node : *cellNodes) {
815 : if (++cnt % 10000 == 0) {
816 : std::cout << "Testing node " << cnt << std::endl;
817 : }
818 : assert(contains(node));
819 : }
820 : delete cellNodes;
821 : #endif
822 0 : typename std::vector<const N*>::const_iterator first = mySortedNodes->begin() + myFromInclusive;
823 0 : typename std::vector<const N*>::const_iterator last = mySortedNodes->begin() + myToExclusive;
824 : typename std::vector<const N*>::const_iterator iter;
825 : #ifdef KDTP_WRITE_QGIS_FILTERS
826 : size_t numberOfNodes = myToExclusive - myFromInclusive;
827 : size_t k = 0;
828 : std::string qgisFilterString = "id IN (";
829 : // go through the nodes of the cell
830 : for (iter = first; iter != last; iter++) {
831 : k++;
832 : qgisFilterString += (*iter)->getID() + (k < numberOfNodes ? ", " : "");
833 : }
834 : qgisFilterString += ")";
835 : std::ostringstream pathAndFileName;
836 : pathAndFileName << "./filter_nodes_of_cell_" << myNumber << ".qqf";
837 : std::ofstream file;
838 : file.open(pathAndFileName.str());
839 : std::ostringstream content;
840 : content << "<Query>" << qgisFilterString << "</Query>";
841 : file << content.str();
842 : file.close();
843 : #endif
844 : // compute the set of of edges inside the cell
845 : #ifdef KDTP_DEBUG_LEVEL_1
846 : std::cout << "Computing the set of of edges inside the cell..." << std::endl;
847 : #endif
848 0 : std::unordered_set<const E*>* edgeSet = this->edgeSet(vehicle);
849 : #ifdef KDTP_DEBUG_LEVEL_1
850 : std::cout << "Done. Now collecting boundary edges..." << std::endl;
851 : int i = 0;
852 : #endif
853 : /// go through the nodes of the cell in order to identify and collect boundary nodes / edges
854 0 : for (iter = first; iter != last; iter++) {
855 : #if defined(KDTP_KEEP_BOUNDARY_EDGES) || defined(KDTP_KEEP_BOUNDARY_NODES) || defined(KDTP_WRITE_QGIS_FILTERS)
856 : std::unordered_set<const E*> nodeEdgeSet;
857 : #endif
858 : #ifdef KDTP_DEBUG_LEVEL_1
859 : if ((i++ % 10000) == 0) {
860 : std::cout << i << " cell nodes processed..." << std::endl;
861 : }
862 : #endif
863 : #if defined(KDTP_KEEP_BOUNDARY_EDGES) || defined(KDTP_KEEP_BOUNDARY_NODES) || defined(KDTP_WRITE_QGIS_FILTERS)
864 : const std::vector<const E*>& incomingEdges = (*iter)->getIncoming();
865 : for (const E* incomingEdge : incomingEdges) {
866 : if (isProhibited(incomingEdge, vehicle)) {
867 : continue;
868 : }
869 : #ifdef KDTP_EXCLUDE_INTERNAL_EDGES
870 : if (incomingEdge->isInternal()) {
871 : continue;
872 : }
873 : #endif
874 : nodeEdgeSet.insert(incomingEdge);
875 : }
876 : const std::vector<const E*>& outgoingEdges = (*iter)->getOutgoing();
877 : for (const E* outgoingEdge : outgoingEdges) {
878 : if (isProhibited(outgoingEdge, vehicle)) {
879 : continue;
880 : }
881 : #ifdef KDTP_EXCLUDE_INTERNAL_EDGES
882 : if (outgoingEdge->isInternal()) {
883 : continue;
884 : }
885 : #endif
886 : nodeEdgeSet.insert(outgoingEdge);
887 : }
888 : bool containsAll = true;
889 : for (const E* nodeEdge : nodeEdgeSet) {
890 : if (edgeSet->find(nodeEdge) == edgeSet->end()) {
891 : containsAll = false;
892 : #ifndef KDTP_KEEP_BOUNDARY_EDGES
893 : break; // cancel at first element which is not found
894 : #else
895 : myBoundaryEdges.insert(nodeEdge); // boundary edge!
896 : #endif
897 : }
898 : }
899 : #if defined(KDTP_KEEP_BOUNDARY_NODES) || defined(KDTP_WRITE_QGIS_FILTERS)
900 : if (!containsAll) {
901 : myBoundaryNodes.push_back(*iter);
902 : }
903 : #endif
904 : #endif
905 : #if defined(KDTP_KEEP_INCOMING_BOUNDARY_EDGES) || defined(KDTP_KEEP_BOUNDARY_TO_NODES)
906 0 : const std::vector<const E*>& incoming = (*iter)->getIncoming();
907 : std::unordered_set<const N*> boundaryToNodesSet;
908 0 : for (const E* nodeEdge : incoming) {
909 0 : if (isProhibited(nodeEdge, vehicle)) {
910 0 : continue;
911 : }
912 : #ifdef KDTP_EXCLUDE_INTERNAL_EDGES
913 0 : if (nodeEdge->isInternal()) {
914 0 : continue;
915 : }
916 : #endif
917 : assert(nodeEdge->getToJunction() == *iter);
918 0 : if (edgeSet->find(nodeEdge) == edgeSet->end()) {
919 : #ifdef KDTP_KEEP_INCOMING_BOUNDARY_EDGES
920 : myIncomingBoundaryEdges.insert(nodeEdge); // incoming boundary edge
921 : #endif
922 : #ifdef KDTP_KEEP_BOUNDARY_TO_NODES
923 : boundaryToNodesSet.insert(*iter); ; // boundary to-node!
924 : #endif
925 : }
926 : }
927 : #ifdef KDTP_KEEP_BOUNDARY_TO_NODES
928 : std::copy(boundaryToNodesSet.begin(), boundaryToNodesSet.end(),
929 : std::back_inserter(myBoundaryToNodes));
930 : #endif
931 : #endif
932 : #if defined(KDTP_KEEP_OUTGOING_BOUNDARY_EDGES) || defined(KDTP_KEEP_BOUNDARY_FROM_NODES)
933 0 : const std::vector<const E*>& outgoing = (*iter)->getOutgoing();
934 : std::unordered_set<const N*> boundaryFromNodesSet;
935 0 : for (const E* nodeEdge : outgoing) {
936 0 : if (isProhibited(nodeEdge, vehicle)) {
937 0 : continue;
938 : }
939 : #ifdef KDTP_EXCLUDE_INTERNAL_EDGES
940 0 : if (nodeEdge->isInternal()) {
941 0 : continue;
942 : }
943 : #endif
944 : assert(nodeEdge->getFromJunction() == *iter);
945 0 : if (edgeSet->find(nodeEdge) == edgeSet->end()) {
946 : #ifdef KDTP_KEEP_OUTGOING_BOUNDARY_EDGES
947 : myOutgoingBoundaryEdges.insert(nodeEdge); // outgoing boundary edge
948 : #endif
949 : #ifdef KDTP_KEEP_BOUNDARY_FROM_NODES
950 : boundaryFromNodesSet.insert(*iter); // boundary from-node
951 : #endif
952 : }
953 : }
954 : #ifdef KDTP_KEEP_BOUNDARY_FROM_NODES
955 : std::copy(boundaryFromNodesSet.begin(), boundaryFromNodesSet.end(),
956 0 : std::back_inserter(myBoundaryFromNodes));
957 : #endif
958 : #endif
959 : }
960 0 : delete edgeSet;
961 : #if defined(KDTP_KEEP_BOUNDARY_NODES) || defined(KDTP_WRITE_QGIS_FILTERS)
962 : size_t numberOfBoundaryNodes = myBoundaryNodes.size();
963 : #ifdef KDTP_DEBUG_LEVEL_1
964 : std::cout << "Number of boundary nodes: " << numberOfBoundaryNodes << std::endl;
965 : #endif
966 : #endif
967 : #ifdef KDTP_KEEP_BOUNDARY_FROM_NODES
968 : #ifdef KDTP_DEBUG_LEVEL_1
969 : size_t numberOfBoundaryFromNodes = myBoundaryFromNodes.size();
970 : std::cout << "Number of boundary from nodes: " << numberOfBoundaryFromNodes << std::endl;
971 : #endif
972 : #endif
973 : #ifdef KDTP_KEEP_BOUNDARY_TO_NODES
974 : size_t numberOfBoundaryToNodes = myBoundaryToNodes.size();
975 : #ifdef KDTP_DEBUG_LEVEL_1
976 : std::cout << "Number of boundary to nodes: " << numberOfBoundaryToNodes << std::endl;
977 : #endif
978 : #endif
979 : #ifdef KDTP_KEEP_BOUNDARY_EDGES
980 : size_t numberOfBoundaryEdges = myBoundaryEdges.size();
981 : #ifdef KDTP_DEBUG_LEVEL_1
982 : std::cout << "Number of boundary edges: " << numberOfBoundaryEdges << std::endl;
983 : #endif
984 : #endif
985 : #ifdef KDTP_KEEP_INCOMING_BOUNDARY_EDGES
986 : #ifdef KDTP_DEBUG_LEVEL_1
987 : size_t numberOfIncomingBoundaryEdges = myIncomingBoundaryEdges.size();
988 : std::cout << "Number of incoming boundary edges: " << numberOfIncomingBoundaryEdges << std::endl;
989 : #endif
990 : #endif
991 : #ifdef KDTP_KEEP_OUTGOING_BOUNDARY_EDGES
992 : #ifdef KDTP_DEBUG_LEVEL_1
993 : size_t numberOfOutgoingBoundaryEdges = myOutgoingBoundaryEdges.size();
994 : std::cout << "Number of outgoing boundary edges: " << numberOfOutgoingBoundaryEdges << std::endl;
995 : #endif
996 : #endif
997 : #ifdef KDTP_WRITE_QGIS_FILTERS
998 : k = 0;
999 : qgisFilterString.clear();
1000 : qgisFilterString = "id IN (";
1001 : // go through the boundary nodes of the cell
1002 : for (const N* node : myBoundaryNodes) {
1003 : k++;
1004 : qgisFilterString += node->getID() + (k < numberOfBoundaryNodes ? ", " : "");
1005 : }
1006 : #ifndef KDTP_KEEP_BOUNDARY_NODES
1007 : myBoundaryNodes.clear();
1008 : #endif
1009 : qgisFilterString += ")";
1010 : pathAndFileName.str("");
1011 : pathAndFileName.clear();
1012 : pathAndFileName << "./filter_boundary_nodes_of_cell_" << myNumber << ".qqf";
1013 : file.clear();
1014 : file.open(pathAndFileName.str());
1015 : content.str("");
1016 : content.clear();
1017 : content << "<Query>" << qgisFilterString << "</Query>";
1018 : file << content.str();
1019 : file.close();
1020 : #endif
1021 : #ifdef KDTP_DEBUG_LEVEL_1
1022 : std::cout << "Done. Now calling the constructor recursively to instantiate the subcells (left or lower one first)..." << std::endl;
1023 : #endif
1024 0 : size_t medianIndex = partition();
1025 0 : completeSpatialInfo();
1026 : // create subcells
1027 0 : if (myLevel < myNumberOfLevels - 1) {
1028 : // determine min/max X/Y-values to pass on to left or lower subcell
1029 : double passMinX = -1, passMaxX = -1, passMinY = -1, passMaxY = -1;
1030 0 : if (myAxis == Axis::X) { // left subcell
1031 0 : passMinX = myMinX;
1032 0 : passMaxX = (*mySortedNodes)[medianIndex]->getPosition().x();
1033 0 : passMinY = myMinY;
1034 0 : passMaxY = myMaxY;
1035 : } else { // lower subcell
1036 0 : passMinX = myMinX;
1037 0 : passMaxX = myMaxX;
1038 0 : passMinY = myMinY;
1039 0 : passMaxY = (*mySortedNodes)[medianIndex]->getPosition().y();
1040 : }
1041 : #ifdef KDTP_FOR_SYNTHETIC_NETWORKS
1042 : // notice that nodes with indexes medianIndex and medianIndex+1 may have the same coordinate in the direction of myAxis
1043 : // (in that case, they are ordered / distributed between the two subcells with a tie-breaking rule)
1044 : // consequently, the spatial extent of cells alone does not suffice to decide whether a node belongs to a certain cell or not
1045 : // therefore, for each of the two subcells, we compute the set of nodes sharing said coordinate (called the set of "spatial conflict nodes")
1046 : // the size of this set is usually very small compared to the size of the set of all nodes of the subcell
1047 : // with the spatial extent of a cell and its spatial conflict nodes set, a valid test whether a node belongs to the cell becomes available
1048 : // this test is quicker and requires significantly less memory than one based on pre-computed sets of all cell nodes
1049 : std::pair<std::unordered_set<const N*>*, std::unordered_set<const N*>*> conflictNodes = spatialConflictNodes(medianIndex);
1050 : // determine northern/eastern/southern/western conflict nodes to pass on to left or lower subcell
1051 : std::unordered_set<const N*>* passNorthernConflictNodes;
1052 : std::unordered_set<const N*>* passEasternConflictNodes;
1053 : std::unordered_set<const N*>* passSouthernConflictNodes;
1054 : std::unordered_set<const N*>* passWesternConflictNodes;
1055 : if (myAxis == Axis::X) { // left subcell
1056 : passNorthernConflictNodes = myNorthernConflictNodes;
1057 : passEasternConflictNodes = conflictNodes.first;
1058 : passSouthernConflictNodes = mySouthernConflictNodes;
1059 : passWesternConflictNodes = myWesternConflictNodes;
1060 : } else { // lower subcell
1061 : passNorthernConflictNodes = conflictNodes.first;
1062 : passEasternConflictNodes = myEasternConflictNodes;
1063 : passSouthernConflictNodes = mySouthernConflictNodes;
1064 : passWesternConflictNodes = myWesternConflictNodes;
1065 : }
1066 : myLeftOrLowerSubcell = new Cell(myCells, myLevelCells, mySortedNodes, myNumberOfLevels, myLevel + 1, KDTreePartition::flip(myAxis), myFromInclusive, medianIndex + 1, this,
1067 : passMinX, passMaxX, passMinY, passMaxY, passNorthernConflictNodes, passEasternConflictNodes, passSouthernConflictNodes, passWesternConflictNodes, true,
1068 : vehicle, myHavePermissions, myHaveRestrictions);
1069 : #else
1070 0 : myLeftOrLowerSubcell = new Cell(myCells, myLevelCells, mySortedNodes, myNumberOfLevels, myLevel + 1, KDTreePartition::flip(myAxis), myFromInclusive, medianIndex + 1, this,
1071 0 : passMinX, passMaxX, passMinY, passMaxY, true, vehicle, myHavePermissions, myHaveRestrictions);
1072 : #endif
1073 : #ifdef KDTP_DEBUG_LEVEL_1
1074 : std::cout << "Left or lower call done. Now calling it for the right or upper subcell..." << std::endl;
1075 : #endif
1076 : // determine min/max X/Y-values to pass on to right or upper subcell
1077 : passMinX = -1, passMaxX = -1, passMinY = -1, passMaxY = -1;
1078 0 : if (myAxis == Axis::X) { // right subcell
1079 0 : passMinX = (*mySortedNodes)[medianIndex + 1]->getPosition().x();
1080 0 : passMaxX = myMaxX;
1081 0 : passMinY = myMinY;
1082 0 : passMaxY = myMaxY;
1083 : } else { // upper subcell
1084 0 : passMinX = myMinX;
1085 0 : passMaxX = myMaxX;
1086 0 : passMinY = (*mySortedNodes)[medianIndex + 1]->getPosition().y();
1087 0 : passMaxY = myMaxY;
1088 : }
1089 : #ifdef KDTP_FOR_SYNTHETIC_NETWORKS
1090 : // determine northern/eastern/southern/western conflict nodes to pass on to right or upper subcell
1091 : if (myAxis == Axis::X) { // right subcell
1092 : passNorthernConflictNodes = myNorthernConflictNodes;
1093 : passEasternConflictNodes = myEasternConflictNodes;
1094 : passSouthernConflictNodes = mySouthernConflictNodes;
1095 : passWesternConflictNodes = conflictNodes.second;
1096 : } else { // upper subcell
1097 : passNorthernConflictNodes = myNorthernConflictNodes;
1098 : passEasternConflictNodes = myEasternConflictNodes;
1099 : passSouthernConflictNodes = conflictNodes.second;
1100 : passWesternConflictNodes = myWesternConflictNodes;
1101 : }
1102 : myRightOrUpperSubcell = new Cell(myCells, myLevelCells, mySortedNodes, myNumberOfLevels, myLevel + 1, KDTreePartition::flip(myAxis), medianIndex + 1, myToExclusive, this,
1103 : passMinX, passMaxX, passMinY, passMaxY, passNorthernConflictNodes, passEasternConflictNodes, passSouthernConflictNodes, passWesternConflictNodes, false,
1104 : vehicle, myHavePermissions, myHaveRestrictions);
1105 : #else
1106 0 : myRightOrUpperSubcell = new Cell(myCells, myLevelCells, mySortedNodes, myNumberOfLevels, myLevel + 1, KDTreePartition::flip(myAxis), medianIndex + 1, myToExclusive, this,
1107 0 : passMinX, passMaxX, passMinY, passMaxY, false, vehicle, myHavePermissions, myHaveRestrictions);
1108 : #endif
1109 : #ifdef KDTP_DEBUG_LEVEL_1
1110 : std::cout << "Right or upper call done. Returning from constructor..." << std::endl;
1111 : #endif
1112 : } else {
1113 : // leaves of the k-d tree: subcell pointers equal nullptr
1114 0 : myLeftOrLowerSubcell = nullptr;
1115 0 : myRightOrUpperSubcell = nullptr;
1116 : }
1117 0 : } // end of cell constructor
1118 :
1119 : template<class E, class N, class V>
1120 0 : double KDTreePartition<E, N, V>::Cell::minAxisValue(Axis axis) const {
1121 : #ifdef KDTP_DEBUG_LEVEL_1
1122 : double returnValue = -1;
1123 : #endif
1124 0 : if (myHasNodesSortedWrtToMyAxis && axis == myAxis) {
1125 : #ifndef KDTP_DEBUG_LEVEL_1
1126 0 : return (myAxis == Axis::Y ? (*mySortedNodes)[myFromInclusive]->getPosition().y()
1127 0 : : (*mySortedNodes)[myFromInclusive]->getPosition().x());
1128 : #else
1129 : returnValue = (myAxis == Axis::Y ? (*mySortedNodes)[myFromInclusive]->getPosition().y()
1130 : : (*mySortedNodes)[myFromInclusive]->getPosition().x());
1131 : #endif
1132 : }
1133 : double minAxisValue = std::numeric_limits<double>::max();
1134 0 : typename std::vector<const N*>::const_iterator first = mySortedNodes->begin() + myFromInclusive;
1135 0 : typename std::vector<const N*>::const_iterator last = mySortedNodes->begin() + myToExclusive;
1136 : typename std::vector<const N*>::const_iterator iter;
1137 : // go through the nodes of the cell
1138 0 : for (iter = first; iter != last; iter++) {
1139 : double nodeAxisValue;
1140 0 : nodeAxisValue = (axis == Axis::Y ? (*iter)->getPosition().y() : (*iter)->getPosition().x());
1141 0 : if (nodeAxisValue < minAxisValue) {
1142 : minAxisValue = nodeAxisValue;
1143 : }
1144 : }
1145 : assert(minAxisValue < std::numeric_limits<double>::max());
1146 : #ifdef KDTP_DEBUG_LEVEL_1
1147 : assert((!myHasNodesSortedWrtToMyAxis || axis != myAxis) || minAxisValue == returnValue);
1148 : #endif
1149 : return minAxisValue;
1150 : }
1151 :
1152 : template<class E, class N, class V>
1153 : double KDTreePartition<E, N, V>::Cell::minAxisValue() const {
1154 : return minAxisValue(myAxis);
1155 : }
1156 :
1157 : template<class E, class N, class V>
1158 0 : double KDTreePartition<E, N, V>::Cell::maxAxisValue(Axis axis) const {
1159 : #ifdef KDTP_DEBUG_LEVEL_1
1160 : double returnValue = -1;
1161 : #endif
1162 0 : if (myHasNodesSortedWrtToMyAxis && axis == myAxis) {
1163 : #ifndef KDTP_DEBUG_LEVEL_1
1164 0 : return (myAxis == Axis::Y ? (*mySortedNodes)[myToExclusive - 1]->getPosition().y()
1165 0 : : (*mySortedNodes)[myToExclusive - 1]->getPosition().x());
1166 : #else
1167 : returnValue = (myAxis == Axis::Y ? (*mySortedNodes)[myToExclusive - 1]->getPosition().y()
1168 : : (*mySortedNodes)[myToExclusive - 1]->getPosition().x());
1169 : #endif
1170 :
1171 : }
1172 : double maxAxisValue = -1;
1173 0 : typename std::vector<const N*>::const_iterator first = mySortedNodes->begin() + myFromInclusive;
1174 0 : typename std::vector<const N*>::const_iterator last = mySortedNodes->begin() + myToExclusive;
1175 : typename std::vector<const N*>::const_iterator iter;
1176 : // go through the nodes of the cell
1177 0 : for (iter = first; iter != last; iter++) {
1178 : double nodeAxisValue;
1179 0 : nodeAxisValue = (axis == Axis::Y ? (*iter)->getPosition().y() : (*iter)->getPosition().x());
1180 0 : if (nodeAxisValue > maxAxisValue) {
1181 : maxAxisValue = nodeAxisValue;
1182 : }
1183 : }
1184 : assert(maxAxisValue > 0);
1185 : #ifdef KDTP_DEBUG_LEVEL_1
1186 : assert((!myHasNodesSortedWrtToMyAxis || axis != myAxis) || maxAxisValue == returnValue);
1187 : #endif
1188 : return maxAxisValue;
1189 : }
1190 :
1191 : template<class E, class N, class V>
1192 : double KDTreePartition<E, N, V>::Cell::maxAxisValue() const {
1193 : return maxAxisValue(myAxis);
1194 : }
1195 :
1196 : template<class E, class N, class V>
1197 0 : size_t KDTreePartition<E, N, V>::Cell::partition() {
1198 0 : typename std::vector<const N*>::iterator first = mySortedNodes->begin() + myFromInclusive;
1199 0 : typename std::vector<const N*>::iterator last = mySortedNodes->begin() + myToExclusive;
1200 0 : if (myAxis == Axis::Y) {
1201 0 : std::sort(first, last, NodeYComparator());
1202 : } else {
1203 0 : std::sort(first, last, NodeXComparator());
1204 : }
1205 0 : myHasNodesSortedWrtToMyAxis = true;
1206 0 : if (mySupercell) {
1207 : // sorting at one level destroys order at higher levels (but preserves the median split property)
1208 0 : mySupercell->myHasNodesSortedWrtToMyAxis = false;
1209 : }
1210 0 : size_t length = myToExclusive - myFromInclusive;
1211 0 : size_t medianIndex = myFromInclusive + (length % 2 == 0 ? length / 2 - 1 : (length + 1) / 2 - 1);
1212 : #ifndef KDTP_FOR_SYNTHETIC_NETWORKS
1213 : // notice that nodes with indexes medianIndex and medianIndex+1 may have the same coordinate in the direction of myAxis -
1214 : // in that case, they would be ordered and distributed between the two subcells with a tie-breaking rule,
1215 : // and the spatial extent of cells alone would not suffice to decide whether a node belongs to a certain cell or not
1216 : // for real-world road networks, this should occur very rarely
1217 : // therefore, a simple but perfectly acceptable remedy is to shift the median index until the nodes with indexes
1218 : // medianIndex and medianIndex+1 have different coordinates
1219 : // this will distort the 50%/50% distribution of nodes just a little bit (and only under very rare circumstances)
1220 : // hence we can assume zero impact on performance
1221 : // note that this may happen more frequently for e.g. synthetic networks placing nodes at a regular, constant grid.
1222 : // for this case, a different solution is provided, see code within #ifdef KDTP_FOR_SYNTHETIC_NETWORKS ... #endif
1223 0 : const N* medianNode = (*mySortedNodes)[medianIndex];
1224 0 : const N* afterMedianNode = (*mySortedNodes)[medianIndex + 1];
1225 0 : double medianNodeCoordinate = myAxis == Axis::X ? medianNode->getPosition().x()
1226 : : medianNode->getPosition().y();
1227 0 : double afterMedianNodeCoordinate = myAxis == Axis::X ? afterMedianNode->getPosition().x()
1228 : : afterMedianNode->getPosition().y();
1229 0 : while (medianNodeCoordinate == afterMedianNodeCoordinate && medianIndex < myToExclusive - 3) {
1230 : #ifdef KDTP_DEBUG_LEVEL_2
1231 : std::cout << "Found spatially conflicting nodes." << std::endl;
1232 : #endif
1233 0 : medianIndex++;
1234 0 : medianNode = (*mySortedNodes)[medianIndex];
1235 0 : afterMedianNode = (*mySortedNodes)[medianIndex + 1];
1236 0 : medianNodeCoordinate = myAxis == Axis::X ? medianNode->getPosition().x()
1237 : : medianNode->getPosition().y();
1238 0 : afterMedianNodeCoordinate = myAxis == Axis::X ? afterMedianNode->getPosition().x()
1239 : : afterMedianNode->getPosition().y();
1240 : }
1241 : #endif
1242 0 : myMedianCoordinate = medianNodeCoordinate;
1243 0 : return medianIndex;
1244 : }
1245 :
1246 : template<class E, class N, class V>
1247 0 : void KDTreePartition<E, N, V>::Cell::completeSpatialInfo() {
1248 : // complete missing information about the cell's spatial extent
1249 0 : if (myMinX < 0) {
1250 0 : myMinX = minAxisValue(Axis::X);
1251 : }
1252 0 : if (myMaxX < 0) {
1253 0 : myMaxX = maxAxisValue(Axis::X);
1254 : }
1255 0 : if (myMinY < 0) {
1256 0 : myMinY = minAxisValue(Axis::Y);
1257 : }
1258 0 : if (myMaxY < 0) {
1259 0 : myMaxY = maxAxisValue(Axis::Y);
1260 : }
1261 0 : myHasCompleteSpatialInfo = true;
1262 0 : }
1263 :
1264 : template<class E, class N, class V>
1265 0 : std::unordered_set<const E*>* KDTreePartition<E, N, V>::Cell::edgeSet(const V* const vehicle) const {
1266 0 : std::unordered_set<const E*>* edgeSet = new std::unordered_set<const E*>();
1267 0 : typename std::vector<const N*>::const_iterator first = mySortedNodes->begin() + myFromInclusive;
1268 0 : typename std::vector<const N*>::const_iterator last = mySortedNodes->begin() + myToExclusive;
1269 : typename std::vector<const N*>::const_iterator iter;
1270 0 : for (iter = first; iter != last; iter++) {
1271 : const N* edgeNode;
1272 0 : const std::vector<const E*>& incomingEdges = (*iter)->getIncoming();
1273 0 : for (const E* incomingEdge : incomingEdges) {
1274 0 : if (isProhibited(incomingEdge, vehicle)) {
1275 0 : continue;
1276 : }
1277 : #ifdef KDTP_EXCLUDE_INTERNAL_EDGES
1278 0 : if (incomingEdge->isInternal()) {
1279 0 : continue;
1280 : }
1281 : #endif
1282 : edgeNode = incomingEdge->getFromJunction();
1283 : if (contains(edgeNode)) {
1284 : edgeSet->insert(incomingEdge);
1285 : }
1286 : }
1287 0 : const std::vector<const E*>& outgoingEdges = (*iter)->getOutgoing();
1288 0 : for (const E* outgoingEdge : outgoingEdges) {
1289 0 : if (isProhibited(outgoingEdge, vehicle)) {
1290 0 : continue;
1291 : }
1292 : #ifdef KDTP_EXCLUDE_INTERNAL_EDGES
1293 0 : if (outgoingEdge->isInternal()) {
1294 0 : continue;
1295 : }
1296 : #endif
1297 : edgeNode = outgoingEdge->getToJunction();
1298 : if (contains(edgeNode)) {
1299 : edgeSet->insert(outgoingEdge);
1300 : }
1301 : }
1302 : }
1303 0 : return edgeSet;
1304 : }
1305 :
1306 : template<class E, class N, class V>
1307 : size_t KDTreePartition<E, N, V>::Cell::numberOfEdgesEndingInCell(const V* const vehicle) const {
1308 : typename std::vector<const N*>::const_iterator first = mySortedNodes->begin() + myFromInclusive;
1309 : typename std::vector<const N*>::const_iterator last = mySortedNodes->begin() + myToExclusive;
1310 : typename std::vector<const N*>::const_iterator iter;
1311 : size_t edgeCounter = 0;
1312 : for (iter = first; iter != last; iter++) {
1313 : const std::vector<const E*>& incomingEdges = (*iter)->getIncoming();
1314 : for (const E* incomingEdge : incomingEdges) {
1315 : if (isProhibited(incomingEdge, vehicle)) {
1316 : continue;
1317 : }
1318 : #ifdef KDTP_EXCLUDE_INTERNAL_EDGES
1319 : if (incomingEdge->isInternal()) {
1320 : continue;
1321 : } else {
1322 : edgeCounter++;
1323 : }
1324 : #endif
1325 : }
1326 : }
1327 : return edgeCounter;
1328 : }
1329 :
1330 : template<class E, class N, class V>
1331 : size_t KDTreePartition<E, N, V>::Cell::numberOfEdgesStartingInCell(const V* const vehicle) const {
1332 : typename std::vector<const N*>::const_iterator first = mySortedNodes->begin() + myFromInclusive;
1333 : typename std::vector<const N*>::const_iterator last = mySortedNodes->begin() + myToExclusive;
1334 : typename std::vector<const N*>::const_iterator iter;
1335 : size_t edgeCounter = 0;
1336 : for (iter = first; iter != last; iter++) {
1337 : const std::vector<const E*>& outgoingEdges = (*iter)->getOutgoing();
1338 : for (const E* outgoingEdge : outgoingEdges) {
1339 : if (isProhibited(outgoingEdge, vehicle)) {
1340 : continue;
1341 : }
1342 : #ifdef KDTP_EXCLUDE_INTERNAL_EDGES
1343 : if (outgoingEdge->isInternal()) {
1344 : continue;
1345 : } else {
1346 : edgeCounter++;
1347 : }
1348 : }
1349 : #endif
1350 : }
1351 : return edgeCounter;
1352 : }
1353 :
1354 : template<class E, class N, class V>
1355 : std::pair<typename std::vector<const N*>::const_iterator,
1356 : typename std::vector<const N*>::const_iterator> KDTreePartition<E, N, V>::Cell::nodeIterators() const {
1357 0 : typename std::vector<const N*>::const_iterator first = mySortedNodes->begin() + myFromInclusive;
1358 0 : typename std::vector<const N*>::const_iterator last = mySortedNodes->begin() + myToExclusive;
1359 : std::pair<typename std::vector<const N*>::const_iterator,
1360 : typename std::vector<const N*>::const_iterator> iterators = std::make_pair(first, last);
1361 : return iterators;
1362 : }
1363 :
1364 : template<class E, class N, class V>
1365 : std::vector<const N*>* KDTreePartition<E, N, V>::Cell::nodes() const {
1366 : typename std::vector<const N*>::const_iterator first = mySortedNodes->begin() + myFromInclusive;
1367 : typename std::vector<const N*>::const_iterator last = mySortedNodes->begin() + myToExclusive;
1368 : std::vector<const N*>* nodes = new std::vector<const N*>(first, last);
1369 : return nodes;
1370 : }
1371 :
1372 : #ifdef KDTP_FOR_SYNTHETIC_NETWORKS
1373 : template<class E, class N, class V>
1374 : std::pair<std::unordered_set<const N*>*,
1375 : std::unordered_set<const N*>*> KDTreePartition<E, N, V>::Cell::spatialConflictNodes(size_t medianIndex) const {
1376 : std::unordered_set<const N*>* leftOrLowerSpatialConflictNodes = new std::unordered_set<const N*>();
1377 : std::unordered_set<const N*>* rightOrUpperSpatialConflictNodes = new std::unordered_set<const N*>();
1378 : std::pair<std::unordered_set<const N*>*, std::unordered_set<const N*>*> spatialConflictNodes
1379 : = std::make_pair(leftOrLowerSpatialConflictNodes, rightOrUpperSpatialConflictNodes);
1380 : std::vector<const N*>::const_iterator iter, prev;
1381 : if (myAxis == Axis::X) {
1382 : if ((*mySortedNodes)[medianIndex]->getPosition().x() == (*mySortedNodes)[medianIndex + 1]->getPosition().x()) {
1383 : double sharedCoordinate = (*mySortedNodes)[medianIndex]->getPosition().x();
1384 : std::vector<const N*>::iterator firstLeftOrLower = mySortedNodes->begin() + medianIndex;
1385 : // this is last since we go backward
1386 : std::vector<const N*>::iterator last = mySortedNodes->begin() + myFromInclusive - 1;
1387 : for (iter = firstLeftOrLower; ((*iter)->getPosition().x() == sharedCoordinate) && (iter != last); iter--) {
1388 : leftOrLowerSpatialConflictNodes->insert(*iter);
1389 : }
1390 : }
1391 : } else {
1392 : if ((*mySortedNodes)[medianIndex]->getPosition().y() == (*mySortedNodes)[medianIndex + 1]->getPosition().y()) {
1393 : double sharedCoordinate = (*mySortedNodes)[medianIndex]->getPosition().y();
1394 : std::vector<const N*>::iterator firstRightOrUpper = mySortedNodes->begin() + medianIndex + 1;
1395 : std::vector<const N*>::const_iterator last = mySortedNodes->begin() + myToExclusive;
1396 : for (iter = firstRightOrUpper; ((*iter)->getPosition().y() == sharedCoordinate) && (iter != last); iter++) {
1397 : rightOrUpperSpatialConflictNodes->insert(*iter);
1398 : }
1399 : }
1400 : }
1401 : return spatialConflictNodes;
1402 : }
1403 : #endif
1404 :
1405 : template<class E, class N, class V>
1406 : bool KDTreePartition<E, N, V>::Cell::isInBounds(const N* node) const {
1407 : double nodeX = node->getPosition().x();
1408 : double nodeY = node->getPosition().y();
1409 0 : if (nodeX < myMinX || nodeX > myMaxX || nodeY < myMinY || nodeY > myMaxY) {
1410 : return false;
1411 : }
1412 : return true;
1413 : }
1414 :
1415 : template<class E, class N, class V>
1416 : bool KDTreePartition<E, N, V>::Cell::contains(const N* node) const {
1417 0 : if (myLevel == 0) { // p.d., the root cell contains all nodes
1418 : return true;
1419 : }
1420 : if (!isInBounds(node)) {
1421 : #ifdef KDTP_DEBUG_LEVEL_2
1422 : std::cout << "Not in bounds, nX: " << node->getPosition().x() << ", nY: " << node->getPosition().y() << std::endl;
1423 : #endif
1424 : return false;
1425 : }
1426 : #ifdef KDTP_DEBUG_LEVEL_2
1427 : else {
1428 : std::cout << "Node is in bounds!" << std::endl;
1429 : }
1430 : #endif
1431 : #ifdef KDTP_FOR_SYNTHETIC_NETWORKS
1432 : //
1433 : double nodeX = node->getPosition().x();
1434 : double nodeY = node->getPosition().y();
1435 : #ifdef KDTP_DEBUG_LEVEL_2
1436 : std::cout << "Entered contains, nodeX: " << nodeX << ", nodeY: " << nodeY << std::endl;
1437 : #endif
1438 : double northY = -1;
1439 : if (myNorthernConflictNodes && !myNorthernConflictNodes->empty()) {
1440 : northY = (*(myNorthernConflictNodes->begin()))->getPosition().y();
1441 : }
1442 : double eastX = -1;
1443 : if (myEasternConflictNodes && !myEasternConflictNodes->empty()) {
1444 : eastX = (*(myEasternConflictNodes->begin()))->getPosition().x();
1445 : }
1446 : double southY = -1;
1447 : if (mySouthernConflictNodes && !mySouthernConflictNodes->empty()) {
1448 : southY = (*(mySouthernConflictNodes->begin()))->getPosition().y();
1449 : }
1450 : double westX = -1;
1451 : if (myWesternConflictNodes && !myWesternConflictNodes->empty()) {
1452 : westX = (*(myWesternConflictNodes->begin()))->getPosition().x();
1453 : }
1454 : #ifdef KDTP_DEBUG_LEVEL_2
1455 : std::cout << "northY: " << northY << ", eastX: " << eastX << ", southY: " << southY << ", westX: " << westX << std::endl;
1456 : #endif
1457 : if (nodeX == eastX) { // we have to test the eastern conflict nodes
1458 : #ifdef KDTP_DEBUG_LEVEL_2
1459 : std::cout << "On eastern bound, nX: " << node->getPosition().x() << ", nY: " << node->getPosition().y() << std::endl;
1460 : #endif
1461 : return myEasternConflictNodes->find(node) != myEasternConflictNodes->end();
1462 : }
1463 : if (nodeX == westX) { // we have to test the western conflict nodes
1464 : #ifdef KDTP_DEBUG_LEVEL_2
1465 : std::cout << "On western bound, nX: " << node->getPosition().x() << ", nY: " << node->getPosition().y() << std::endl;
1466 : #endif
1467 : return myWesternConflictNodes->find(node) != myWesternConflictNodes->end();
1468 : }
1469 : if (nodeY == northY) { // we have to test the northern conflict nodes
1470 : #ifdef KDTP_DEBUG_LEVEL_2
1471 : std::cout << "On northern bound, nX: " << node->getPosition().x() << ", nY: " << node->getPosition().y() << std::endl;
1472 : #endif
1473 : return myNorthernConflictNodes->find(node) != myNorthernConflictNodes->end();
1474 : }
1475 : if (nodeY == southY) { // we have to test the southern conflict nodes
1476 : #ifdef KDTP_DEBUG_LEVEL_2
1477 : std::cout << "On southern bound, nX: " << node->getPosition().x() << ", nY: " << node->getPosition().y() << std::endl;
1478 : #endif
1479 : return mySouthernConflictNodes->find(node) != myEasternConflictNodes->end();
1480 : }
1481 : #endif
1482 : #ifdef KDTP_DEBUG_LEVEL_2
1483 : std::cout << "Node does not lie on a border!" << std::endl;
1484 : #endif
1485 : return true; // if the node does not lie on a spatial border, the passed in-bounds test suffices
1486 : }
1487 :
1488 : template<class E, class N, class V>
1489 0 : const typename KDTreePartition<E, N, V>::Cell* KDTreePartition<E, N, V>::searchNode(const N* node) const {
1490 0 : const typename KDTreePartition<E, N, V>::Cell* cell = myRoot;
1491 : if (!myRoot->contains(node)) {
1492 : return nullptr;
1493 : }
1494 : while (true) {
1495 : assert(cell);
1496 0 : double nodeCoordinate = cell->getAxis() == Axis::X ? node->getPosition().x() : node->getPosition().y();
1497 0 : const typename KDTreePartition<E, N, V>::Cell* nextCell = nodeCoordinate <= cell->getMedianCoordinate() ?
1498 : cell->getLeftOrLowerSubcell() : cell->getRightOrUpperSubcell();
1499 0 : if (nextCell == nullptr) {
1500 0 : return cell;
1501 : } else {
1502 : cell = nextCell;
1503 : }
1504 : }
1505 : }
|
