GCC Code Coverage Report

Directory: ./
File: points/kd_tree.cpp
Date: 2026-09-27 03:12:47
Exec Total Coverage
Lines: 251 264 95.1%
Functions: 28 30 93.3%
Branches: 136 180 75.6%
Line Branch Exec Source
1 /*
2 * Copyright (c) 2000-2022 Inria
3 * All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 *
8 * * Redistributions of source code must retain the above copyright notice,
9 * this list of conditions and the following disclaimer.
10 * * Redistributions in binary form must reproduce the above copyright notice,
11 * this list of conditions and the following disclaimer in the documentation
12 * and/or other materials provided with the distribution.
13 * * Neither the name of the ALICE Project-Team nor the names of its
14 * contributors may be used to endorse or promote products derived from this
15 * software without specific prior written permission.
16 *
17 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 * POSSIBILITY OF SUCH DAMAGE.
28 *
29 * Contact: Bruno Levy
30 *
31 * https://www.inria.fr/fr/bruno-levy
32 *
33 * Inria,
34 * Domaine de Voluceau,
35 * 78150 Le Chesnay - Rocquencourt
36 * FRANCE
37 *
38 */
39
40 #include <geogram/points/kd_tree.h>
41 #include <geogram/basic/geometry_nd.h>
42 #include <geogram/basic/process.h>
43 #include <geogram/basic/algorithm.h>
44
45 namespace {
46
47 using namespace GEO;
48 using GEO::index_t;
49
50 /**
51 * \brief Comparison functor used to
52 * sort the point indices. Used by
53 * BalancedKdTree.
54 */
55 class ComparePointCoord {
56 public:
57 /**
58 * \brief Creates a new ComparePointCoord
59 * \param[in] nb_points number of points
60 * \param[in] points pointer to first point
61 * \param[in] stride number of doubles between two
62 * consecutive points in array (=dimension if point
63 * array is compact).
64 * \param[in] splitting_coord the coordinate to compare
65 */
66 ComparePointCoord(
67 index_t nb_points,
68 const double* points,
69 index_t stride,
70 coord_index_t splitting_coord
71 182121 ) :
72 182121 nb_points_(nb_points),
73 182121 points_(points),
74 182121 stride_(stride),
75 182121 splitting_coord_(splitting_coord) {
76 geo_argused(nb_points_);
77 }
78
79 /**
80 * \brief Compares to point indices (does the
81 * indirection and coordinate lookup).
82 * \param[in] i index of first point to compare
83 * \param[in] j index of second point to compare
84 * \return true if point \p i is before point \p j, false otherwise
85 */
86 bool operator() (index_t i, index_t j) const {
87 geo_debug_assert(i < nb_points_);
88 geo_debug_assert(j < nb_points_);
89 return
90 25983242 (points_ + i * stride_)[splitting_coord_] <
91
22/22
✓ Branch 0 taken 28 times.
✓ Branch 1 taken 15 times.
✓ Branch 2 taken 21 times.
✓ Branch 3 taken 4 times.
✓ Branch 4 taken 31651 times.
✓ Branch 5 taken 97320 times.
✓ Branch 6 taken 15208612 times.
✓ Branch 7 taken 9472022 times.
✓ Branch 8 taken 13275749 times.
✓ Branch 9 taken 9472022 times.
✓ Branch 10 taken 469950 times.
✓ Branch 11 taken 446564 times.
✓ Branch 12 taken 469950 times.
✓ Branch 13 taken 446564 times.
✓ Branch 14 taken 216009 times.
✓ Branch 15 taken 253941 times.
✓ Branch 16 taken 179710 times.
✓ Branch 17 taken 266854 times.
✓ Branch 18 taken 159691 times.
✓ Branch 19 taken 97320 times.
✓ Branch 20 taken 32 times.
✓ Branch 21 taken 12 times.
 49647527 (points_ + j * stride_)[splitting_coord_]
92 ;
93 }
94
95 private:
96 index_t nb_points_;
97 const double* points_;
98 index_t stride_;
99 coord_index_t splitting_coord_;
100 };
101 }
102
103 /****************************************************************************/
104
105 namespace GEO {
106
107 145 KdTree::KdTree(coord_index_t dim) :
108 NearestNeighborSearch(dim),
109 bbox_min_(dim),
110 bbox_max_(dim),
111
1/2
✓ Branch 2 taken 145 times.
✗ Branch 3 not taken.
 145 root_(NO_INDEX) {
112 145 }
113
114
1/2
✓ Branch 0 taken 145 times.
✗ Branch 1 not taken.
 290 KdTree::~KdTree() {
115 290 }
116
117 ✗ bool KdTree::stride_supported() const {
118 ✗ return true;
119 }
120
121 585 void KdTree::set_points(
122 index_t nb_points, const double* points, index_t stride
123 ) {
124 585 nb_points_ = nb_points;
125 585 points_ = points;
126 585 stride_ = stride;
127
128 585 point_index_.resize(nb_points);
129
2/2
✓ Branch 0 taken 1885581 times.
✓ Branch 1 taken 585 times.
 1886166 for(index_t i = 0; i < nb_points; i++) {
130 1885581 point_index_[i] = i;
131 }
132
133 // Compute the bounding box.
134
2/2
✓ Branch 0 taken 2692 times.
✓ Branch 1 taken 585 times.
 3277 for(coord_index_t c = 0; c < dimension(); ++c) {
135 2692 bbox_min_[c] = Numeric::max_float64();
136 2692 bbox_max_[c] = -Numeric::max_float64();
137 }
138
2/2
✓ Branch 0 taken 1885581 times.
✓ Branch 1 taken 585 times.
 1886166 for(index_t i = 0; i < nb_points; ++i) {
139 const double* p = point_ptr(i);
140
2/2
✓ Branch 0 taken 8715543 times.
✓ Branch 1 taken 1885581 times.
 10601124 for(coord_index_t c = 0; c < dimension(); ++c) {
141
4/4
✓ Branch 0 taken 68719 times.
✓ Branch 1 taken 8646824 times.
✓ Branch 2 taken 38514 times.
✓ Branch 3 taken 8677029 times.
 8784262 bbox_min_[c] = std::min(bbox_min_[c], p[c]);
142 8715543 bbox_max_[c] = std::max(bbox_max_[c], p[c]);
143 }
144 }
145
146 585 root_ = build_tree();
147 585 }
148
149 537 void KdTree::set_points(
150 index_t nb_points, const double* points
151 ) {
152 537 set_points(nb_points, points, dimension());
153 537 }
154
155
156 1978810 void KdTree::get_nearest_neighbors(
157 index_t nb_neighbors,
158 const double* query_point,
159 index_t* neighbors,
160 double* neighbors_sq_dist
161 ) const {
162
163 geo_debug_assert(nb_neighbors <= nb_points());
164
165 // Compute distance between query point and global bounding box
166 // and copy global bounding box to local variables (bbox_min, bbox_max),
167 // allocated on the stack. bbox_min and bbox_max are updated during the
168 // traversal of the BalancedKdTree (see
169 // get_nearest_neighbors_recursive()). They are necessary to
170 // compute the distance between the query point and the
171 // bbox of the current node.
172 1978810 double box_dist = 0.0;
173 1978810 double* bbox_min = (double*) (alloca(dimension() * sizeof(double)));
174 1978810 double* bbox_max = (double*) (alloca(dimension() * sizeof(double)));
175 1978810 init_bbox_and_bbox_dist_for_traversal(
176 bbox_min, bbox_max, box_dist, query_point
177 );
178 NearestNeighbors NN(
179 nb_neighbors,
180 neighbors,
181 neighbors_sq_dist,
182 1978810 (index_t*)alloca(sizeof(index_t) * (nb_neighbors+1)),
183 1978810 (double*)alloca(sizeof(double) * (nb_neighbors+1))
184 );
185 1978810 get_nearest_neighbors_recursive(
186 1978810 root_, 0, nb_points(), bbox_min, bbox_max, box_dist, query_point, NN
187 );
188 NN.copy_to_user();
189 1978810 }
190
191 ✗ void KdTree::get_nearest_neighbors(
192 index_t nb_neighbors,
193 const double* query_point,
194 index_t* neighbors,
195 double* neighbors_sq_dist,
196 KeepInitialValues KV
197 ) const {
198 geo_debug_assert(nb_neighbors <= nb_points());
199 geo_argused(KV);
200 // Compute distance between query point and global bounding box
201 // and copy global bounding box to local variables (bbox_min, bbox_max),
202 // allocated on the stack. bbox_min and bbox_max are updated during the
203 // traversal of the BalancedKdTree
204 // (see get_nearest_neighbors_recursive()). They
205 // are necessary to compute the distance between the query point and the
206 // bbox of the current node.
207 ✗ double box_dist = 0.0;
208 ✗ double* bbox_min = (double*) (alloca(dimension() * sizeof(double)));
209 ✗ double* bbox_max = (double*) (alloca(dimension() * sizeof(double)));
210 ✗ init_bbox_and_bbox_dist_for_traversal(
211 bbox_min, bbox_max, box_dist, query_point
212 );
213 NearestNeighbors NN(
214 nb_neighbors,
215 neighbors,
216 neighbors_sq_dist,
217 ✗ (index_t*)alloca(sizeof(index_t) * (nb_neighbors+1)),
218 ✗ (double*)alloca(sizeof(double) * (nb_neighbors+1))
219 );
220 ✗ NN.copy_from_user();
221 ✗ get_nearest_neighbors_recursive(
222 ✗ root_, 0, nb_points(), bbox_min, bbox_max, box_dist, query_point, NN
223 );
224 NN.copy_to_user();
225 ✗ }
226
227 1873025 void KdTree::get_nearest_neighbors(
228 index_t nb_neighbors,
229 index_t q_index,
230 index_t* neighbors,
231 double* neighbors_sq_dist
232 ) const {
233 // TODO: optimized version that uses the fact that
234 // we know that query_point is in the search data
235 // structure already.
236 // (I tried something already, see in the Attic,
237 // but it did not give any significant speedup).
238 1873025 get_nearest_neighbors(
239 nb_neighbors, point_ptr(q_index),
240 neighbors, neighbors_sq_dist
241 );
242 1873025 }
243
244 64786802 void KdTree::get_nearest_neighbors_recursive(
245 index_t node_index, index_t b, index_t e,
246 double* bbox_min, double* bbox_max, double box_dist,
247 const double* query_point, NearestNeighbors& NN
248 ) const {
249 geo_debug_assert(e > b);
250
251 // Simple case (node is a leaf)
252
2/2
✓ Branch 0 taken 19752365 times.
✓ Branch 1 taken 45034437 times.
 64786802 if((e - b) <= MAX_LEAF_SIZE) {
253 19752365 get_nearest_neighbors_leaf(node_index, b, e, query_point, NN);
254 19752365 return;
255 }
256
257 // Get node attributes (virtual function call).
258
259 index_t left_node_index;
260 index_t right_node_index;
261 coord_index_t coord;
262 index_t m;
263 double val;
264
265 45034437 get_node(
266 node_index, b, e,
267 left_node_index, right_node_index,
268 coord, m, val
269 );
270
271 45034437 double cut_diff = query_point[coord] - val;
272
273 // If the query point is on the left side
274
2/2
✓ Branch 0 taken 22253646 times.
✓ Branch 1 taken 22780791 times.
 45034437 if(cut_diff < 0.0) {
275
276 // Traverse left subtree
277 {
278 22253646 double bbox_max_save = bbox_max[coord];
279 22253646 bbox_max[coord] = val;
280 22253646 get_nearest_neighbors_recursive(
281 left_node_index, b, m,
282 bbox_min, bbox_max, box_dist, query_point, NN
283 );
284 22253646 bbox_max[coord] = bbox_max_save;
285 }
286
287 // Update bbox distance (now measures the
288 // distance to the bbox of the right subtree)
289 22253646 double box_diff = bbox_min[coord] - query_point[coord];
290
2/2
✓ Branch 0 taken 5607721 times.
✓ Branch 1 taken 16645925 times.
 22253646 if(box_diff > 0.0) {
291 5607721 box_dist -= geo_sqr(box_diff);
292 }
293
2/2
✓ Branch 0 taken 20203256 times.
✓ Branch 1 taken 2050390 times.
 22253646 box_dist += geo_sqr(cut_diff);
294
295 // Traverse the right subtree, only if bbox
296 // distance is nearer than furthest neighbor,
297 // else there is no chance that the right
298 // subtree contains points that will change
299 // anything in the nearest neighbors NN.
300
2/2
✓ Branch 0 taken 8911767 times.
✓ Branch 1 taken 13341879 times.
 22253646 if(box_dist <= NN.furthest_neighbor_sq_dist()) {
301 double bbox_min_save = bbox_min[coord];
302 8911767 bbox_min[coord] = val;
303 8911767 get_nearest_neighbors_recursive(
304 right_node_index, m, e,
305 bbox_min, bbox_max, box_dist, query_point, NN
306 );
307 8911767 bbox_min[coord] = bbox_min_save;
308 }
309 } else {
310 // else the query point is on the right side
311 // (then do the same with left and right subtree
312 // permutted).
313 {
314 22780791 double bbox_min_save = bbox_min[coord];
315 22780791 bbox_min[coord] = val;
316 22780791 get_nearest_neighbors_recursive(
317 right_node_index, m, e,
318 bbox_min, bbox_max, box_dist, query_point, NN
319 );
320 22780791 bbox_min[coord] = bbox_min_save;
321 }
322
323 // Update bbox distance (now measures the
324 // distance to the bbox of the left subtree)
325 22780791 double box_diff = query_point[coord] - bbox_max[coord];
326
2/2
✓ Branch 0 taken 5548587 times.
✓ Branch 1 taken 17232204 times.
 22780791 if(box_diff > 0.0) {
327 5548587 box_dist -= geo_sqr(box_diff);
328 }
329
2/2
✓ Branch 0 taken 20811574 times.
✓ Branch 1 taken 1969217 times.
 22780791 box_dist += geo_sqr(cut_diff);
330
331
2/2
✓ Branch 0 taken 8861788 times.
✓ Branch 1 taken 13919003 times.
 22780791 if(box_dist <= NN.furthest_neighbor_sq_dist()) {
332 double bbox_max_save = bbox_max[coord];
333 8861788 bbox_max[coord] = val;
334 8861788 get_nearest_neighbors_recursive(
335 left_node_index, b, m,
336 bbox_min, bbox_max, box_dist, query_point, NN
337 );
338 8861788 bbox_max[coord] = bbox_max_save;
339 }
340 }
341 }
342
343 19752365 void KdTree::get_nearest_neighbors_leaf(
344 index_t node_index, index_t b, index_t e,
345 const double* query_point,
346 NearestNeighbors& NN
347 ) const {
348 geo_argused(node_index);
349
2/2
✓ Branch 0 taken 13753948 times.
✓ Branch 1 taken 5998417 times.
 19752365 NN.nb_visited += (e-b);
350 double R = NN.furthest_neighbor_sq_dist();
351 index_t nb = e-b;
352 const index_t* geo_restrict idx = &point_index_[b];
353
354 // TODO: check generated ASM (I'd like to have AVX here).
355 // We may need to dispatch according to dimension.
356
357 index_t local_idx[MAX_LEAF_SIZE];
358 double local_sq_dist[MAX_LEAF_SIZE];
359
360 // Cache indices and computed distances in local
361 // array. I guess AVX likes that (to be checked).
362 // Not sure, because access to p is indirect, maybe
363 // I should copy the points to local memory before
364 // computing the distances (or having another copy
365 // of the points array that I pre-reorder so that
366 // leaf's points are in a contiguous chunk of memory),
367 // to be tested...
368
2/2
✓ Branch 0 taken 211804435 times.
✓ Branch 1 taken 19752365 times.
 231556800 for(index_t ii=0; ii<nb; ++ii) {
369 211804435 index_t i = idx[ii];
370 const double* geo_restrict p = point_ptr(i);
371 double sq_dist = Geom::distance2(
372 query_point, p, dimension()
373 );
374 211804435 local_idx[ii] = i;
375 211804435 local_sq_dist[ii] = sq_dist;
376 }
377
378 // Now insert the points that are nearer to query
379 // point than NN's bounding ball.
380
2/2
✓ Branch 0 taken 211804435 times.
✓ Branch 1 taken 19752365 times.
 231556800 for(index_t ii=0; ii<nb; ++ii) {
381 211804435 double sq_dist = local_sq_dist[ii];
382
2/2
✓ Branch 0 taken 86137314 times.
✓ Branch 1 taken 125667121 times.
 211804435 if(sq_dist <= R) {
383 86137314 NN.insert(local_idx[ii],sq_dist);
384 R = NN.furthest_neighbor_sq_dist();
385 }
386 }
387 19752365 }
388
389 1978810 void KdTree::init_bbox_and_bbox_dist_for_traversal(
390 double* bbox_min, double* bbox_max,
391 double& box_dist, const double* query_point
392 ) const {
393 // Compute distance between query point and global bounding box
394 // and copy global bounding box to local variables (bbox_min, bbox_max),
395 // allocated on the stack. bbox_min and bbox_max are updated during the
396 // traversal of the KdTree (see get_nearest_neighbors_recursive()). They
397 // are necessary to compute the distance between the query point and the
398 // bbox of the current node.
399 1978810 box_dist = 0.0;
400
2/2
✓ Branch 0 taken 9016294 times.
✓ Branch 1 taken 1978810 times.
 10995104 for(coord_index_t c = 0; c < dimension(); ++c) {
401 9016294 bbox_min[c] = bbox_min_[c];
402 9016294 bbox_max[c] = bbox_max_[c];
403
2/2
✓ Branch 0 taken 446 times.
✓ Branch 1 taken 9015848 times.
 9016294 if(query_point[c] < bbox_min_[c]) {
404 446 box_dist += geo_sqr(bbox_min_[c] - query_point[c]);
405
2/2
✓ Branch 0 taken 538 times.
✓ Branch 1 taken 9015310 times.
 9015848 } else if(query_point[c] > bbox_max_[c]) {
406 538 box_dist += geo_sqr(bbox_max_[c] - query_point[c]);
407 }
408 }
409 1978810 }
410
411 /****************************************************************************/
412
413 129 BalancedKdTree::BalancedKdTree(coord_index_t dim) :
414 KdTree(dim),
415 129 m0_(max_index_t()),
416 129 m1_(max_index_t()),
417 129 m2_(max_index_t()),
418 129 m3_(max_index_t()),
419 129 m4_(max_index_t()),
420 129 m5_(max_index_t()),
421 129 m6_(max_index_t()),
422 129 m7_(max_index_t()),
423 129 m8_(max_index_t()) {
424 129 }
425
426
2/2
✓ Branch 0 taken 128 times.
✓ Branch 1 taken 1 times.
 516 BalancedKdTree::~BalancedKdTree() {
427 516 }
428
429 569 index_t BalancedKdTree::build_tree() {
430 569 index_t sz = max_node_index(1, 0, nb_points()) + 1;
431 569 splitting_coord_.resize(sz);
432 569 splitting_val_.resize(sz);
433
434 // If there are more than 16*MAX_LEAF_SIZE (=256) points,
435 // create the tree in parallel
436 if(
437
3/4
✓ Branch 0 taken 499 times.
✓ Branch 1 taken 70 times.
✓ Branch 2 taken 499 times.
✗ Branch 3 not taken.
 1068 nb_points() >= (16 * MAX_LEAF_SIZE) &&
438 499 Process::maximum_concurrent_threads() > 1
439 ) {
440 499 m0_ = 0;
441 499 m8_ = nb_points();
442 // Create the first level of the tree
443
1/2
✓ Branch 2 taken 499 times.
✗ Branch 3 not taken.
 499 m4_ = split_kd_node(1, m0_, m8_);
444
445 // Create the second level of the tree
446 // (using two threads)
447
1/2
✓ Branch 1 taken 499 times.
✗ Branch 2 not taken.
 499 parallel(
448
1/4
✓ Branch 0 taken 499 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 998 [this]() { m2_ = split_kd_node(2, m0_, m4_); },
449
1/2
✓ Branch 1 taken 499 times.
✗ Branch 2 not taken.
 998 [this]() { m6_ = split_kd_node(3, m4_, m8_); }
450 );
451
452 // Create the third level of the tree
453 // (using four threads)
454 parallel(
455
1/4
✓ Branch 0 taken 499 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 998 [this]() { m1_ = split_kd_node(4, m0_, m2_); },
456
1/4
✓ Branch 0 taken 499 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 998 [this]() { m3_ = split_kd_node(5, m2_, m4_); },
457
1/4
✓ Branch 0 taken 499 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 998 [this]() { m5_ = split_kd_node(6, m4_, m6_); },
458
1/2
✓ Branch 1 taken 499 times.
✗ Branch 2 not taken.
 998 [this]() { m7_ = split_kd_node(7, m6_, m8_); }
459 );
460
461 // Create the fourth level of the tree
462 // (using eight threads)
463
1/2
✓ Branch 1 taken 499 times.
✗ Branch 2 not taken.
 499 parallel(
464
1/4
✓ Branch 1 taken 499 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 998 [this]() { create_kd_tree_recursive(8 , m0_, m1_); },
465
1/4
✓ Branch 1 taken 499 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 998 [this]() { create_kd_tree_recursive(9 , m1_, m2_); },
466
1/4
✓ Branch 1 taken 499 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 998 [this]() { create_kd_tree_recursive(10, m2_, m3_); },
467
1/4
✓ Branch 1 taken 499 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 998 [this]() { create_kd_tree_recursive(11, m3_, m4_); },
468
1/4
✓ Branch 1 taken 499 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 998 [this]() { create_kd_tree_recursive(12, m4_, m5_); },
469
1/4
✓ Branch 1 taken 499 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 998 [this]() { create_kd_tree_recursive(13, m5_, m6_); },
470
1/4
✓ Branch 1 taken 499 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 998 [this]() { create_kd_tree_recursive(14, m6_, m7_); },
471 998 [this]() { create_kd_tree_recursive(15, m7_, m8_); }
472 );
473
474 } else {
475 70 create_kd_tree_recursive(1, 0, nb_points());
476 }
477
478 // Root node is number 1.
479 // This is because "children at 2*n and 2*n+1" does not
480 // work with 0 !!
481 569 return 1;
482 }
483
484 182121 index_t BalancedKdTree::split_kd_node(
485 index_t node_index, index_t b, index_t e
486 ) {
487
488 geo_debug_assert(e > b);
489 // Do not split leafs
490
1/2
✓ Branch 0 taken 182121 times.
✗ Branch 1 not taken.
 182121 if(b + 1 == e) {
491 return b;
492 }
493
494 182121 coord_index_t splitting_coord = best_splitting_coord(b, e);
495 182121 index_t m = b + (e - b) / 2;
496 geo_debug_assert(m < e);
497
498 // sorts the indices in such a way that points's
499 // coordinates splitting_coord in [b,m) are smaller
500 // than m's and points in [m,e) are
501 // greater or equal to m's
502 182121 std::nth_element(
503 point_index_.begin() + std::ptrdiff_t(b),
504 point_index_.begin() + std::ptrdiff_t(m),
505 point_index_.begin() + std::ptrdiff_t(e),
506 ComparePointCoord(
507 nb_points_, points_, stride_, splitting_coord
508 182121 )
509 );
510
511 // Initialize node's variables (splitting coord and
512 // splitting value)
513 182121 splitting_coord_[node_index] = splitting_coord;
514 182121 splitting_val_[node_index] =
515 182121 point_ptr(point_index_[m])[splitting_coord];
516 182121 return m;
517 }
518
519 182121 coord_index_t BalancedKdTree::best_splitting_coord(
520 index_t b, index_t e
521 ) {
522 // Returns the coordinates that maximizes
523 // point's spread. We should probably
524 // use a tradeoff between spread and
525 // bbox shape ratio, as done in ANN, but
526 // this simple method seems to give good
527 // results in our case.
528 coord_index_t result = 0;
529 182121 double max_spread = spread(b, e, 0);
530
2/2
✓ Branch 0 taken 676969 times.
✓ Branch 1 taken 182121 times.
 859090 for(coord_index_t c = 1; c < dimension(); ++c) {
531 double coord_spread = spread(b, e, c);
532
2/2
✓ Branch 0 taken 152333 times.
✓ Branch 1 taken 524636 times.
 676969 if(coord_spread > max_spread) {
533 result = c;
534 max_spread = coord_spread;
535 }
536 }
537 182121 return result;
538 }
539
540 44734899 void BalancedKdTree::get_node(
541 index_t n, index_t b, index_t e,
542 index_t& left_child, index_t& right_child,
543 coord_index_t& splitting_coord,
544 index_t& m,
545 double& splitting_val
546 ) const {
547 44734899 left_child = 2*n;
548 44734899 right_child = 2*n+1;
549 44734899 splitting_coord = splitting_coord_[n];
550 44734899 m = b + (e - b) / 2;
551 44734899 splitting_val = splitting_val_[n];
552 44734899 }
553
554 /**************************************************************************/
555
556 16 AdaptiveKdTree::AdaptiveKdTree(coord_index_t dim) : KdTree(dim) {
557 16 }
558
559 1860 index_t AdaptiveKdTree::new_node() {
560 1860 splitting_coord_.push_back(0);
561 1860 splitting_val_.push_back(0.0);
562 1860 node_m_.push_back(0);
563
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1860 times.
 1860 node_right_child_.push_back(0);
564 1860 return nb_nodes()-1;
565 }
566
567 16 index_t AdaptiveKdTree::build_tree() {
568 // Create kd-tree. Local copy of the bbox is used, because it
569 // is modified during traversal.
570 16 double* bbox_min = (double*) (alloca(dimension() * sizeof(double)));
571 16 double* bbox_max = (double*) (alloca(dimension() * sizeof(double)));
572
2/2
✓ Branch 0 taken 48 times.
✓ Branch 1 taken 16 times.
 64 for(coord_index_t c = 0; c < dimension(); ++c) {
573 48 bbox_min[c] = bbox_min_[c];
574 48 bbox_max[c] = bbox_max_[c];
575 }
576
577 16 splitting_coord_.resize(0);
578 16 splitting_val_.resize(0);
579 16 node_m_.resize(0);
580 16 node_right_child_.resize(0);
581
582 16 return create_kd_tree_recursive(0, nb_points(), bbox_min, bbox_max);
583 }
584
585
586 3736 index_t AdaptiveKdTree::create_kd_tree_recursive(
587 index_t b, index_t e,
588 double* bbox_min, double* bbox_max
589 ) {
590
2/2
✓ Branch 0 taken 1860 times.
✓ Branch 1 taken 1876 times.
 3736 if(e - b <= MAX_LEAF_SIZE) {
591 return NO_INDEX;
592 }
593
594 index_t m;
595 coord_index_t cut_dim;
596 double cut_val;
597
598 // Compute m, cut_dim and cut_val,
599 // and reorganize indices along cut_val.
600 1860 split_kd_node(
601 b, e, bbox_min, bbox_max,
602 m, cut_dim, cut_val
603 );
604
605 1860 index_t n = new_node();
606 1860 splitting_coord_[n] = cut_dim;
607 1860 splitting_val_[n] = cut_val;
608 1860 node_m_[n] = m;
609
610 {
611 1860 double bbox_max_save = bbox_max[cut_dim];
612 1860 bbox_max[cut_dim] = cut_val;
613 // This creates the left child. It does not need to be stored
614 // because the tree is created in an order such that the
615 // left child of node n is n+1.
616 1860 create_kd_tree_recursive(b, m, bbox_min, bbox_max);
617 1860 bbox_max[cut_dim] = bbox_max_save;
618 }
619
620 {
621 1860 double bbox_min_save = bbox_min[cut_dim];
622 1860 bbox_min[cut_dim] = cut_val;
623 // Note: right_child needs to be copied to local variables
624 // before being set in node_right_child_[n] because
625 // create_kd_tree_recursive() modifies node_right_child_
626 // (reallocates). If the following two lines are
627 // done in a single assignment, then the computed reference to
628 // node_right_child_[n]
629 // in the lhs is no longer valid after the rhs is evaluated !
630 1860 index_t right_child = create_kd_tree_recursive(
631 m, e, bbox_min, bbox_max
632 );
633 1860 node_right_child_[n] = right_child;
634 1860 bbox_min[cut_dim] = bbox_min_save;
635 }
636
637 1860 return n;
638 }
639
640 1860 void AdaptiveKdTree::split_kd_node(
641 index_t b, index_t e,
642 double* bbox_min, double* bbox_max,
643 index_t& m, coord_index_t& cut_dim, double& cut_val
644 ) {
645 // Like "sliding midpoint split" in ANN.
646
647 const double ERR=0.001;
648
649 // Find length of longest box size
650 1860 double max_length = -1.0;
651
2/2
✓ Branch 0 taken 5580 times.
✓ Branch 1 taken 1860 times.
 7440 for(coord_index_t d=0; d<dimension(); ++d) {
652 5580 double length = bbox_max[d] - bbox_min[d];
653 5580 max_length = std::max(max_length, length);
654 }
655
656 // Cutting coordinate
657 1860 cut_dim=0;
658
659 // Find long side with most spread
660 double max_spread = -1.0;
661
2/2
✓ Branch 0 taken 5580 times.
✓ Branch 1 taken 1860 times.
 7440 for(coord_index_t d=0; d<dimension(); ++d) {
662 5580 double length = bbox_max[d] - bbox_min[d];
663 // Is it among longest ?
664
2/2
✓ Branch 0 taken 3252 times.
✓ Branch 1 taken 2328 times.
 5580 if(length >= (1.0 - ERR)*max_length) {
665 2328 double spr = spread(b, e, d);
666
2/2
✓ Branch 0 taken 339 times.
✓ Branch 1 taken 1989 times.
 2328 if(spr > max_spread) {
667 max_spread = spr;
668 1989 cut_dim = d;
669 }
670 }
671 }
672
673 1860 double ideal_cut_val = 0.5*(bbox_min[cut_dim] + bbox_max[cut_dim]);
674
675 double coord_min, coord_max;
676 1860 get_minmax(b, e, cut_dim, coord_min, coord_max);
677
678 1860 cut_val = ideal_cut_val;
679
680 // Make it slide if need be.
681
2/2
✓ Branch 0 taken 77 times.
✓ Branch 1 taken 1783 times.
 1860 if(ideal_cut_val < coord_min) {
682 77 cut_val = coord_min;
683
2/2
✓ Branch 0 taken 99 times.
✓ Branch 1 taken 1684 times.
 1783 } else if (ideal_cut_val > coord_max) {
684 99 cut_val = coord_max;
685 }
686
687 index_t br1,br2;
688 1860 plane_split(b,e,cut_dim,cut_val,br1,br2);
689
690 1860 index_t m0 = b + (e-b)/2;
691 1860 m = m0;
692
693
2/2
✓ Branch 0 taken 77 times.
✓ Branch 1 taken 1783 times.
 1860 if(ideal_cut_val < coord_min) {
694 77 m = b+1;
695
2/2
✓ Branch 0 taken 99 times.
✓ Branch 1 taken 1684 times.
 1783 } else if(ideal_cut_val > coord_max) {
696 99 m = e-1;
697
2/2
✓ Branch 0 taken 584 times.
✓ Branch 1 taken 1100 times.
 1684 } else if(br1 > m0) {
698 584 m = br1;
699
2/2
✓ Branch 0 taken 451 times.
✓ Branch 1 taken 649 times.
 1100 } else if(br2 < m0) {
700 451 m = br2;
701 }
702 1860 }
703
704 1860 void AdaptiveKdTree::plane_split(
705 index_t b_in, index_t e_in, coord_index_t coord, double val,
706 index_t& br1_out, index_t& br2_out
707 ) {
708 1860 int b = int(b_in);
709 1860 int e = int(e_in);
710 int l=b;
711 1860 int r=e-1;
712 while(true) {
713
3/4
✗ Branch 0 not taken.
✓ Branch 1 taken 81226 times.
✓ Branch 2 taken 36928 times.
✓ Branch 3 taken 44298 times.
 81226 while(l < e && point_coord(l,coord) < val) {
714 44298 ++l;
715 }
716
4/4
✓ Branch 0 taken 89566 times.
✓ Branch 1 taken 9 times.
✓ Branch 2 taken 36919 times.
✓ Branch 3 taken 52647 times.
 89575 while(r >= 0 && point_coord(r,coord) >= val) {
717 52647 --r;
718 }
719
2/2
✓ Branch 0 taken 1860 times.
✓ Branch 1 taken 35068 times.
 36928 if(l > r) {
720 break;
721 }
722 std::swap(point_index_[l], point_index_[r]);
723 35068 ++l; --r;
724 }
725 int br1 = l;
726 r = e-1;
727 while(true) {
728
4/4
✓ Branch 0 taken 99 times.
✓ Branch 1 taken 6213 times.
✓ Branch 2 taken 4589 times.
✓ Branch 3 taken 1624 times.
 6312 while(l < e && point_coord(l,coord) <= val) {
729 1624 ++l;
730 }
731
4/4
✓ Branch 0 taken 80453 times.
✓ Branch 1 taken 1109 times.
✓ Branch 2 taken 3579 times.
✓ Branch 3 taken 76874 times.
 81562 while(r >= br1 && point_coord(r,coord) > val) {
732 76874 --r;
733 }
734
2/2
✓ Branch 0 taken 2828 times.
✓ Branch 1 taken 1860 times.
 4688 if(l > r) {
735 break;
736 }
737 std::swap(point_index_[l], point_index_[r]);
738 2828 ++l; --r;
739 }
740 int br2 = l;
741 1860 br1_out = index_t(br1);
742 1860 br2_out = index_t(br2);
743 1860 }
744
745 299538 void AdaptiveKdTree::get_node(
746 index_t n, index_t b, index_t e,
747 index_t& left_child, index_t& right_child,
748 coord_index_t& splitting_coord,
749 index_t& m,
750 double& splitting_val
751 ) const {
752 geo_debug_assert(n < nb_nodes());
753 geo_argused(b);
754 geo_argused(e);
755 299538 left_child = n+1;
756 299538 right_child = node_right_child_[n];
757 299538 splitting_coord = splitting_coord_[n];
758 299538 m = node_m_[n];
759 299538 splitting_val = splitting_val_[n];
760 299538 }
761
762 /*************************************************************************/
763
764 }
765