GRASS GIS 8 Programmer's Manual  8.2.1dev(2022)-e71518dcd
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  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 
61 static 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 
76 static void kdtree_free_node(struct kdnode *n)
77 {
78  G_free(n->c);
79  G_free(n);
80 }
81 
82 static 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 */
110 struct 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 */
138 void 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 */
166 void kdtree_destroy(struct kdtree *t)
167 {
168  /* remove all entries */
169  kdtree_clear(t);
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 */
178 int 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 */
201 int 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 */
331 void 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 */
501 int 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 */
624 int 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 */
739 int 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  */
835 int 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  */
850 int 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 
874 static 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 
1113 static 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 
1184 static 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  */
1342 static 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  */
1359 static 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 }
#define G_malloc(n)
Definition: defs/gis.h:112
void void void void G_fatal_error(const char *,...) __attribute__((format(printf
int csize
Definition: kdtree.h:84
unsigned char dim
Definition: kdtree.h:69
Node for k-d tree.
Definition: kdtree.h:67
void kdtree_clear(struct kdtree *t)
Definition: kdtree.c:138
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.
double * c
Definition: kdtree.h:72
int kdtree_rnn(struct kdtree *t, double *c, int **puid, int *skip)
Definition: kdtree.c:739
size_t count
Definition: kdtree.h:86
void G_free(void *)
Free allocated memory.
Definition: gis/alloc.c:149
int count
int kdtree_dnn(struct kdtree *t, double *c, int **puid, double **pd, double maxdist, int *skip)
Definition: kdtree.c:624
int btol
Definition: kdtree.h:85
unsigned char depth
Definition: kdtree.h:70
#define NULL
Definition: ccmath.h:32
unsigned char balance
Definition: kdtree.h:71
struct kdtree * kdtree_create(char ndims, int *btol)
Definition: kdtree.c:110
void G_message(const char *,...) __attribute__((format(printf
unsigned char ndims
Definition: kdtree.h:82
struct kdnode * up[256]
Definition: kdtree.h:97
struct kdtree * tree
Definition: kdtree.h:95
double t
Definition: r_raster.c:39
double b
Definition: r_raster.c:39
int kdtree_traverse(struct kdtrav *trav, double *c, int *uid)
Definition: kdtree.c:850
k-d tree
Definition: kdtree.h:80
int first
Definition: kdtree.h:99
#define MAX(a, b)
Definition: gis.h:135
void kdtree_destroy(struct kdtree *t)
Definition: kdtree.c:166
int kdtree_knn(struct kdtree *t, double *c, int *uid, double *d, int k, int *skip)
Definition: kdtree.c:501
int kdtree_insert(struct kdtree *t, double *c, int uid, int dc)
Definition: kdtree.c:178
unsigned char * nextdim
Definition: kdtree.h:83
struct kdnode * root
Definition: kdtree.h:87
struct kdnode * child[2]
Definition: kdtree.h:74
void G_warning(const char *,...) __attribute__((format(printf
struct kdnode * curr_node
Definition: kdtree.h:96
int uid
Definition: kdtree.h:73
#define G_realloc(p, n)
Definition: defs/gis.h:114
int top
Definition: kdtree.h:98
k-d tree traversal
Definition: kdtree.h:93
void kdtree_optimize(struct kdtree *t, int level)
Definition: kdtree.c:331
int G_debug(int, const char *,...) __attribute__((format(printf
#define KD_BTOL
Definition: kdtree.c:23
double r
Definition: r_raster.c:39