GRASS GIS 8 Programmer's Manual 8.4.1(2025)-45ca3179ab
Loading...
Searching...
No Matches
kdtree.c
Go to the documentation of this file.
1/*!
2 * \file kdtree.c
3 *
4 * \brief binary search tree
5 *
6 * Dynamic balanced k-d tree implementation
7 *
8 * (C) 2014 by the GRASS Development Team
9 *
10 * This program is free software under the GNU General Public License
11 * (>=v2). Read the file COPYING that comes with GRASS for details.
12 *
13 * \author Markus Metz
14 */
15
16#include <stdlib.h>
17#include <string.h>
18#include <math.h>
19#include <grass/gis.h>
20#include <grass/glocale.h>
21#include "kdtree.h"
22
23#define KD_BTOL 7
24
25#ifdef KD_DEBUG
26#undef KD_DEBUG
27#endif
28
29static int rcalls = 0;
30static int rcallsmax = 0;
31
32static struct kdnode *kdtree_insert2(struct kdtree *, struct kdnode *,
33 struct kdnode *, int, int);
34static int kdtree_replace(struct kdtree *, struct kdnode *);
35static int kdtree_balance(struct kdtree *, struct kdnode *, int);
36static int kdtree_first(struct kdtrav *, double *, int *);
37static int kdtree_next(struct kdtrav *, double *, int *);
38
39static int cmp(struct kdnode *a, struct kdnode *b, int p)
40{
41 if (a->c[p] < b->c[p])
42 return -1;
43 if (a->c[p] > b->c[p])
44 return 1;
45
46 return (a->uid < b->uid ? -1 : a->uid > b->uid);
47}
48
49static int cmpc(struct kdnode *a, struct kdnode *b, struct kdtree *t)
50{
51 int i;
52
53 for (i = 0; i < t->ndims; i++) {
54 if (a->c[i] != b->c[i]) {
55 return 1;
56 }
57 }
58
59 return 0;
60}
61
62static struct kdnode *kdtree_newnode(struct kdtree *t)
63{
64 struct kdnode *n = G_malloc(sizeof(struct kdnode));
65
66 n->c = G_malloc(t->ndims * sizeof(double));
67 n->dim = 0;
68 n->depth = 0;
69 n->balance = 0;
70 n->uid = 0;
71 n->child[0] = NULL;
72 n->child[1] = NULL;
73
74 return n;
75}
76
77static void kdtree_free_node(struct kdnode *n)
78{
79 G_free(n->c);
80 G_free(n);
81}
82
83static void kdtree_update_node(struct kdtree *t, struct kdnode *n)
84{
85 int ld, rd, btol;
86
87 ld = (!n->child[0] ? -1 : n->child[0]->depth);
88 rd = (!n->child[1] ? -1 : n->child[1]->depth);
89 n->depth = MAX(ld, rd) + 1;
90
91 n->balance = 0;
92 /* set balance flag if any of the node's subtrees needs balancing
93 * or if the node itself needs balancing */
94 if ((n->child[0] && n->child[0]->balance) ||
95 (n->child[1] && n->child[1]->balance)) {
96 n->balance = 1;
97
98 return;
99 }
100
101 btol = t->btol;
102 if (!n->child[0] || !n->child[1])
103 btol = 2;
104
105 if (ld > rd + btol || rd > ld + btol)
106 n->balance = 1;
107}
108
109/* create a new k-d tree with ndims dimensions,
110 * optionally set balancing tolerance */
111struct kdtree *kdtree_create(char ndims, int *btol)
112{
113 int i;
114 struct kdtree *t;
115
116 t = G_malloc(sizeof(struct kdtree));
117
118 t->ndims = ndims;
119 t->csize = ndims * sizeof(double);
120 t->btol = KD_BTOL;
121 if (btol) {
122 t->btol = *btol;
123 if (t->btol < 2)
124 t->btol = 2;
125 }
126
127 t->nextdim = G_malloc(ndims * sizeof(char));
128 for (i = 0; i < ndims - 1; i++)
129 t->nextdim[i] = i + 1;
130 t->nextdim[ndims - 1] = 0;
131
132 t->count = 0;
133 t->root = NULL;
134
135 return t;
136}
137
138/* clear the tree, removing all entries */
139void kdtree_clear(struct kdtree *t)
140{
141 struct kdnode *it;
142 struct kdnode *save = t->root;
143
144 /*
145 Rotate away the left links so that
146 we can treat this like the destruction
147 of a linked list
148 */
149 while ((it = save) != NULL) {
150 if (it->child[0] == NULL) {
151 /* No left links, just kill the node and move on */
152 save = it->child[1];
153 kdtree_free_node(it);
154 it = NULL;
155 }
156 else {
157 /* Rotate away the left link and check again */
158 save = it->child[0];
159 it->child[0] = save->child[1];
160 save->child[1] = it;
161 }
162 }
163 t->root = NULL;
164}
165
166/* destroy the tree */
168{
169 /* remove all entries */
171 G_free(t->nextdim);
172
173 G_free(t);
174 t = NULL;
175}
176
177/* insert an item (coordinates c and uid) into the k-d tree
178 * dc == 1: allow duplicate coordinates */
179int kdtree_insert(struct kdtree *t, double *c, int uid, int dc)
180{
181 struct kdnode *nnew;
182 size_t count = t->count;
183
184 nnew = kdtree_newnode(t);
185 memcpy(nnew->c, c, t->csize);
186 nnew->uid = uid;
187
188 t->root = kdtree_insert2(t, t->root, nnew, 1, dc);
189
190 /* print depth of recursion
191 * recursively called fns are insert2, balance, and replace */
192 /*
193 if (rcallsmax > 1)
194 fprintf(stdout, "%d\n", rcallsmax);
195 */
196
197 return count < t->count;
198}
199
200/* remove an item from the k-d tree
201 * coordinates c and uid must match */
202int kdtree_remove(struct kdtree *t, double *c, int uid)
203{
204 struct kdnode sn, *n;
205 struct kdstack {
206 struct kdnode *n;
207 int dir;
208 } s[256];
209 int top;
210 int dir, found;
211 int balance, bmode;
212
213 sn.c = c;
214 sn.uid = uid;
215
216 /* find sn node */
217 top = 0;
218 s[top].n = t->root;
219 dir = 1;
220 found = 0;
221 while (!found) {
222 n = s[top].n;
223 found = (!cmpc(&sn, n, t) && sn.uid == n->uid);
224 if (!found) {
225 dir = cmp(&sn, n, n->dim) > 0;
226 s[top].dir = dir;
227 top++;
228 s[top].n = n->child[dir];
229
230 if (!s[top].n) {
231 G_warning("Node does not exist");
232
233 return 0;
234 }
235 }
236 }
237
238 if (s[top].n->depth == 0) {
239 kdtree_free_node(s[top].n);
240 s[top].n = NULL;
241 if (top) {
242 top--;
243 n = s[top].n;
244 dir = s[top].dir;
245 n->child[dir] = NULL;
246
247 /* update node */
248 kdtree_update_node(t, n);
249 }
250 else {
251 t->root = NULL;
252
253 return 1;
254 }
255 }
256 else
257 kdtree_replace(t, s[top].n);
258
259 while (top) {
260 top--;
261 n = s[top].n;
262
263 /* update node */
264 kdtree_update_node(t, n);
265 }
266
267 balance = 1;
268 bmode = 1;
269 if (balance) {
270 struct kdnode *r;
271 int iter, bmode2;
272
273 /* fix any inconsistencies in the (sub-)tree */
274 iter = 0;
275 bmode2 = 0;
276 top = 0;
277 r = t->root;
278 s[top].n = r;
279 while (top >= 0) {
280
281 n = s[top].n;
282
283 /* top-down balancing
284 * slower but more compact */
285 if (!bmode2) {
286 while (kdtree_balance(t, n, bmode))
287 ;
288 }
289
290 /* go down */
291 if (n->child[0] && n->child[0]->balance) {
292 dir = 0;
293 top++;
294 s[top].n = n->child[dir];
295 }
296 else if (n->child[1] && n->child[1]->balance) {
297 dir = 1;
298 top++;
299 s[top].n = n->child[dir];
300 }
301 /* go back up */
302 else {
303
304 /* bottom-up balancing
305 * faster but less compact */
306 kdtree_update_node(t, n);
307 if (bmode2) {
308 while (kdtree_balance(t, n, bmode))
309 ;
310 }
311 top--;
312 if (top >= 0) {
313 kdtree_update_node(t, s[top].n);
314 }
315 if (!bmode2 && top == 0) {
316 iter++;
317 if (iter == 2) {
318 /* the top node has been visited twice,
319 * switch from top-down to bottom-up balancing */
320 iter = 0;
321 bmode2 = 1;
322 }
323 }
324 }
325 }
326 }
327
328 return 1;
329}
330
331/* k-d tree optimization, only useful if the tree will be used heavily
332 * (more searches than items in the tree)
333 * level 0 = a bit, 1 = more, 2 = a lot */
334void kdtree_optimize(struct kdtree *t, int level)
335{
336 struct kdnode *n, *n2;
337 struct kdstack {
338 struct kdnode *n;
339 int dir;
340 char v;
341 } s[256];
342 int dir;
343 int top;
344 int ld, rd;
345 int diffl, diffr;
346 int nbal;
347
348 if (!t->root)
349 return;
350
351 G_debug(1, "k-d tree optimization for %zd items, tree depth %d", t->count,
352 t->root->depth);
353
354 nbal = 0;
355 top = 0;
356 s[top].n = t->root;
357 while (s[top].n) {
358 n = s[top].n;
359
360 ld = (!n->child[0] ? -1 : n->child[0]->depth);
361 rd = (!n->child[1] ? -1 : n->child[1]->depth);
362
363 if (ld < rd)
364 while (kdtree_balance(t, n->child[0], level))
365 ;
366 else if (ld > rd)
367 while (kdtree_balance(t, n->child[1], level))
368 ;
369
370 ld = (!n->child[0] ? -1 : n->child[0]->depth);
371 rd = (!n->child[1] ? -1 : n->child[1]->depth);
372 n->depth = MAX(ld, rd) + 1;
373
374 dir = (rd > ld);
375
376 top++;
377 s[top].n = n->child[dir];
378 }
379
380 while (top) {
381 top--;
382 n = s[top].n;
383
384 /* balance node */
385 while (kdtree_balance(t, n, level)) {
386 nbal++;
387 }
388 while (kdtree_balance(t, n->child[0], level))
389 ;
390 while (kdtree_balance(t, n->child[1], level))
391 ;
392
393 ld = (!n->child[0] ? -1 : n->child[0]->depth);
394 rd = (!n->child[1] ? -1 : n->child[1]->depth);
395 n->depth = MAX(ld, rd) + 1;
396
397 while (kdtree_balance(t, n, level)) {
398 nbal++;
399 }
400 }
401
402 while (s[top].n) {
403 n = s[top].n;
404
405 /* balance node */
406 while (kdtree_balance(t, n, level)) {
407 nbal++;
408 }
409 while (kdtree_balance(t, n->child[0], level))
410 ;
411 while (kdtree_balance(t, n->child[1], level))
412 ;
413
414 ld = (!n->child[0] ? -1 : n->child[0]->depth);
415 rd = (!n->child[1] ? -1 : n->child[1]->depth);
416 n->depth = MAX(ld, rd) + 1;
417
418 while (kdtree_balance(t, n, level)) {
419 nbal++;
420 }
421
422 ld = (!n->child[0] ? -1 : n->child[0]->depth);
423 rd = (!n->child[1] ? -1 : n->child[1]->depth);
424
425 dir = (rd > ld);
426
427 top++;
428 s[top].n = n->child[dir];
429 }
430
431 while (top) {
432 top--;
433 n = s[top].n;
434
435 /* update node depth */
436 ld = (!n->child[0] ? -1 : n->child[0]->depth);
437 rd = (!n->child[1] ? -1 : n->child[1]->depth);
438 n->depth = MAX(ld, rd) + 1;
439 }
440
441 if (level) {
442 top = 0;
443 s[top].n = t->root;
444 while (s[top].n) {
445 n = s[top].n;
446
447 /* balance node */
448 while (kdtree_balance(t, n, level)) {
449 nbal++;
450 }
451 while (kdtree_balance(t, n->child[0], level))
452 ;
453 while (kdtree_balance(t, n->child[1], level))
454 ;
455
456 ld = (!n->child[0] ? -1 : n->child[0]->depth);
457 rd = (!n->child[1] ? -1 : n->child[1]->depth);
458 n->depth = MAX(ld, rd) + 1;
459
460 while (kdtree_balance(t, n, level)) {
461 nbal++;
462 }
463
464 diffl = diffr = -1;
465 if (n->child[0]) {
466 n2 = n->child[0];
467 ld = (!n2->child[0] ? -1 : n2->child[0]->depth);
468 rd = (!n2->child[1] ? -1 : n2->child[1]->depth);
469
470 diffl = ld - rd;
471 if (diffl < 0)
472 diffl = -diffl;
473 }
474 if (n->child[1]) {
475 n2 = n->child[1];
476 ld = (!n2->child[0] ? -1 : n2->child[0]->depth);
477 rd = (!n2->child[1] ? -1 : n2->child[1]->depth);
478
479 diffr = ld - rd;
480 if (diffr < 0)
481 diffr = -diffr;
482 }
483
484 dir = (diffr > diffl);
485
486 top++;
487 s[top].n = n->child[dir];
488 }
489
490 while (top) {
491 top--;
492 n = s[top].n;
493
494 /* update node depth */
495 ld = (!n->child[0] ? -1 : n->child[0]->depth);
496 rd = (!n->child[1] ? -1 : n->child[1]->depth);
497 n->depth = MAX(ld, rd) + 1;
498 }
499 }
500
501 G_debug(1, "k-d tree optimization: %d times balanced, new depth %d", nbal,
502 t->root->depth);
503
504 return;
505}
506
507/* find k nearest neighbors
508 * results are stored in uid (uids) and d (squared distances)
509 * optionally an uid to be skipped can be given
510 * useful when searching for the nearest neighbors of an item
511 * that is also in the tree */
512int kdtree_knn(struct kdtree *t, double *c, int *uid, double *d, int k,
513 int *skip)
514{
515 int i, found;
516 double diff, dist, maxdist;
517 struct kdnode sn, *n;
518 struct kdstack {
519 struct kdnode *n;
520 int dir;
521 char v;
522 } s[256];
523 int dir;
524 int top;
525
526 if (!t->root)
527 return 0;
528
529 sn.c = c;
530 sn.uid = (int)0x80000000;
531 if (skip)
532 sn.uid = *skip;
533
534 maxdist = INFINITY;
535 found = 0;
536
537 /* go down */
538 top = 0;
539 s[top].n = t->root;
540 while (s[top].n) {
541 n = s[top].n;
542 dir = cmp(&sn, n, n->dim) > 0;
543 s[top].dir = dir;
544 s[top].v = 0;
545 top++;
546 s[top].n = n->child[dir];
547 }
548
549 /* go back up */
550 while (top) {
551 top--;
552
553 if (!s[top].v) {
554 s[top].v = 1;
555 n = s[top].n;
556
557 if (n->uid != sn.uid) {
558 if (found < k) {
559 dist = 0.0;
560 i = t->ndims - 1;
561 do {
562 diff = sn.c[i] - n->c[i];
563 dist += diff * diff;
564
565 } while (i--);
566
567 i = found;
568 while (i > 0 && d[i - 1] > dist) {
569 d[i] = d[i - 1];
570 uid[i] = uid[i - 1];
571 i--;
572 }
573 if (i < found && d[i] == dist && uid[i] == n->uid)
574 G_fatal_error("knn: inserting duplicate");
575 d[i] = dist;
576 uid[i] = n->uid;
577 maxdist = d[found];
578 found++;
579 }
580 else {
581 dist = 0.0;
582 i = t->ndims - 1;
583 do {
584 diff = sn.c[i] - n->c[i];
585 dist += diff * diff;
586
587 } while (i-- && dist <= maxdist);
588
589 if (dist < maxdist) {
590 i = k - 1;
591 while (i > 0 && d[i - 1] > dist) {
592 d[i] = d[i - 1];
593 uid[i] = uid[i - 1];
594 i--;
595 }
596 if (d[i] == dist && uid[i] == n->uid)
597 G_fatal_error("knn: inserting duplicate");
598 d[i] = dist;
599 uid[i] = n->uid;
600
601 maxdist = d[k - 1];
602 }
603 }
604 if (found == k && maxdist == 0.0)
605 break;
606 }
607
608 /* look on the other side ? */
609 dir = s[top].dir;
610 diff = sn.c[(int)n->dim] - n->c[(int)n->dim];
611 dist = diff * diff;
612
613 if (dist <= maxdist) {
614 /* go down the other side */
615 top++;
616 s[top].n = n->child[!dir];
617 while (s[top].n) {
618 n = s[top].n;
619 dir = cmp(&sn, n, n->dim) > 0;
620 s[top].dir = dir;
621 s[top].v = 0;
622 top++;
623 s[top].n = n->child[dir];
624 }
625 }
626 }
627 }
628
629 return found;
630}
631
632/* find all nearest neighbors within distance aka radius search
633 * results are stored in puid (uids) and pd (squared distances)
634 * memory is allocated as needed, the calling fn must free the memory
635 * optionally an uid to be skipped can be given */
636int kdtree_dnn(struct kdtree *t, double *c, int **puid, double **pd,
637 double maxdist, int *skip)
638{
639 int i, k, found;
640 double diff, dist;
641 struct kdnode sn, *n;
642 struct kdstack {
643 struct kdnode *n;
644 int dir;
645 char v;
646 } s[256];
647 int dir;
648 int top;
649 int *uid;
650 double *d, maxdistsq;
651
652 if (!t->root)
653 return 0;
654
655 sn.c = c;
656 sn.uid = (int)0x80000000;
657 if (skip)
658 sn.uid = *skip;
659
660 *pd = NULL;
661 *puid = NULL;
662
663 k = 0;
664 uid = NULL;
665 d = NULL;
666
667 found = 0;
668 maxdistsq = maxdist * maxdist;
669
670 /* go down */
671 top = 0;
672 s[top].n = t->root;
673 while (s[top].n) {
674 n = s[top].n;
675 dir = cmp(&sn, n, n->dim) > 0;
676 s[top].dir = dir;
677 s[top].v = 0;
678 top++;
679 s[top].n = n->child[dir];
680 }
681
682 /* go back up */
683 while (top) {
684 top--;
685
686 if (!s[top].v) {
687 s[top].v = 1;
688 n = s[top].n;
689
690 if (n->uid != sn.uid) {
691 dist = 0;
692 i = t->ndims - 1;
693 do {
694 diff = sn.c[i] - n->c[i];
695 dist += diff * diff;
696
697 } while (i-- && dist <= maxdistsq);
698
699 if (dist <= maxdistsq) {
700 if (found + 1 >= k) {
701 k = found + 10;
702 uid = G_realloc(uid, k * sizeof(int));
703 d = G_realloc(d, k * sizeof(double));
704 }
705 i = found;
706 while (i > 0 && d[i - 1] > dist) {
707 d[i] = d[i - 1];
708 uid[i] = uid[i - 1];
709 i--;
710 }
711 if (i < found && d[i] == dist && uid[i] == n->uid)
712 G_fatal_error("dnn: inserting duplicate");
713 d[i] = dist;
714 uid[i] = n->uid;
715 found++;
716 }
717 }
718
719 /* look on the other side ? */
720 dir = s[top].dir;
721
722 diff = fabs(sn.c[(int)n->dim] - n->c[(int)n->dim]);
723 if (diff <= maxdist) {
724 /* go down the other side */
725 top++;
726 s[top].n = n->child[!dir];
727 while (s[top].n) {
728 n = s[top].n;
729 dir = cmp(&sn, n, n->dim) > 0;
730 s[top].dir = dir;
731 s[top].v = 0;
732 top++;
733 s[top].n = n->child[dir];
734 }
735 }
736 }
737 }
738
739 *pd = d;
740 *puid = uid;
741
742 return found;
743}
744
745/* find all nearest neighbors within range aka box search
746 * the range is specified with min and max for each dimension as
747 * (min1, min2, ..., minn, max1, max2, ..., maxn)
748 * results are stored in puid (uids)
749 * memory is allocated as needed, the calling fn must free the memory
750 * optionally an uid to be skipped can be given */
751int kdtree_rnn(struct kdtree *t, double *c, int **puid, int *skip)
752{
753 int i, k, found, inside;
754 struct kdnode sn, *n;
755 struct kdstack {
756 struct kdnode *n;
757 int dir;
758 char v;
759 } s[256];
760 int dir;
761 int top;
762 int *uid;
763
764 if (!t->root)
765 return 0;
766
767 sn.c = c;
768 sn.uid = (int)0x80000000;
769 if (skip)
770 sn.uid = *skip;
771
772 *puid = NULL;
773
774 k = 0;
775 uid = NULL;
776
777 found = 0;
778
779 /* go down */
780 top = 0;
781 s[top].n = t->root;
782 while (s[top].n) {
783 n = s[top].n;
784 dir = cmp(&sn, n, n->dim) > 0;
785 s[top].dir = dir;
786 s[top].v = 0;
787 top++;
788 s[top].n = n->child[dir];
789 }
790
791 /* go back up */
792 while (top) {
793 top--;
794
795 if (!s[top].v) {
796 s[top].v = 1;
797 n = s[top].n;
798
799 if (n->uid != sn.uid) {
800 inside = 1;
801 for (i = 0; i < t->ndims; i++) {
802 if (n->c[i] < sn.c[i] || n->c[i] > sn.c[i + t->ndims]) {
803 inside = 0;
804 break;
805 }
806 }
807
808 if (inside) {
809 if (found + 1 >= k) {
810 k = found + 10;
811 uid = G_realloc(uid, k * sizeof(int));
812 }
813 i = found;
814 uid[i] = n->uid;
815 found++;
816 }
817 }
818
819 /* look on the other side ? */
820 dir = s[top].dir;
821 if (n->c[(int)n->dim] >= sn.c[(int)n->dim] &&
822 n->c[(int)n->dim] <= sn.c[(int)n->dim + t->ndims]) {
823 /* go down the other side */
824 top++;
825 s[top].n = n->child[!dir];
826 while (s[top].n) {
827 n = s[top].n;
828 dir = cmp(&sn, n, n->dim) > 0;
829 s[top].dir = dir;
830 s[top].v = 0;
831 top++;
832 s[top].n = n->child[dir];
833 }
834 }
835 }
836 }
837
838 *puid = uid;
839
840 return found;
841}
842
843/* initialize tree traversal
844 * (re-)sets trav structure
845 * returns 0
846 */
847int kdtree_init_trav(struct kdtrav *trav, struct kdtree *tree)
848{
849 trav->tree = tree;
850 trav->curr_node = tree->root;
851 trav->first = 1;
852 trav->top = 0;
853
854 return 0;
855}
856
857/* traverse the tree
858 * useful to get all items in the tree non-recursively
859 * struct kdtrav *trav needs to be initialized first
860 * returns 1, 0 when finished
861 */
862int kdtree_traverse(struct kdtrav *trav, double *c, int *uid)
863{
864 if (trav->curr_node == NULL) {
865 if (trav->first)
866 G_debug(1, "k-d tree: empty tree");
867 else
868 G_debug(1, "k-d tree: finished traversing");
869
870 return 0;
871 }
872
873 if (trav->first) {
874 trav->first = 0;
875 return kdtree_first(trav, c, uid);
876 }
877
878 return kdtree_next(trav, c, uid);
879}
880
881/**********************************************/
882/* internal functions */
883
884/**********************************************/
885
886static int kdtree_replace(struct kdtree *t, struct kdnode *r)
887{
888 double mindist;
889 int rdir, ordir, dir;
890 int ld, rd;
891 struct kdnode *n, *rn, * or ;
892 struct kdstack {
893 struct kdnode *n;
894 int dir;
895 char v;
896 } s[256];
897 int top, top2;
898 int is_leaf;
899 int nr;
900
901 if (!r)
902 return 0;
903 if (!r->child[0] && !r->child[1])
904 return 0;
905
906 /* do not call kdtree_balance in this fn, this can cause
907 * stack overflow due to too many recursive calls */
908
909 /* find replacement for r
910 * overwrite r, delete replacement */
911 nr = 0;
912
913 /* pick a subtree */
914 rdir = 1;
915
916 or = r;
917 ld = (! or->child[0] ? -1 : or->child[0]->depth);
918 rd = (! or->child[1] ? -1 : or->child[1]->depth);
919
920 if (ld > rd) {
921 rdir = 0;
922 }
923
924 /* replace old root, make replacement the new root
925 * repeat until replacement is leaf */
926 ordir = rdir;
927 is_leaf = 0;
928 s[0].n = or ;
929 s[0].dir = ordir;
930 top2 = 1;
931 mindist = -1;
932 while (!is_leaf) {
933 rn = NULL;
934
935 /* find replacement for old root */
936 top = top2;
937 s[top].n = or->child[ordir];
938
939 n = s[top].n;
940 rn = n;
941 mindist = or->c[(int) or->dim] - n->c[(int) or->dim];
942 if (ordir)
943 mindist = -mindist;
944
945 /* go down */
946 while (s[top].n) {
947 n = s[top].n;
948 dir = !ordir;
949 if (n->dim != or->dim)
950 dir = cmp(or, n, n->dim) > 0;
951 s[top].dir = dir;
952 s[top].v = 0;
953 top++;
954 s[top].n = n->child[dir];
955 }
956
957 /* go back up */
958 while (top > top2) {
959 top--;
960
961 if (!s[top].v) {
962 s[top].v = 1;
963 n = s[top].n;
964 if ((cmp(rn, n, or->dim) > 0) == ordir) {
965 rn = n;
966 mindist = or->c[(int) or->dim] - n->c[(int) or->dim];
967 if (ordir)
968 mindist = -mindist;
969 }
970
971 /* look on the other side ? */
972 dir = s[top].dir;
973 if (n->dim != or->dim && mindist >= fabs(n->c[(int)n->dim] -
974 n->c[(int)n->dim])) {
975 /* go down the other side */
976 top++;
977 s[top].n = n->child[!dir];
978 while (s[top].n) {
979 n = s[top].n;
980 dir = !ordir;
981 if (n->dim != or->dim)
982 dir = cmp(or, n, n->dim) > 0;
983 s[top].dir = dir;
984 s[top].v = 0;
985 top++;
986 s[top].n = n->child[dir];
987 }
988 }
989 }
990 }
991
992#ifdef KD_DEBUG
993 if (!rn)
994 G_fatal_error("No replacement");
995 if (ordir && or->c[(int) or->dim] > rn->c[(int) or->dim])
996 G_fatal_error("rn is smaller");
997
998 if (!ordir && or->c[(int) or->dim] < rn->c[(int) or->dim])
999 G_fatal_error("rn is larger");
1000
1001 if (or->child[1]) {
1002 dir = cmp(or->child[1], rn, or->dim);
1003 if (dir < 0) {
1004 int i;
1005
1006 for (i = 0; i < t->ndims; i++)
1007 G_message("rn c %g, or child c %g", rn->c[i],
1008 or->child[1]->c[i]);
1010 "Right child of old root is smaller than rn, dir is %d",
1011 ordir);
1012 }
1013 }
1014 if (or->child[0]) {
1015 dir = cmp(or->child[0], rn, or->dim);
1016 if (dir > 0) {
1017 int i;
1018
1019 for (i = 0; i < t->ndims; i++)
1020 G_message("rn c %g, or child c %g", rn->c[i],
1021 or->child[0]->c[i]);
1023 "Left child of old root is larger than rn, dir is %d",
1024 ordir);
1025 }
1026 }
1027#endif
1028
1029 is_leaf = (rn->child[0] == NULL && rn->child[1] == NULL);
1030
1031#ifdef KD_DEBUG
1032 if (is_leaf && rn->depth != 0)
1033 G_fatal_error("rn is leaf but depth is %d", (int)rn->depth);
1034 if (!is_leaf && rn->depth <= 0)
1035 G_fatal_error("rn is not leaf but depth is %d", (int)rn->depth);
1036#endif
1037
1038 nr++;
1039
1040 /* go to replacement from or->child[ordir] */
1041 top = top2;
1042 dir = 1;
1043 while (dir) {
1044 n = s[top].n;
1045 dir = cmp(rn, n, n->dim);
1046 if (dir) {
1047 s[top].dir = dir > 0;
1048 top++;
1049 s[top].n = n->child[dir > 0];
1050
1051 if (!s[top].n) {
1052 G_fatal_error("(Last) replacement disappeared %d", nr);
1053 }
1054 }
1055 }
1056
1057#ifdef KD_DEBUG
1058 if (s[top].n != rn)
1059 G_fatal_error("rn is unreachable from or");
1060#endif
1061
1062 top2 = top;
1063 s[top2 + 1].n = NULL;
1064
1065 /* copy replacement to old root */
1066 memcpy(or->c, rn->c, t->csize);
1067 or->uid = rn->uid;
1068
1069 if (!is_leaf) {
1070 /* make replacement the old root */
1071 or = rn;
1072
1073 /* pick a subtree */
1074 ordir = 1;
1075 ld = (! or->child[0] ? -1 : or->child[0]->depth);
1076 rd = (! or->child[1] ? -1 : or->child[1]->depth);
1077 if (ld > rd) {
1078 ordir = 0;
1079 }
1080 s[top2].dir = ordir;
1081 top2++;
1082 }
1083 }
1084
1085 if (!rn)
1086 G_fatal_error("No replacement at all");
1087
1088 /* delete last replacement */
1089 if (s[top2].n != rn) {
1090 G_fatal_error("Wrong top2 for last replacement");
1091 }
1092 top = top2 - 1;
1093 n = s[top].n;
1094 dir = s[top].dir;
1095 if (n->child[dir] != rn) {
1096 G_fatal_error("Last replacement disappeared");
1097 }
1098 kdtree_free_node(rn);
1099 n->child[dir] = NULL;
1100 t->count--;
1101
1102 kdtree_update_node(t, n);
1103 top++;
1104
1105 /* go back up */
1106 while (top) {
1107 top--;
1108 n = s[top].n;
1109
1110#ifdef KD_DEBUG
1111 /* debug directions */
1112 if (n->child[0]) {
1113 if (cmp(n->child[0], n, n->dim) > 0)
1114 G_warning("Left child is larger");
1115 }
1116 if (n->child[1]) {
1117 if (cmp(n->child[1], n, n->dim) < 1)
1118 G_warning("Right child is not larger");
1119 }
1120#endif
1121
1122 /* update node */
1123 kdtree_update_node(t, n);
1124 }
1125
1126 return nr;
1127}
1128
1129static int kdtree_balance(struct kdtree *t, struct kdnode *r, int bmode)
1130{
1131 struct kdnode * or ;
1132 int dir;
1133 int rd, ld;
1134 int old_depth;
1135 int btol;
1136
1137 if (!r) {
1138 return 0;
1139 }
1140
1141 ld = (!r->child[0] ? -1 : r->child[0]->depth);
1142 rd = (!r->child[1] ? -1 : r->child[1]->depth);
1143 old_depth = MAX(ld, rd) + 1;
1144
1145 if (old_depth != r->depth) {
1146 G_warning("balancing: depth is wrong: %d != %d", r->depth, old_depth);
1147 kdtree_update_node(t, r);
1148 }
1149
1150 /* subtree difference */
1151 btol = t->btol;
1152 if (!r->child[0] || !r->child[1])
1153 btol = 2;
1154 dir = -1;
1155 ld = (!r->child[0] ? -1 : r->child[0]->depth);
1156 rd = (!r->child[1] ? -1 : r->child[1]->depth);
1157 if (ld > rd + btol) {
1158 dir = 0;
1159 }
1160 else if (rd > ld + btol) {
1161 dir = 1;
1162 }
1163 else {
1164 return 0;
1165 }
1166
1167 or = kdtree_newnode(t);
1168 memcpy(or->c, r->c, t->csize);
1169 or->uid = r->uid;
1170 or->dim = t->nextdim[r->dim];
1171
1172 if (!kdtree_replace(t, r))
1173 G_fatal_error("kdtree_balance: nothing replaced");
1174
1175#ifdef KD_DEBUG
1176 if (!cmp(r, or, r->dim)) {
1177 G_warning("kdtree_balance: replacement failed");
1178 kdtree_free_node(or);
1179
1180 return 0;
1181 }
1182#endif
1183
1184 r->child[!dir] =
1185 kdtree_insert2(t, r->child[!dir], or, bmode, 1); /* bmode */
1186
1187 /* update node */
1188 kdtree_update_node(t, r);
1189
1190 if (r->depth == old_depth) {
1191 G_debug(4, "balancing had no effect");
1192 return 1;
1193 }
1194
1195 if (r->depth > old_depth)
1196 G_fatal_error("balancing failed");
1197
1198 return 1;
1199}
1200
1201static struct kdnode *kdtree_insert2(struct kdtree *t, struct kdnode *r,
1202 struct kdnode *nnew, int balance, int dc)
1203{
1204 struct kdnode *n;
1205 struct kdstack {
1206 struct kdnode *n;
1207 int dir;
1208 } s[256];
1209 int top;
1210 int dir;
1211 int bmode;
1212
1213 if (!r) {
1214 r = nnew;
1215 t->count++;
1216
1217 return r;
1218 }
1219
1220 /* level of recursion */
1221 rcalls++;
1222 if (rcallsmax < rcalls)
1223 rcallsmax = rcalls;
1224
1225 /* balancing modes
1226 * bmode = 0: no recursion (only insert -> balance -> insert)
1227 * slower, higher tree depth
1228 * bmode = 1: recursion (insert -> balance -> insert -> balance ...)
1229 * faster, more compact tree
1230 * */
1231 bmode = 1;
1232
1233 /* find node with free child */
1234 top = 0;
1235 s[top].n = r;
1236 while (s[top].n) {
1237
1238 n = s[top].n;
1239
1240 if (!cmpc(nnew, n, t) && (!dc || nnew->uid == n->uid)) {
1241
1242 G_debug(1, "KD node exists already, nothing to do");
1243 kdtree_free_node(nnew);
1244
1245 if (!balance) {
1246 rcalls--;
1247 return r;
1248 }
1249
1250 break;
1251 }
1252 dir = cmp(nnew, n, n->dim) > 0;
1253 s[top].dir = dir;
1254
1255 top++;
1256 if (top > 255)
1257 G_fatal_error("depth too large: %d", top);
1258 s[top].n = n->child[dir];
1259 }
1260
1261 if (!s[top].n) {
1262 /* insert to child pointer of parent */
1263 top--;
1264 n = s[top].n;
1265 dir = s[top].dir;
1266 n->child[dir] = nnew;
1267 nnew->dim = t->nextdim[n->dim];
1268
1269 t->count++;
1270 top++;
1271 }
1272
1273 /* go back up */
1274 while (top) {
1275 top--;
1276 n = s[top].n;
1277
1278 /* update node */
1279 kdtree_update_node(t, n);
1280
1281 /* do not balance on the way back up */
1282
1283#ifdef KD_DEBUG
1284 /* debug directions */
1285 if (n->child[0]) {
1286 if (cmp(n->child[0], n, n->dim) > 0)
1287 G_warning("Insert2: Left child is larger");
1288 }
1289 if (n->child[1]) {
1290 if (cmp(n->child[1], n, n->dim) < 1)
1291 G_warning("Insert2: Right child is not larger");
1292 }
1293#endif
1294 }
1295
1296 if (balance) {
1297 int iter, bmode2;
1298
1299 /* fix any inconsistencies in the (sub-)tree */
1300 iter = 0;
1301 bmode2 = 0;
1302 top = 0;
1303 s[top].n = r;
1304 while (top >= 0) {
1305
1306 n = s[top].n;
1307
1308 /* top-down balancing
1309 * slower but more compact */
1310 if (!bmode2) {
1311 while (kdtree_balance(t, n, bmode))
1312 ;
1313 }
1314
1315 /* go down */
1316 if (n->child[0] && n->child[0]->balance) {
1317 dir = 0;
1318 top++;
1319 s[top].n = n->child[dir];
1320 }
1321 else if (n->child[1] && n->child[1]->balance) {
1322 dir = 1;
1323 top++;
1324 s[top].n = n->child[dir];
1325 }
1326 /* go back up */
1327 else {
1328
1329 /* bottom-up balancing
1330 * faster but less compact */
1331 if (bmode2) {
1332 while (kdtree_balance(t, n, bmode))
1333 ;
1334 }
1335 top--;
1336 if (top >= 0) {
1337 kdtree_update_node(t, s[top].n);
1338 }
1339 if (!bmode2 && top == 0) {
1340 iter++;
1341 if (iter == 2) {
1342 /* the top node has been visited twice,
1343 * switch from top-down to bottom-up balancing */
1344 iter = 0;
1345 bmode2 = 1;
1346 }
1347 }
1348 }
1349 }
1350 }
1351
1352 rcalls--;
1353
1354 return r;
1355}
1356
1357/* start traversing the tree
1358 * returns pointer to first item
1359 */
1360static int kdtree_first(struct kdtrav *trav, double *c, int *uid)
1361{
1362 /* get smallest item */
1363 while (trav->curr_node->child[0] != NULL) {
1364 trav->up[trav->top++] = trav->curr_node;
1365 trav->curr_node = trav->curr_node->child[0];
1366 }
1367
1368 memcpy(c, trav->curr_node->c, trav->tree->csize);
1369 *uid = trav->curr_node->uid;
1370
1371 return 1;
1372}
1373
1374/* continue traversing the tree in ascending order
1375 * returns pointer to data item, NULL when finished
1376 */
1377static int kdtree_next(struct kdtrav *trav, double *c, int *uid)
1378{
1379 if (trav->curr_node->child[1] != NULL) {
1380 /* something on the right side: larger item */
1381 trav->up[trav->top++] = trav->curr_node;
1382 trav->curr_node = trav->curr_node->child[1];
1383
1384 /* go down, find smallest item in this branch */
1385 while (trav->curr_node->child[0] != NULL) {
1386 trav->up[trav->top++] = trav->curr_node;
1387 trav->curr_node = trav->curr_node->child[0];
1388 }
1389 }
1390 else {
1391 /* at smallest item in this branch, go back up */
1392 struct kdnode *last;
1393
1394 do {
1395 if (trav->top == 0) {
1396 trav->curr_node = NULL;
1397 break;
1398 }
1399 last = trav->curr_node;
1400 trav->curr_node = trav->up[--trav->top];
1401 } while (last == trav->curr_node->child[1]);
1402 }
1403
1404 if (trav->curr_node != NULL) {
1405 memcpy(c, trav->curr_node->c, trav->tree->csize);
1406 *uid = trav->curr_node->uid;
1407
1408 return 1;
1409 }
1410
1411 return 0; /* finished traversing */
1412}
void G_free(void *buf)
Free allocated memory.
Definition alloc.c:150
#define NULL
Definition ccmath.h:32
int G_debug(int level, const char *msg,...)
Print debugging message.
Definition debug.c:66
double b
double t
double r
void G_fatal_error(const char *msg,...)
Print a fatal error message to stderr.
Definition gis/error.c:159
void G_message(const char *msg,...)
Print a message to stderr.
Definition gis/error.c:89
void G_warning(const char *msg,...)
Print a warning message to stderr.
Definition gis/error.c:203
int count
void kdtree_optimize(struct kdtree *t, int level)
Definition kdtree.c:334
int kdtree_rnn(struct kdtree *t, double *c, int **puid, int *skip)
Definition kdtree.c:751
struct kdtree * kdtree_create(char ndims, int *btol)
Definition kdtree.c:111
int kdtree_traverse(struct kdtrav *trav, double *c, int *uid)
Definition kdtree.c:862
void kdtree_clear(struct kdtree *t)
Definition kdtree.c:139
int kdtree_insert(struct kdtree *t, double *c, int uid, int dc)
Definition kdtree.c:179
int kdtree_knn(struct kdtree *t, double *c, int *uid, double *d, int k, int *skip)
Definition kdtree.c:512
void kdtree_destroy(struct kdtree *t)
Definition kdtree.c:167
int kdtree_dnn(struct kdtree *t, double *c, int **puid, double **pd, double maxdist, int *skip)
Definition kdtree.c:636
#define KD_BTOL
Definition kdtree.c:23
int kdtree_init_trav(struct kdtrav *trav, struct kdtree *tree)
Definition kdtree.c:847
int kdtree_remove(struct kdtree *t, double *c, int uid)
Definition kdtree.c:202
Dynamic balanced k-d tree implementation.
#define MAX(a, b)
Definition parson.c:87
Node for k-d tree.
Definition kdtree.h:67
unsigned char dim
Definition kdtree.h:68
unsigned char balance
Definition kdtree.h:70
unsigned char depth
Definition kdtree.h:69
struct kdnode * child[2]
Definition kdtree.h:73
int uid
Definition kdtree.h:72
double * c
Definition kdtree.h:71
k-d tree traversal
Definition kdtree.h:92
struct kdtree * tree
Definition kdtree.h:93
struct kdnode * curr_node
Definition kdtree.h:94
struct kdnode * up[256]
Definition kdtree.h:95
int top
Definition kdtree.h:96
int first
Definition kdtree.h:97
k-d tree
Definition kdtree.h:80
unsigned char ndims
Definition kdtree.h:81
int btol
Definition kdtree.h:84
int csize
Definition kdtree.h:83
struct kdnode * root
Definition kdtree.h:86