RTree UDI support (written by David Vaz).
This commit is contained in:
Vitor Santos Costa
524
packages/udi/rtree.c
Normal file
524
packages/udi/rtree.c
Normal file
@@ -0,0 +1,524 @@
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <assert.h>
|
||||
#include <float.h>
|
||||
|
||||
#include "rtree.h"
|
||||
|
||||
static node_t RTreeNewNode (void);
|
||||
static void RTreeDestroyNode (node_t);
|
||||
static void RTreeNodeInit (node_t);
|
||||
|
||||
static int RTreeSearchNode (node_t, rect_t, SearchHitCallback, void *);
|
||||
static int RTreeInsertNode (node_t, int, rect_t,void *,node_t *);
|
||||
|
||||
static int RTreePickBranch (rect_t, node_t);
|
||||
static int RTreeAddBranch(node_t, branch_t, node_t *);
|
||||
static void RTreeSplitNode (node_t, branch_t, node_t *);
|
||||
|
||||
static void RTreePickSeeds(partition_t *, node_t, node_t);
|
||||
static void RTreeNodeAddBranch(rect_t *, node_t, branch_t);
|
||||
static void RTreePickNext(partition_t *, node_t, node_t);
|
||||
|
||||
static rect_t RTreeNodeCover(node_t);
|
||||
|
||||
static double RectArea (rect_t);
|
||||
static rect_t RectCombine (rect_t, rect_t);
|
||||
static int RectOverlap (rect_t, rect_t);
|
||||
static void RectPrint (rect_t);
|
||||
|
||||
static partition_t PartitionNew (void);
|
||||
static void PartitionPush (partition_t *, branch_t);
|
||||
static branch_t PartitionPop (partition_t *);
|
||||
static branch_t PartitionGet (partition_t *, int);
|
||||
|
||||
rtree_t RTreeNew (void)
|
||||
{
|
||||
rtree_t t;
|
||||
t = RTreeNewNode();
|
||||
t->level = 0; /*leaf*/
|
||||
return t;
|
||||
}
|
||||
|
||||
void RTreeDestroy (rtree_t t)
|
||||
{
|
||||
if (t)
|
||||
RTreeDestroyNode (t);
|
||||
}
|
||||
|
||||
static node_t RTreeNewNode (void)
|
||||
{
|
||||
node_t n;
|
||||
|
||||
n = (node_t) malloc (sizeof(*n));
|
||||
assert(n);
|
||||
RTreeNodeInit(n);
|
||||
return n;
|
||||
}
|
||||
|
||||
static void RTreeDestroyNode (node_t node)
|
||||
{
|
||||
int i;
|
||||
|
||||
if (node->level == 0) /* leaf level*/
|
||||
{
|
||||
for (i = 0; i < MAXCARD; i++)
|
||||
if (node->branch[i].child)
|
||||
;/* allow user free data*/
|
||||
else
|
||||
break;
|
||||
}
|
||||
else
|
||||
{
|
||||
for (i = 0; i < MAXCARD; i++)
|
||||
if (node->branch[i].child)
|
||||
RTreeDestroyNode (node->branch[i].child);
|
||||
else
|
||||
break;
|
||||
}
|
||||
free (node);
|
||||
}
|
||||
|
||||
static void RTreeNodeInit (node_t n)
|
||||
{
|
||||
memset((void *) n,0, sizeof(*n));
|
||||
n->level = -1;
|
||||
}
|
||||
|
||||
int RTreeSearch (rtree_t t, rect_t s, SearchHitCallback f, void *arg)
|
||||
{
|
||||
assert(t);
|
||||
return RTreeSearchNode(t,s,f,arg);
|
||||
}
|
||||
|
||||
static int RTreeSearchNode (node_t n, rect_t s, SearchHitCallback f, void *arg)
|
||||
{
|
||||
int i;
|
||||
int c = 0;
|
||||
|
||||
if (n->level > 0)
|
||||
{
|
||||
for (i = 0; i < MAXCARD; i++)
|
||||
if (n->branch[i].child &&
|
||||
RectOverlap (s,n->branch[i].mbr))
|
||||
c += RTreeSearchNode ((node_t) n->branch[i].child, s, f, arg);
|
||||
}
|
||||
else
|
||||
{
|
||||
for (i = 0; i < MAXCARD; i++)
|
||||
if (n->branch[i].child &&
|
||||
RectOverlap (s,n->branch[i].mbr))
|
||||
{
|
||||
c ++;
|
||||
if (f)
|
||||
if ( !f(n->branch[i].mbr,n->branch[i].child,arg))
|
||||
return c;
|
||||
}
|
||||
}
|
||||
return c;
|
||||
}
|
||||
|
||||
void RTreeInsert (rtree_t *t, rect_t r, void *data)
|
||||
{
|
||||
node_t n2;
|
||||
node_t new_root;
|
||||
branch_t b;
|
||||
assert(t && *t);
|
||||
|
||||
if (RTreeInsertNode(*t, 0, r, data, &n2))
|
||||
/* deal with root split */
|
||||
{
|
||||
new_root = RTreeNewNode();
|
||||
new_root->level = (*t)->level + 1;
|
||||
b.mbr = RTreeNodeCover(*t);
|
||||
b.child = (void *) *t;
|
||||
RTreeAddBranch(new_root, b, NULL);
|
||||
b.mbr = RTreeNodeCover(n2);
|
||||
b.child = (void *) n2;
|
||||
RTreeAddBranch(new_root, b, NULL);
|
||||
*t = new_root;
|
||||
}
|
||||
}
|
||||
|
||||
static int RTreeInsertNode (node_t n, int level,
|
||||
rect_t r, void *data,
|
||||
node_t *new_node)
|
||||
{
|
||||
int i;
|
||||
node_t n2;
|
||||
branch_t b;
|
||||
|
||||
assert(n && new_node);
|
||||
assert(level >= 0 && level <= n->level);
|
||||
|
||||
if (n->level > level)
|
||||
{
|
||||
i = RTreePickBranch(r,n);
|
||||
if (!RTreeInsertNode((node_t) n->branch[i].child, level,
|
||||
r, data,&n2)) /* not split */
|
||||
{
|
||||
n->branch[i].mbr = RectCombine(r,n->branch[i].mbr);
|
||||
return FALSE;
|
||||
}
|
||||
else /* node split */
|
||||
{
|
||||
n->branch[i].mbr = RTreeNodeCover(n->branch[i].child);
|
||||
b.child = n2;
|
||||
b.mbr = RTreeNodeCover(n2);
|
||||
return RTreeAddBranch(n, b, new_node);
|
||||
}
|
||||
}
|
||||
else /*insert level*/
|
||||
{
|
||||
b.mbr = r;
|
||||
b.child = data;
|
||||
return RTreeAddBranch(n, b, new_node);
|
||||
}
|
||||
}
|
||||
|
||||
static int RTreeAddBranch(node_t n, branch_t b, node_t *new_node)
|
||||
{
|
||||
int i;
|
||||
|
||||
assert(n);
|
||||
|
||||
if (n->count < MAXCARD) /*split not necessary*/
|
||||
{
|
||||
for (i = 0; i < MAXCARD; i++)
|
||||
if (n->branch[i].child == NULL)
|
||||
{
|
||||
n->branch[i] = b;
|
||||
n->count ++;
|
||||
break;
|
||||
}
|
||||
return FALSE;
|
||||
}
|
||||
else /*needs to split*/
|
||||
{
|
||||
assert(new_node);
|
||||
RTreeSplitNode (n, b, new_node);
|
||||
return TRUE;
|
||||
}
|
||||
}
|
||||
|
||||
static int RTreePickBranch (rect_t r, node_t n)
|
||||
{
|
||||
int i;
|
||||
double area;
|
||||
double inc_area;
|
||||
rect_t tmp;
|
||||
int best_i;
|
||||
double best_inc;
|
||||
double best_i_area;
|
||||
|
||||
best_i = 0;
|
||||
best_inc = DBL_MAX; /* double Max value */
|
||||
best_i_area = DBL_MAX;
|
||||
|
||||
for (i = 0; i < MAXCARD; i++)
|
||||
if (n->branch[i].child)
|
||||
{
|
||||
area = RectArea (n->branch[i].mbr);
|
||||
tmp = RectCombine (r, n->branch[i].mbr);
|
||||
inc_area = RectArea (tmp) - area;
|
||||
|
||||
if (inc_area < best_inc)
|
||||
{
|
||||
best_inc = inc_area;
|
||||
best_i = i;
|
||||
best_i_area = area;
|
||||
}
|
||||
else if (inc_area == best_inc && best_i_area > area)
|
||||
{
|
||||
best_inc = inc_area;
|
||||
best_i = i;
|
||||
best_i_area = area;
|
||||
}
|
||||
}
|
||||
else
|
||||
break;
|
||||
return best_i;
|
||||
}
|
||||
|
||||
static void RTreeSplitNode (node_t n, branch_t b, node_t *new_node)
|
||||
{
|
||||
partition_t p;
|
||||
int level;
|
||||
int i;
|
||||
|
||||
assert(n);
|
||||
assert(new_node);
|
||||
|
||||
p = PartitionNew();
|
||||
|
||||
for (i = 0; i < MAXCARD; i ++)
|
||||
PartitionPush(&p,n->branch[i]);
|
||||
PartitionPush(&p,b);
|
||||
|
||||
level = n->level;
|
||||
RTreeNodeInit(n);
|
||||
n->level = level;
|
||||
*new_node = RTreeNewNode();
|
||||
(*new_node)->level = level;
|
||||
|
||||
RTreePickSeeds(&p, n, *new_node);
|
||||
|
||||
while (p.n)
|
||||
if (n->count + p.n <= MINCARD)
|
||||
/* first group (n) needs all entries */
|
||||
RTreeNodeAddBranch(&(p.cover[0]), n, PartitionPop(&p));
|
||||
else if ((*new_node)->count + p.n <= MINCARD)
|
||||
/* second group (new_node) needs all entries */
|
||||
RTreeNodeAddBranch(&(p.cover[1]), *new_node, PartitionPop(&p));
|
||||
else
|
||||
RTreePickNext(&p, n, *new_node);
|
||||
}
|
||||
|
||||
static void RTreePickNext(partition_t *p, node_t n1, node_t n2)
|
||||
/* linear version */
|
||||
{
|
||||
branch_t b;
|
||||
double area[2], inc_area[2];
|
||||
rect_t tmp;
|
||||
|
||||
b = PartitionPop(p);
|
||||
|
||||
area[0] = RectArea (p->cover[0]);
|
||||
tmp = RectCombine (p->cover[0], b.mbr);
|
||||
inc_area[0] = RectArea (tmp) - area[0];
|
||||
|
||||
area[1] = RectArea (p->cover[1]);
|
||||
tmp = RectCombine (p->cover[1], b.mbr);
|
||||
inc_area[1] = RectArea (tmp) - area[1];
|
||||
|
||||
if (inc_area[0] < inc_area[1] ||
|
||||
(inc_area[0] == inc_area[1] && area[0] < area[1]))
|
||||
RTreeNodeAddBranch(&(p->cover[0]),n1,b);
|
||||
else
|
||||
RTreeNodeAddBranch(&(p->cover[1]),n2,b);
|
||||
}
|
||||
|
||||
static void RTreePickSeeds(partition_t *p, node_t n1, node_t n2)
|
||||
/* puts in index 0 of each node the resulting entry, forming the two
|
||||
groups
|
||||
This is the linear version
|
||||
*/
|
||||
{
|
||||
int dim,high, i;
|
||||
int highestLow[NUMDIMS], lowestHigh[NUMDIMS];
|
||||
double width[NUMDIMS];
|
||||
int seed0, seed1;
|
||||
double sep, best_sep;
|
||||
|
||||
assert(p->n == MAXCARD + 1);
|
||||
|
||||
for (dim = 0; dim < NUMDIMS; dim++)
|
||||
{
|
||||
high = dim + NUMDIMS;
|
||||
highestLow[dim] = lowestHigh[dim] = 0;
|
||||
for (i = 1; i < MAXCARD +1; i++)
|
||||
{
|
||||
if (p->buffer[i].mbr.coords[dim] >
|
||||
p->buffer[highestLow[dim]].mbr.coords[dim])
|
||||
highestLow[dim] = i;
|
||||
if (p->buffer[i].mbr.coords[high] <
|
||||
p->buffer[lowestHigh[dim]].mbr.coords[high])
|
||||
lowestHigh[dim] = i;
|
||||
}
|
||||
width[dim] = p->cover_all.coords[high] - p->cover_all.coords[dim];
|
||||
assert(width[dim] >= 0);
|
||||
}
|
||||
|
||||
seed0 = lowestHigh[0];
|
||||
seed1 = highestLow[0];
|
||||
best_sep = 0;
|
||||
for (dim = 0; dim < NUMDIMS; dim ++)
|
||||
{
|
||||
high = dim + NUMDIMS;
|
||||
|
||||
sep = (p->buffer[highestLow[dim]].mbr.coords[dim] -
|
||||
p->buffer[lowestHigh[dim]].mbr.coords[high]) / width[dim];
|
||||
if (sep > best_sep)
|
||||
{
|
||||
seed0 = lowestHigh[dim];
|
||||
seed1 = highestLow[dim];
|
||||
best_sep = sep;
|
||||
}
|
||||
}
|
||||
/* assert (seed0 != seed1); */
|
||||
if (seed0 > seed1)
|
||||
{
|
||||
RTreeNodeAddBranch(&(p->cover[0]),n1,PartitionGet(p,seed0));
|
||||
RTreeNodeAddBranch(&(p->cover[1]),n2,PartitionGet(p,seed1));
|
||||
}
|
||||
else if (seed0 < seed1)
|
||||
{
|
||||
RTreeNodeAddBranch(&(p->cover[0]),n1,PartitionGet(p,seed1));
|
||||
RTreeNodeAddBranch(&(p->cover[1]),n2,PartitionGet(p,seed0));
|
||||
}
|
||||
}
|
||||
|
||||
static void RTreeNodeAddBranch(rect_t *r, node_t n, branch_t b)
|
||||
{
|
||||
int i;
|
||||
|
||||
assert(n);
|
||||
assert(n->count < MAXCARD);
|
||||
|
||||
for (i = 0; i < MAXCARD; i++)
|
||||
if (n->branch[i].child == NULL)
|
||||
{
|
||||
n->branch[i] = b;
|
||||
n->count ++;
|
||||
break;
|
||||
}
|
||||
*r = RectCombine(*r,b.mbr);
|
||||
}
|
||||
|
||||
|
||||
void RTreePrint(node_t t)
|
||||
{
|
||||
int i;
|
||||
|
||||
/* printf("rtree([_,_,_,_,_]).\n"); */
|
||||
printf("rtree(%p,%d,[",t,t->level);
|
||||
for (i = 0; i < MAXCARD; i++)
|
||||
{
|
||||
if (t->branch[i].child != NULL)
|
||||
{
|
||||
printf("(%p,",t->branch[i].child);
|
||||
RectPrint(t->branch[i].mbr);
|
||||
printf(")");
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("nil");
|
||||
}
|
||||
if (i < MAXCARD-1)
|
||||
printf(",");
|
||||
}
|
||||
printf("]).\n");
|
||||
|
||||
if (t->level != 0)
|
||||
for (i = 0; i < MAXCARD; i++)
|
||||
if (t->branch[i].child != NULL)
|
||||
RTreePrint((node_t) t->branch[i].child);
|
||||
else
|
||||
break;
|
||||
}
|
||||
|
||||
/*
|
||||
* Partition related
|
||||
*/
|
||||
|
||||
static partition_t PartitionNew (void)
|
||||
{
|
||||
partition_t p;
|
||||
memset((void *) &p,0, sizeof(p));
|
||||
p.cover[0] = p.cover[1] = p.cover_all = RectInit();
|
||||
return p;
|
||||
}
|
||||
|
||||
static void PartitionPush (partition_t *p, branch_t b)
|
||||
{
|
||||
assert(p->n < MAXCARD + 1);
|
||||
p->buffer[p->n] = b;
|
||||
p->n ++;
|
||||
p->cover_all = RectCombine(p->cover_all,b.mbr);
|
||||
}
|
||||
|
||||
static branch_t PartitionPop (partition_t *p)
|
||||
{
|
||||
assert(p->n > 0);
|
||||
p->n --;
|
||||
return p->buffer[p->n];
|
||||
}
|
||||
|
||||
static branch_t PartitionGet (partition_t *p, int n)
|
||||
{
|
||||
branch_t b;
|
||||
assert (p->n > n);
|
||||
b = p->buffer[n];
|
||||
p->buffer[n] = PartitionPop(p);
|
||||
return b;
|
||||
}
|
||||
|
||||
/*
|
||||
* Rect related
|
||||
*/
|
||||
|
||||
rect_t RectInit (void)
|
||||
{
|
||||
rect_t r = {{DBL_MAX, DBL_MAX, DBL_MIN, DBL_MIN}};
|
||||
return (r);
|
||||
}
|
||||
|
||||
static double RectArea (rect_t r)
|
||||
{
|
||||
int i;
|
||||
double area;
|
||||
|
||||
for (i = 0,area = 1; i < NUMDIMS; i++)
|
||||
area *= r.coords[i+NUMDIMS] - r.coords[i];
|
||||
|
||||
/* area = (r.coords[1] - r.coords[0]) * */
|
||||
/* (r.coords[3] - r.coords[2]); */
|
||||
|
||||
return area;
|
||||
}
|
||||
|
||||
static rect_t RectCombine (rect_t r, rect_t s)
|
||||
{
|
||||
int i;
|
||||
rect_t new_rect;
|
||||
|
||||
for (i = 0; i < NUMDIMS; i++)
|
||||
{
|
||||
new_rect.coords[i] = MIN(r.coords[i],s.coords[i]);
|
||||
new_rect.coords[i+NUMDIMS] = MAX(r.coords[i+NUMDIMS],s.coords[i+NUMDIMS]);
|
||||
}
|
||||
|
||||
return new_rect;
|
||||
}
|
||||
|
||||
static int RectOverlap (rect_t r, rect_t s)
|
||||
{
|
||||
int i;
|
||||
|
||||
for (i = 0; i < NUMDIMS; i++)
|
||||
if (r.coords[i] > s.coords[i + NUMDIMS] ||
|
||||
s.coords[i] > r.coords[i + NUMDIMS])
|
||||
return FALSE;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
static rect_t RTreeNodeCover(node_t n)
|
||||
{
|
||||
int i;
|
||||
rect_t r = RectInit();
|
||||
|
||||
for (i = 0; i < MAXCARD; i++)
|
||||
if (n->branch[i].child)
|
||||
{
|
||||
r = RectCombine (r, n->branch[i].mbr);
|
||||
}
|
||||
else
|
||||
break;
|
||||
|
||||
return r;
|
||||
}
|
||||
|
||||
static void RectPrint (rect_t r)
|
||||
{
|
||||
int i;
|
||||
|
||||
printf("[");
|
||||
for (i = 0; i < 2*NUMDIMS; i++)
|
||||
{
|
||||
printf("%f",r.coords[i]);
|
||||
if ( i < 2*NUMDIMS - 1)
|
||||
printf(",");
|
||||
}
|
||||
printf("]");
|
||||
}
|
||||
Reference in New Issue
Block a user