GRASS GIS 7 Programmer's Manual  7.9.dev(2021)-e5379bbd7
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 #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 
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 
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 
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 
81 static void kdtree_free_node(struct kdnode *n)
82 {
83  G_free(n->c);
84  G_free(n);
85 }
86 
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 */
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 */
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 */
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 */
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 */
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 */
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 */
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 */
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 */
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  */
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  */
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 
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 
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 
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  */
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  */
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 }
G_malloc
#define G_malloc(n)
Definition: defs/gis.h:112
G_fatal_error
void void void void G_fatal_error(const char *,...) __attribute__((format(printf
kdtree::csize
int csize
Definition: kdtree.h:84
kdnode::dim
unsigned char dim
Definition: kdtree.h:69
kdnode
Node for k-d tree.
Definition: kdtree.h:67
kdtree_clear
void kdtree_clear(struct kdtree *t)
Definition: kdtree.c:143
kdtree_init_trav
int kdtree_init_trav(struct kdtrav *trav, struct kdtree *tree)
Definition: kdtree.c:840
kdtree_remove
int kdtree_remove(struct kdtree *t, double *c, int uid)
Definition: kdtree.c:206
MAX
#define MAX(a, b)
Definition: kdtree.c:26
kdtree.h
Dynamic balanced k-d tree implementation.
kdnode::c
double * c
Definition: kdtree.h:72
kdtree_rnn
int kdtree_rnn(struct kdtree *t, double *c, int **puid, int *skip)
Definition: kdtree.c:744
kdtree::count
size_t count
Definition: kdtree.h:86
G_free
void G_free(void *)
Free allocated memory.
Definition: gis/alloc.c:149
count
int count
kdtree_dnn
int kdtree_dnn(struct kdtree *t, double *c, int **puid, double **pd, double maxdist, int *skip)
Definition: kdtree.c:629
kdtree::btol
int btol
Definition: kdtree.h:85
kdnode::depth
unsigned char depth
Definition: kdtree.h:70
NULL
#define NULL
Definition: ccmath.h:32
kdnode::balance
unsigned char balance
Definition: kdtree.h:71
kdtree_create
struct kdtree * kdtree_create(char ndims, int *btol)
Definition: kdtree.c:115
G_message
void G_message(const char *,...) __attribute__((format(printf
kdtree::ndims
unsigned char ndims
Definition: kdtree.h:82
kdtrav::up
struct kdnode * up[256]
Definition: kdtree.h:97
kdtrav::tree
struct kdtree * tree
Definition: kdtree.h:95
t
double t
Definition: r_raster.c:39
b
double b
Definition: r_raster.c:39
kdtree_traverse
int kdtree_traverse(struct kdtrav *trav, double *c, int *uid)
Definition: kdtree.c:855
kdtree
k-d tree
Definition: kdtree.h:80
kdtrav::first
int first
Definition: kdtree.h:99
kdtree_destroy
void kdtree_destroy(struct kdtree *t)
Definition: kdtree.c:171
kdtree_knn
int kdtree_knn(struct kdtree *t, double *c, int *uid, double *d, int k, int *skip)
Definition: kdtree.c:506
kdtree_insert
int kdtree_insert(struct kdtree *t, double *c, int uid, int dc)
Definition: kdtree.c:183
kdtree::nextdim
unsigned char * nextdim
Definition: kdtree.h:83
kdtree::root
struct kdnode * root
Definition: kdtree.h:87
kdnode::child
struct kdnode * child[2]
Definition: kdtree.h:74
G_warning
void G_warning(const char *,...) __attribute__((format(printf
kdtrav::curr_node
struct kdnode * curr_node
Definition: kdtree.h:96
kdnode::uid
int uid
Definition: kdtree.h:73
G_realloc
#define G_realloc(p, n)
Definition: defs/gis.h:114
kdtrav::top
int top
Definition: kdtree.h:98
kdtrav
k-d tree traversal
Definition: kdtree.h:93
kdtree_optimize
void kdtree_optimize(struct kdtree *t, int level)
Definition: kdtree.c:336
G_debug
int G_debug(int, const char *,...) __attribute__((format(printf
KD_BTOL
#define KD_BTOL
Definition: kdtree.c:28
r
double r
Definition: r_raster.c:39