diff --git a/packages/udi/roads.yap.gz b/packages/udi/roads.yap.gz new file mode 100644 index 000000000..422f40736 Binary files /dev/null and b/packages/udi/roads.yap.gz differ diff --git a/packages/udi/rtree.c b/packages/udi/rtree.c new file mode 100644 index 000000000..90bd5b219 --- /dev/null +++ b/packages/udi/rtree.c @@ -0,0 +1,524 @@ +#include +#include +#include +#include +#include + +#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("]"); +} diff --git a/packages/udi/rtree.h b/packages/udi/rtree.h new file mode 100644 index 000000000..f05ae86e8 --- /dev/null +++ b/packages/udi/rtree.h @@ -0,0 +1,63 @@ +#ifndef _RTREE_ +#define _RTREE_ + +#ifndef FALSE +#define FALSE 0 +#endif +#ifndef TRUE +#define TRUE !FALSE +#endif + +#define NUMDIMS 2 /* 2d */ + +struct Rect +{ + double coords[2*NUMDIMS]; /* x1min, y1min, ... , x1max, y1max, ...*/ +}; +typedef struct Rect rect_t; + +struct Branch +{ + rect_t mbr; + void * child; /*void * so user can store whatever he needs, in case + of non-leaf ndes it stores the child-pointer*/ +}; +typedef struct Branch branch_t; + +#define PGSIZE 196 +#define MAXCARD (int)((PGSIZE-(2*sizeof(int)))/ sizeof(struct Branch)) +#define MINCARD (MAXCARD / 2) + +struct Node +{ + int count; + int level; + branch_t branch[MAXCARD]; +}; +typedef struct Node * node_t; + +typedef node_t rtree_t; + +#define MIN(a, b) ((a) < (b) ? (a) : (b)) +#define MAX(a, b) ((a) > (b) ? (a) : (b)) + +/* CallBack to search function */ +typedef int (*SearchHitCallback)(rect_t r, void *data, void *arg); + +extern rtree_t RTreeNew (void); +extern void RTreeInsert (rtree_t *, rect_t, void *); +extern int RTreeSearch (rtree_t, rect_t, SearchHitCallback, void *); +extern void RTreeDestroy (rtree_t); +extern void RTreePrint(node_t); +extern rect_t RectInit (void); + +struct Partition +{ + branch_t buffer[MAXCARD+1]; + int n; + rect_t cover_all; + rect_t cover[2]; +}; +typedef struct Partition partition_t; + +#endif /* _RTREE_ */ diff --git a/packages/udi/rtree_udi.c b/packages/udi/rtree_udi.c new file mode 100644 index 000000000..74da5ca56 --- /dev/null +++ b/packages/udi/rtree_udi.c @@ -0,0 +1,178 @@ +#include +#include +#include +#include + +#include + +#include "Yap.h" + +#include "rtree.h" +#include "clause_list.h" +#include "rtree_udi_i.h" +#include "rtree_udi.h" + +static int YAP_IsNumberTerm (YAP_Term term, YAP_Float *n) +{ + if (YAP_IsIntTerm (term) != FALSE) + { + if (n != NULL) + *n = (YAP_Float) YAP_IntOfTerm (term); + return (TRUE); + } + if (YAP_IsFloatTerm (term) != FALSE) + { + if (n != NULL) + *n = YAP_FloatOfTerm (term); + return (TRUE); + } + return (FALSE); +} + +static rect_t RectOfTerm (YAP_Term term) +{ + YAP_Term tmp; + rect_t rect; + int i; + + if (!YAP_IsPairTerm(term)) + return (RectInit()); + + for (i = 0; YAP_IsPairTerm(term) && i < 4; i++) + { + tmp = YAP_HeadOfTerm (term); + if (!YAP_IsNumberTerm(tmp,&(rect.coords[i]))) + return (RectInit()); + term = YAP_TailOfTerm (term); + } + + return (rect); +} + +control_t *RtreeUdiInit (YAP_Term spec, + void * pred, + int arity){ + control_t *control; + YAP_Term arg; + int i, c; + /* YAP_Term mod; */ + + /* spec = Yap_StripModule(spec, &mod); */ + if (! YAP_IsApplTerm(spec)) + return (NULL); + + control = (control_t *) malloc (sizeof(*control)); + assert(control); + memset((void *) control,0, sizeof(*control)); + + c = 0; + for (i = 1; i <= arity; i ++) + { + arg = YAP_ArgOfTerm(i,spec); + if (YAP_IsAtomTerm(arg) + && strcmp("+",YAP_AtomName(YAP_AtomOfTerm(arg))) == 0) + { + + (*control)[c].pred = pred; + (*control)[c++].arg = i; + + } + } + + for (i = 0; i < NARGS; i++) + printf("%d,%p\t",(*control)[i].arg,(*control)[i].tree); + printf("\n"); + + return control; +} + +control_t *RtreeUdiInsert (YAP_Term term,control_t *control,void *clausule) +{ + int i; + rect_t r; + + assert(control); + + for (i = 0; i < NARGS && (*control)[i].arg != 0 ; i++) + { + r = RectOfTerm(YAP_ArgOfTerm((*control)[i].arg,term)); + if (!(*control)[i].tree) + (*control)[i].tree = RTreeNew(); + RTreeInsert(&(*control)[i].tree,r,clausule); + } + + /* printf("insert %p\n", clausule); */ + + return (control); +} + +static int callback(rect_t r, void *data, void *arg) +{ + callback_m_t x; + x = (callback_m_t) arg; + return Yap_ClauseListExtend(x->cl,data,x->pred); +} + +/*ARGS ARE AVAILABLE*/ +void *RtreeUdiSearch (control_t *control) +{ + rect_t r; + int i; + struct ClauseList clauselist; + struct CallbackM cm; + callback_m_t c; + YAP_Term Constraints; + + /*RTreePrint ((*control)[0].tree);*/ + + for (i = 0; i < NARGS && (*control)[i].arg != 0 ; i++) + if (YAP_IsAttVar(YAP_A((*control)[i].arg))) + { + + /*get the constraits rect*/ + Constraints = YAP_AttsOfVar(YAP_A((*control)[i].arg)); + /* Yap_DebugPlWrite(Constraints); */ + r = RectOfTerm(YAP_ArgOfTerm(2,Constraints)); + + c = &cm; + c->cl = Yap_ClauseListInit(&clauselist); + c->pred = (*control)[i].pred; + if (!c->cl) + return NULL; /*? or fail*/ + RTreeSearch((*control)[i].tree, r, callback, c); + Yap_ClauseListClose(c->cl); + + if (Yap_ClauseListCount(c->cl) == 0) + { + Yap_ClauseListDestroy(c->cl); + return Yap_FAILCODE(); + } + + if (Yap_ClauseListCount(c->cl) == 1) + { + return Yap_ClauseListToClause(c->cl); + } + + return Yap_ClauseListCode(c->cl); + } + + return NULL; /*YAP FALLBACK*/ +} + +int RtreeUdiDestroy(control_t *control) +{ + int i; + + assert(control); + + for (i = 0; i < NARGS && (*control)[i].arg != 0; i++) + { + if ((*control)[i].tree) + RTreeDestroy((*control)[i].tree); + } + + free(control); + control = NULL; + + return TRUE; +} diff --git a/packages/udi/rtree_udi_i.h b/packages/udi/rtree_udi_i.h new file mode 100644 index 000000000..d16aeb04e --- /dev/null +++ b/packages/udi/rtree_udi_i.h @@ -0,0 +1,20 @@ +#ifndef _RTREE_UDI_I_ +#define _RTREE_UDI_I_ + +#define NARGS 5 +struct Control +{ + int arg; + void *pred; + rtree_t tree; +}; +typedef struct Control control_t[NARGS]; + +struct CallbackM +{ + clause_list_t cl; + void * pred; +}; +typedef struct CallbackM * callback_m_t; + +#endif /* _RTREE_UDI_I_ */ diff --git a/packages/udi/test.yap b/packages/udi/test.yap new file mode 100644 index 000000000..ea315674d --- /dev/null +++ b/packages/udi/test.yap @@ -0,0 +1,15 @@ +:- nogc. + +%% {A,B} :- +%% {A},{B}. +overlap(A,B) :- + attributes:get_all_atts(A,C), + attributes:put_att_term(A,overlap(C,B)). + +:- udi(rect(+,-)). +rect([0,0,2,2],1). +rect([5,5,7,7],2). +rect([8, 5, 9, 6],3). +rect([7, 1, 9, 2],4). + +%:- overlap(R,[6, 4, 10, 6]), rect(R,ID). \ No newline at end of file diff --git a/packages/udi/test2.yap b/packages/udi/test2.yap new file mode 100644 index 000000000..db31973a4 --- /dev/null +++ b/packages/udi/test2.yap @@ -0,0 +1,14 @@ +:- nogc. + +overlap(A,B) :- + attributes:get_all_atts(A,C), + attributes:put_att_term(A,overlap(C,B)). + +:- udi(rect(-,+)). + +:- ['roads.yap']. + +r(ID1,ID2) :- + rect(ID1,R1), + overlap(R2,R1), + rect(ID2,R2). \ No newline at end of file