19 #include <grass/gis.h>
20 #include <grass/glocale.h>
26 #define MAX(a, b) ((a) > (b) ? (a) : (b))
34 static int rcalls = 0;
35 static int rcallsmax = 0;
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 *);
46 if (a->
c[p] <
b->c[p])
48 if (a->
c[p] >
b->c[p])
51 return (a->
uid <
b->uid ? -1 : a->
uid >
b->uid);
57 for (i = 0; i <
t->ndims; i++) {
58 if (a->
c[i] !=
b->c[i]) {
70 n->
c = G_malloc(
t->ndims *
sizeof(
double));
81 static void kdtree_free_node(
struct kdnode *n)
87 static void kdtree_update_node(
struct kdtree *
t,
struct kdnode *n)
109 if (ld > rd + btol || rd > ld + btol)
120 t = G_malloc(
sizeof(
struct kdtree));
123 t->csize =
ndims *
sizeof(double);
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;
153 while((it = save) !=
NULL) {
157 kdtree_free_node(it);
188 nnew = kdtree_newnode(
t);
189 memcpy(nnew->
c,
c,
t->csize);
192 t->root = kdtree_insert2(
t,
t->root, nnew, 1, dc);
227 found = (!cmpc(&sn, n,
t) && sn.
uid == n->
uid);
229 dir = cmp(&sn, n, n->
dim) > 0;
232 s[top].n = n->
child[dir];
242 if (s[top].n->
depth == 0) {
243 kdtree_free_node(s[top].n);
249 n->child[dir] =
NULL;
252 kdtree_update_node(
t, n);
261 kdtree_replace(
t, s[top].n);
268 kdtree_update_node(
t, n);
290 while (kdtree_balance(
t, n, bmode));
297 s[top].n = n->
child[dir];
302 s[top].n = n->
child[dir];
309 kdtree_update_node(
t, n);
311 while (kdtree_balance(
t, n, bmode));
315 kdtree_update_node(
t, s[top].n);
317 if (!bmode2 && top == 0) {
353 G_debug(1,
"k-d tree optimization for %zd items, tree depth %d",
354 t->count,
t->root->depth);
366 while (kdtree_balance(
t, n->
child[0], level));
368 while (kdtree_balance(
t, n->
child[1], level));
377 s[top].n = n->
child[dir];
385 while (kdtree_balance(
t, n, level)) {
388 while (kdtree_balance(
t, n->
child[0], level));
389 while (kdtree_balance(
t, n->
child[1], level));
395 while (kdtree_balance(
t, n, level)) {
404 while (kdtree_balance(
t, n, level)) {
407 while (kdtree_balance(
t, n->
child[0], level));
408 while (kdtree_balance(
t, n->
child[1], level));
414 while (kdtree_balance(
t, n, level)) {
424 s[top].n = n->
child[dir];
444 while (kdtree_balance(
t, n, level)) {
447 while (kdtree_balance(
t, n->child[0], level));
448 while (kdtree_balance(
t, n->child[1], level));
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;
454 while (kdtree_balance(
t, n, level)) {
478 dir = (diffr > diffl);
481 s[top].n = n->
child[dir];
489 ld = (!n->child[0] ? -1 : n->child[0]->depth);
490 rd = (!n->child[1] ? -1 : n->child[1]->depth);
495 G_debug(1,
"k-d tree optimization: %d times balanced, new depth %d",
496 nbal,
t->root->depth);
509 double diff, dist, maxdist;
523 sn.
uid = (int)0x80000000;
535 dir = cmp(&sn, n, n->
dim) > 0;
539 s[top].n = n->
child[dir];
555 diff = sn.
c[i] - n->
c[i];
561 while (i > 0 && d[i - 1] > dist) {
566 if (i < found && d[i] == dist &&
uid[i] == n->
uid)
577 diff = sn.
c[i] - n->
c[i];
580 }
while (i-- && dist <= maxdist);
582 if (dist < maxdist) {
584 while (i > 0 && d[i - 1] > dist) {
589 if (d[i] == dist &&
uid[i] == n->
uid)
597 if (found == k && maxdist == 0.0)
603 diff = sn.
c[(int)n->
dim] - n->
c[(
int)n->
dim];
606 if (dist <= maxdist) {
609 s[top].n = n->
child[!dir];
612 dir = cmp(&sn, n, n->
dim) > 0;
616 s[top].n = n->child[dir];
630 double maxdist,
int *skip)
643 double *d, maxdistsq;
649 sn.
uid = (int)0x80000000;
661 maxdistsq = maxdist * maxdist;
668 dir = cmp(&sn, n, n->
dim) > 0;
672 s[top].n = n->
child[dir];
687 diff = sn.
c[i] - n->
c[i];
690 }
while (i-- && dist <= maxdistsq);
692 if (dist <= maxdistsq) {
693 if (found + 1 >= k) {
695 uid = G_realloc(
uid, k *
sizeof(
int));
696 d = G_realloc(d, k *
sizeof(
double));
699 while (i > 0 && d[i - 1] > dist) {
704 if (i < found && d[i] == dist &&
uid[i] == n->
uid)
715 diff = fabs(sn.
c[(
int)n->
dim] - n->
c[(
int)n->
dim]);
716 if (diff <= maxdist) {
719 s[top].n = n->
child[!dir];
722 dir = cmp(&sn, n, n->
dim) > 0;
726 s[top].n = n->child[dir];
746 int i, k, found, inside;
761 sn.
uid = (int)0x80000000;
777 dir = cmp(&sn, n, n->
dim) > 0;
781 s[top].n = n->
child[dir];
794 for (i = 0; i <
t->ndims; i++) {
795 if (n->
c[i] < sn.
c[i] || n->
c[i] > sn.
c[i +
t->ndims]) {
802 if (found + 1 >= k) {
804 uid = G_realloc(
uid, k *
sizeof(
int));
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]) {
818 s[top].n = n->
child[!dir];
821 dir = cmp(&sn, n, n->
dim) > 0;
825 s[top].n = n->child[dir];
859 G_debug(1,
"k-d tree: empty tree");
861 G_debug(1,
"k-d tree: finished traversing");
868 return kdtree_first(trav,
c,
uid);
871 return kdtree_next(trav,
c,
uid);
882 int rdir, ordir, dir;
884 struct kdnode *n, *rn, *or;
896 if (!
r->child[0] && !
r->child[1])
930 s[top].n = or->
child[ordir];
934 mindist = or->
c[(int)or->
dim] - n->
c[(
int)or->
dim];
942 if (n->dim != or->
dim)
943 dir = cmp(or, n, n->dim) > 0;
947 s[top].n = n->child[dir];
957 if ((cmp(rn, n, or->
dim) > 0) == ordir) {
959 mindist = or->
c[(int)or->
dim] - n->c[(
int)or->
dim];
966 if (n->dim != or->
dim &&
967 mindist >= fabs(n->c[(
int)n->dim] - n->c[(
int)n->dim])) {
970 s[top].n = n->child[!dir];
974 if (n->dim != or->
dim)
975 dir = cmp(or, n, n->dim) > 0;
979 s[top].n = n->child[dir];
988 if (ordir && or->
c[(
int)or->
dim] > rn->
c[(
int)or->
dim])
991 if (!ordir && or->
c[(
int)or->
dim] < rn->
c[(
int)or->
dim])
995 dir = cmp(or->
child[1], rn, or->
dim);
999 for (i = 0; i <
t->ndims; i++)
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);
1006 dir = cmp(or->
child[0], rn, or->
dim);
1010 for (i = 0; i <
t->ndims; i++)
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);
1021 if (is_leaf && rn->
depth != 0)
1023 if (!is_leaf && rn->
depth <= 0)
1034 dir = cmp(rn, n, n->dim);
1036 s[top].dir = dir > 0;
1038 s[top].n = n->child[dir > 0];
1052 s[top2 + 1].n =
NULL;
1055 memcpy(or->
c, rn->
c,
t->csize);
1069 s[top2].dir = ordir;
1078 if (s[top2].n != rn) {
1084 if (n->child[dir] != rn) {
1087 kdtree_free_node(rn);
1088 n->child[dir] =
NULL;
1091 kdtree_update_node(
t, n);
1102 if (cmp(n->child[0], n, n->dim) > 0)
1106 if (cmp(n->child[1], n, n->dim) < 1)
1112 kdtree_update_node(
t, n);
1118 static int kdtree_balance(
struct kdtree *
t,
struct kdnode *
r,
int bmode)
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;
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);
1141 if (!
r->child[0] || !
r->child[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) {
1149 else if (rd > ld + btol) {
1156 or = kdtree_newnode(
t);
1157 memcpy(or->
c,
r->c,
t->csize);
1159 or->
dim =
t->nextdim[
r->dim];
1161 if (!kdtree_replace(
t,
r))
1165 if (!cmp(
r, or,
r->dim)) {
1166 G_warning(
"kdtree_balance: replacement failed");
1167 kdtree_free_node(or);
1173 r->child[!dir] = kdtree_insert2(
t,
r->child[!dir], or, bmode, 1);
1176 kdtree_update_node(
t,
r);
1178 if (
r->depth == old_depth) {
1179 G_debug(4,
"balancing had no effect");
1183 if (
r->depth > old_depth)
1211 if (rcallsmax < rcalls)
1229 if (!cmpc(nnew, n,
t) && (!dc || nnew->
uid == n->uid)) {
1231 G_debug(1,
"KD node exists already, nothing to do");
1232 kdtree_free_node(nnew);
1241 dir = cmp(nnew, n, n->dim) > 0;
1247 s[top].n = n->child[dir];
1255 n->child[dir] = nnew;
1256 nnew->
dim =
t->nextdim[n->dim];
1268 kdtree_update_node(
t, n);
1275 if (cmp(n->child[0], n, n->dim) > 0)
1276 G_warning(
"Insert2: Left child is larger");
1279 if (cmp(n->child[1], n, n->dim) < 1)
1280 G_warning(
"Insert2: Right child is not larger");
1300 while (kdtree_balance(
t, n, bmode));
1304 if (n->child[0] && n->child[0]->balance) {
1307 s[top].n = n->child[dir];
1309 else if (n->child[1] && n->child[1]->balance) {
1312 s[top].n = n->child[dir];
1320 while (kdtree_balance(
t, n, bmode));
1324 kdtree_update_node(
t, s[top].n);
1326 if (!bmode2 && top == 0) {
1347 static int kdtree_first(
struct kdtrav *trav,
double *
c,
int *
uid)
1364 static int kdtree_next(
struct kdtrav *trav,
double *
c,
int *
uid)
1382 if (trav->
top == 0) {
void G_free(void *buf)
Free allocated memory.
int G_debug(int level, const char *msg,...)
Print debugging message.
void G_fatal_error(const char *msg,...)
Print a fatal error message to stderr.
void G_message(const char *msg,...)
Print a message to stderr.
void G_warning(const char *msg,...)
Print a warning message to stderr.
void kdtree_optimize(struct kdtree *t, int level)
int kdtree_rnn(struct kdtree *t, double *c, int **puid, int *skip)
struct kdtree * kdtree_create(char ndims, int *btol)
int kdtree_traverse(struct kdtrav *trav, double *c, int *uid)
void kdtree_clear(struct kdtree *t)
int kdtree_insert(struct kdtree *t, double *c, int uid, int dc)
int kdtree_knn(struct kdtree *t, double *c, int *uid, double *d, int k, int *skip)
void kdtree_destroy(struct kdtree *t)
int kdtree_dnn(struct kdtree *t, double *c, int **puid, double **pd, double maxdist, int *skip)
int kdtree_init_trav(struct kdtrav *trav, struct kdtree *tree)
int kdtree_remove(struct kdtree *t, double *c, int uid)
Dynamic balanced k-d tree implementation.
struct kdnode * curr_node