--- ray/src/hd/sm_qtree.c 1998/08/19 17:45:24 3.1 +++ ray/src/hd/sm_qtree.c 2003/06/20 00:25:49 3.16 @@ -1,9 +1,6 @@ -/* Copyright (c) 1998 Silicon Graphics, Inc. */ - #ifndef lint -static char SCCSid[] = "$SunId$ SGI"; +static const char RCSid[] = "$Id: sm_qtree.c,v 3.16 2003/06/20 00:25:49 greg Exp $"; #endif - /* * sm_qtree.c: adapted from octree.c from radiance code */ @@ -14,17 +11,27 @@ static char SCCSid[] = "$SunId$ SGI"; */ #include "standard.h" - +#include "sm_flag.h" #include "sm_geom.h" -#include "sm_qtree.h" #include "object.h" +#include "sm_qtree.h" -QUADTREE *quad_block[QT_MAX_BLK]; /* our quadtree */ +QUADTREE *quad_block[QT_MAX_BLK]; /* our quadtree */ static QUADTREE quad_free_list = EMPTY; /* freed octree nodes */ -static QUADTREE treetop = 0; /* next free node */ +static QUADTREE treetop = 0; /* next free node */ +int32 *quad_flag= NULL; /* quadtree flags */ - +qtremovelast(qt,id) +QUADTREE qt; +OBJECT id; +{ +#ifdef DEBUG + if(qtqueryset(qt)[(*(qtqueryset(qt)))] != id) + error(CONSISTENCY,"not removed\n"); +#endif + ((*(qtqueryset(qt)))--); +} QUADTREE qtAlloc() /* allocate a quadtree */ { @@ -41,14 +48,31 @@ qtAlloc() /* allocate a quadtree */ if (QT_BLOCK(freet) >= QT_MAX_BLK) return(EMPTY); if ((quad_block[QT_BLOCK(freet)] = (QUADTREE *)malloc( - (unsigned)QT_BLOCK_SIZE*4*sizeof(QUADTREE))) == NULL) - return(EMPTY); + QT_BLOCK_SIZE*4*sizeof(QUADTREE))) == NULL) + error(SYSTEM,"qtAlloc(): Unable to allocate memory\n"); + + /* Realloc the per/node flags */ + quad_flag = (int32 *)realloc((void *)quad_flag, + (QT_BLOCK(freet)+1)*((QT_BLOCK_SIZE+7)>>3)); + if (quad_flag == NULL) + error(SYSTEM,"qtAlloc(): Unable to allocate memory\n"); } treetop += 4; return(freet); } +qtClearAllFlags() /* clear all quadtree branch flags */ +{ + if (!treetop) + return; + + /* Clear the node flags*/ + bzero((char *)quad_flag, (QT_BLOCK(treetop-4)+1)*((QT_BLOCK_SIZE+7)>>3)); + /* Clear set flags */ + qtclearsetflags(); +} + qtFree(qt) /* free a quadtree */ register QUADTREE qt; { @@ -69,497 +93,1470 @@ qtFree(qt) /* free a quadtree */ qtDone() /* free EVERYTHING */ { register int i; - + + qtfreeleaves(); for (i = 0; i < QT_MAX_BLK; i++) { - free((char *)quad_block[i], - (unsigned)QT_BLOCK_SIZE*4*sizeof(QUADTREE)); + if (quad_block[i] == NULL) + break; + free((void *)quad_block[i]); quad_block[i] = NULL; } + /* Free the flags */ + if (i) free((void *)quad_flag); + quad_flag = NULL; quad_free_list = EMPTY; treetop = 0; } -QUADTREE -qtCompress(qt) /* recursively combine nodes */ -register QUADTREE qt; +/* + * bary_parent(coord,i) : Set coord to be value of pt at one level up in + * quadtree, given it was the ith child + * BCOORD coord[3]; :barycentric coordinates of point, also will hold + * result as side effect + * int i; :designates which child coord was + */ +bary_parent(coord,i) +BCOORD coord[3]; +int i; { - register int i; - register QUADTREE qres; + switch(i) { + case 0: + /* update bary for child */ + coord[0] = (coord[0] >> 1) + MAXBCOORD4; + coord[1] >>= 1; + coord[2] >>= 1; + break; + case 1: + coord[0] >>= 1; + coord[1] = (coord[1] >> 1) + MAXBCOORD4; + coord[2] >>= 1; + break; + + case 2: + coord[0] >>= 1; + coord[1] >>= 1; + coord[2] = (coord[2] >> 1) + MAXBCOORD4; + break; + + case 3: + coord[0] = MAXBCOORD4 - (coord[0] >> 1); + coord[1] = MAXBCOORD4 - (coord[1] >> 1); + coord[2] = MAXBCOORD4 - (coord[2] >> 1); + break; +#ifdef DEBUG + default: + eputs("bary_parent():Invalid child\n"); + break; +#endif + } +} - if (!QT_IS_TREE(qt)) /* not a tree */ - return(qt); - qres = QT_NTH_CHILD(qt,0) = qtCompress(QT_NTH_CHILD(qt,0)); - for (i = 1; i < 4; i++) - if((QT_NTH_CHILD(qt,i) = qtCompress(QT_NTH_CHILD(qt,i))) != qres) - qres = qt; - if(!QT_IS_TREE(qres)) - { /* all were identical leaves */ - QT_NTH_CHILD(qt,0) = quad_free_list; - quad_free_list = qt; +/* + * bary_from_child(coord,child,next): Given that coord was the (child) child + * of the quadtree node, calculate what the (next) child + * is at the same level. + * BCOORD coord[3]; :At current level (child)th child of node,will also hold + * result as side effect + * int child,next; :Which child coord was (child), and which one + * is now desired(next) + */ +bary_from_child(coord,child,next) +BCOORD coord[3]; +int child,next; +{ +#ifdef DEBUG + if(child <0 || child > 3) + { + eputs("bary_from_child():Invalid child\n"); + return; + } + if(next <0 || next > 3) + { + eputs("bary_from_child():Invalid next\n"); + return; + } +#endif + if(next == child) + return; + + switch(child){ + case 0: + coord[0] = 0; + coord[1] = MAXBCOORD2 - coord[1]; + coord[2] = MAXBCOORD2 - coord[2]; + break; + case 1: + coord[0] = MAXBCOORD2 - coord[0]; + coord[1] = 0; + coord[2] = MAXBCOORD2 - coord[2]; + break; + case 2: + coord[0] = MAXBCOORD2 - coord[0]; + coord[1] = MAXBCOORD2 - coord[1]; + coord[2] = 0; + break; + case 3: + switch(next){ + case 0: + coord[0] = 0; + coord[1] = MAXBCOORD2 - coord[1]; + coord[2] = MAXBCOORD2 - coord[2]; + break; + case 1: + coord[0] = MAXBCOORD2 - coord[0]; + coord[1] = 0; + coord[2] = MAXBCOORD2 - coord[2]; + break; + case 2: + coord[0] = MAXBCOORD2 - coord[0]; + coord[1] = MAXBCOORD2 - coord[1]; + coord[2] = 0; + break; } - return(qres); + break; + } } -QUADTREE -qtPoint_locate(qtptr,v1,v2,v3,pt,type,which,p0,p1,p2) -QUADTREE *qtptr; -FVECT v1,v2,v3; -FVECT pt; -char *type,*which; -FVECT p0,p1,p2; +/* + * int + * bary_child(coord): Return which child coord is of node at current level + * As side effect recalculate coord at next level + * BCOORD coord[3]; :At current level coordinates of point, will also set + * to coords at next level as side effect + */ +int +bary_child(coord) +BCOORD coord[3]; { - char d,w; - int i; - QUADTREE *child; - QUADTREE qt; - FVECT a,b,c; - FVECT t1,t2,t3; + int i; - /* Determine if point lies within pyramid (and therefore - inside a spherical quadtree cell):GT_INTERIOR, on one of the - pyramid sides (and on cell edge):GT_EDGE(1,2 or 3), - or on pyramid vertex (and on cell vertex):GT_VERTEX(1,2, or 3). - For each triangle edge: compare the - point against the plane formed by the edge and the view center - */ - d = test_single_point_against_spherical_tri(v1,v2,v3,pt,&w); - - /* Not in this triangle */ - if(!d) + if(coord[0] > MAXBCOORD4) + { + /* update bary for child */ + coord[0] = (coord[0]<< 1) - MAXBCOORD2; + coord[1] <<= 1; + coord[2] <<= 1; + return(0); + } + else + if(coord[1] > MAXBCOORD4) { - if(which) - *which = 0; - return(EMPTY); + coord[0] <<= 1; + coord[1] = (coord[1] << 1) - MAXBCOORD2; + coord[2] <<= 1; + return(1); } - - /* Will return lowest level triangle containing point: It the - point is on an edge or vertex: will return first associated - triangle encountered in the child traversal- the others can - be derived using triangle adjacency information - */ - - if(QT_IS_TREE(*qtptr)) - { - qtSubdivide_tri(v1,v2,v3,a,b,c); - child = QT_NTH_CHILD_PTR(*qtptr,0); - if(!QT_IS_EMPTY(*child)) + else + if(coord[2] > MAXBCOORD4) { - qt = qtPoint_locate(child,v1,a,c,pt,type,which,p0,p1,p2); - if(!QT_IS_EMPTY(qt)) - return(qt); + coord[0] <<= 1; + coord[1] <<= 1; + coord[2] = (coord[2] << 1) - MAXBCOORD2; + return(2); } - child = QT_NTH_CHILD_PTR(*qtptr,1); - if(!QT_IS_EMPTY(*child)) - { - qt = qtPoint_locate(child,a,b,c,pt,type,which,p0,p1,p2); - if(!QT_IS_EMPTY(qt)) - return(qt); - } - child = QT_NTH_CHILD_PTR(*qtptr,2); - if(!QT_IS_EMPTY(*child)) - { - qt = qtPoint_locate(child,a,v2,b,pt,type,which,p0,p1,p2); - if(!QT_IS_EMPTY(qt)) - return(qt); - } - child = QT_NTH_CHILD_PTR(*qtptr,3); - if(!QT_IS_EMPTY(*child)) - { - qt = qtPoint_locate(child,c,b,v3,pt,type,which,p0,p1,p2); - if(!QT_IS_EMPTY(qt)) - return(qt); - } - } - else - if(!QT_IS_EMPTY(*qtptr)) - { - /* map GT_VERTEX,GT_EDGE,GT_FACE GT_INTERIOR of pyramid to - spherical triangle primitives - */ - if(type) - *type = d; - if(which) - *which = w; - VCOPY(p0,v1); - VCOPY(p1,v2); - VCOPY(p2,v3); - return(*qtptr); - } - return(EMPTY); + else + { + coord[0] = MAXBCOORD2 - (coord[0] << 1); + coord[1] = MAXBCOORD2 - (coord[1] << 1); + coord[2] = MAXBCOORD2 - (coord[2] << 1); + return(3); + } } + + +QUADTREE +qtLocate(qt,bcoord) +QUADTREE qt; +BCOORD bcoord[3]; +{ + int i; + + if(QT_IS_TREE(qt)) + { + i = bary_child(bcoord); + + return(qtLocate(QT_NTH_CHILD(qt,i),bcoord)); + } + else + return(qt); +} + int -qtPoint_in_tri(qtptr,v0,v1,v2,pt,type,which) -QUADTREE *qtptr; -FVECT v0,v1,v2; -FVECT pt; -char *type,*which; +move_to_nbr(b,db0,db1,db2,tptr) +BCOORD b[3]; +BDIR db0,db1,db2; +double *tptr; { - QUADTREE qt; - OBJECT os[MAXSET+1],*optr; - char d,w; - int i,id; - FVECT p0,p1,p2; + double t,dt; + BCOORD bc; + int nbr; - qt = qtPoint_locate(qtptr,v0,v1,v2,pt,type,which,p0,p1,p2); - if(QT_IS_EMPTY(qt)) - return(EMPTY); - - /* Get the set */ - qtgetset(os,qt); - for (i = QT_SET_CNT(os),optr = QT_SET_PTR(os); i > 0; i--) + nbr = -1; + *tptr = 0.0; + /* Advance to next node */ + if(b[0]==0 && db0 < 0) + return(0); + if(b[1]==0 && db1 < 0) + return(1); + if(b[2]==0 && db2 < 0) + return(2); + if(db0 < 0) + { + t = ((double)b[0])/-db0; + nbr = 0; + } + else + t = MAXFLOAT; + if(db1 < 0) + { + dt = ((double)b[1])/-db1; + if( dt < t) { - /* Find the triangle that pt falls in (NOTE:FOR now return first 1) */ - id = QT_SET_NEXT_ELEM(optr); - qtTri_verts_from_id(id,p0,p1,p2); - d = test_single_point_against_spherical_tri(p0,p1,p2,pt,&w); - if(d) - { - if(type) - *type = d; - if(which) - *which = w; - return(id); - } + t = dt; + nbr = 1; } - return(EMPTY); + } + if(db2 < 0) + { + dt = ((double)b[2])/-db2; + if( dt < t) + { + t = dt; + nbr = 2; + } + } + *tptr = t; + return(nbr); } -QUADTREE -qtSubdivide(qtptr) -QUADTREE *qtptr; +qtTrace_ray(qt,b,db0,db1,db2,nextptr,sign,sfactor,func,f) + QUADTREE qt; + BCOORD b[3]; + BDIR db0,db1,db2; + int *nextptr; + int sign,sfactor; + FUNC func; + int *f; { - QUADTREE node; - node = qtAlloc(); - QT_CLEAR_CHILDREN(node); - *qtptr = node; - return(node); + int i,found; + QUADTREE child; + int nbr,next,w; + double t; + + if(QT_IS_TREE(qt)) + { + /* Find the appropriate child and reset the coord */ + i = bary_child(b); + + for(;;) + { + child = QT_NTH_CHILD(qt,i); + if(i != 3) + qtTrace_ray(child,b,db0,db1,db2,nextptr,sign,sfactor+1,func,f); + else + /* If the center cell- must flip direction signs */ + qtTrace_ray(child,b,-db0,-db1,-db2,nextptr,1-sign,sfactor+1,func,f); + + if(QT_FLAG_IS_DONE(*f)) + return; + /* If in same block: traverse */ + if(i==3) + next = *nextptr; + else + if(*nextptr == i) + next = 3; + else + { + /* reset the barycentric coordinates in the parents*/ + bary_parent(b,i); + /* Else pop up to parent and traverse from there */ + return(qt); + } + bary_from_child(b,i,next); + i = next; + } + } + else + { +#ifdef TEST_DRIVER + if(Pick_cnt < 500) + Pick_q[Pick_cnt++]=qt; +#endif; + F_FUNC(func)(qt,F_ARGS(func),f); + if(QT_FLAG_IS_DONE(*f)) + return; + /* Advance to next node */ + nbr = move_to_nbr(b,db0,db1,db2,&t); + + if(nbr==-1) + { + QT_FLAG_SET_DONE(*f); + return; + } + b[0] += (BCOORD)(t *db0); + b[1] += (BCOORD)(t *db1); + b[2] += (BCOORD)(t *db2); + *nextptr = nbr; + return; + + } +} + +#define TEST_INT(tl,th,d,q0,q1,h,l) \ + tl=d>q0;th=d> 1) | (s0 << 2 & 4))); + /* See if edge can be trivially rejected from intersetion testing */ + test_t0t1 = !(((s0 & 6)==0) || ((s1 & 6)==0)|| ((s2 & 6)==0) || + ((sq0 & 6)==6) ||((sq1 & 6)==6)|| ((sq2 & 6)==6)); + bl=bh=0; + if(test_t0t1 && (e0 & 2) ) + { + /* Must do double calculation to avoid overflow */ + db = t0[1] + dt10[1]*((double)(a-t0[0]))/dt10[0]; + TEST_INT(testl,testh,db,b,q1[1],bl,bh) + } + test_t1t2= !(((s0 & 3)==0) || ((s1 & 3)==0)|| ((s2 & 3)==0) || + ((sq0 & 3)==3) ||((sq1 & 3)==3)|| ((sq2 & 3)==3)); + if(test_t1t2 && (e0 & 1)) + { /* test t1t2 against a */ + db = t1[1] + dt21[1]*((double)(a - t1[0]))/dt21[0]; + TEST_INT(testl,testh,db,b,q1[1],bl,bh) + } + test_t2t0 = !(((s0 & 5)==0) || ((s1 & 5)==0)|| ((s2 & 5)==0) || + ((sq0 & 5)==5) ||((sq1 & 5)==5)|| ((sq2 & 5)==5)); + if(test_t2t0 && (e0 & 4)) + { + db = t2[1] + dt02[1]*((double)(a - t2[0]))/dt02[0]; + TEST_INT(testl,testh,db,b,q1[1],bl,bh) + } + e1 = (s1 ^ ((s1 >> 1) | (s1 << 2 & 4))); + cl = ch = 0; + if(test_t0t1 && (e1 & 2)) + {/* test t0t1 against b */ + db = t0[2] + dt10[2]*((double)(b-t0[1]))/dt10[1]; + TEST_INT(testl,testh,db,c,q2[2],cl,ch) + } + if(test_t1t2 && (e1 & 1)) + {/* test t1t2 against b */ + db = t1[2] + dt21[2]*((double)(q0[1] - t1[1]))/dt21[1]; + TEST_INT(testl,testh,db,c,q2[2],cl,ch) + } + if(test_t2t0 && (e1 & 4)) + {/* test t2t0 against b */ + db = t2[2] + dt02[2]*((double)(q0[1] - t2[1]))/dt02[1]; + TEST_INT(testl,testh,db,c,q2[2],cl,ch) + } + + /* test edges against c */ + e2 = (s2 ^ ((s2 >> 1) | (s2 << 2 & 4))); + al = ah = 0; + if(test_t0t1 && (e2 & 2)) + { /* test t0t1 against c */ + db = t0[0] + dt10[0]*((double)(c-t0[2]))/dt10[2]; + TEST_INT(testl,testh,db,a,q0[0],al,ah) + } + if(test_t1t2 && (e2 & 1)) + { + db = t1[0] + dt21[0]*((double)(c - t1[2]))/dt21[2]; + TEST_INT(testl,testh,db,a,q0[0],al,ah) + } + if(test_t2t0 && (e2 & 4)) + { /* test t2t0 against c */ + db = t2[0] + dt02[0]*((double)(c - t2[2]))/dt02[2]; + TEST_INT(testl,testh,db,a,q0[0],al,ah) + } + /* Only remaining case is if some edge trivially rejected */ + if(!e0 || !e1 || !e2) + return(qt); + + /* Only one/none got tested - pick either of other two/any two */ + /* Only need to test one edge */ + if(!test_t0t1 && (e0 & 2)) + { + db = t0[1] + dt10[1]*((double)(a-t0[0]))/dt10[0]; + TEST_INT(testl,testh,db,b,q1[1],bl,bh) + } + if(!test_t1t2 && (e0 & 1)) + {/* test t1t2 against a */ + db = t1[1] + dt21[1]*((double)(a - t1[0]))/dt21[0]; + TEST_INT(testl,testh,db,b,q1[1],bl,bh) + } + if(!test_t2t0 && (e0 & 4)) + { + db = t2[1] + dt02[1]*((double)(a - t2[0]))/dt02[0]; + TEST_INT(testl,testh,db,b,q1[1],bl,bh) + } + + return(qt); + +Lfunc_call: + qt = f.func(f.argptr,root,qt,q0,q1,q2,t0,t1,t2,dt10,dt21,dt02,scale, + s0,s1,s2,sq0,sq1,sq2,0,f,n); + return(qt); + } + +/* Leaf node: Do definitive test */ QUADTREE -qtSubdivide_nth_child(qt,n) +qtLeaf_insert_tri_rev(root,qt,q0,q1,q2,t0,t1,t2,dt10,dt21,dt02, + scale, s0,s1,s2,sq0,sq1,sq2,f,n) +int root; QUADTREE qt; +BCOORD q0[3],q1[3],q2[3]; +BCOORD t0[3],t1[3],t2[3]; +BCOORD dt10[3],dt21[3],dt02[3]; +unsigned int scale,s0,s1,s2,sq0,sq1,sq2; +FUNC f; int n; -{ - QUADTREE node; +{ + double db; + unsigned int e0,e1,e2; + BCOORD a,b,c; + double p0[2], p1[2],cp; + char al,ah,bl,bh,cl,ch; + char testl,testh,test_t0t1,test_t1t2,test_t2t0; + unsigned int ls0,ls1,ls2; + + + /* May have gotten passed trivial reject if one/two vertices are ON */ + a = q1[0];b= q0[1]; c= q0[2]; + SIDES_LESS(t0,t1,t2,ls0,ls1,ls2,a,b,c); + + /* First check if can trivial accept: if vertex in cell */ + if(ls0 & ls1 & ls2) + { + goto Lfunc_call; + } + if(ls0==0 || ls1 == 0 || ls2 ==0) + return(qt); + /* Assumption: Could not trivial reject*/ + /* IF any coords lie on edge- must be in if couldnt reject */ - node = qtSubdivide(&(QT_NTH_CHILD(qt,n))); + if(t0[0]== a || t0[1] == b || t0[2] == c) + { + goto Lfunc_call; + } + if(t1[0]== a || t1[1] == b || t1[2] == c) + { + goto Lfunc_call; + } + if(t2[0]== a || t2[1] == b || t2[2] == c) + { + goto Lfunc_call; + } + /* Test for edge crossings */ + /* Test t0t1 against a,b,c */ + /* First test if t0t1 can be trivially rejected */ + /* If both edges are outside bounds- dont need to test */ - return(node); -} + /* Test t0t1,t1t2,t2t0 against a */ + test_t0t1 = !(((ls0 & 6)==0) || ((ls1 & 6)==0)|| ((ls2 & 6)==0) || + ((sq0 & 6)==0) ||((sq1 & 6)==0)|| ((sq2 & 6)==0)); + e0 = (ls0 ^ ((ls0 >> 1) | (ls0 << 2 & 4))); + bl=bh=0; + /* Test t0t1,t1t2,t2t0 against a */ + if(test_t0t1 && (e0 & 2) ) + { + db = t0[1] + dt10[1]*((double)(a-t0[0])/dt10[0]); + TEST_INT(testl,testh,db,q1[1],b,bl,bh) + } + test_t1t2= !(((ls0 & 3)==0) || ((ls1 & 3)==0)|| ((ls2 & 3)==0) || + ((sq0 & 3)==0) ||((sq1 & 3)==0)|| ((sq2 & 3)==0)); + if(test_t1t2 && (e0 & 1)) + { /* test t1t2 against a */ + db = t1[1] + dt21[1]*((double)(a - t1[0]))/dt21[0]; + TEST_INT(testl,testh,db,q1[1],b,bl,bh) + } + test_t2t0 = !(((ls0 & 5)==0) || ((ls1 & 5)==0)|| ((ls2 & 5)==0) || + ((sq0 & 5)==0) ||((sq1 & 5)==0)|| ((sq2 & 5)==0)); + if(test_t2t0 && (e0 & 4)) + { + db = t2[1] + dt02[1]*((double)(a - t2[0]))/dt02[0]; + TEST_INT(testl,testh,db,q1[1],b,bl,bh) + } + cl = ch = 0; + e1 = (ls1 ^ ((ls1 >> 1) | (ls1 << 2 & 4))); + if(test_t0t1 && (e1 & 2)) + {/* test t0t1 against b */ + db = t0[2] + dt10[2]*(((double)(b-t0[1]))/dt10[1]); + TEST_INT(testl,testh,db,q1[2],c,cl,ch) + } + if(test_t1t2 && (e1 & 1)) + {/* test t1t2 against b */ + db = t1[2] + dt21[2]*((double)(b - t1[1]))/dt21[1]; + TEST_INT(testl,testh,db,q1[2],c,cl,ch) + } + if(test_t2t0 && (e1 & 4)) + {/* test t2t0 against b */ + db = t2[2] + dt02[2]*((double)(b - t2[1]))/dt02[1]; + TEST_INT(testl,testh,db,q1[2],c,cl,ch) + } + al = ah = 0; + e2 = (ls2 ^ ((ls2 >> 1) | (ls2 << 2 & 4))); + if(test_t0t1 && (e2 & 2)) + { /* test t0t1 against c */ + db = t0[0] + dt10[0]*(((double)(c-t0[2]))/dt10[2]); + TEST_INT(testl,testh,db,q0[0],a,al,ah) + } + if(test_t1t2 && (e2 & 1)) + { + db = t1[0] + dt21[0]*((double)(c - t1[2]))/dt21[2]; + TEST_INT(testl,testh,db,q0[0],a,al,ah) + } + if(test_t2t0 && (e2 & 4)) + { /* test t2t0 against c */ + db = t2[0] + dt02[0]*((double)(c - t2[2]))/dt02[2]; + TEST_INT(testl,testh,db,q0[0],a,al,ah) + } + /* Only remaining case is if some edge trivially rejected */ + if(!e0 || !e1 || !e2) + return(qt); -/* for triangle v1-v2-v3- returns a,b,c: children are: + /* Only one/none got tested - pick either of other two/any two */ + /* Only need to test one edge */ + if(!test_t0t1 && (e0 & 2)) + { + db = t0[1] + dt10[1]*((double)(a-t0[0]))/dt10[0]; + TEST_INT(testl,testh,db,q1[1],b,bl,bh) + } + if(!test_t1t2 && (e0 & 1)) + {/* test t1t2 against a */ + db = t1[1] + dt21[1]*((double)(a - t1[0]))/dt21[0]; + TEST_INT(testl,testh,db,q1[1],b,bl,bh) + } + if(!test_t2t0 && (e0 & 4)) + { + db = t2[1] + dt02[1]*((double)(a - t2[0]))/dt02[0]; + TEST_INT(testl,testh,db,q1[1],b,bl,bh) + } - v3 0: v1,a,c - /\ 1: a,b,c - /3 \ 2: a,v2,b - c/____\b 3: c,b,v3 - /\ /\ - /0 \1 /2 \ - v1/____\/____\v2 - a - */ + return(qt); +Lfunc_call: + qt = f.func(f.argptr,root,qt,q0,q1,q2,t0,t1,t2,dt10,dt21,dt02,scale, + s0,s1,s2,sq0,sq1,sq2,1,f,n); + return(qt); + } -qtSubdivide_tri(v1,v2,v3,a,b,c) -FVECT v1,v2,v3; -FVECT a,b,c; + + +/* ASSUMPTION: that triangle t0,t1,t2 intersects quad cell q0,q1,q2 */ + +/*-------q2--------- sq2 + / \ +s1 /sc \ s0 + qb_____qa + / \ / \ +\sq0/sa \ /sb \ /sq1 + \ q0____qc____q1/ + \ / + \ s2 / +*/ + +int Find_id=0; + +QUADTREE +qtInsert_tri(root,qt,q0,q1,q2,t0,t1,t2,dt10,dt21,dt02,scale, + s0,s1,s2,sq0,sq1,sq2,f,n) +int root; +QUADTREE qt; +BCOORD q0[3],q1[3],q2[3]; +BCOORD t0[3],t1[3],t2[3]; +BCOORD dt10[3],dt21[3],dt02[3]; +BCOORD scale; +unsigned int s0,s1,s2,sq0,sq1,sq2; +FUNC f; +int n; { - EDGE_MIDPOINT_VEC3(a,v1,v2); - normalize(a); - EDGE_MIDPOINT_VEC3(b,v2,v3); - normalize(b); - EDGE_MIDPOINT_VEC3(c,v3,v1); - normalize(c); + BCOORD a,b,c; + BCOORD va[3],vb[3],vc[3]; + unsigned int sa,sb,sc; + + /* If a tree: trivial reject and recurse on potential children */ + if(QT_IS_TREE(qt)) + { + /* Test against new a edge: if entirely to left: only need + to visit child 0 + */ + a = q1[0] + scale; + b = q0[1] + scale; + c = q0[2] + scale; + + SIDES_GTR(t0,t1,t2,sa,sb,sc,a,b,c); + + if(sa==7) + { + vb[1] = q0[1]; + vc[2] = q0[2]; + vc[1] = b; + vb[2] = c; + vb[0] = vc[0] = a; + QT_NTH_CHILD(qt,0) = qtInsert_tri(root,QT_NTH_CHILD(qt,0),q0,vc, + vb,t0,t1,t2,dt10,dt21,dt02, scale>>1,sa,s1,s2,sq0,sb,sc,f,n+1); + return(qt); + } + if(sb==7) + { + va[0] = q1[0]; + vc[2] = q0[2]; + va[1] = vc[1] = b; + va[2] = c; + vc[0] = a; + QT_NTH_CHILD(qt,1) = qtInsert_tri(root,QT_NTH_CHILD(qt,1),vc,q1,va, + t0,t1,t2,dt10,dt21,dt02,scale>>1,s0,sb,s2,sa,sq1,sc,f,n+1); + return(qt); + } + if(sc==7) + { + va[0] = q1[0]; + vb[1] = q0[1]; + va[1] = b; + va[2] = vb[2] = c; + vb[0] = a; + QT_NTH_CHILD(qt,2) = qtInsert_tri(root,QT_NTH_CHILD(qt,2),vb,va, + q2,t0,t1,t2,dt10,dt21,dt02,scale>>1,s0,s1,sc,sa,sb,sq2,f,n+1); + return(qt); + } + + va[0] = q1[0]; + vb[1] = q0[1]; + vc[2] = q0[2]; + va[1] = vc[1] = b; + va[2] = vb[2] = c; + vb[0] = vc[0] = a; + /* If outside of all edges: only need to Visit 3 */ + if(sa==0 && sb==0 && sc==0) + { + QT_NTH_CHILD(qt,3) = qtInsert_tri_rev(root,QT_NTH_CHILD(qt,3),va,vb, + vc,t0,t1,t2,dt10,dt21,dt02, scale>>1,sa,sb,sc,s0,s1,s2,f,n+1); + return(qt); + } + + if(sa) + QT_NTH_CHILD(qt,0) = qtInsert_tri(root,QT_NTH_CHILD(qt,0),q0,vc,vb,t0, + t1,t2,dt10,dt21,dt02,scale>>1,sa,s1,s2,sq0,sb,sc,f,n+1); + if(sb) + QT_NTH_CHILD(qt,1) = qtInsert_tri(root,QT_NTH_CHILD(qt,1),vc,q1,va,t0, + t1,t2,dt10,dt21,dt02,scale>>1,s0,sb,s2,sa,sq1,sc,f,n+1); + if(sc) + QT_NTH_CHILD(qt,2) = qtInsert_tri(root,QT_NTH_CHILD(qt,2),vb,va,q2,t0, + t1,t2,dt10,dt21,dt02,scale>>1,s0,s1,sc,sa,sb,sq2,f,n+1); + QT_NTH_CHILD(qt,3) = qtInsert_tri_rev(root,QT_NTH_CHILD(qt,3),va,vb,vc,t0, + t1,t2,dt10,dt21,dt02,scale>>1,sa,sb,sc,s0,s1,s2,f,n+1); + return(qt); + } + /* Leaf node: Do definitive test */ + else + return(qt = qtLeaf_insert_tri(root,qt,q0,q1,q2,t0,t1,t2,dt10,dt21,dt02, + scale,s0,s1,s2,sq0,sq1,sq2,f,n)); } -qtNth_child_tri(v1,v2,v3,a,b,c,i,r1,r2,r3) -FVECT v1,v2,v3; -FVECT a,b,c; -int i; -FVECT r1,r2,r3; + +QUADTREE +qtInsert_tri_rev(root,qt,q0,q1,q2,t0,t1,t2,dt10,dt21,dt02,scale, + s0,s1,s2,sq0,sq1,sq2,f,n) +int root; +QUADTREE qt; +BCOORD q0[3],q1[3],q2[3]; +BCOORD t0[3],t1[3],t2[3]; +BCOORD dt10[3],dt21[3],dt02[3]; +BCOORD scale; +unsigned int s0,s1,s2,sq0,sq1,sq2; +FUNC f; +int n; { - VCOPY(r1,a); VCOPY(r2,b); VCOPY(r3,c); - switch(i){ - case 0: - VCOPY(r2,r1); - VCOPY(r1,v1); - break; - case 1: - break; - case 2: - VCOPY(r3,r2); - VCOPY(r2,v2); - break; - case 3: - VCOPY(r1,r3); - VCOPY(r3,v3); - break; + BCOORD a,b,c; + BCOORD va[3],vb[3],vc[3]; + unsigned int sa,sb,sc; + + /* If a tree: trivial reject and recurse on potential children */ + if(QT_IS_TREE(qt)) + { + /* Test against new a edge: if entirely to left: only need + to visit child 0 + */ + a = q1[0] - scale; + b = q0[1] - scale; + c = q0[2] - scale; + + SIDES_GTR(t0,t1,t2,sa,sb,sc,a,b,c); + + if(sa==0) + { + vb[1] = q0[1]; + vc[2] = q0[2]; + vc[1] = b; + vb[2] = c; + vb[0] = vc[0] = a; + + QT_NTH_CHILD(qt,0) = qtInsert_tri_rev(root,QT_NTH_CHILD(qt,0),q0,vc, + vb,t0,t1,t2,dt10,dt21,dt02, scale>>1,sa,s1,s2,sq0,sb,sc,f,n+1); + return(qt); + } + if(sb==0) + { + va[0] = q1[0]; + vc[2] = q0[2]; + va[1] = vc[1] = b; + va[2] = c; + vc[0] = a; + QT_NTH_CHILD(qt,1) = qtInsert_tri_rev(root,QT_NTH_CHILD(qt,1),vc,q1,va, + t0,t1,t2,dt10,dt21,dt02,scale>>1,s0,sb,s2,sa,sq1,sc,f,n+1); + return(qt); + } + if(sc==0) + { + va[0] = q1[0]; + vb[1] = q0[1]; + va[1] = b; + va[2] = vb[2] = c; + vb[0] = a; + QT_NTH_CHILD(qt,2) = qtInsert_tri_rev(root,QT_NTH_CHILD(qt,2),vb,va, + q2,t0,t1,t2,dt10,dt21,dt02,scale>>1,s0,s1,sc,sa,sb,sq2,f,n+1); + return(qt); + } + va[0] = q1[0]; + vb[1] = q0[1]; + vc[2] = q0[2]; + va[1] = vc[1] = b; + va[2] = vb[2] = c; + vb[0] = vc[0] = a; + /* If outside of all edges: only need to Visit 3 */ + if(sa==7 && sb==7 && sc==7) + { + QT_NTH_CHILD(qt,3) = qtInsert_tri(root,QT_NTH_CHILD(qt,3),va,vb, + vc,t0,t1,t2,dt10,dt21,dt02, scale>>1,sa,sb,sc,s0,s1,s2,f,n+1); + return(qt); + } + if(sa!=7) + QT_NTH_CHILD(qt,0) = qtInsert_tri_rev(root,QT_NTH_CHILD(qt,0),q0,vc,vb, + t0,t1,t2,dt10,dt21,dt02,scale>>1,sa,s1,s2,sq0,sb,sc,f,n+1); + if(sb!=7) + QT_NTH_CHILD(qt,1) = qtInsert_tri_rev(root,QT_NTH_CHILD(qt,1),vc,q1,va, + t0,t1,t2,dt10,dt21,dt02,scale>>1,s0,sb,s2,sa,sq1,sc,f,n+1); + if(sc!=7) + QT_NTH_CHILD(qt,2) = qtInsert_tri_rev(root,QT_NTH_CHILD(qt,2),vb,va,q2, + t0,t1,t2,dt10,dt21,dt02,scale>>1,s0,s1,sc,sa,sb,sq2,f,n+1); + + QT_NTH_CHILD(qt,3) = qtInsert_tri(root,QT_NTH_CHILD(qt,3),va,vb,vc,t0,t1, + t2,dt10,dt21,dt02,scale>>1,sa,sb,sc,s0,s1,s2,f,n+1); + return(qt); } + /* Leaf node: Do definitive test */ + else + return(qt = qtLeaf_insert_tri_rev(root,qt,q0,q1,q2,t0,t1,t2,dt10,dt21,dt02, + scale,s0,s1,s2,sq0,sq1,sq2,f,n)); } -/* Add triangle "id" to all leaf level cells that are children of -quadtree pointed to by "qtptr" with cell vertices "t1,t2,t3" -that it overlaps (vertex and edge adjacencies do not count -as overlapping). If the addition of the triangle causes the cell to go over -threshold- the cell is split- and the triangle must be recursively inserted -into the new child cells: it is assumed that "v1,v2,v3" are normalized + + + +/*************************************************************************/ +/* Visit code for applying functions: NOTE THIS IS THE SAME AS INSERT CODE: + except sets flags: wanted insert to be as efficient as possible: so + broke into two sets of routines. Also traverses only to n levels. */ -int -qtAdd_tri(qtptr,id,t1,t2,t3,v1,v2,v3,n) -QUADTREE *qtptr; -int id; -FVECT t1,t2,t3; -FVECT v1,v2,v3; +qtVisit_tri(root,qt,q0,q1,q2,t0,t1,t2,dt10,dt21,dt02,scale, + s0,s1,s2,sq0,sq1,sq2,f,n) +int root; +QUADTREE qt; +BCOORD q0[3],q1[3],q2[3]; +BCOORD t0[3],t1[3],t2[3]; +BCOORD dt10[3],dt21[3],dt02[3]; +BCOORD scale; +unsigned int s0,s1,s2,sq0,sq1,sq2; +FUNC f; int n; { - - char test; - int i,index; - FVECT a,b,c; - OBJECT os[MAXSET+1],*optr; - QUADTREE qt; - int found; - FVECT r1,r2,r3; + BCOORD a,b,c; + BCOORD va[3],vb[3],vc[3]; + unsigned int sa,sb,sc; - /* test if triangle (t1,t2,t3) overlaps cell triangle (v1,v2,v3) */ - test = spherical_tri_intersect(t1,t2,t3,v1,v2,v3); + if(n == 0) + return; + /* If a tree: trivial reject and recurse on potential children */ + if(QT_IS_TREE(qt)) + { + QT_SET_FLAG(qt); - /* If triangles do not overlap: done */ - if(!test) - return(FALSE); - found = 0; + /* Test against new a edge: if entirely to left: only need + to visit child 0 + */ + a = q1[0] + scale; + b = q0[1] + scale; + c = q0[2] + scale; - /* if this is tree: recurse */ - if(QT_IS_TREE(*qtptr)) - { - n++; - qtSubdivide_tri(v1,v2,v3,a,b,c); - found |= qtAdd_tri(QT_NTH_CHILD_PTR(*qtptr,0),id,t1,t2,t3,v1,a,c,n); - found |= qtAdd_tri(QT_NTH_CHILD_PTR(*qtptr,1),id,t1,t2,t3,a,b,c,n); - found |= qtAdd_tri(QT_NTH_CHILD_PTR(*qtptr,2),id,t1,t2,t3,a,v2,b,n); - found |= qtAdd_tri(QT_NTH_CHILD_PTR(*qtptr,3),id,t1,t2,t3,c,b,v3,n); + SIDES_GTR(t0,t1,t2,sa,sb,sc,a,b,c); -#if 0 - if(!found) + if(sa==7) { -#ifdef TEST_DRIVER - HANDLE_ERROR("qtAdd_tri():Found in parent but not children\n"); -#else - eputs("qtAdd_tri():Found in parent but not children\n"); -#endif + vb[1] = q0[1]; + vc[2] = q0[2]; + vc[1] = b; + vb[2] = c; + vb[0] = vc[0] = a; + qtVisit_tri(root,QT_NTH_CHILD(qt,0),q0,vc, + vb,t0,t1,t2,dt10,dt21,dt02, scale>>1,sa,s1,s2,sq0,sb,sc,f,n-1); + return; } -#endif + if(sb==7) + { + va[0] = q1[0]; + vc[2] = q0[2]; + va[1] = vc[1] = b; + va[2] = c; + vc[0] = a; + qtVisit_tri(root,QT_NTH_CHILD(qt,1),vc,q1,va, + t0,t1,t2,dt10,dt21,dt02,scale>>1,s0,sb,s2,sa,sq1,sc,f,n-1); + return; + } + if(sc==7) + { + va[0] = q1[0]; + vb[1] = q0[1]; + va[1] = b; + va[2] = vb[2] = c; + vb[0] = a; + qtVisit_tri(root,QT_NTH_CHILD(qt,2),vb,va, + q2,t0,t1,t2,dt10,dt21,dt02,scale>>1,s0,s1,sc,sa,sb,sq2,f,n-1); + return; + } + + va[0] = q1[0]; + vb[1] = q0[1]; + vc[2] = q0[2]; + va[1] = vc[1] = b; + va[2] = vb[2] = c; + vb[0] = vc[0] = a; + /* If outside of all edges: only need to Visit 3 */ + if(sa==0 && sb==0 && sc==0) + { + qtVisit_tri_rev(root,QT_NTH_CHILD(qt,3),va,vb, + vc,t0,t1,t2,dt10,dt21,dt02, scale>>1,sa,sb,sc,s0,s1,s2,f,n-1); + return; + } + + if(sa) + qtVisit_tri(root,QT_NTH_CHILD(qt,0),q0,vc,vb,t0, + t1,t2,dt10,dt21,dt02,scale>>1,sa,s1,s2,sq0,sb,sc,f,n-1); + if(sb) + qtVisit_tri(root,QT_NTH_CHILD(qt,1),vc,q1,va,t0, + t1,t2,dt10,dt21,dt02,scale>>1,s0,sb,s2,sa,sq1,sc,f,n-1); + if(sc) + qtVisit_tri(root,QT_NTH_CHILD(qt,2),vb,va,q2,t0, + t1,t2,dt10,dt21,dt02,scale>>1,s0,s1,sc,sa,sb,sq2,f,n-1); + qtVisit_tri_rev(root,QT_NTH_CHILD(qt,3),va,vb,vc,t0, + t1,t2,dt10,dt21,dt02,scale>>1,sa,sb,sc,s0,s1,s2,f,n-1); } + /* Leaf node: Do definitive test */ else + qtLeaf_visit_tri(root,qt,q0,q1,q2,t0,t1,t2,dt10,dt21,dt02, + scale,s0,s1,s2,sq0,sq1,sq2,f,n); + +} + + +qtVisit_tri_rev(root,qt,q0,q1,q2,t0,t1,t2,dt10,dt21,dt02,scale, + s0,s1,s2,sq0,sq1,sq2,f,n) +int root; +QUADTREE qt; +BCOORD q0[3],q1[3],q2[3]; +BCOORD t0[3],t1[3],t2[3]; +BCOORD dt10[3],dt21[3],dt02[3]; +BCOORD scale; +unsigned int s0,s1,s2,sq0,sq1,sq2; +FUNC f; +int n; +{ + BCOORD a,b,c; + BCOORD va[3],vb[3],vc[3]; + unsigned int sa,sb,sc; + + if(n==0) + return; + /* If a tree: trivial reject and recurse on potential children */ + if(QT_IS_TREE(qt)) { - /* If this leave node emptry- create a new set */ - if(QT_IS_EMPTY(*qtptr)) - { - *qtptr = qtaddelem(*qtptr,id); -#if 0 - { - int k; - qtgetset(os,*qtptr); - printf("\n%d:\n",os[0]); - for(k=1; k <= os[0];k++) - printf("%d ",os[k]); - printf("\n"); - } -#endif - /* - os[0] = 0; - insertelem(os,id); - qt = fullnode(os); - *qtptr = qt; - */ - } - else - { - qtgetset(os,*qtptr); - /* If the set is too large: subdivide */ - if(QT_SET_CNT(os) < QT_SET_THRESHOLD) - { - *qtptr = qtaddelem(*qtptr,id); -#if 0 - { - int k; - qtgetset(os,*qtptr); - printf("\n%d:\n",os[0]); - for(k=1; k <= os[0];k++) - printf("%d ",os[k]); - printf("\n"); - } -#endif - /* - insertelem(os,id); - *qtptr = fullnode(os); - */ - } - else - { - if (n < QT_MAX_LEVELS) - { - /* If set size exceeds threshold: subdivide cell and - reinsert set tris into cell - */ - n++; - qtfreeleaf(*qtptr); - qtSubdivide(qtptr); - found = qtAdd_tri(qtptr,id,t1,t2,t3,v1,v2,v3,n); -#if 0 - if(!found) - { -#ifdef TEST_DRIVER - HANDLE_ERROR("qtAdd_tri():Found in parent but not children\n"); -#else - eputs("qtAdd_tri():Found in parent but not children\n"); -#endif - } -#endif - for(optr = &(os[1]),i = QT_SET_CNT(os); i > 0; i--) - { - id = QT_SET_NEXT_ELEM(optr); - qtTri_verts_from_id(id,r1,r2,r3); - found=qtAdd_tri(qtptr,id,r1,r2,r3,v1,v2,v3,n); -#ifdef DEBUG - if(!found) - eputs("qtAdd_tri():Reinsert-in parent but not children\n"); -#endif - } - } - else - if(QT_SET_CNT(os) < QT_MAX_SET) - { - *qtptr = qtaddelem(*qtptr,id); -#if 0 - { - int k; - qtgetset(os,*qtptr); - printf("\n%d:\n",os[0]); - for(k=1; k <= os[0];k++) - printf("%d ",os[k]); - printf("\n"); - } -#endif - /* - insertelem(os,id); - *qtptr = fullnode(os); - */ - } - else - { -#ifdef DEBUG - eputs("qtAdd_tri():two many levels\n"); -#endif - return(FALSE); - } - } - } + QT_SET_FLAG(qt); + /* Test against new a edge: if entirely to left: only need + to visit child 0 + */ + a = q1[0] - scale; + b = q0[1] - scale; + c = q0[2] - scale; + + SIDES_GTR(t0,t1,t2,sa,sb,sc,a,b,c); + + if(sa==0) + { + vb[1] = q0[1]; + vc[2] = q0[2]; + vc[1] = b; + vb[2] = c; + vb[0] = vc[0] = a; + qtVisit_tri_rev(root,QT_NTH_CHILD(qt,0),q0,vc, + vb,t0,t1,t2,dt10,dt21,dt02, scale>>1,sa,s1,s2,sq0,sb,sc,f,n-1); + return; + } + if(sb==0) + { + va[0] = q1[0]; + vc[2] = q0[2]; + va[1] = vc[1] = b; + va[2] = c; + vc[0] = a; + qtVisit_tri_rev(root,QT_NTH_CHILD(qt,1),vc,q1,va, + t0,t1,t2,dt10,dt21,dt02,scale>>1,s0,sb,s2,sa,sq1,sc,f,n-1); + return; + } + if(sc==0) + { + va[0] = q1[0]; + vb[1] = q0[1]; + va[1] = b; + va[2] = vb[2] = c; + vb[0] = a; + qtVisit_tri_rev(root,QT_NTH_CHILD(qt,2),vb,va, + q2,t0,t1,t2,dt10,dt21,dt02,scale>>1,s0,s1,sc,sa,sb,sq2,f,n-1); + return; + } + va[0] = q1[0]; + vb[1] = q0[1]; + vc[2] = q0[2]; + va[1] = vc[1] = b; + va[2] = vb[2] = c; + vb[0] = vc[0] = a; + /* If outside of all edges: only need to Visit 3 */ + if(sa==7 && sb==7 && sc==7) + { + qtVisit_tri(root,QT_NTH_CHILD(qt,3),va,vb, + vc,t0,t1,t2,dt10,dt21,dt02, scale>>1,sa,sb,sc,s0,s1,s2,f,n-1); + return; + } + if(sa!=7) + qtVisit_tri_rev(root,QT_NTH_CHILD(qt,0),q0,vc,vb, + t0,t1,t2,dt10,dt21,dt02,scale>>1,sa,s1,s2,sq0,sb,sc,f,n-1); + if(sb!=7) + qtVisit_tri_rev(root,QT_NTH_CHILD(qt,1),vc,q1,va, + t0,t1,t2,dt10,dt21,dt02,scale>>1,s0,sb,s2,sa,sq1,sc,f,n-1); + if(sc!=7) + qtVisit_tri_rev(root,QT_NTH_CHILD(qt,2),vb,va,q2, + t0,t1,t2,dt10,dt21,dt02,scale>>1,s0,s1,sc,sa,sb,sq2,f,n-1); + + qtVisit_tri(root,QT_NTH_CHILD(qt,3),va,vb,vc,t0,t1, + t2,dt10,dt21,dt02,scale>>1,sa,sb,sc,s0,s1,s2,f,n-1); + return; } - return(TRUE); + /* Leaf node: Do definitive test */ + else + qtLeaf_visit_tri_rev(root,qt,q0,q1,q2,t0,t1,t2,dt10,dt21,dt02, + scale,s0,s1,s2,sq0,sq1,sq2,f,n); } -int -qtApply_to_tri_cells(qtptr,t1,t2,t3,v1,v2,v3,func,arg) -QUADTREE *qtptr; -FVECT t1,t2,t3; -FVECT v1,v2,v3; -int (*func)(); -char *arg; -{ - char test; - FVECT a,b,c; - /* test if triangle (t1,t2,t3) overlaps cell triangle (v1,v2,v3) */ - test = spherical_tri_intersect(t1,t2,t3,v1,v2,v3); +qtLeaf_visit_tri(root,qt,q0,q1,q2,t0,t1,t2,dt10,dt21,dt02, + scale, s0,s1,s2,sq0,sq1,sq2,f,n) +int root; +QUADTREE qt; +BCOORD q0[3],q1[3],q2[3]; +BCOORD t0[3],t1[3],t2[3]; +BCOORD dt10[3],dt21[3],dt02[3]; +unsigned int scale,s0,s1,s2,sq0,sq1,sq2; +FUNC f; +int n; +{ + double db; + unsigned int e0,e1,e2; + char al,ah,bl,bh,cl,ch,testl,testh; + char test_t0t1,test_t1t2,test_t2t0; + BCOORD a,b,c; + + /* First check if can trivial accept: if vertex in cell */ + if(s0 & s1 & s2) + goto Lfunc_call; - /* If triangles do not overlap: done */ - if(!test) - return(FALSE); + /* Assumption: Could not trivial reject*/ + /* IF any coords lie on edge- must be in if couldnt reject */ + a = q1[0];b= q0[1]; c= q0[2]; + if(t0[0]== a || t0[1] == b || t0[2] == c) + goto Lfunc_call; + if(t1[0]== a || t1[1] == b || t1[2] == c) + goto Lfunc_call; + if(t2[0]== a || t2[1] == b || t2[2] == c) + goto Lfunc_call; + + /* Test for edge crossings */ + /* Test t0t1,t1t2,t2t0 against a */ + /* Calculate edge crossings */ + e0 = (s0 ^ ((s0 >> 1) | (s0 << 2 & 4))); + /* See if edge can be trivially rejected from intersetion testing */ + test_t0t1 = !(((s0 & 6)==0) || ((s1 & 6)==0)|| ((s2 & 6)==0) || + ((sq0 & 6)==6) ||((sq1 & 6)==6)|| ((sq2 & 6)==6)); + bl=bh=0; + if(test_t0t1 && (e0 & 2) ) + { + /* Must do double calculation to avoid overflow */ + db = t0[1] + dt10[1]*((double)(a-t0[0]))/dt10[0]; + TEST_INT(testl,testh,db,b,q1[1],bl,bh) + } + test_t1t2= !(((s0 & 3)==0) || ((s1 & 3)==0)|| ((s2 & 3)==0) || + ((sq0 & 3)==3) ||((sq1 & 3)==3)|| ((sq2 & 3)==3)); + if(test_t1t2 && (e0 & 1)) + { /* test t1t2 against a */ + db = t1[1] + dt21[1]*((double)(a - t1[0]))/dt21[0]; + TEST_INT(testl,testh,db,b,q1[1],bl,bh) + } + test_t2t0 = !(((s0 & 5)==0) || ((s1 & 5)==0)|| ((s2 & 5)==0) || + ((sq0 & 5)==5) ||((sq1 & 5)==5)|| ((sq2 & 5)==5)); + if(test_t2t0 && (e0 & 4)) + { + db = t2[1] + dt02[1]*((double)(a - t2[0]))/dt02[0]; + TEST_INT(testl,testh,db,b,q1[1],bl,bh) + } + e1 = (s1 ^ ((s1 >> 1) | (s1 << 2 & 4))); + cl = ch = 0; + if(test_t0t1 && (e1 & 2)) + {/* test t0t1 against b */ + db = t0[2] + dt10[2]*((double)(b-t0[1]))/dt10[1]; + TEST_INT(testl,testh,db,c,q2[2],cl,ch) + } + if(test_t1t2 && (e1 & 1)) + {/* test t1t2 against b */ + db = t1[2] + dt21[2]*((double)(q0[1] - t1[1]))/dt21[1]; + TEST_INT(testl,testh,db,c,q2[2],cl,ch) + } + if(test_t2t0 && (e1 & 4)) + {/* test t2t0 against b */ + db = t2[2] + dt02[2]*((double)(q0[1] - t2[1]))/dt02[1]; + TEST_INT(testl,testh,db,c,q2[2],cl,ch) + } + + /* test edges against c */ + e2 = (s2 ^ ((s2 >> 1) | (s2 << 2 & 4))); + al = ah = 0; + if(test_t0t1 && (e2 & 2)) + { /* test t0t1 against c */ + db = t0[0] + dt10[0]*((double)(c-t0[2]))/dt10[2]; + TEST_INT(testl,testh,db,a,q0[0],al,ah) + } + if(test_t1t2 && (e2 & 1)) + { + db = t1[0] + dt21[0]*((double)(c - t1[2]))/dt21[2]; + TEST_INT(testl,testh,db,a,q0[0],al,ah) + } + if(test_t2t0 && (e2 & 4)) + { /* test t2t0 against c */ + db = t2[0] + dt02[0]*((double)(c - t2[2]))/dt02[2]; + TEST_INT(testl,testh,db,a,q0[0],al,ah) + } + /* Only remaining case is if some edge trivially rejected */ + if(!e0 || !e1 || !e2) + return; - /* if this is tree: recurse */ - if(QT_IS_TREE(*qtptr)) + /* Only one/none got tested - pick either of other two/any two */ + /* Only need to test one edge */ + if(!test_t0t1 && (e0 & 2)) { - qtSubdivide_tri(v1,v2,v3,a,b,c); - qtApply_to_tri_cells(QT_NTH_CHILD_PTR(*qtptr,0),t1,t2,t3,v1,a,c,func,arg); - qtApply_to_tri_cells(QT_NTH_CHILD_PTR(*qtptr,1),t1,t2,t3,a,b,c,func,arg); - qtApply_to_tri_cells(QT_NTH_CHILD_PTR(*qtptr,2),t1,t2,t3,a,v2,b,func,arg); - qtApply_to_tri_cells(QT_NTH_CHILD_PTR(*qtptr,3),t1,t2,t3,c,b,v3,func,arg); + db = t0[1] + dt10[1]*((double)(a-t0[0]))/dt10[0]; + TEST_INT(testl,testh,db,b,q1[1],bl,bh) } - else - return(func(qtptr,arg)); + if(!test_t1t2 && (e0 & 1)) + {/* test t1t2 against a */ + db = t1[1] + dt21[1]*((double)(a - t1[0]))/dt21[0]; + TEST_INT(testl,testh,db,b,q1[1],bl,bh) + } + if(!test_t2t0 && (e0 & 4)) + { + db = t2[1] + dt02[1]*((double)(a - t2[0]))/dt02[0]; + TEST_INT(testl,testh,db,b,q1[1],bl,bh) + } + + return; + +Lfunc_call: + + f.func(f.argptr,root,qt,n); + if(!QT_IS_EMPTY(qt)) + QT_LEAF_SET_FLAG(qt); + } -int -qtRemove_tri(qtptr,id,t1,t2,t3,v1,v2,v3) -QUADTREE *qtptr; -int id; -FVECT t1,t2,t3; -FVECT v1,v2,v3; -{ + +/* Leaf node: Do definitive test */ +QUADTREE +qtLeaf_visit_tri_rev(root,qt,q0,q1,q2,t0,t1,t2,dt10,dt21,dt02, + scale, s0,s1,s2,sq0,sq1,sq2,f,n) +int root; +QUADTREE qt; +BCOORD q0[3],q1[3],q2[3]; +BCOORD t0[3],t1[3],t2[3]; +BCOORD dt10[3],dt21[3],dt02[3]; +unsigned int scale,s0,s1,s2,sq0,sq1,sq2; +FUNC f; +int n; +{ + double db; + unsigned int e0,e1,e2; + BCOORD a,b,c; + double p0[2], p1[2],cp; + char al,ah,bl,bh,cl,ch; + char testl,testh,test_t0t1,test_t1t2,test_t2t0; + unsigned int ls0,ls1,ls2; - char test; - int i; - FVECT a,b,c; - OBJECT os[MAXSET+1]; + /* May have gotten passed trivial reject if one/two vertices are ON */ + a = q1[0];b= q0[1]; c= q0[2]; + SIDES_LESS(t0,t1,t2,ls0,ls1,ls2,a,b,c); - /* test if triangle (t1,t2,t3) overlaps cell triangle (v1,v2,v3) */ - test = spherical_tri_intersect(t1,t2,t3,v1,v2,v3); + /* First check if can trivial accept: if vertex in cell */ + if(ls0 & ls1 & ls2) + goto Lfunc_call; - /* If triangles do not overlap: done */ - if(!test) - return(FALSE); + if(ls0==0 || ls1 == 0 || ls2 ==0) + return; + /* Assumption: Could not trivial reject*/ + /* IF any coords lie on edge- must be in if couldnt reject */ - /* if this is tree: recurse */ - if(QT_IS_TREE(*qtptr)) + if(t0[0]== a || t0[1] == b || t0[2] == c) + goto Lfunc_call; + if(t1[0]== a || t1[1] == b || t1[2] == c) + goto Lfunc_call; + if(t2[0]== a || t2[1] == b || t2[2] == c) + goto Lfunc_call; + + /* Test for edge crossings */ + /* Test t0t1 against a,b,c */ + /* First test if t0t1 can be trivially rejected */ + /* If both edges are outside bounds- dont need to test */ + + /* Test t0t1,t1t2,t2t0 against a */ + test_t0t1 = !(((ls0 & 6)==0) || ((ls1 & 6)==0)|| ((ls2 & 6)==0) || + ((sq0 & 6)==0) ||((sq1 & 6)==0)|| ((sq2 & 6)==0)); + e0 = (ls0 ^ ((ls0 >> 1) | (ls0 << 2 & 4))); + bl=bh=0; + /* Test t0t1,t1t2,t2t0 against a */ + if(test_t0t1 && (e0 & 2) ) { - qtSubdivide_tri(v1,v2,v3,a,b,c); - qtRemove_tri(QT_NTH_CHILD_PTR(*qtptr,0),id,t1,t2,t3,v1,a,c); - qtRemove_tri(QT_NTH_CHILD_PTR(*qtptr,1),id,t1,t2,t3,a,b,c); - qtRemove_tri(QT_NTH_CHILD_PTR(*qtptr,2),id,t1,t2,t3,a,v2,b); - qtRemove_tri(QT_NTH_CHILD_PTR(*qtptr,3),id,t1,t2,t3,c,b,v3); + db = t0[1] + dt10[1]*((double)(a-t0[0])/dt10[0]); + TEST_INT(testl,testh,db,q1[1],b,bl,bh) } - else + test_t1t2= !(((ls0 & 3)==0) || ((ls1 & 3)==0)|| ((ls2 & 3)==0) || + ((sq0 & 3)==0) ||((sq1 & 3)==0)|| ((sq2 & 3)==0)); + if(test_t1t2 && (e0 & 1)) + { /* test t1t2 against a */ + db = t1[1] + dt21[1]*((double)(a - t1[0]))/dt21[0]; + TEST_INT(testl,testh,db,q1[1],b,bl,bh) + } + test_t2t0 = !(((ls0 & 5)==0) || ((ls1 & 5)==0)|| ((ls2 & 5)==0) || + ((sq0 & 5)==0) ||((sq1 & 5)==0)|| ((sq2 & 5)==0)); + if(test_t2t0 && (e0 & 4)) { - if(QT_IS_EMPTY(*qtptr)) + db = t2[1] + dt02[1]*((double)(a - t2[0]))/dt02[0]; + TEST_INT(testl,testh,db,q1[1],b,bl,bh) + } + cl = ch = 0; + e1 = (ls1 ^ ((ls1 >> 1) | (ls1 << 2 & 4))); + if(test_t0t1 && (e1 & 2)) + {/* test t0t1 against b */ + db = t0[2] + dt10[2]*(((double)(b-t0[1]))/dt10[1]); + TEST_INT(testl,testh,db,q1[2],c,cl,ch) + } + if(test_t1t2 && (e1 & 1)) + {/* test t1t2 against b */ + db = t1[2] + dt21[2]*((double)(b - t1[1]))/dt21[1]; + TEST_INT(testl,testh,db,q1[2],c,cl,ch) + } + if(test_t2t0 && (e1 & 4)) + {/* test t2t0 against b */ + db = t2[2] + dt02[2]*((double)(b - t2[1]))/dt02[1]; + TEST_INT(testl,testh,db,q1[2],c,cl,ch) + } + al = ah = 0; + e2 = (ls2 ^ ((ls2 >> 1) | (ls2 << 2 & 4))); + if(test_t0t1 && (e2 & 2)) + { /* test t0t1 against c */ + db = t0[0] + dt10[0]*(((double)(c-t0[2]))/dt10[2]); + TEST_INT(testl,testh,db,q0[0],a,al,ah) + } + if(test_t1t2 && (e2 & 1)) + { + db = t1[0] + dt21[0]*((double)(c - t1[2]))/dt21[2]; + TEST_INT(testl,testh,db,q0[0],a,al,ah) + } + if(test_t2t0 && (e2 & 4)) + { /* test t2t0 against c */ + db = t2[0] + dt02[0]*((double)(c - t2[2]))/dt02[2]; + TEST_INT(testl,testh,db,q0[0],a,al,ah) + } + /* Only remaining case is if some edge trivially rejected */ + if(!e0 || !e1 || !e2) + return; + + /* Only one/none got tested - pick either of other two/any two */ + /* Only need to test one edge */ + if(!test_t0t1 && (e0 & 2)) + { + db = t0[1] + dt10[1]*((double)(a-t0[0]))/dt10[0]; + TEST_INT(testl,testh,db,q1[1],b,bl,bh) + } + if(!test_t1t2 && (e0 & 1)) + {/* test t1t2 against a */ + db = t1[1] + dt21[1]*((double)(a - t1[0]))/dt21[0]; + TEST_INT(testl,testh,db,q1[1],b,bl,bh) + } + if(!test_t2t0 && (e0 & 4)) + { + db = t2[1] + dt02[1]*((double)(a - t2[0]))/dt02[0]; + TEST_INT(testl,testh,db,q1[1],b,bl,bh) + } + + return; + +Lfunc_call: + f.func(f.argptr,root,qt,n); + if(!QT_IS_EMPTY(qt)) + QT_LEAF_SET_FLAG(qt); +} + + +QUADTREE +qtInsert_point(root,qt,parent,q0,q1,q2,bc,scale,f,n,doneptr) +int root; +QUADTREE qt,parent; +BCOORD q0[3],q1[3],q2[3],bc[3],scale; +FUNC f; +int n,*doneptr; +{ + BCOORD a,b,c; + BCOORD va[3],vb[3],vc[3]; + + if(QT_IS_TREE(qt)) + { + a = q1[0] + scale; + b = q0[1] + scale; + c = q0[2] + scale; + if(bc[0] > a) + { + vc[0] = a; vc[1] = b; vc[2] = q0[2]; + vb[0] = a; vb[1] = q0[1]; vb[2] = c; + QT_NTH_CHILD(qt,0) = qtInsert_point(root,QT_NTH_CHILD(qt,0), + qt,q0,vc,vb,bc,scale>>1,f,n+1,doneptr); + if(!*doneptr) { -#ifdef DEBUG - eputs("qtRemove_tri(): triangle not found\n"); -#endif - } - /* remove id from set */ - else + f.func_after(f.argptr,QT_NTH_CHILD(qt,1),f,doneptr); + if(!*doneptr) + f.func_after(f.argptr,QT_NTH_CHILD(qt,2),f,doneptr); + if(!*doneptr) + f.func_after(f.argptr,QT_NTH_CHILD(qt,3),f,doneptr); + } + return(qt); + } + if(bc[1] > b) + { + vc[0] = a; vc[1] = b; vc[2] = q0[2]; + va[0] = q1[0]; va[1] = b; va[2] = c; + QT_NTH_CHILD(qt,1) =qtInsert_point(root,QT_NTH_CHILD(qt,1), + qt,vc,q1,va,bc,scale >>1,f,n+1,doneptr); + if(!*doneptr) { - qtgetset(os,*qtptr); - if(!inset(os,id)) - { -#ifdef DEBUG - eputs("qtRemove_tri(): tri not in set\n"); -#endif - } - else - { - *qtptr = qtdelelem(*qtptr,id); -#if 0 - { - int k; - if(!QT_IS_EMPTY(*qtptr)) - {qtgetset(os,*qtptr); - printf("\n%d:\n",os[0]); - for(k=1; k <= os[0];k++) - printf("%d ",os[k]); - printf("\n"); - } + f.func_after(f.argptr,QT_NTH_CHILD(qt,0),f,doneptr); + if(!*doneptr) + f.func_after(f.argptr,QT_NTH_CHILD(qt,2),f,doneptr); + if(!*doneptr) + f.func_after(f.argptr,QT_NTH_CHILD(qt,3),f,doneptr); + } + return(qt); + } + if(bc[2] > c) + { + va[0] = q1[0]; va[1] = b; va[2] = c; + vb[0] = a; vb[1] = q0[1]; vb[2] = c; + QT_NTH_CHILD(qt,2) = qtInsert_point(root,QT_NTH_CHILD(qt,2), + qt,vb,va,q2,bc,scale>>1,f,n+1,doneptr); + if(!*doneptr) + { + f.func_after(f.argptr,QT_NTH_CHILD(qt,0),f,doneptr); + if(!*doneptr) + f.func_after(f.argptr,QT_NTH_CHILD(qt,1),f,doneptr); + if(!*doneptr) + f.func_after(f.argptr,QT_NTH_CHILD(qt,3),f,doneptr); + } + return(qt); + } + else + { + va[0] = q1[0]; va[1] = b; va[2] = c; + vb[0] = a; vb[1] = q0[1]; vb[2] = c; + vc[0] = a; vc[1] = b; vc[2] = q0[2]; + QT_NTH_CHILD(qt,3) = qtInsert_point_rev(root,QT_NTH_CHILD(qt,3), + qt,va,vb,vc,bc,scale>>1,f,n+1,doneptr); + if(!*doneptr) + { + f.func_after(f.argptr,QT_NTH_CHILD(qt,0),f,doneptr); + if(!*doneptr) + f.func_after(f.argptr,QT_NTH_CHILD(qt,1),f,doneptr); + if(!*doneptr) + f.func_after(f.argptr,QT_NTH_CHILD(qt,2),f,doneptr); + } + return(qt); + } + } + else + { + qt = f.func(f.argptr,root,qt,parent,q0,q1,q2,bc,scale,0,f,n,doneptr); + return(qt); + } +} - } -#endif - } - } + +QUADTREE +qtInsert_point_rev(root,qt,parent,q0,q1,q2,bc,scale,f,n,doneptr) +int root; +QUADTREE qt,parent; +BCOORD q0[3],q1[3],q2[3],bc[3]; +BCOORD scale; +FUNC f; +int n,*doneptr; +{ + BCOORD a,b,c; + BCOORD va[3],vb[3],vc[3]; + + if(QT_IS_TREE(qt)) + { + a = q1[0] - scale; + b = q0[1] - scale; + c = q0[2] - scale; + if(bc[0] < a) + { + vc[0] = a; vc[1] = b; vc[2] = q0[2]; + vb[0] = a; vb[1] = q0[1]; vb[2] = c; + QT_NTH_CHILD(qt,0) = qtInsert_point_rev(root,QT_NTH_CHILD(qt,0), + qt,q0,vc,vb,bc,scale>>1,f,n+1,doneptr); + if(!*doneptr) + { + f.func_after(f.argptr,QT_NTH_CHILD(qt,1),f,doneptr); + if(!*doneptr) + f.func_after(f.argptr,QT_NTH_CHILD(qt,2),f,doneptr); + if(!*doneptr) + f.func_after(f.argptr,QT_NTH_CHILD(qt,3),f,doneptr); + } + return(qt); + } + if(bc[1] < b) + { + vc[0] = a; vc[1] = b; vc[2] = q0[2]; + va[0] = q1[0]; va[1] = b; va[2] = c; + QT_NTH_CHILD(qt,1) = qtInsert_point_rev(root,QT_NTH_CHILD(qt,1), + qt,vc,q1,va,bc,scale>>1,f,n+1,doneptr); + if(!*doneptr) + { + f.func_after(f.argptr,QT_NTH_CHILD(qt,0),f,doneptr); + if(!*doneptr) + f.func_after(f.argptr,QT_NTH_CHILD(qt,2),f,doneptr); + if(!*doneptr) + f.func_after(f.argptr,QT_NTH_CHILD(qt,3),f,doneptr); + } + return(qt); + } + if(bc[2] < c) + { + va[0] = q1[0]; va[1] = b; va[2] = c; + vb[0] = a; vb[1] = q0[1]; vb[2] = c; + QT_NTH_CHILD(qt,2) = qtInsert_point_rev(root,QT_NTH_CHILD(qt,2), + qt,vb,va,q2,bc,scale>>1,f,n+1,doneptr); + if(!*doneptr) + { + f.func_after(f.argptr,QT_NTH_CHILD(qt,0),f,doneptr); + if(!*doneptr) + f.func_after(f.argptr,QT_NTH_CHILD(qt,1),f,doneptr); + if(!*doneptr) + f.func_after(f.argptr,QT_NTH_CHILD(qt,3),f,doneptr); + } + return(qt); + } + else + { + va[0] = q1[0]; va[1] = b; va[2] = c; + vb[0] = a; vb[1] = q0[1]; vb[2] = c; + vc[0] = a; vc[1] = b; vc[2] = q0[2]; + QT_NTH_CHILD(qt,3) = qtInsert_point(root,QT_NTH_CHILD(qt,3), + qt,va,vb,vc,bc,scale>>1,f,n+1,doneptr); + if(!*doneptr) + { + f.func_after(f.argptr,QT_NTH_CHILD(qt,0),f,doneptr); + if(!*doneptr) + f.func_after(f.argptr,QT_NTH_CHILD(qt,1),f,doneptr); + if(!*doneptr) + f.func_after(f.argptr,QT_NTH_CHILD(qt,2),f,doneptr); + } + return(qt); + } } - return(TRUE); + else + { + qt = f.func(f.argptr,root,qt,parent,q0,q1,q2,bc,scale,1,f,n,doneptr); + return(qt); + } } + + + + + + + + + + +