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