Loading...
Searching...
No Matches
kdtree.c
Go to the documentation of this file.
/*!
* \file kdtree.c
*
* \brief binary search tree
*
* Dynamic balanced k-d tree implementation
*
* (C) 2014 by the GRASS Development Team
*
* This program is free software under the GNU General Public License
* (>=v2). Read the file COPYING that comes with GRASS for details.
*
* \author Markus Metz
*/
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <grass/gis.h>
#include <grass/glocale.h>
#include "kdtree.h"
#define KD_BTOL 7
#ifdef KD_DEBUG
#undef KD_DEBUG
#endif
static int rcalls = 0;
static int rcallsmax = 0;
static struct kdnode *kdtree_insert2(struct kdtree *, struct kdnode *,
struct kdnode *, int, int);
static int kdtree_replace(struct kdtree *, struct kdnode *);
static int kdtree_balance(struct kdtree *, struct kdnode *, int);
static int kdtree_first(struct kdtrav *, double *, int *);
static int kdtree_next(struct kdtrav *, double *, int *);
static int cmp(struct kdnode *a, struct kdnode *b, int p)
{
if (a->c[p] < b->c[p])
return -1;
if (a->c[p] > b->c[p])
return 1;
return (a->uid < b->uid ? -1 : a->uid > b->uid);
}
static int cmpc(struct kdnode *a, struct kdnode *b, struct kdtree *t)
{
int i;
for (i = 0; i < t->ndims; i++) {
if (a->c[i] != b->c[i]) {
return 1;
}
}
return 0;
}
static struct kdnode *kdtree_newnode(struct kdtree *t)
{
struct kdnode *n = G_malloc(sizeof(struct kdnode));
n->c = G_malloc(t->ndims * sizeof(double));
n->dim = 0;
n->depth = 0;
n->balance = 0;
n->uid = 0;
n->child[0] = NULL;
n->child[1] = NULL;
return n;
}
static void kdtree_free_node(struct kdnode *n)
{
G_free(n->c);
G_free(n);
}
static void kdtree_update_node(struct kdtree *t, struct kdnode *n)
{
int ld, rd, btol;
ld = (!n->child[0] ? -1 : n->child[0]->depth);
rd = (!n->child[1] ? -1 : n->child[1]->depth);
n->depth = MAX(ld, rd) + 1;
n->balance = 0;
/* set balance flag if any of the node's subtrees needs balancing
* or if the node itself needs balancing */
if ((n->child[0] && n->child[0]->balance) ||
(n->child[1] && n->child[1]->balance)) {
n->balance = 1;
return;
}
btol = t->btol;
if (!n->child[0] || !n->child[1])
btol = 2;
if (ld > rd + btol || rd > ld + btol)
n->balance = 1;
}
/* create a new k-d tree with ndims dimensions,
* optionally set balancing tolerance */
struct kdtree *kdtree_create(char ndims, int *btol)
{
int i;
struct kdtree *t;
t = G_malloc(sizeof(struct kdtree));
t->ndims = ndims;
t->csize = ndims * sizeof(double);
t->btol = KD_BTOL;
if (btol) {
t->btol = *btol;
if (t->btol < 2)
t->btol = 2;
}
t->nextdim = G_malloc(ndims * sizeof(char));
for (i = 0; i < ndims - 1; i++)
t->nextdim[i] = i + 1;
t->nextdim[ndims - 1] = 0;
t->count = 0;
t->root = NULL;
return t;
}
/* clear the tree, removing all entries */
void kdtree_clear(struct kdtree *t)
{
struct kdnode *it;
struct kdnode *save = t->root;
/*
Rotate away the left links so that
we can treat this like the destruction
of a linked list
*/
while ((it = save) != NULL) {
if (it->child[0] == NULL) {
/* No left links, just kill the node and move on */
save = it->child[1];
kdtree_free_node(it);
it = NULL;
}
else {
/* Rotate away the left link and check again */
save = it->child[0];
it->child[0] = save->child[1];
save->child[1] = it;
}
}
t->root = NULL;
}
/* destroy the tree */
void kdtree_destroy(struct kdtree *t)
{
/* remove all entries */
kdtree_clear(t);
G_free(t->nextdim);
G_free(t);
t = NULL;
}
/* insert an item (coordinates c and uid) into the k-d tree
* dc == 1: allow duplicate coordinates */
int kdtree_insert(struct kdtree *t, double *c, int uid, int dc)
{
struct kdnode *nnew;
size_t count = t->count;
nnew = kdtree_newnode(t);
memcpy(nnew->c, c, t->csize);
nnew->uid = uid;
t->root = kdtree_insert2(t, t->root, nnew, 1, dc);
/* print depth of recursion
* recursively called fns are insert2, balance, and replace */
/*
if (rcallsmax > 1)
fprintf(stdout, "%d\n", rcallsmax);
*/
return count < t->count;
}
/* remove an item from the k-d tree
* coordinates c and uid must match */
int kdtree_remove(struct kdtree *t, double *c, int uid)
{
struct kdnode sn, *n;
struct kdstack {
struct kdnode *n;
int dir;
} s[256];
int top;
int dir, found;
int balance, bmode;
sn.c = c;
sn.uid = uid;
/* find sn node */
top = 0;
s[top].n = t->root;
dir = 1;
found = 0;
while (!found) {
n = s[top].n;
found = (!cmpc(&sn, n, t) && sn.uid == n->uid);
if (!found) {
dir = cmp(&sn, n, n->dim) > 0;
s[top].dir = dir;
top++;
s[top].n = n->child[dir];
if (!s[top].n) {
G_warning("Node does not exist");
return 0;
}
}
}
if (s[top].n->depth == 0) {
kdtree_free_node(s[top].n);
s[top].n = NULL;
if (top) {
top--;
n = s[top].n;
dir = s[top].dir;
n->child[dir] = NULL;
/* update node */
kdtree_update_node(t, n);
}
else {
t->root = NULL;
return 1;
}
}
else
kdtree_replace(t, s[top].n);
while (top) {
top--;
n = s[top].n;
/* update node */
kdtree_update_node(t, n);
}
balance = 1;
bmode = 1;
if (balance) {
struct kdnode *r;
int iter, bmode2;
/* fix any inconsistencies in the (sub-)tree */
iter = 0;
bmode2 = 0;
top = 0;
r = t->root;
s[top].n = r;
while (top >= 0) {
n = s[top].n;
/* top-down balancing
* slower but more compact */
if (!bmode2) {
while (kdtree_balance(t, n, bmode))
;
}
/* go down */
if (n->child[0] && n->child[0]->balance) {
dir = 0;
top++;
s[top].n = n->child[dir];
}
else if (n->child[1] && n->child[1]->balance) {
dir = 1;
top++;
s[top].n = n->child[dir];
}
/* go back up */
else {
/* bottom-up balancing
* faster but less compact */
kdtree_update_node(t, n);
if (bmode2) {
while (kdtree_balance(t, n, bmode))
;
}
top--;
if (top >= 0) {
kdtree_update_node(t, s[top].n);
}
if (!bmode2 && top == 0) {
iter++;
if (iter == 2) {
/* the top node has been visited twice,
* switch from top-down to bottom-up balancing */
iter = 0;
bmode2 = 1;
}
}
}
}
}
return 1;
}
/* k-d tree optimization, only useful if the tree will be used heavily
* (more searches than items in the tree)
* level 0 = a bit, 1 = more, 2 = a lot */
void kdtree_optimize(struct kdtree *t, int level)
{
struct kdnode *n, *n2;
struct kdstack {
struct kdnode *n;
int dir;
char v;
} s[256];
int dir;
int top;
int ld, rd;
int diffl, diffr;
int nbal;
if (!t->root)
return;
G_debug(1, "k-d tree optimization for %zd items, tree depth %d", t->count,
t->root->depth);
nbal = 0;
top = 0;
s[top].n = t->root;
while (s[top].n) {
n = s[top].n;
ld = (!n->child[0] ? -1 : n->child[0]->depth);
rd = (!n->child[1] ? -1 : n->child[1]->depth);
if (ld < rd)
while (kdtree_balance(t, n->child[0], level))
;
else if (ld > rd)
while (kdtree_balance(t, n->child[1], level))
;
ld = (!n->child[0] ? -1 : n->child[0]->depth);
rd = (!n->child[1] ? -1 : n->child[1]->depth);
n->depth = MAX(ld, rd) + 1;
dir = (rd > ld);
top++;
s[top].n = n->child[dir];
}
while (top) {
top--;
n = s[top].n;
/* balance node */
while (kdtree_balance(t, n, level)) {
nbal++;
}
while (kdtree_balance(t, n->child[0], level))
;
while (kdtree_balance(t, n->child[1], level))
;
ld = (!n->child[0] ? -1 : n->child[0]->depth);
rd = (!n->child[1] ? -1 : n->child[1]->depth);
n->depth = MAX(ld, rd) + 1;
while (kdtree_balance(t, n, level)) {
nbal++;
}
}
while (s[top].n) {
n = s[top].n;
/* balance node */
while (kdtree_balance(t, n, level)) {
nbal++;
}
while (kdtree_balance(t, n->child[0], level))
;
while (kdtree_balance(t, n->child[1], level))
;
ld = (!n->child[0] ? -1 : n->child[0]->depth);
rd = (!n->child[1] ? -1 : n->child[1]->depth);
n->depth = MAX(ld, rd) + 1;
while (kdtree_balance(t, n, level)) {
nbal++;
}
ld = (!n->child[0] ? -1 : n->child[0]->depth);
rd = (!n->child[1] ? -1 : n->child[1]->depth);
dir = (rd > ld);
top++;
s[top].n = n->child[dir];
}
while (top) {
top--;
n = s[top].n;
/* update node depth */
ld = (!n->child[0] ? -1 : n->child[0]->depth);
rd = (!n->child[1] ? -1 : n->child[1]->depth);
n->depth = MAX(ld, rd) + 1;
}
if (level) {
top = 0;
s[top].n = t->root;
while (s[top].n) {
n = s[top].n;
/* balance node */
while (kdtree_balance(t, n, level)) {
nbal++;
}
while (kdtree_balance(t, n->child[0], level))
;
while (kdtree_balance(t, n->child[1], level))
;
ld = (!n->child[0] ? -1 : n->child[0]->depth);
rd = (!n->child[1] ? -1 : n->child[1]->depth);
n->depth = MAX(ld, rd) + 1;
while (kdtree_balance(t, n, level)) {
nbal++;
}
diffl = diffr = -1;
if (n->child[0]) {
n2 = n->child[0];
ld = (!n2->child[0] ? -1 : n2->child[0]->depth);
rd = (!n2->child[1] ? -1 : n2->child[1]->depth);
diffl = ld - rd;
if (diffl < 0)
diffl = -diffl;
}
if (n->child[1]) {
n2 = n->child[1];
ld = (!n2->child[0] ? -1 : n2->child[0]->depth);
rd = (!n2->child[1] ? -1 : n2->child[1]->depth);
diffr = ld - rd;
if (diffr < 0)
diffr = -diffr;
}
dir = (diffr > diffl);
top++;
s[top].n = n->child[dir];
}
while (top) {
top--;
n = s[top].n;
/* update node depth */
ld = (!n->child[0] ? -1 : n->child[0]->depth);
rd = (!n->child[1] ? -1 : n->child[1]->depth);
n->depth = MAX(ld, rd) + 1;
}
}
G_debug(1, "k-d tree optimization: %d times balanced, new depth %d", nbal,
t->root->depth);
return;
}
/* find k nearest neighbors
* results are stored in uid (uids) and d (squared distances)
* optionally an uid to be skipped can be given
* useful when searching for the nearest neighbors of an item
* that is also in the tree */
int kdtree_knn(struct kdtree *t, double *c, int *uid, double *d, int k,
int *skip)
{
int i, found;
double diff, dist, maxdist;
struct kdnode sn, *n;
struct kdstack {
struct kdnode *n;
int dir;
char v;
} s[256];
int dir;
int top;
if (!t->root)
return 0;
sn.c = c;
sn.uid = (int)0x80000000;
if (skip)
sn.uid = *skip;
maxdist = INFINITY;
found = 0;
/* go down */
top = 0;
s[top].n = t->root;
while (s[top].n) {
n = s[top].n;
dir = cmp(&sn, n, n->dim) > 0;
s[top].dir = dir;
s[top].v = 0;
top++;
s[top].n = n->child[dir];
}
/* go back up */
while (top) {
top--;
if (!s[top].v) {
s[top].v = 1;
n = s[top].n;
if (n->uid != sn.uid) {
if (found < k) {
dist = 0.0;
i = t->ndims - 1;
do {
diff = sn.c[i] - n->c[i];
dist += diff * diff;
} while (i--);
i = found;
while (i > 0 && d[i - 1] > dist) {
d[i] = d[i - 1];
uid[i] = uid[i - 1];
i--;
}
if (i < found && d[i] == dist && uid[i] == n->uid)
G_fatal_error("knn: inserting duplicate");
d[i] = dist;
uid[i] = n->uid;
maxdist = d[found];
found++;
}
else {
dist = 0.0;
i = t->ndims - 1;
do {
diff = sn.c[i] - n->c[i];
dist += diff * diff;
} while (i-- && dist <= maxdist);
if (dist < maxdist) {
i = k - 1;
while (i > 0 && d[i - 1] > dist) {
d[i] = d[i - 1];
uid[i] = uid[i - 1];
i--;
}
if (d[i] == dist && uid[i] == n->uid)
G_fatal_error("knn: inserting duplicate");
d[i] = dist;
uid[i] = n->uid;
maxdist = d[k - 1];
}
}
if (found == k && maxdist == 0.0)
break;
}
/* look on the other side ? */
dir = s[top].dir;
diff = sn.c[(int)n->dim] - n->c[(int)n->dim];
dist = diff * diff;
if (dist <= maxdist) {
/* go down the other side */
top++;
s[top].n = n->child[!dir];
while (s[top].n) {
n = s[top].n;
dir = cmp(&sn, n, n->dim) > 0;
s[top].dir = dir;
s[top].v = 0;
top++;
s[top].n = n->child[dir];
}
}
}
}
return found;
}
/* find all nearest neighbors within distance aka radius search
* results are stored in puid (uids) and pd (squared distances)
* memory is allocated as needed, the calling fn must free the memory
* optionally an uid to be skipped can be given */
int kdtree_dnn(struct kdtree *t, double *c, int **puid, double **pd,
double maxdist, int *skip)
{
int i, k, found;
double diff, dist;
struct kdnode sn, *n;
struct kdstack {
struct kdnode *n;
int dir;
char v;
} s[256];
int dir;
int top;
int *uid;
double *d, maxdistsq;
if (!t->root)
return 0;
sn.c = c;
sn.uid = (int)0x80000000;
if (skip)
sn.uid = *skip;
*pd = NULL;
*puid = NULL;
k = 0;
uid = NULL;
d = NULL;
found = 0;
maxdistsq = maxdist * maxdist;
/* go down */
top = 0;
s[top].n = t->root;
while (s[top].n) {
n = s[top].n;
dir = cmp(&sn, n, n->dim) > 0;
s[top].dir = dir;
s[top].v = 0;
top++;
s[top].n = n->child[dir];
}
/* go back up */
while (top) {
top--;
if (!s[top].v) {
s[top].v = 1;
n = s[top].n;
if (n->uid != sn.uid) {
dist = 0;
i = t->ndims - 1;
do {
diff = sn.c[i] - n->c[i];
dist += diff * diff;
} while (i-- && dist <= maxdistsq);
if (dist <= maxdistsq) {
if (found + 1 >= k) {
k = found + 10;
uid = G_realloc(uid, k * sizeof(int));
d = G_realloc(d, k * sizeof(double));
}
i = found;
while (i > 0 && d[i - 1] > dist) {
d[i] = d[i - 1];
uid[i] = uid[i - 1];
i--;
}
if (i < found && d[i] == dist && uid[i] == n->uid)
G_fatal_error("dnn: inserting duplicate");
d[i] = dist;
uid[i] = n->uid;
found++;
}
}
/* look on the other side ? */
dir = s[top].dir;
diff = fabs(sn.c[(int)n->dim] - n->c[(int)n->dim]);
if (diff <= maxdist) {
/* go down the other side */
top++;
s[top].n = n->child[!dir];
while (s[top].n) {
n = s[top].n;
dir = cmp(&sn, n, n->dim) > 0;
s[top].dir = dir;
s[top].v = 0;
top++;
s[top].n = n->child[dir];
}
}
}
}
*pd = d;
*puid = uid;
return found;
}
/* find all nearest neighbors within range aka box search
* the range is specified with min and max for each dimension as
* (min1, min2, ..., minn, max1, max2, ..., maxn)
* results are stored in puid (uids)
* memory is allocated as needed, the calling fn must free the memory
* optionally an uid to be skipped can be given */
int kdtree_rnn(struct kdtree *t, double *c, int **puid, int *skip)
{
int i, k, found, inside;
struct kdnode sn, *n;
struct kdstack {
struct kdnode *n;
int dir;
char v;
} s[256];
int dir;
int top;
int *uid;
if (!t->root)
return 0;
sn.c = c;
sn.uid = (int)0x80000000;
if (skip)
sn.uid = *skip;
*puid = NULL;
k = 0;
uid = NULL;
found = 0;
/* go down */
top = 0;
s[top].n = t->root;
while (s[top].n) {
n = s[top].n;
dir = cmp(&sn, n, n->dim) > 0;
s[top].dir = dir;
s[top].v = 0;
top++;
s[top].n = n->child[dir];
}
/* go back up */
while (top) {
top--;
if (!s[top].v) {
s[top].v = 1;
n = s[top].n;
if (n->uid != sn.uid) {
inside = 1;
for (i = 0; i < t->ndims; i++) {
if (n->c[i] < sn.c[i] || n->c[i] > sn.c[i + t->ndims]) {
inside = 0;
break;
}
}
if (inside) {
if (found + 1 >= k) {
k = found + 10;
uid = G_realloc(uid, k * sizeof(int));
}
i = found;
uid[i] = n->uid;
found++;
}
}
/* look on the other side ? */
dir = s[top].dir;
if (n->c[(int)n->dim] >= sn.c[(int)n->dim] &&
n->c[(int)n->dim] <= sn.c[(int)n->dim + t->ndims]) {
/* go down the other side */
top++;
s[top].n = n->child[!dir];
while (s[top].n) {
n = s[top].n;
dir = cmp(&sn, n, n->dim) > 0;
s[top].dir = dir;
s[top].v = 0;
top++;
s[top].n = n->child[dir];
}
}
}
}
*puid = uid;
return found;
}
/* initialize tree traversal
* (re-)sets trav structure
* returns 0
*/
int kdtree_init_trav(struct kdtrav *trav, struct kdtree *tree)
{
trav->tree = tree;
trav->curr_node = tree->root;
trav->first = 1;
trav->top = 0;
return 0;
}
/* traverse the tree
* useful to get all items in the tree non-recursively
* struct kdtrav *trav needs to be initialized first
* returns 1, 0 when finished
*/
int kdtree_traverse(struct kdtrav *trav, double *c, int *uid)
{
if (trav->curr_node == NULL) {
if (trav->first)
G_debug(1, "k-d tree: empty tree");
else
G_debug(1, "k-d tree: finished traversing");
return 0;
}
if (trav->first) {
trav->first = 0;
return kdtree_first(trav, c, uid);
}
return kdtree_next(trav, c, uid);
}
/**********************************************/
/* internal functions */
/**********************************************/
static int kdtree_replace(struct kdtree *t, struct kdnode *r)
{
double mindist;
int rdir, ordir, dir;
int ld, rd;
struct kdnode *n, *rn, *or;
struct kdstack {
struct kdnode *n;
int dir;
char v;
} s[256];
int top, top2;
int is_leaf;
int nr;
if (!r)
return 0;
if (!r->child[0] && !r->child[1])
return 0;
/* do not call kdtree_balance in this fn, this can cause
* stack overflow due to too many recursive calls */
/* find replacement for r
* overwrite r, delete replacement */
nr = 0;
/* pick a subtree */
rdir = 1;
or = r;
ld = (!or->child[0] ? -1 : or->child[0]->depth);
rd = (!or->child[1] ? -1 : or->child[1]->depth);
if (ld > rd) {
rdir = 0;
}
/* replace old root, make replacement the new root
* repeat until replacement is leaf */
ordir = rdir;
is_leaf = 0;
s[0].n = or;
s[0].dir = ordir;
top2 = 1;
mindist = -1;
while (!is_leaf) {
rn = NULL;
/* find replacement for old root */
top = top2;
s[top].n = or->child[ordir];
n = s[top].n;
rn = n;
mindist = or->c[(int)or->dim] - n->c[(int)or->dim];
if (ordir)
mindist = -mindist;
/* go down */
while (s[top].n) {
n = s[top].n;
dir = !ordir;
if (n->dim != or->dim)
dir = cmp(or, n, n->dim) > 0;
s[top].dir = dir;
s[top].v = 0;
top++;
s[top].n = n->child[dir];
}
/* go back up */
while (top > top2) {
top--;
if (!s[top].v) {
s[top].v = 1;
n = s[top].n;
if ((cmp(rn, n, or->dim) > 0) == ordir) {
rn = n;
mindist = or->c[(int)or->dim] - n->c[(int)or->dim];
if (ordir)
mindist = -mindist;
}
/* look on the other side ? */
dir = s[top].dir;
if (n->dim != or->dim &&
mindist >= fabs(n->c[(int)n->dim] - n->c[(int)n->dim])) {
/* go down the other side */
top++;
s[top].n = n->child[!dir];
while (s[top].n) {
n = s[top].n;
dir = !ordir;
if (n->dim != or->dim)
dir = cmp(or, n, n->dim) > 0;
s[top].dir = dir;
s[top].v = 0;
top++;
s[top].n = n->child[dir];
}
}
}
}
#ifdef KD_DEBUG
if (!rn)
G_fatal_error("No replacement");
if (ordir && or->c[(int)or->dim] > rn->c[(int)or->dim])
G_fatal_error("rn is smaller");
if (!ordir && or->c[(int)or->dim] < rn->c[(int)or->dim])
G_fatal_error("rn is larger");
if (or->child[1]) {
dir = cmp(or->child[1], rn, or->dim);
if (dir < 0) {
int i;
for (i = 0; i < t->ndims; i++)
G_message("rn c %g, or child c %g", rn->c[i],
or->child[1]->c[i]);
G_fatal_error(
"Right child of old root is smaller than rn, dir is %d",
ordir);
}
}
if (or->child[0]) {
dir = cmp(or->child[0], rn, or->dim);
if (dir > 0) {
int i;
for (i = 0; i < t->ndims; i++)
G_message("rn c %g, or child c %g", rn->c[i],
or->child[0]->c[i]);
G_fatal_error(
"Left child of old root is larger than rn, dir is %d",
ordir);
}
}
#endif
is_leaf = (rn->child[0] == NULL && rn->child[1] == NULL);
#ifdef KD_DEBUG
if (is_leaf && rn->depth != 0)
G_fatal_error("rn is leaf but depth is %d", (int)rn->depth);
if (!is_leaf && rn->depth <= 0)
G_fatal_error("rn is not leaf but depth is %d", (int)rn->depth);
#endif
nr++;
/* go to replacement from or->child[ordir] */
top = top2;
dir = 1;
while (dir) {
n = s[top].n;
dir = cmp(rn, n, n->dim);
if (dir) {
s[top].dir = dir > 0;
top++;
s[top].n = n->child[dir > 0];
if (!s[top].n) {
G_fatal_error("(Last) replacement disappeared %d", nr);
}
}
}
#ifdef KD_DEBUG
if (s[top].n != rn)
G_fatal_error("rn is unreachable from or");
#endif
top2 = top;
s[top2 + 1].n = NULL;
/* copy replacement to old root */
memcpy(or->c, rn->c, t->csize);
or->uid = rn->uid;
if (!is_leaf) {
/* make replacement the old root */
or = rn;
/* pick a subtree */
ordir = 1;
ld = (!or->child[0] ? -1 : or->child[0]->depth);
rd = (!or->child[1] ? -1 : or->child[1]->depth);
if (ld > rd) {
ordir = 0;
}
s[top2].dir = ordir;
top2++;
}
}
if (!rn)
G_fatal_error("No replacement at all");
/* delete last replacement */
if (s[top2].n != rn) {
G_fatal_error("Wrong top2 for last replacement");
}
top = top2 - 1;
n = s[top].n;
dir = s[top].dir;
if (n->child[dir] != rn) {
G_fatal_error("Last replacement disappeared");
}
kdtree_free_node(rn);
n->child[dir] = NULL;
t->count--;
kdtree_update_node(t, n);
top++;
/* go back up */
while (top) {
top--;
n = s[top].n;
#ifdef KD_DEBUG
/* debug directions */
if (n->child[0]) {
if (cmp(n->child[0], n, n->dim) > 0)
G_warning("Left child is larger");
}
if (n->child[1]) {
if (cmp(n->child[1], n, n->dim) < 1)
G_warning("Right child is not larger");
}
#endif
/* update node */
kdtree_update_node(t, n);
}
return nr;
}
static int kdtree_balance(struct kdtree *t, struct kdnode *r, int bmode)
{
struct kdnode *or;
int dir;
int rd, ld;
int old_depth;
int btol;
if (!r) {
return 0;
}
ld = (!r->child[0] ? -1 : r->child[0]->depth);
rd = (!r->child[1] ? -1 : r->child[1]->depth);
old_depth = MAX(ld, rd) + 1;
if (old_depth != r->depth) {
G_warning("balancing: depth is wrong: %d != %d", r->depth, old_depth);
kdtree_update_node(t, r);
}
/* subtree difference */
btol = t->btol;
if (!r->child[0] || !r->child[1])
btol = 2;
dir = -1;
ld = (!r->child[0] ? -1 : r->child[0]->depth);
rd = (!r->child[1] ? -1 : r->child[1]->depth);
if (ld > rd + btol) {
dir = 0;
}
else if (rd > ld + btol) {
dir = 1;
}
else {
return 0;
}
or = kdtree_newnode(t);
memcpy(or->c, r->c, t->csize);
or->uid = r->uid;
or->dim = t->nextdim[r->dim];
if (!kdtree_replace(t, r))
G_fatal_error("kdtree_balance: nothing replaced");
#ifdef KD_DEBUG
if (!cmp(r, or, r->dim)) {
G_warning("kdtree_balance: replacement failed");
kdtree_free_node(or);
return 0;
}
#endif
r->child[!dir] =
kdtree_insert2(t, r->child[!dir], or, bmode, 1); /* bmode */
/* update node */
kdtree_update_node(t, r);
if (r->depth == old_depth) {
G_debug(4, "balancing had no effect");
return 1;
}
if (r->depth > old_depth)
G_fatal_error("balancing failed");
return 1;
}
static struct kdnode *kdtree_insert2(struct kdtree *t, struct kdnode *r,
struct kdnode *nnew, int balance, int dc)
{
struct kdnode *n;
struct kdstack {
struct kdnode *n;
int dir;
} s[256];
int top;
int dir;
int bmode;
if (!r) {
r = nnew;
t->count++;
return r;
}
/* level of recursion */
rcalls++;
if (rcallsmax < rcalls)
rcallsmax = rcalls;
/* balancing modes
* bmode = 0: no recursion (only insert -> balance -> insert)
* slower, higher tree depth
* bmode = 1: recursion (insert -> balance -> insert -> balance ...)
* faster, more compact tree
* */
bmode = 1;
/* find node with free child */
top = 0;
s[top].n = r;
while (s[top].n) {
n = s[top].n;
if (!cmpc(nnew, n, t) && (!dc || nnew->uid == n->uid)) {
G_debug(1, "KD node exists already, nothing to do");
kdtree_free_node(nnew);
if (!balance) {
rcalls--;
return r;
}
break;
}
dir = cmp(nnew, n, n->dim) > 0;
s[top].dir = dir;
top++;
if (top > 255)
G_fatal_error("depth too large: %d", top);
s[top].n = n->child[dir];
}
if (!s[top].n) {
/* insert to child pointer of parent */
top--;
n = s[top].n;
dir = s[top].dir;
n->child[dir] = nnew;
nnew->dim = t->nextdim[n->dim];
t->count++;
top++;
}
/* go back up */
while (top) {
top--;
n = s[top].n;
/* update node */
kdtree_update_node(t, n);
/* do not balance on the way back up */
#ifdef KD_DEBUG
/* debug directions */
if (n->child[0]) {
if (cmp(n->child[0], n, n->dim) > 0)
G_warning("Insert2: Left child is larger");
}
if (n->child[1]) {
if (cmp(n->child[1], n, n->dim) < 1)
G_warning("Insert2: Right child is not larger");
}
#endif
}
if (balance) {
int iter, bmode2;
/* fix any inconsistencies in the (sub-)tree */
iter = 0;
bmode2 = 0;
top = 0;
s[top].n = r;
while (top >= 0) {
n = s[top].n;
/* top-down balancing
* slower but more compact */
if (!bmode2) {
while (kdtree_balance(t, n, bmode))
;
}
/* go down */
if (n->child[0] && n->child[0]->balance) {
dir = 0;
top++;
s[top].n = n->child[dir];
}
else if (n->child[1] && n->child[1]->balance) {
dir = 1;
top++;
s[top].n = n->child[dir];
}
/* go back up */
else {
/* bottom-up balancing
* faster but less compact */
if (bmode2) {
while (kdtree_balance(t, n, bmode))
;
}
top--;
if (top >= 0) {
kdtree_update_node(t, s[top].n);
}
if (!bmode2 && top == 0) {
iter++;
if (iter == 2) {
/* the top node has been visited twice,
* switch from top-down to bottom-up balancing */
iter = 0;
bmode2 = 1;
}
}
}
}
}
rcalls--;
return r;
}
/* start traversing the tree
* returns pointer to first item
*/
static int kdtree_first(struct kdtrav *trav, double *c, int *uid)
{
/* get smallest item */
while (trav->curr_node->child[0] != NULL) {
trav->up[trav->top++] = trav->curr_node;
trav->curr_node = trav->curr_node->child[0];
}
memcpy(c, trav->curr_node->c, trav->tree->csize);
*uid = trav->curr_node->uid;
return 1;
}
/* continue traversing the tree in ascending order
* returns pointer to data item, NULL when finished
*/
static int kdtree_next(struct kdtrav *trav, double *c, int *uid)
{
if (trav->curr_node->child[1] != NULL) {
/* something on the right side: larger item */
trav->up[trav->top++] = trav->curr_node;
trav->curr_node = trav->curr_node->child[1];
/* go down, find smallest item in this branch */
while (trav->curr_node->child[0] != NULL) {
trav->up[trav->top++] = trav->curr_node;
trav->curr_node = trav->curr_node->child[0];
}
}
else {
/* at smallest item in this branch, go back up */
struct kdnode *last;
do {
if (trav->top == 0) {
trav->curr_node = NULL;
break;
}
last = trav->curr_node;
trav->curr_node = trav->up[--trav->top];
} while (last == trav->curr_node->child[1]);
}
if (trav->curr_node != NULL) {
memcpy(c, trav->curr_node->c, trav->tree->csize);
*uid = trav->curr_node->uid;
return 1;
}
return 0; /* finished traversing */
}
G_free
void G_free(void *buf)
Free allocated memory.
Definition alloc.c:147
NULL
#define NULL
Definition ccmath.h:32
G_debug
int G_debug(int level, const char *msg,...)
Print debugging message.
Definition debug.c:66
b
double b
Definition driver/set_window.c:5
t
double t
Definition driver/set_window.c:5
r
double r
Definition driver/set_window.c:5
G_fatal_error
void G_fatal_error(const char *msg,...)
Print a fatal error message to stderr.
Definition gis/error.c:159
G_message
void G_message(const char *msg,...)
Print a message to stderr.
Definition gis/error.c:89
G_warning
void G_warning(const char *msg,...)
Print a warning message to stderr.
Definition gis/error.c:203
count
int count
kdtree_optimize
void kdtree_optimize(struct kdtree *t, int level)
Definition kdtree.c:334
kdtree_rnn
int kdtree_rnn(struct kdtree *t, double *c, int **puid, int *skip)
Definition kdtree.c:751
kdtree_create
struct kdtree * kdtree_create(char ndims, int *btol)
Definition kdtree.c:111
kdtree_traverse
int kdtree_traverse(struct kdtrav *trav, double *c, int *uid)
Definition kdtree.c:862
kdtree_clear
void kdtree_clear(struct kdtree *t)
Definition kdtree.c:139
kdtree_insert
int kdtree_insert(struct kdtree *t, double *c, int uid, int dc)
Definition kdtree.c:179
kdtree_knn
int kdtree_knn(struct kdtree *t, double *c, int *uid, double *d, int k, int *skip)
Definition kdtree.c:512
kdtree_destroy
void kdtree_destroy(struct kdtree *t)
Definition kdtree.c:167
kdtree_dnn
int kdtree_dnn(struct kdtree *t, double *c, int **puid, double **pd, double maxdist, int *skip)
Definition kdtree.c:636
KD_BTOL
#define KD_BTOL
Definition kdtree.c:23
kdtree_init_trav
int kdtree_init_trav(struct kdtrav *trav, struct kdtree *tree)
Definition kdtree.c:847
kdtree_remove
int kdtree_remove(struct kdtree *t, double *c, int uid)
Definition kdtree.c:202
kdtree.h
Dynamic balanced k-d tree implementation.
MAX
#define MAX(a, b)
Definition parson.c:91
kdnode
Node for k-d tree.
Definition kdtree.h:67
kdnode::dim
unsigned char dim
Definition kdtree.h:68
kdnode::balance
unsigned char balance
Definition kdtree.h:70
kdnode::depth
unsigned char depth
Definition kdtree.h:69
kdnode::child
struct kdnode * child[2]
Definition kdtree.h:73
kdnode::uid
int uid
Definition kdtree.h:72
kdnode::c
double * c
Definition kdtree.h:71
kdtrav
k-d tree traversal
Definition kdtree.h:92
kdtrav::tree
struct kdtree * tree
Definition kdtree.h:93
kdtrav::curr_node
struct kdnode * curr_node
Definition kdtree.h:94
kdtrav::up
struct kdnode * up[256]
Definition kdtree.h:95
kdtrav::top
int top
Definition kdtree.h:96
kdtrav::first
int first
Definition kdtree.h:97
kdtree
k-d tree
Definition kdtree.h:80
kdtree::ndims
unsigned char ndims
Definition kdtree.h:81
kdtree::btol
int btol
Definition kdtree.h:84
kdtree::csize
int csize
Definition kdtree.h:83
kdtree::root
struct kdnode * root
Definition kdtree.h:86