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 *,
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 *);
46 if (a->
c[p] < b->
c[p])
48 if (a->
c[p] > b->
c[p])
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));
80 static void kdtree_free_node(
struct kdnode *n)
93 t = G_malloc(
sizeof(
struct kdtree));
96 t->
csize = ndims *
sizeof(double);
105 t->
nextdim = G_malloc(ndims *
sizeof(
char));
106 for (i = 0; i < ndims - 1; i++)
127 while((it = save) !=
NULL) {
131 kdtree_free_node(it);
162 nnew = kdtree_newnode(t);
163 memcpy(nnew->
c, c, t->
csize);
166 t->
root = kdtree_insert2(t, t->
root, nnew, 1, dc);
175 return count < t->
count;
201 found = (!cmpc(&sn, n, t) && sn.
uid == n->
uid);
203 dir = cmp(&sn, n, n->
dim) > 0;
206 s[top].n = n->
child[dir];
216 if (s[top].n->
depth == 0) {
217 kdtree_free_node(s[top].n);
223 n->child[dir] =
NULL;
226 n->depth = (!n->child[!dir] ? 0 : n->child[!dir]->depth + 1);
235 kdtree_replace(t, s[top].n, 1);
242 while (kdtree_balance(t, n->child[dir], 0));
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;
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;
259 while (kdtree_balance(t, t->
root, 0));
284 G_debug(1,
"k-d tree optimization for %zd items, tree depth %d",
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));
298 ld = (!n->child[0] ? -1 : n->child[0]->depth);
299 rd = (!n->child[1] ? -1 : n->child[1]->depth);
304 ld = (!n->child[0] ? -1 : n->child[0]->depth);
305 rd = (!n->child[1] ? -1 : n->child[1]->depth);
310 s[top].n = n->child[dir];
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;
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));
334 ld = (!n->child[0] ? -1 : n->child[0]->depth);
335 rd = (!n->child[1] ? -1 : n->child[1]->depth);
360 dir = (diffr > diffl);
363 s[top].n = n->
child[dir];
371 ld = (!n->child[0] ? -1 : n->child[0]->depth);
372 rd = (!n->child[1] ? -1 : n->child[1]->depth);
377 G_debug(1,
"k-d tree optimization: %d times balanced, new depth %d",
391 double diff, dist, maxdist;
405 sn.
uid = (int)0x80000000;
417 dir = cmp(&sn, n, n->
dim) > 0;
421 s[top].n = n->child[dir];
432 if (n->uid != sn.
uid) {
437 diff = sn.
c[i] - n->c[i];
443 while (i > 0 && d[i - 1] > dist) {
448 if (i < found && d[i] == dist && uid[i] == n->uid)
459 diff = sn.
c[i] - n->c[i];
462 }
while (i-- && dist <= maxdist);
464 if (dist < maxdist) {
466 while (i > 0 && d[i - 1] > dist) {
471 if (d[i] == dist && uid[i] == n->uid)
479 if (found == k && maxdist == 0.0)
485 diff = sn.
c[(int)n->dim] - n->c[(
int)n->dim];
488 if (dist <= maxdist) {
491 s[top].n = n->child[!dir];
494 dir = cmp(&sn, n, n->
dim) > 0;
498 s[top].n = n->child[dir];
512 double maxdist,
int *skip)
525 double *d, maxdistsq;
531 sn.
uid = (int)0x80000000;
543 maxdistsq = maxdist * maxdist;
550 dir = cmp(&sn, n, n->
dim) > 0;
554 s[top].n = n->child[dir];
565 if (n->uid != sn.
uid) {
569 diff = sn.
c[i] - n->c[i];
572 }
while (i-- && dist <= maxdistsq);
574 if (dist <= maxdistsq) {
575 if (found + 1 >= k) {
577 uid = G_realloc(uid, k *
sizeof(
int));
578 d = G_realloc(d, k *
sizeof(
double));
581 while (i > 0 && d[i - 1] > dist) {
586 if (i < found && d[i] == dist && uid[i] == n->uid)
597 diff = fabs(sn.
c[(
int)n->dim] - n->c[(
int)n->dim]);
598 if (diff <= maxdist) {
601 s[top].n = n->child[!dir];
604 dir = cmp(&sn, n, n->
dim) > 0;
608 s[top].n = n->child[dir];
628 int i, k, found, inside;
643 sn.
uid = (int)0x80000000;
659 dir = cmp(&sn, n, n->
dim) > 0;
663 s[top].n = n->child[dir];
674 if (n->uid != sn.
uid) {
676 for (i = 0; i < t->
ndims; i++) {
677 if (n->c[i] < sn.
c[i] || n->c[i] > sn.
c[i + t->
ndims]) {
684 if (found + 1 >= k) {
686 uid = G_realloc(uid, k *
sizeof(
int));
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]) {
700 s[top].n = n->child[!dir];
703 dir = cmp(&sn, n, n->
dim) > 0;
707 s[top].n = n->child[dir];
741 G_debug(1,
"k-d tree: empty tree");
743 G_debug(1,
"k-d tree: finished traversing");
750 return kdtree_first(trav, c, uid);
753 return kdtree_next(trav, c, uid);
761 static int kdtree_replace(
struct kdtree *t,
struct kdnode *
r,
int bmode)
764 int rdir, ordir, dir;
765 int ld, rd, old_depth;
766 struct kdnode *n, *rn, *or;
812 s[top].n = or->
child[ordir];
816 mindist = or->
c[(int)or->
dim] - n->
c[(
int)or->
dim];
824 if (n->dim != or->
dim)
825 dir = cmp(or, n, n->
dim) > 0;
829 s[top].n = n->child[dir];
839 if ((cmp(rn, n, or->
dim) > 0) == ordir) {
841 mindist = or->
c[(int)or->
dim] - n->c[(
int)or->
dim];
848 if (n->dim != or->
dim &&
849 mindist >= fabs(n->c[(
int)n->dim] - n->c[(
int)n->dim])) {
852 s[top].n = n->child[!dir];
856 if (n->dim != or->
dim)
857 dir = cmp(or, n, n->
dim) > 0;
861 s[top].n = n->child[dir];
870 if (ordir && or->
c[(
int)or->
dim] > rn->
c[(
int)or->
dim])
873 if (!ordir && or->
c[(
int)or->
dim] < rn->
c[(
int)or->
dim])
877 dir = cmp(or->
child[1], rn, or->
dim);
881 for (i = 0; i < t->
ndims; i++)
884 G_fatal_error(
"Right child of old root is smaller than rn, dir is %d", ordir);
888 dir = cmp(or->
child[0], rn, or->
dim);
892 for (i = 0; i < t->
ndims; i++)
895 G_fatal_error(
"Left child of old root is larger than rn, dir is %d", ordir);
903 if (is_leaf && rn->
depth != 0)
905 if (!is_leaf && rn->
depth <= 0)
916 dir = cmp(rn, n, n->
dim);
918 s[top].dir = dir > 0;
920 s[top].n = n->child[dir > 0];
934 s[top2 + 1].n =
NULL;
937 memcpy(or->
c, rn->
c, t->
csize);
960 if (s[top2].n != rn) {
966 if (n->
child[dir] != rn) {
969 kdtree_free_node(rn);
973 old_depth = n->
depth;
980 while (kdtree_balance(t, n, bmode));
982 if (n->
depth == old_depth)
997 if (cmp(n->
child[0], n, n->
dim) > 0)
1001 if (cmp(n->
child[1], n, n->
dim) < 1)
1015 static int kdtree_balance(
struct kdtree *t,
struct kdnode *r,
int bmode)
1029 old_depth =
MAX(ld, rd) + 1;
1031 if (old_depth != r->
depth) {
1032 G_warning(
"balancing: depth is wrong: %d != %d", r->
depth, old_depth);
1033 r->
depth = old_depth;
1043 if (ld > rd + btol) {
1046 else if (rd > ld + btol) {
1053 or = kdtree_newnode(t);
1054 memcpy(or->
c, r->
c, t->
csize);
1058 if (!kdtree_replace(t, r, bmode))
1062 if (!cmp(r, or, r->
dim)) {
1063 G_warning(
"kdtree_balance: replacement failed");
1064 kdtree_free_node(or);
1070 r->
child[!dir] = kdtree_insert2(t, r->
child[!dir], or, bmode, 1);
1077 if (r->
depth == old_depth) {
1078 G_debug(4,
"balancing had no effect");
1082 if (r->
depth > old_depth)
1090 int balance,
int dc)
1113 if (rcallsmax < rcalls)
1121 if (balance && bmode == 0) {
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));
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;
1159 dir = (diffr > diffl);
1162 s[top].n = n->
child[dir];
1170 ld = (!n->child[0] ? -1 : n->child[0]->depth);
1171 rd = (!n->child[1] ? -1 : n->child[1]->depth);
1184 if (balance && bmode < 2) {
1185 old_depth = n->depth;
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));
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;
1197 if (old_depth != n->depth)
1201 if (!cmpc(nnew, n, t) && (!dc || nnew->
uid == n->uid)) {
1203 G_debug(1,
"KD node exists already, nothing to do");
1204 kdtree_free_node(nnew);
1213 dir = cmp(nnew, n, n->
dim) > 0;
1219 s[top].n = n->child[dir];
1227 n->child[dir] = nnew;
1232 old_depth = n->depth;
1233 n->depth = (!n->child[!dir] ? 1 : n->child[!dir]->depth + 1);
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));
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;
1247 if (old_depth != n->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;
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));
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;
1281 if (cmp(n->child[0], n, n->dim) > 0)
1282 G_warning(
"Insert2: Left child is larger");
1285 if (cmp(n->child[1], n, n->dim) < 1)
1286 G_warning(
"Insert2: Right child is not larger");
1299 static int kdtree_first(
struct kdtrav *trav,
double *
c,
int *
uid)
1316 static int kdtree_next(
struct kdtrav *trav,
double *c,
int *uid)
1334 if (trav->
top == 0) {
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)
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_fatal_error(const char *msg,...)
Print a fatal error message to stderr.
int kdtree_traverse(struct kdtrav *trav, double *c, int *uid)
int G_debug(int level, const char *msg,...)
Print debugging message.
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)
struct kdnode * curr_node
void G_message(const char *msg,...)
Print a message to stderr.
void G_free(void *buf)
Free allocated memory.
void kdtree_optimize(struct kdtree *t, int level)
void G_warning(const char *msg,...)
Print a warning message to stderr.