GCC Code Coverage Report

Directory: ./
File: lib/geogram/points/kd_tree.cpp
Date: 2026-09-07 02:28:19
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 141733 ) :
72 141733 nb_points_(nb_points),
73 141733 points_(points),
74 141733 stride_(stride),
75 141733 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 20029091 (points_ + i * stride_)[splitting_coord_] <
91
22/22
✓ Branch 0 taken 23 times.
✓ Branch 1 taken 17 times.
✓ Branch 2 taken 21 times.
✓ Branch 3 taken 4 times.
✓ Branch 4 taken 24657 times.
✓ Branch 5 taken 76806 times.
✓ Branch 6 taken 11547063 times.
✓ Branch 7 taken 7464850 times.
✓ Branch 8 taken 10268597 times.
✓ Branch 9 taken 7464850 times.
✓ Branch 10 taken 362280 times.
✓ Branch 11 taken 352997 times.
✓ Branch 12 taken 362280 times.
✓ Branch 13 taken 352997 times.
✓ Branch 14 taken 162773 times.
✓ Branch 15 taken 199507 times.
✓ Branch 16 taken 139895 times.
✓ Branch 17 taken 213102 times.
✓ Branch 18 taken 123528 times.
✓ Branch 19 taken 76806 times.
✓ Branch 20 taken 30 times.
✓ Branch 21 taken 9 times.
 38477815 (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 142 KdTree::KdTree(coord_index_t dim) :
108 NearestNeighborSearch(dim),
109 bbox_min_(dim),
110 bbox_max_(dim),
111
1/2
✓ Branch 2 taken 142 times.
✗ Branch 3 not taken.
 142 root_(NO_INDEX) {
112 142 }
113
114
1/2
✓ Branch 0 taken 142 times.
✗ Branch 1 not taken.
 284 KdTree::~KdTree() {
115 284 }
116
117 ✗ bool KdTree::stride_supported() const {
118 ✗ return true;
119 }
120
121 525 void KdTree::set_points(
122 index_t nb_points, const double* points, index_t stride
123 ) {
124 525 nb_points_ = nb_points;
125 525 points_ = points;
126 525 stride_ = stride;
127
128 525 point_index_.resize(nb_points);
129
2/2
✓ Branch 0 taken 1485426 times.
✓ Branch 1 taken 525 times.
 1485951 for(index_t i = 0; i < nb_points; i++) {
130 1485426 point_index_[i] = i;
131 }
132
133 // Compute the bounding box.
134
2/2
✓ Branch 0 taken 2389 times.
✓ Branch 1 taken 525 times.
 2914 for(coord_index_t c = 0; c < dimension(); ++c) {
135 2389 bbox_min_[c] = Numeric::max_float64();
136 2389 bbox_max_[c] = -Numeric::max_float64();
137 }
138
2/2
✓ Branch 0 taken 1485426 times.
✓ Branch 1 taken 525 times.
 1485951 for(index_t i = 0; i < nb_points; ++i) {
139 const double* p = point_ptr(i);
140
2/2
✓ Branch 0 taken 6900078 times.
✓ Branch 1 taken 1485426 times.
 8385504 for(coord_index_t c = 0; c < dimension(); ++c) {
141
4/4
✓ Branch 0 taken 55707 times.
✓ Branch 1 taken 6844371 times.
✓ Branch 2 taken 30916 times.
✓ Branch 3 taken 6869162 times.
 6955785 bbox_min_[c] = std::min(bbox_min_[c], p[c]);
142 6900078 bbox_max_[c] = std::max(bbox_max_[c], p[c]);
143 }
144 }
145
146 525 root_ = build_tree();
147 525 }
148
149 477 void KdTree::set_points(
150 index_t nb_points, const double* points
151 ) {
152 477 set_points(nb_points, points, dimension());
153 477 }
154
155
156 1546041 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 1546041 double box_dist = 0.0;
173 1546041 double* bbox_min = (double*) (alloca(dimension() * sizeof(double)));
174 1546041 double* bbox_max = (double*) (alloca(dimension() * sizeof(double)));
175 1546041 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 1546041 (index_t*)alloca(sizeof(index_t) * (nb_neighbors+1)),
183 1546041 (double*)alloca(sizeof(double) * (nb_neighbors+1))
184 );
185 1546041 get_nearest_neighbors_recursive(
186 1546041 root_, 0, nb_points(), bbox_min, bbox_max, box_dist, query_point, NN
187 );
188 NN.copy_to_user();
189 1546041 }
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 1472387 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 1472387 get_nearest_neighbors(
239 nb_neighbors, point_ptr(q_index),
240 neighbors, neighbors_sq_dist
241 );
242 1472387 }
243
244 48952196 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 15138117 times.
✓ Branch 1 taken 33814079 times.
 48952196 if((e - b) <= MAX_LEAF_SIZE) {
253 15138117 get_nearest_neighbors_leaf(node_index, b, e, query_point, NN);
254 15138117 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 33814079 get_node(
266 node_index, b, e,
267 left_node_index, right_node_index,
268 coord, m, val
269 );
270
271 33814079 double cut_diff = query_point[coord] - val;
272
273 // If the query point is on the left side
274
2/2
✓ Branch 0 taken 16998111 times.
✓ Branch 1 taken 16815968 times.
 33814079 if(cut_diff < 0.0) {
275
276 // Traverse left subtree
277 {
278 16998111 double bbox_max_save = bbox_max[coord];
279 16998111 bbox_max[coord] = val;
280 16998111 get_nearest_neighbors_recursive(
281 left_node_index, b, m,
282 bbox_min, bbox_max, box_dist, query_point, NN
283 );
284 16998111 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 16998111 double box_diff = bbox_min[coord] - query_point[coord];
290
2/2
✓ Branch 0 taken 4285893 times.
✓ Branch 1 taken 12712218 times.
 16998111 if(box_diff > 0.0) {
291 4285893 box_dist -= geo_sqr(box_diff);
292 }
293
2/2
✓ Branch 0 taken 15332383 times.
✓ Branch 1 taken 1665728 times.
 16998111 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 6872151 times.
✓ Branch 1 taken 10125960 times.
 16998111 if(box_dist <= NN.furthest_neighbor_sq_dist()) {
301 double bbox_min_save = bbox_min[coord];
302 6872151 bbox_min[coord] = val;
303 6872151 get_nearest_neighbors_recursive(
304 right_node_index, m, e,
305 bbox_min, bbox_max, box_dist, query_point, NN
306 );
307 6872151 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 16815968 double bbox_min_save = bbox_min[coord];
315 16815968 bbox_min[coord] = val;
316 16815968 get_nearest_neighbors_recursive(
317 right_node_index, m, e,
318 bbox_min, bbox_max, box_dist, query_point, NN
319 );
320 16815968 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 16815968 double box_diff = query_point[coord] - bbox_max[coord];
326
2/2
✓ Branch 0 taken 4059643 times.
✓ Branch 1 taken 12756325 times.
 16815968 if(box_diff > 0.0) {
327 4059643 box_dist -= geo_sqr(box_diff);
328 }
329
2/2
✓ Branch 0 taken 15250638 times.
✓ Branch 1 taken 1565330 times.
 16815968 box_dist += geo_sqr(cut_diff);
330
331
2/2
✓ Branch 0 taken 6719925 times.
✓ Branch 1 taken 10096043 times.
 16815968 if(box_dist <= NN.furthest_neighbor_sq_dist()) {
332 double bbox_max_save = bbox_max[coord];
333 6719925 bbox_max[coord] = val;
334 6719925 get_nearest_neighbors_recursive(
335 left_node_index, b, m,
336 bbox_min, bbox_max, box_dist, query_point, NN
337 );
338 6719925 bbox_max[coord] = bbox_max_save;
339 }
340 }
341 }
342
343 15138117 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 10361018 times.
✓ Branch 1 taken 4777099 times.
 15138117 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 162059661 times.
✓ Branch 1 taken 15138117 times.
 177197778 for(index_t ii=0; ii<nb; ++ii) {
369 162059661 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 162059661 local_idx[ii] = i;
375 162059661 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 162059661 times.
✓ Branch 1 taken 15138117 times.
 177197778 for(index_t ii=0; ii<nb; ++ii) {
381 162059661 double sq_dist = local_sq_dist[ii];
382
2/2
✓ Branch 0 taken 70377105 times.
✓ Branch 1 taken 91682556 times.
 162059661 if(sq_dist <= R) {
383 70377105 NN.insert(local_idx[ii],sq_dist);
384 R = NN.furthest_neighbor_sq_dist();
385 }
386 }
387 15138117 }
388
389 1546041 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 1546041 box_dist = 0.0;
400
2/2
✓ Branch 0 taken 7100836 times.
✓ Branch 1 taken 1546041 times.
 8646877 for(coord_index_t c = 0; c < dimension(); ++c) {
401 7100836 bbox_min[c] = bbox_min_[c];
402 7100836 bbox_max[c] = bbox_max_[c];
403
2/2
✓ Branch 0 taken 446 times.
✓ Branch 1 taken 7100390 times.
 7100836 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 7099852 times.
 7100390 } else if(query_point[c] > bbox_max_[c]) {
406 538 box_dist += geo_sqr(bbox_max_[c] - query_point[c]);
407 }
408 }
409 1546041 }
410
411 /****************************************************************************/
412
413 126 BalancedKdTree::BalancedKdTree(coord_index_t dim) :
414 KdTree(dim),
415 126 m0_(max_index_t()),
416 126 m1_(max_index_t()),
417 126 m2_(max_index_t()),
418 126 m3_(max_index_t()),
419 126 m4_(max_index_t()),
420 126 m5_(max_index_t()),
421 126 m6_(max_index_t()),
422 126 m7_(max_index_t()),
423 126 m8_(max_index_t()) {
424 126 }
425
426
2/2
✓ Branch 0 taken 125 times.
✓ Branch 1 taken 1 times.
 504 BalancedKdTree::~BalancedKdTree() {
427 504 }
428
429 509 index_t BalancedKdTree::build_tree() {
430 509 index_t sz = max_node_index(1, 0, nb_points()) + 1;
431 509 splitting_coord_.resize(sz);
432 509 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 439 times.
✓ Branch 1 taken 70 times.
✓ Branch 2 taken 439 times.
✗ Branch 3 not taken.
 948 nb_points() >= (16 * MAX_LEAF_SIZE) &&
438 439 Process::maximum_concurrent_threads() > 1
439 ) {
440 439 m0_ = 0;
441 439 m8_ = nb_points();
442 // Create the first level of the tree
443
1/2
✓ Branch 2 taken 439 times.
✗ Branch 3 not taken.
 439 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 439 times.
✗ Branch 2 not taken.
 439 parallel(
448
1/4
✓ Branch 0 taken 439 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 878 [this]() { m2_ = split_kd_node(2, m0_, m4_); },
449
1/2
✓ Branch 1 taken 439 times.
✗ Branch 2 not taken.
 878 [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 439 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 878 [this]() { m1_ = split_kd_node(4, m0_, m2_); },
456
1/4
✓ Branch 0 taken 439 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 878 [this]() { m3_ = split_kd_node(5, m2_, m4_); },
457
1/4
✓ Branch 0 taken 439 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 878 [this]() { m5_ = split_kd_node(6, m4_, m6_); },
458
1/2
✓ Branch 1 taken 439 times.
✗ Branch 2 not taken.
 878 [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 439 times.
✗ Branch 2 not taken.
 439 parallel(
464
1/4
✓ Branch 1 taken 439 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 878 [this]() { create_kd_tree_recursive(8 , m0_, m1_); },
465
1/4
✓ Branch 1 taken 439 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 878 [this]() { create_kd_tree_recursive(9 , m1_, m2_); },
466
1/4
✓ Branch 1 taken 439 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 878 [this]() { create_kd_tree_recursive(10, m2_, m3_); },
467
1/4
✓ Branch 1 taken 439 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 878 [this]() { create_kd_tree_recursive(11, m3_, m4_); },
468
1/4
✓ Branch 1 taken 439 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 878 [this]() { create_kd_tree_recursive(12, m4_, m5_); },
469
1/4
✓ Branch 1 taken 439 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 878 [this]() { create_kd_tree_recursive(13, m5_, m6_); },
470
1/4
✓ Branch 1 taken 439 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 878 [this]() { create_kd_tree_recursive(14, m6_, m7_); },
471 878 [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 509 return 1;
482 }
483
484 141733 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 141733 times.
✗ Branch 1 not taken.
 141733 if(b + 1 == e) {
491 return b;
492 }
493
494 141733 coord_index_t splitting_coord = best_splitting_coord(b, e);
495 141733 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 141733 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 141733 )
509 );
510
511 // Initialize node's variables (splitting coord and
512 // splitting value)
513 141733 splitting_coord_[node_index] = splitting_coord;
514 141733 splitting_val_[node_index] =
515 141733 point_ptr(point_index_[m])[splitting_coord];
516 141733 return m;
517 }
518
519 141733 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 141733 double max_spread = spread(b, e, 0);
530
2/2
✓ Branch 0 taken 533340 times.
✓ Branch 1 taken 141733 times.
 675073 for(coord_index_t c = 1; c < dimension(); ++c) {
531 double coord_spread = spread(b, e, c);
532
2/2
✓ Branch 0 taken 121239 times.
✓ Branch 1 taken 412101 times.
 533340 if(coord_spread > max_spread) {
533 result = c;
534 max_spread = coord_spread;
535 }
536 }
537 141733 return result;
538 }
539
540 33514541 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 33514541 left_child = 2*n;
548 33514541 right_child = 2*n+1;
549 33514541 splitting_coord = splitting_coord_[n];
550 33514541 m = b + (e - b) / 2;
551 33514541 splitting_val = splitting_val_[n];
552 33514541 }
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