29 static int rcalls = 0;
30 static int rcallsmax = 0;
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 *);
41 if (a->
c[p] < b->
c[p])
43 if (a->
c[p] > b->
c[p])
52 for (i = 0; i < t->
ndims; i++) {
53 if (a->
c[i] != b->
c[i]) {
76 static void kdtree_free_node(
struct kdnode *n)
82 static void kdtree_update_node(
struct kdtree *t,
struct kdnode *n)
104 if (ld > rd + btol || rd > ld + btol)
118 t->
csize = ndims *
sizeof(double);
127 for (i = 0; i < ndims - 1; i++)
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);
196 return count < t->
count;
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));
289 if (n->child[0] && n->child[0]->balance) {
292 s[top].n = n->child[dir];
294 else if (n->child[1] && n->child[1]->balance) {
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",
357 ld = (!n->child[0] ? -1 : n->child[0]->depth);
358 rd = (!n->child[1] ? -1 : n->child[1]->depth);
361 while (kdtree_balance(t, n->child[0], level));
363 while (kdtree_balance(t, n->child[1], level));
365 ld = (!n->child[0] ? -1 : n->child[0]->depth);
366 rd = (!n->child[1] ? -1 : n->child[1]->depth);
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));
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;
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));
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;
409 while (kdtree_balance(t, n, level)) {
413 ld = (!n->child[0] ? -1 : n->child[0]->depth);
414 rd = (!n->child[1] ? -1 : n->child[1]->depth);
419 s[top].n = n->child[dir];
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;
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);
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",
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];
545 if (n->uid != sn.
uid) {
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];
678 if (n->uid != sn.
uid) {
682 diff = sn.
c[i] - n->c[i];
685 }
while (i-- && dist <= maxdistsq);
687 if (dist <= maxdistsq) {
688 if (found + 1 >= k) {
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];
787 if (n->uid != sn.
uid) {
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) {
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);
874 static int kdtree_replace(
struct kdtree *t,
struct kdnode *
r)
877 int rdir, ordir, dir;
879 struct kdnode *n, *rn, *or;
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);
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);
1113 static int kdtree_balance(
struct kdtree *t,
struct kdnode *r,
int bmode)
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);
1141 if (ld > rd + btol) {
1144 else if (rd > ld + btol) {
1151 or = kdtree_newnode(t);
1152 memcpy(or->
c, r->
c, t->
csize);
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;
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) {
1342 static int kdtree_first(
struct kdtrav *trav,
double *
c,
int *
uid)
1359 static int kdtree_next(
struct kdtrav *trav,
double *c,
int *uid)
1377 if (trav->
top == 0) {
void void void void G_fatal_error(const char *,...) __attribute__((format(printf
void kdtree_clear(struct kdtree *t)
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.
int kdtree_rnn(struct kdtree *t, double *c, int **puid, int *skip)
void G_free(void *)
Free allocated memory.
int kdtree_dnn(struct kdtree *t, double *c, int **puid, double **pd, double maxdist, int *skip)
struct kdtree * kdtree_create(char ndims, int *btol)
void G_message(const char *,...) __attribute__((format(printf
int kdtree_traverse(struct kdtrav *trav, double *c, int *uid)
void kdtree_destroy(struct kdtree *t)
int kdtree_knn(struct kdtree *t, double *c, int *uid, double *d, int k, int *skip)
int kdtree_insert(struct kdtree *t, double *c, int uid, int dc)
void G_warning(const char *,...) __attribute__((format(printf
struct kdnode * curr_node
void kdtree_optimize(struct kdtree *t, int level)
int G_debug(int, const char *,...) __attribute__((format(printf