GRASS GIS 8 Programmer's Manual  8.4.0dev(2024)-6c790bf5c0
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 
29 static int rcalls = 0;
30 static int rcallsmax = 0;
31 
32 static struct kdnode *kdtree_insert2(struct kdtree *, struct kdnode *,
33  struct kdnode *, int, int);
34 static int kdtree_replace(struct kdtree *, struct kdnode *);
35 static int kdtree_balance(struct kdtree *, struct kdnode *, int);
36 static int kdtree_first(struct kdtrav *, double *, int *);
37 static int kdtree_next(struct kdtrav *, double *, int *);
38 
39 static 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 
49 static 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 
62 static 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 
77 static void kdtree_free_node(struct kdnode *n)
78 {
79  G_free(n->c);
80  G_free(n);
81 }
82 
83 static 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 */
111 struct 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 */
139 void 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 */
167 void kdtree_destroy(struct kdtree *t)
168 {
169  /* remove all entries */
170  kdtree_clear(t);
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 */
179 int 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 */
202 int 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 */
334 void 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 */
512 int 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 */
636 int 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 */
751 int 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  */
847 int 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  */
862 int 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 
886 static 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]);
1009  G_fatal_error(
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]);
1022  G_fatal_error(
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 
1129 static 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 
1201 static 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  */
1360 static 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  */
1377 static 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 }
#define NULL
Definition: ccmath.h:32
void G_free(void *)
Free allocated memory.
Definition: gis/alloc.c:150
#define G_realloc(p, n)
Definition: defs/gis.h:96
void void void void G_fatal_error(const char *,...) __attribute__((format(printf
void G_warning(const char *,...) __attribute__((format(printf
#define G_malloc(n)
Definition: defs/gis.h:94
void G_message(const char *,...) __attribute__((format(printf
int G_debug(int, const char *,...) __attribute__((format(printf
#define MAX(a, b)
Definition: gis.h:149
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.
double b
Definition: r_raster.c:39
double t
Definition: r_raster.c:39
double r
Definition: r_raster.c:39
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