20#include <grass/glocale.h>
30static int rcallsmax = 0;
33 struct kdnode *,
int,
int);
34static int kdtree_replace(
struct kdtree *,
struct kdnode *);
35static int kdtree_balance(
struct kdtree *,
struct kdnode *,
int);
36static int kdtree_first(
struct kdtrav *,
double *,
int *);
37static int kdtree_next(
struct kdtrav *,
double *,
int *);
41 if (a->
c[p] <
b->c[p])
43 if (a->
c[p] >
b->c[p])
46 return (a->
uid <
b->uid ? -1 : a->
uid >
b->uid);
52 for (i = 0; i <
t->ndims; i++) {
53 if (a->
c[i] !=
b->c[i]) {
65 n->
c = G_malloc(
t->ndims *
sizeof(
double));
76static void kdtree_free_node(
struct kdnode *n)
82static void kdtree_update_node(
struct kdtree *
t,
struct kdnode *n)
104 if (ld > rd + btol || rd > ld + btol)
115 t = G_malloc(
sizeof(
struct kdtree));
118 t->csize =
ndims *
sizeof(double);
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;
148 while((it = save) !=
NULL) {
152 kdtree_free_node(it);
183 nnew = kdtree_newnode(
t);
184 memcpy(nnew->
c,
c,
t->csize);
187 t->root = kdtree_insert2(
t,
t->root, nnew, 1, dc);
222 found = (!cmpc(&sn, n,
t) && sn.
uid == n->
uid);
224 dir = cmp(&sn, n, n->
dim) > 0;
227 s[top].n = n->
child[dir];
237 if (s[top].n->
depth == 0) {
238 kdtree_free_node(s[top].n);
244 n->child[dir] =
NULL;
247 kdtree_update_node(
t, n);
256 kdtree_replace(
t, s[top].n);
263 kdtree_update_node(
t, n);
285 while (kdtree_balance(
t, n, bmode));
292 s[top].n = n->
child[dir];
297 s[top].n = n->
child[dir];
304 kdtree_update_node(
t, n);
306 while (kdtree_balance(
t, n, bmode));
310 kdtree_update_node(
t, s[top].n);
312 if (!bmode2 && top == 0) {
348 G_debug(1,
"k-d tree optimization for %zd items, tree depth %d",
349 t->count,
t->root->depth);
361 while (kdtree_balance(
t, n->
child[0], level));
363 while (kdtree_balance(
t, n->
child[1], level));
372 s[top].n = n->
child[dir];
380 while (kdtree_balance(
t, n, level)) {
383 while (kdtree_balance(
t, n->
child[0], level));
384 while (kdtree_balance(
t, n->
child[1], level));
390 while (kdtree_balance(
t, n, level)) {
399 while (kdtree_balance(
t, n, level)) {
402 while (kdtree_balance(
t, n->
child[0], level));
403 while (kdtree_balance(
t, n->
child[1], level));
409 while (kdtree_balance(
t, n, level)) {
419 s[top].n = n->
child[dir];
439 while (kdtree_balance(
t, n, level)) {
442 while (kdtree_balance(
t, n->child[0], level));
443 while (kdtree_balance(
t, n->child[1], level));
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;
449 while (kdtree_balance(
t, n, level)) {
473 dir = (diffr > diffl);
476 s[top].n = n->
child[dir];
484 ld = (!n->child[0] ? -1 : n->child[0]->depth);
485 rd = (!n->child[1] ? -1 : n->child[1]->depth);
490 G_debug(1,
"k-d tree optimization: %d times balanced, new depth %d",
491 nbal,
t->root->depth);
504 double diff, dist, maxdist;
518 sn.
uid = (int)0x80000000;
530 dir = cmp(&sn, n, n->
dim) > 0;
534 s[top].n = n->
child[dir];
550 diff = sn.
c[i] - n->
c[i];
556 while (i > 0 && d[i - 1] > dist) {
561 if (i < found && d[i] == dist &&
uid[i] == n->
uid)
572 diff = sn.
c[i] - n->
c[i];
575 }
while (i-- && dist <= maxdist);
577 if (dist < maxdist) {
579 while (i > 0 && d[i - 1] > dist) {
584 if (d[i] == dist &&
uid[i] == n->
uid)
592 if (found == k && maxdist == 0.0)
598 diff = sn.
c[(int)n->
dim] - n->
c[(
int)n->
dim];
601 if (dist <= maxdist) {
604 s[top].n = n->
child[!dir];
607 dir = cmp(&sn, n, n->
dim) > 0;
611 s[top].n = n->child[dir];
625 double maxdist,
int *skip)
638 double *d, maxdistsq;
644 sn.
uid = (int)0x80000000;
656 maxdistsq = maxdist * maxdist;
663 dir = cmp(&sn, n, n->
dim) > 0;
667 s[top].n = n->
child[dir];
682 diff = sn.
c[i] - n->
c[i];
685 }
while (i-- && dist <= maxdistsq);
687 if (dist <= maxdistsq) {
688 if (found + 1 >= k) {
690 uid = G_realloc(
uid, k *
sizeof(
int));
691 d = G_realloc(d, k *
sizeof(
double));
694 while (i > 0 && d[i - 1] > dist) {
699 if (i < found && d[i] == dist &&
uid[i] == n->
uid)
710 diff = fabs(sn.
c[(
int)n->
dim] - n->
c[(
int)n->
dim]);
711 if (diff <= maxdist) {
714 s[top].n = n->
child[!dir];
717 dir = cmp(&sn, n, n->
dim) > 0;
721 s[top].n = n->child[dir];
741 int i, k, found, inside;
756 sn.
uid = (int)0x80000000;
772 dir = cmp(&sn, n, n->
dim) > 0;
776 s[top].n = n->
child[dir];
789 for (i = 0; i <
t->ndims; i++) {
790 if (n->
c[i] < sn.
c[i] || n->
c[i] > sn.
c[i +
t->ndims]) {
797 if (found + 1 >= k) {
799 uid = G_realloc(
uid, k *
sizeof(
int));
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]) {
813 s[top].n = n->
child[!dir];
816 dir = cmp(&sn, n, n->
dim) > 0;
820 s[top].n = n->child[dir];
854 G_debug(1,
"k-d tree: empty tree");
856 G_debug(1,
"k-d tree: finished traversing");
863 return kdtree_first(trav,
c,
uid);
866 return kdtree_next(trav,
c,
uid);
877 int rdir, ordir, dir;
879 struct kdnode *n, *rn, *or;
891 if (!
r->child[0] && !
r->child[1])
925 s[top].n = or->
child[ordir];
929 mindist = or->
c[(int)or->
dim] - n->
c[(
int)or->
dim];
937 if (n->dim != or->
dim)
938 dir = cmp(or, n, n->dim) > 0;
942 s[top].n = n->child[dir];
952 if ((cmp(rn, n, or->
dim) > 0) == ordir) {
954 mindist = or->
c[(int)or->
dim] - n->c[(
int)or->
dim];
961 if (n->dim != or->
dim &&
962 mindist >= fabs(n->c[(
int)n->dim] - n->c[(
int)n->dim])) {
965 s[top].n = n->child[!dir];
969 if (n->dim != or->
dim)
970 dir = cmp(or, n, n->dim) > 0;
974 s[top].n = n->child[dir];
983 if (ordir && or->
c[(
int)or->
dim] > rn->
c[(
int)or->
dim])
986 if (!ordir && or->
c[(
int)or->
dim] < rn->
c[(
int)or->
dim])
990 dir = cmp(or->
child[1], rn, or->
dim);
994 for (i = 0; i <
t->ndims; i++)
997 G_fatal_error(
"Right child of old root is smaller than rn, dir is %d", ordir);
1001 dir = cmp(or->
child[0], rn, or->
dim);
1005 for (i = 0; i <
t->ndims; i++)
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);
1016 if (is_leaf && rn->
depth != 0)
1018 if (!is_leaf && rn->
depth <= 0)
1029 dir = cmp(rn, n, n->dim);
1031 s[top].dir = dir > 0;
1033 s[top].n = n->child[dir > 0];
1047 s[top2 + 1].n =
NULL;
1050 memcpy(or->
c, rn->
c,
t->csize);
1064 s[top2].dir = ordir;
1073 if (s[top2].n != rn) {
1079 if (n->child[dir] != rn) {
1082 kdtree_free_node(rn);
1083 n->child[dir] =
NULL;
1086 kdtree_update_node(
t, n);
1097 if (cmp(n->child[0], n, n->dim) > 0)
1101 if (cmp(n->child[1], n, n->dim) < 1)
1107 kdtree_update_node(
t, n);
1113static int kdtree_balance(
struct kdtree *
t,
struct kdnode *
r,
int bmode)
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;
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);
1136 if (!
r->child[0] || !
r->child[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) {
1144 else if (rd > ld + btol) {
1151 or = kdtree_newnode(
t);
1152 memcpy(or->
c,
r->c,
t->csize);
1154 or->
dim =
t->nextdim[
r->dim];
1156 if (!kdtree_replace(
t,
r))
1160 if (!cmp(
r, or,
r->dim)) {
1161 G_warning(
"kdtree_balance: replacement failed");
1162 kdtree_free_node(or);
1168 r->child[!dir] = kdtree_insert2(
t,
r->child[!dir], or, bmode, 1);
1171 kdtree_update_node(
t,
r);
1173 if (
r->depth == old_depth) {
1174 G_debug(4,
"balancing had no effect");
1178 if (
r->depth > old_depth)
1206 if (rcallsmax < rcalls)
1224 if (!cmpc(nnew, n,
t) && (!dc || nnew->
uid == n->uid)) {
1226 G_debug(1,
"KD node exists already, nothing to do");
1227 kdtree_free_node(nnew);
1236 dir = cmp(nnew, n, n->dim) > 0;
1242 s[top].n = n->child[dir];
1250 n->child[dir] = nnew;
1251 nnew->
dim =
t->nextdim[n->dim];
1263 kdtree_update_node(
t, n);
1270 if (cmp(n->child[0], n, n->dim) > 0)
1271 G_warning(
"Insert2: Left child is larger");
1274 if (cmp(n->child[1], n, n->dim) < 1)
1275 G_warning(
"Insert2: Right child is not larger");
1295 while (kdtree_balance(
t, n, bmode));
1299 if (n->child[0] && n->child[0]->balance) {
1302 s[top].n = n->child[dir];
1304 else if (n->child[1] && n->child[1]->balance) {
1307 s[top].n = n->child[dir];
1315 while (kdtree_balance(
t, n, bmode));
1319 kdtree_update_node(
t, s[top].n);
1321 if (!bmode2 && top == 0) {
1342static int kdtree_first(
struct kdtrav *trav,
double *
c,
int *
uid)
1359static int kdtree_next(
struct kdtrav *trav,
double *
c,
int *
uid)
1377 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