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