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