--- ray/src/hd/sm_qtree.c 1998/09/16 18:16:29 3.6 +++ ray/src/hd/sm_qtree.c 1999/06/10 15:22:23 3.13 @@ -14,7 +14,7 @@ static char SCCSid[] = "$SunId$ SGI"; */ #include "standard.h" - +#include "sm_flag.h" #include "sm_geom.h" #include "sm_qtree.h" @@ -27,8 +27,18 @@ int4 *quad_flag= NULL; extern FVECT Pick_v0[500],Pick_v1[500],Pick_v2[500]; extern int Pick_cnt,Pick_tri,Pick_samp; extern FVECT Pick_point[500]; +extern int Pick_q[500]; + #endif +qtremovelast(qt,id) +QUADTREE qt; +int id; +{ + if(qtqueryset(qt)[(*(qtqueryset(qt)))] != id) + error(CONSISTENCY,"not removed\n"); + ((*(qtqueryset(qt)))--); +} QUADTREE qtAlloc() /* allocate a quadtree */ { @@ -46,12 +56,13 @@ qtAlloc() /* allocate a quadtree */ return(EMPTY); if ((quad_block[QT_BLOCK(freet)] = (QUADTREE *)malloc( QT_BLOCK_SIZE*4*sizeof(QUADTREE))) == NULL) - return(EMPTY); + error(SYSTEM,"qtAlloc(): Unable to allocate memory\n"); + /* Realloc the per/node flags */ quad_flag = (int4 *)realloc((char *)quad_flag, - (QT_BLOCK(freet)+1)*(QT_BLOCK_SIZE/8)); + (QT_BLOCK(freet)+1)*((QT_BLOCK_SIZE+7)>>3)); if (quad_flag == NULL) - return(EMPTY); + error(SYSTEM,"qtAlloc(): Unable to allocate memory\n"); } treetop += 4; return(freet); @@ -60,11 +71,15 @@ qtAlloc() /* allocate a quadtree */ qtClearAllFlags() /* clear all quadtree branch flags */ { - if (!treetop) return; - bzero((char *)quad_flag, (QT_BLOCK(treetop-4)+1)*(QT_BLOCK_SIZE/8)); + 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; { @@ -85,7 +100,7 @@ qtFree(qt) /* free a quadtree */ qtDone() /* free EVERYTHING */ { register int i; - + qtfreeleaves(); for (i = 0; i < QT_MAX_BLK; i++) { @@ -94,6 +109,7 @@ qtDone() /* free EVERYTHING */ free((char *)quad_block[i]); quad_block[i] = NULL; } + /* Free the flags */ if (i) free((char *)quad_flag); quad_flag = NULL; quad_free_list = EMPTY; @@ -101,1157 +117,1295 @@ qtDone() /* free EVERYTHING */ } QUADTREE -*qtLocate_leaf(qtptr,bcoord,t0,t1,t2) -QUADTREE *qtptr; -double bcoord[3]; -FVECT t0,t1,t2; +qtLocate(qt,bcoord) +QUADTREE qt; +BCOORD bcoord[3]; { int i; - QUADTREE *child; - FVECT a,b,c; - if(QT_IS_TREE(*qtptr)) - { - i = bary_child(bcoord); -#ifdef DEBUG_TEST_DRIVER - qtSubdivide_tri(Pick_v0[Pick_cnt-1],Pick_v1[Pick_cnt-1], - Pick_v2[Pick_cnt-1],a,b,c); - qtNth_child_tri(Pick_v0[Pick_cnt-1],Pick_v1[Pick_cnt-1], - Pick_v2[Pick_cnt-1],a,b,c,i, - Pick_v0[Pick_cnt],Pick_v1[Pick_cnt], - Pick_v2[Pick_cnt]); - Pick_cnt++; -#endif + if(QT_IS_TREE(qt)) + { + i = bary_child(bcoord); - child = QT_NTH_CHILD_PTR(*qtptr,i); - if(t0) + return(qtLocate(QT_NTH_CHILD(qt,i),bcoord)); + } + else + return(qt); +} + +int +move_to_nbr(b,db0,db1,db2,tptr) +BCOORD b[3]; +BDIR db0,db1,db2; +double *tptr; +{ + double t,dt; + BCOORD bc; + int nbr; + + 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) + { + t = dt; + nbr = 1; + } + } + if(db2 < 0) + { + dt = ((double)b[2])/-db2; + if( dt < t) { - qtSubdivide_tri(t0,t1,t2,a,b,c); - qtNth_child_tri(t0,t1,t2,a,b,c,i,t0,t1,t2); + t = dt; + nbr = 2; } - return(qtLocate_leaf(child,bcoord,t0,t1,t2)); } - else - return(qtptr); + *tptr = t; + return(nbr); } - -QUADTREE -*qtRoot_point_locate(qtptr,v0,v1,v2,pt,t0,t1,t2) -QUADTREE *qtptr; -FVECT v0,v1,v2; -FVECT pt; -FVECT t0,t1,t2; +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; { - int d; - int i,x,y; - QUADTREE *child; - FVECT n,i_pt,a,b,c; - double pd,bcoord[3]; + int i,found; + QUADTREE child; + int nbr,next,w; + double t; - /* 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 = point_in_stri(v0,v1,v2,pt); + if(QT_IS_TREE(qt)) + { + /* Find the appropriate child and reset the coord */ + i = bary_child(b); - - /* Not in this triangle */ - if(!d) - return(NULL); - - /* 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)) - { - /* Find the intersection point */ - tri_plane_equation(v0,v1,v2,n,&pd,FALSE); - intersect_vector_plane(pt,n,pd,NULL,i_pt); - - i = max_index(n,NULL); - x = (i+1)%3; - y = (i+2)%3; - /* Calculate barycentric coordinates of i_pt */ - bary2d(v0[x],v0[y],v1[x],v1[y],v2[x],v2[y],i_pt[x],i_pt[y],bcoord); - - i = bary_child(bcoord); - child = QT_NTH_CHILD_PTR(*qtptr,i); -#ifdef DEBUG_TEST_DRIVER - Pick_cnt = 0; - VCOPY(Pick_v0[0],v0); - VCOPY(Pick_v1[0],v1); - VCOPY(Pick_v2[0],v2); - Pick_cnt++; - qtSubdivide_tri(Pick_v0[Pick_cnt-1],Pick_v1[Pick_cnt-1], - Pick_v2[Pick_cnt-1],a,b,c); - qtNth_child_tri(Pick_v0[Pick_cnt-1],Pick_v1[Pick_cnt-1], - Pick_v2[Pick_cnt-1],a,b,c,i, - Pick_v0[Pick_cnt],Pick_v1[Pick_cnt], - Pick_v2[Pick_cnt]); - Pick_cnt++; -#endif - if(t0) + for(;;) { - qtSubdivide_tri(v0,v1,v2,a,b,c); - qtNth_child_tri(v0,v1,v2,a,b,c,i,t0,t1,t2); + 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; } - return(qtLocate_leaf(child,bcoord,t0,t1,t2)); } else - { - if(t0) - { - VCOPY(t0,v0); - VCOPY(t1,v1); - VCOPY(t2,v2); - } - return(qtptr); - } -} + { +#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; + + } +} -QUADTREE -qtSubdivide(qtptr) -QUADTREE *qtptr; -{ - QUADTREE node; - node = qtAlloc(); - QT_CLEAR_CHILDREN(node); - *qtptr = node; - return(node); -} -QUADTREE -qtSubdivide_nth_child(qt,n) -QUADTREE qt; -int n; -{ - QUADTREE node; - node = qtSubdivide(&(QT_NTH_CHILD(qt,n))); - - return(node); -} -/* for triangle v0-v1-v2- returns a,b,c: children are: - v2 0: v0,a,c - /\ 1: a,v1,b - /2 \ 2: c,b,v2 - c/____\b 3: b,c,a - /\ /\ - /0 \3 /1 \ - v0____\/____\v1 - a - */ -qtSubdivide_tri(v0,v1,v2,a,b,c) -FVECT v0,v1,v2; -FVECT a,b,c; -{ - EDGE_MIDPOINT_VEC3(a,v0,v1); - EDGE_MIDPOINT_VEC3(b,v1,v2); - EDGE_MIDPOINT_VEC3(c,v2,v0); -} -qtNth_child_tri(v0,v1,v2,a,b,c,i,r0,r1,r2) -FVECT v0,v1,v2; -FVECT a,b,c; -int i; -FVECT r0,r1,r2; -{ +#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); -int -qtAdd_tri(qtptr,q0,q1,q2,t0,t1,t2,id,n) -QUADTREE *qtptr; -FVECT q0,q1,q2; -FVECT t0,t1,t2; -int id; -int n; -{ - int i,index,test,found; - FVECT a,b,c; - OBJECT os[QT_MAXSET+1],*optr; - QUADTREE qt; - FVECT r0,r1,r2; - - found = 0; - /* 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)) { - n++; - qtSubdivide_tri(q0,q1,q2,a,b,c); - test = stri_intersect(t0,t1,t2,q0,a,c); - if(test) - found |= qtAdd_tri(QT_NTH_CHILD_PTR(*qtptr,0),q0,a,c,t0,t1,t2,id,n); - test = stri_intersect(t0,t1,t2,a,q1,b); - if(test) - found |= qtAdd_tri(QT_NTH_CHILD_PTR(*qtptr,1),a,q1,b,t0,t1,t2,id,n); - test = stri_intersect(t0,t1,t2,c,b,q2); - if(test) - found |= qtAdd_tri(QT_NTH_CHILD_PTR(*qtptr,2),c,b,q2,t0,t1,t2,id,n); - test = stri_intersect(t0,t1,t2,b,c,a); - if(test) - found |= qtAdd_tri(QT_NTH_CHILD_PTR(*qtptr,3),b,c,a,t0,t1,t2,id,n); + db = t0[1] + dt10[1]*((double)(a-t0[0]))/dt10[0]; + TEST_INT(testl,testh,db,b,q1[1],bl,bh) } - else + 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)) { - /* If this leave node emptry- create a new set */ - if(QT_IS_EMPTY(*qtptr)) - *qtptr = qtaddelem(*qtptr,id); - else - { - /* If the set is too large: subdivide */ - optr = qtqueryset(*qtptr); + db = t2[1] + dt02[1]*((double)(a - t2[0]))/dt02[0]; + TEST_INT(testl,testh,db,b,q1[1],bl,bh) + } - if(QT_SET_CNT(optr) < QT_SET_THRESHOLD) - *qtptr = qtaddelem(*qtptr,id); - else - { - if (n < QT_MAX_LEVELS) - { - /* If set size exceeds threshold: subdivide cell and - reinsert set tris into cell - */ - qtgetset(os,*qtptr); + return(qt); - n++; - qtfreeleaf(*qtptr); - qtSubdivide(qtptr); - found = qtAdd_tri(qtptr,q0,q1,q2,t0,t1,t2,id,n); +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); - for(optr = QT_SET_PTR(os),i = QT_SET_CNT(os); i > 0; i--) - { - id = QT_SET_NEXT_ELEM(optr); - qtTri_from_id(id,r0,r1,r2,NULL,NULL,NULL,NULL,NULL,NULL); - found=qtAdd_tri(qtptr,q0,q1,q2,r0,r1,r2,id,n); -#ifdef DEBUG - if(!found) - eputs("qtAdd_tri():Reinsert-in parent but not children\n"); -#endif - } - } - else - if(QT_SET_CNT(optr) < QT_MAXSET) - *qtptr = qtaddelem(*qtptr,id); - else - { -#ifdef DEBUG - eputs("qtAdd_tri():two many levels\n"); -#endif - return(FALSE); - } - } - } - } - return(TRUE); } -int -qtApply_to_tri_cells(qtptr,t0,t1,t2,v0,v1,v2,func,arg) -QUADTREE *qtptr; -FVECT t0,t1,t2; -FVECT v0,v1,v2; -int (*func)(); -int *arg; -{ - int test; - FVECT a,b,c; - /* test if triangle (t0,t1,t2) overlaps cell triangle (v0,v1,v2) */ - test = stri_intersect(t0,t1,t2,v0,v1,v2); - - /* If triangles do not overlap: done */ - if(!test) - return(FALSE); - - /* if this is tree: recurse */ - func(qtptr,arg); - - if(QT_IS_TREE(*qtptr)) +/* Leaf node: Do definitive test */ +QUADTREE +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; +{ + 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) { - QT_SET_FLAG(*qtptr); - qtSubdivide_tri(v0,v1,v2,a,b,c); - qtApply_to_tri_cells(QT_NTH_CHILD_PTR(*qtptr,0),t0,t1,t2,v0,a,c,func,arg); - qtApply_to_tri_cells(QT_NTH_CHILD_PTR(*qtptr,1),t0,t1,t2,a,v1,b,func,arg); - qtApply_to_tri_cells(QT_NTH_CHILD_PTR(*qtptr,2),t0,t1,t2,c,b,v2,func,arg); - qtApply_to_tri_cells(QT_NTH_CHILD_PTR(*qtptr,3),t0,t1,t2,b,c,a,func,arg); + 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 */ -int -qtRemove_tri(qtptr,id,t0,t1,t2,v0,v1,v2) -QUADTREE *qtptr; -int id; -FVECT t0,t1,t2; -FVECT v0,v1,v2; -{ + 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 */ - int test; - int i; - FVECT a,b,c; - OBJECT os[QT_MAXSET+1]; - - /* test if triangle (t0,t1,t2) overlaps cell triangle (v0,v1,v2) */ - test = stri_intersect(t0,t1,t2,v0,v1,v2); - - /* If triangles do not overlap: done */ - if(!test) - return(FALSE); - - /* if this is tree: recurse */ - if(QT_IS_TREE(*qtptr)) + /* 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(v0,v1,v2,a,b,c); - qtRemove_tri(QT_NTH_CHILD_PTR(*qtptr,0),id,t0,t1,t2,v0,a,c); - qtRemove_tri(QT_NTH_CHILD_PTR(*qtptr,1),id,t0,t1,t2,a,v1,b); - qtRemove_tri(QT_NTH_CHILD_PTR(*qtptr,2),id,t0,t1,t2,c,b,v2); - qtRemove_tri(QT_NTH_CHILD_PTR(*qtptr,3),id,t0,t1,t2,b,c,a); + 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)) - { -#ifdef DEBUG - eputs("qtRemove_tri(): triangle not found\n"); -#endif - } - /* remove id from set */ - else - { - if(!qtinset(*qtptr,id)) - { -#ifdef DEBUG - eputs("qtRemove_tri(): tri not in set\n"); -#endif - } - else - { - *qtptr = qtdelelem(*qtptr,id); - } - } + db = t2[1] + dt02[1]*((double)(a - t2[0]))/dt02[0]; + TEST_INT(testl,testh,db,q1[1],b,bl,bh) } - return(TRUE); -} + 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); - -int -move_to_nbr(b,db0,db1,db2,tptr) -double b[3],db0,db1,db2; -double *tptr; -{ - double t,dt; - int nbr; - - nbr = -1; - /* Advance to next node */ - if(!ZERO(db0) && db0 < 0.0) - { - t = -b[0]/db0; - nbr = 0; + /* 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) } - else - t = FHUGE; - if(!ZERO(db1) && db1 < 0.0 ) + if(!test_t2t0 && (e0 & 4)) { - dt = -b[1]/db1; - if( dt < t) - { - t = dt; - nbr = 1; - } + db = t2[1] + dt02[1]*((double)(a - t2[0]))/dt02[0]; + TEST_INT(testl,testh,db,q1[1],b,bl,bh) } - if(!ZERO(db2) && db2 < 0.0 ) - { - dt = -b[2]/db2; - if( dt < t) - { - t = dt; - nbr = 2; - } - } - *tptr = t; - return(nbr); -} -int -qtTrace_ray(qtptr,b,db0,db1,db2,orig,dir,func,arg1,arg2) - QUADTREE *qtptr; - double b[3],db0,db1,db2; - FVECT orig,dir; - int (*func)(); - int *arg1,arg2; -{ + 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); + } - int i,found; - QUADTREE *child; - int nbr,next; - double t; -#ifdef DEBUG_TEST_DRIVER - - FVECT a1,b1,c1; - int Pick_parent = Pick_cnt-1; - qtSubdivide_tri(Pick_v0[Pick_parent],Pick_v1[Pick_parent], - Pick_v2[Pick_parent],a1,b1,c1); -#endif - if(QT_IS_TREE(*qtptr)) - { - /* Find the appropriate child and reset the coord */ - i = bary_child(b); - QT_SET_FLAG(*qtptr); +/* ASSUMPTION: that triangle t0,t1,t2 intersects quad cell q0,q1,q2 */ - for(;;) - { - child = QT_NTH_CHILD_PTR(*qtptr,i); +/*-------q2--------- sq2 + / \ +s1 /sc \ s0 + qb_____qa + / \ / \ +\sq0/sa \ /sb \ /sq1 + \ q0____qc____q1/ + \ / + \ s2 / +*/ - if(i != 3) - nbr = qtTrace_ray(child,b,db0,db1,db2,orig,dir,func,arg1,arg2); - else - /* If the center cell- must flip direction signs */ - nbr =qtTrace_ray(child,b,-db0,-db1,-db2,orig,dir,func,arg1,arg2); - if(nbr == QT_DONE) - return(nbr); +int Find_id=0; - /* If in same block: traverse */ - if(i==3) - next = nbr; - else - if(nbr == 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(nbr); - } - bary_from_child(b,i,next); - i = next; - } - } - else - { -#ifdef DEBUG_TEST_DRIVER - qtNth_child_tri(Pick_v0[Pick_parent],Pick_v1[Pick_parent], - Pick_v2[Pick_parent],a1,b1,c1,i, - Pick_v0[Pick_cnt],Pick_v1[Pick_cnt], - Pick_v2[Pick_cnt]); - Pick_cnt++; -#endif +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; +{ + BCOORD a,b,c; + BCOORD va[3],vb[3],vc[3]; + unsigned int sa,sb,sc; - if(func(qtptr,orig,dir,arg1,arg2) == QT_DONE) - return(QT_DONE); + /* 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; - /* Advance to next node */ - /* NOTE: Optimize: should only have to check 1/2 */ - nbr = move_to_nbr(b,db0,db1,db2,&t); + SIDES_GTR(t0,t1,t2,sa,sb,sc,a,b,c); - if(nbr != -1) - { - b[0] += t * db0; - b[1] += t * db1; - b[2] += t * db2; - } - return(nbr); + 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); + } -int -qtRoot_trace_ray(qtptr,q0,q1,q2,orig,dir,func,arg1,arg2) - QUADTREE *qtptr; - FVECT q0,q1,q2; - FVECT orig,dir; - int (*func)(); - int *arg1,arg2; -{ - int i,x,y,nbr; - QUADTREE *child; - FVECT n,c,i_pt,d; - double pd,b[3],db[3],t; - /* 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 - */ - i = point_in_stri(q0,q1,q2,orig); - - /* Not in this triangle */ - if(!i) - return(INVALID); - /* Project the origin onto the root node plane */ + 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); + } - /* Find the intersection point of the origin */ - tri_plane_equation(q0,q1,q2,n,&pd,FALSE); - intersect_vector_plane(orig,n,pd,NULL,i_pt); - /* project the dir as well */ - VADD(c,orig,dir); - intersect_vector_plane(c,n,pd,&t,c); - - /* map to 2d by dropping maximum magnitude component of normal */ - i = max_index(n,NULL); - x = (i+1)%3; - y = (i+2)%3; - /* Calculate barycentric coordinates of orig */ - bary2d(q0[x],q0[y],q1[x],q1[y],q2[x],q2[y],i_pt[x],i_pt[y],b); - /* Calculate barycentric coordinates of dir */ - bary2d(q0[x],q0[y],q1[x],q1[y],q2[x],q2[y],c[x],c[y],db); - if(t < 0.0) - VSUB(db,b,db); + 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 - VSUB(db,db,b); + 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)); +} -#ifdef DEBUG_TEST_DRIVER - VCOPY(Pick_v0[Pick_cnt],q0); - VCOPY(Pick_v1[Pick_cnt],q1); - VCOPY(Pick_v2[Pick_cnt],q2); - Pick_cnt++; -#endif +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; +{ + BCOORD a,b,c; + BCOORD va[3],vb[3],vc[3]; + unsigned int sa,sb,sc; - /* trace the ray starting with this node */ - nbr = qtTrace_ray(qtptr,b,db[0],db[1],db[2],orig,dir,func,arg1,arg2); - return(nbr); - -} + /* 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; -qtVisit_tri_interior(qtptr,q0,q1,q2,t0,t1,t2,n,func,arg1,arg2,arg3) - QUADTREE *qtptr; - FVECT q0,q1,q2; - FVECT t0,t1,t2; - int n; - int (*func)(); - int *arg1,arg2,*arg3; -{ - int i,found,test; - QUADTREE *child; - FVECT c0,c1,c2,a,b,c; - OBJECT os[QT_MAXSET+1],*optr; - int w; - - /* If qt Flag set, or qt vertices interior to t0t1t2-descend */ -tree_modified: + SIDES_GTR(t0,t1,t2,sa,sb,sc,a,b,c); - if(QT_IS_TREE(*qtptr)) - { - if(QT_IS_FLAG(*qtptr) || point_in_stri(t0,t1,t2,q0)) - { - QT_SET_FLAG(*qtptr); - qtSubdivide_tri(q0,q1,q2,a,b,c); - /* descend to children */ - for(i=0;i < 4; i++) - { - child = QT_NTH_CHILD_PTR(*qtptr,i); - qtNth_child_tri(q0,q1,q2,a,b,c,i,c0,c1,c2); - qtVisit_tri_interior(child,c0,c1,c2,t0,t1,t2,n+1, - func,arg1,arg2,arg3); - } - } + 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); } - else + if(sb==0) { - /* NOTE THIS IN TRI TEST Could be replaced by a flag */ - if(!QT_IS_EMPTY(*qtptr)) - { - if(qtinset(*qtptr,arg2)) - if(func(qtptr,q0,q1,q2,t0,t1,t2,n,arg1,arg2,arg3)==QT_MODIFIED) - goto tree_modified; - else - return; - } - if(point_in_stri(t0,t1,t2,q0) ) - if(func(qtptr,q0,q1,q2,t0,t1,t2,n,arg1,arg2,arg3)==QT_MODIFIED) - goto tree_modified; + 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)); } +/*************************************************************************/ +/* 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. +*/ - -/* NOTE: SINCE DIR could be unit: then we could use integer math */ -int -qtVisit_tri_edges(qtptr,b,db0,db1,db2, - db,wptr,t,sign,sfactor,func,arg1,arg2,arg3) - QUADTREE *qtptr; - double b[3],db0,db1,db2,db[3][3]; - int *wptr; - double t[3]; - int sign; - double sfactor; - int (*func)(); - int *arg1,arg2,*arg3; +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; { - int i,found; - QUADTREE *child; - int nbr,next,w; - double t_l,t_g; -#ifdef DEBUG_TEST_DRIVER - FVECT a1,b1,c1; - int Pick_parent = Pick_cnt-1; - qtSubdivide_tri(Pick_v0[Pick_parent],Pick_v1[Pick_parent], - Pick_v2[Pick_parent],a1,b1,c1); -#endif - if(QT_IS_TREE(*qtptr)) - { - /* Find the appropriate child and reset the coord */ - i = bary_child(b); + BCOORD a,b,c; + BCOORD va[3],vb[3],vc[3]; + unsigned int sa,sb,sc; - QT_SET_FLAG(*qtptr); + if(n == 0) + return; + /* If a tree: trivial reject and recurse on potential children */ + if(QT_IS_TREE(qt)) + { + QT_SET_FLAG(qt); - for(;;) - { - w = *wptr; - child = QT_NTH_CHILD_PTR(*qtptr,i); + /* 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(i != 3) - nbr = qtVisit_tri_edges(child,b,db0,db1,db2, - db,wptr,t,sign, - sfactor*2.0,func,arg1,arg2,arg3); - else - /* If the center cell- must flip direction signs */ - nbr = qtVisit_tri_edges(child,b,-db0,-db1,-db2, - db,wptr,t,1-sign, - sfactor*2.0,func,arg1,arg2,arg3); + SIDES_GTR(t0,t1,t2,sa,sb,sc,a,b,c); - if(nbr == QT_DONE) - return(nbr); - if(*wptr != w) - { - w = *wptr; - db0 = db[w][0];db1 = db[w][1];db2 = db[w][2]; - if(sign) - { db0 *= -1.0;db1 *= -1.0; db2 *= -1.0;} - } - /* If in same block: traverse */ - if(i==3) - next = nbr; - else - if(nbr == 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(nbr); - } - bary_from_child(b,i,next); - i = next; - } + if(sa==7) + { + 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; } - else + if(sb==7) { -#ifdef DEBUG_TEST_DRIVER - qtNth_child_tri(Pick_v0[Pick_parent],Pick_v1[Pick_parent], - Pick_v2[Pick_parent],a1,b1,c1,i,Pick_v0[Pick_cnt], - Pick_v1[Pick_cnt],Pick_v2[Pick_cnt]); - Pick_cnt++; -#endif + 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; + } - if(func(qtptr,arg1,arg2,arg3) == QT_DONE) - return(QT_DONE); + 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; + } - /* Advance to next node */ - w = *wptr; - while(1) - { - nbr = move_to_nbr(b,db0,db1,db2,&t_l); + 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); - t_g = t_l/sfactor; -#ifdef DEBUG - if(t[w] <= 0.0) - eputs("qtVisit_tri_edges():negative t\n"); -#endif - if(t_g >= t[w]) - { - if(w == 2) - return(QT_DONE); - - b[0] += (t[w])*sfactor*db0; - b[1] += (t[w])*sfactor*db1; - b[2] += (t[w])*sfactor*db2; - w++; - db0 = db[w][0]; - db1 = db[w][1]; - db2 = db[w][2]; - if(sign) - { db0 *= -1.0;db1 *= -1.0; db2 *= -1.0;} - } - else - if(nbr != INVALID) - { - b[0] += t_l * db0; - b[1] += t_l * db1; - b[2] += t_l * db2; - - t[w] -= t_g; - *wptr = w; - return(nbr); - } - else - return(INVALID); - } - } - } -int -qtRoot_visit_tri_edges(qtptr,q0,q1,q2,tri,i_pt,wptr,func,arg1,arg2,arg3) - QUADTREE *qtptr; - FVECT q0,q1,q2; - FVECT tri[3],i_pt; - int *wptr; - int (*func)(); - int *arg1,arg2,*arg3; +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; { - int x,y,z,nbr,w,i,j; - QUADTREE *child; - FVECT n,c,d,v[3]; - double pd,b[4][3],db[3][3],et[3],t[3],exit_pt; - - w = *wptr; + BCOORD a,b,c; + BCOORD va[3],vb[3],vc[3]; + unsigned int sa,sb,sc; - /* Project the origin onto the root node plane */ - - /* Find the intersection point of the origin */ - tri_plane_equation(q0,q1,q2,n,&pd,FALSE); - /* map to 2d by dropping maximum magnitude component of normal */ - z = max_index(n,NULL); - x = (z+1)%3; - y = (z+2)%3; - /* Calculate barycentric coordinates for current vertex */ - if(w != -1) + if(n==0) + return; + /* If a tree: trivial reject and recurse on potential children */ + if(QT_IS_TREE(qt)) { - bary2d(q0[x],q0[y],q1[x],q1[y],q2[x],q2[y],i_pt[x],i_pt[y],b[3]); - intersect_vector_plane(tri[w],n,pd,&(et[w]),v[w]); - bary2d(q0[x],q0[y],q1[x],q1[y],q2[x],q2[y],v[w][x],v[w][y],b[w]); - } - else - /* Just starting: b[0] is the origin point: guaranteed to be valid b*/ - { - w = 0; - intersect_vector_plane(tri[0],n,pd,&(et[0]),v[0]); - bary2d(q0[x],q0[y],q1[x],q1[y],q2[x],q2[y],v[0][x],v[0][y],b[0]); - VCOPY(b[3],b[0]); - } + 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); - j = (w+1)%3; - intersect_vector_plane(tri[j],n,pd,&(et[j]),v[j]); - bary2d(q0[x],q0[y],q1[x],q1[y],q2[x],q2[y],v[j][x],v[j][y],b[j]); - if(et[j] < 0.0) - { - VSUB(db[w],b[3],b[j]); - t[w] = FHUGE; - } - else - { - /* NOTE: for stability: do not increment with ipt- use full dir and - calculate t: but for wrap around case: could get same problem? - */ - VSUB(db[w],b[j],b[3]); - t[w] = 1.0; - move_to_nbr(b[3],db[w][0],db[w][1],db[w][2],&exit_pt); - if(exit_pt >= 1.0) - { - for(;j < 3;j++) - { - i = (j+1)%3; - if(i!= w) - { - intersect_vector_plane(tri[i],n,pd,&(et[i]),v[i]); - bary2d(q0[x],q0[y],q1[x],q1[y],q2[x],q2[y],v[i][x], - v[i][y],b[i]); - } - if(et[i] < 0.0) - { - VSUB(db[j],b[j],b[i]); - t[j] = FHUGE; - break; - } - else - { - VSUB(db[j],b[i],b[j]); - t[j] = 1.0; - } - move_to_nbr(b[j],db[j][0],db[j][1],db[j][2],&exit_pt); - if(exit_pt < 1.0) - break; - } - } - } - *wptr = w; - /* trace the ray starting with this node */ - nbr = qtVisit_tri_edges(qtptr,b[3],db[w][0],db[w][1],db[w][2], - db,wptr,t,0,1.0,func,arg1,arg2,arg3); - if(nbr != INVALID && nbr != QT_DONE) + if(sa==0) { - i_pt[x] = b[3][0]*q0[x] + b[3][1]*q1[x] + b[3][2]*q2[x]; - i_pt[y] = b[3][0]*q0[y] + b[3][1]*q1[y] + b[3][2]*q2[y]; - i_pt[z] = (-n[x]*i_pt[x] - n[y]*i_pt[y] -pd)/n[z]; + 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; } - return(nbr); - -} - -int -move_to_nbri(b,db0,db1,db2,tptr) -BCOORD b[3]; -BDIR db0,db1,db2; -TINT *tptr; -{ - TINT t,dt; - BCOORD bc; - int nbr; - - nbr = -1; - /* Advance to next node */ - if(db0 < 0) - { - bc = MAXBCOORD*b[0]; - t = bc/-db0; - nbr = 0; - } - else - t = HUGET; - if(db1 < 0) - { - bc = MAXBCOORD*b[1]; - dt = bc/-db1; - if( dt < t) + if(sb==0) { - t = dt; - nbr = 1; + 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(db2 < 0) - { - bc = MAXBCOORD*b[2]; - dt = bc/-db2; - if( dt < t) - { - t = dt; - nbr = 2; - } + 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; } - *tptr = t; - return(nbr); -} -/* NOTE: SINCE DIR could be unit: then we could use integer math */ -int -qtVisit_tri_edgesi(qtptr,b,db0,db1,db2, - db,wptr,t,sign,sfactor,func,arg1,arg2,arg3) - QUADTREE *qtptr; - BCOORD b[3]; - BDIR db0,db1,db2,db[3][3]; - int *wptr; - TINT t[3]; - int sign; - int sfactor; - int (*func)(); - int *arg1,arg2,*arg3; -{ - int i,found; - QUADTREE *child; - int nbr,next,w; - TINT t_g,t_l,t_i; -#ifdef DEBUG_TEST_DRIVER - FVECT a1,b1,c1; - int Pick_parent = Pick_cnt-1; - qtSubdivide_tri(Pick_v0[Pick_parent],Pick_v1[Pick_parent], - Pick_v2[Pick_parent],a1,b1,c1); -#endif - if(QT_IS_TREE(*qtptr)) + 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) { - /* Find the appropriate child and reset the coord */ - i = baryi_child(b); + 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); - QT_SET_FLAG(*qtptr); + 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; + } + /* 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); +} - for(;;) - { - w = *wptr; - child = QT_NTH_CHILD_PTR(*qtptr,i); - if(i != 3) - nbr = qtVisit_tri_edgesi(child,b,db0,db1,db2, - db,wptr,t,sign, - sfactor+1,func,arg1,arg2,arg3); - else - /* If the center cell- must flip direction signs */ - nbr = qtVisit_tri_edgesi(child,b,-db0,-db1,-db2, - db,wptr,t,1-sign, - sfactor+1,func,arg1,arg2,arg3); - if(nbr == QT_DONE) - return(nbr); - if(*wptr != w) - { - w = *wptr; - db0 = db[w][0];db1 = db[w][1];db2 = db[w][2]; - if(sign) - { db0 *= -1;db1 *= -1; db2 *= -1;} - } - /* If in same block: traverse */ - if(i==3) - next = nbr; - else - if(nbr == i) - next = 3; - else - { - /* reset the barycentric coordinates in the parents*/ - baryi_parent(b,i); - /* Else pop up to parent and traverse from there */ - return(nbr); - } - baryi_from_child(b,i,next); - i = next; - } - } - else - { -#ifdef DEBUG_TEST_DRIVER - qtNth_child_tri(Pick_v0[Pick_parent],Pick_v1[Pick_parent], - Pick_v2[Pick_parent],a1,b1,c1,i,Pick_v0[Pick_cnt], - Pick_v1[Pick_cnt],Pick_v2[Pick_cnt]); - Pick_cnt++; -#endif +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(func(qtptr,arg1,arg2,arg3) == QT_DONE) - return(QT_DONE); + /* 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; - /* Advance to next node */ - w = *wptr; - while(1) - { - nbr = move_to_nbri(b,db0,db1,db2,&t_i); + /* 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) + } - t_g = t_i >> sfactor; - - if(t_g >= t[w]) - { - if(w == 2) - return(QT_DONE); + return; - /* The edge fits in the cell- therefore the result of shifting db by - sfactor is guaranteed to be less than MAXBCOORD - */ - /* Caution: (t[w]*db) must occur before divide by MAXBCOORD - since t[w] will always be < MAXBCOORD - */ - b[0] = b[0] + (t[w]*db0*(1 << sfactor))/MAXBCOORD; - b[1] = b[1] + (t[w]*db1*(1 << sfactor))/MAXBCOORD; - b[2] = b[2] + (t[w]*db2*(1 << sfactor))/MAXBCOORD; - w++; - db0 = db[w][0]; db1 = db[w][1]; db2 = db[w][2]; - if(sign) - { db0 *= -1;db1 *= -1; db2 *= -1;} - } - else - if(nbr != INVALID) - { - /* Caution: (t_i*db) must occur before divide by MAXBCOORD - since t_i will always be < MAXBCOORD - */ - b[0] = b[0] + (t_i *db0) / MAXBCOORD; - b[1] = b[1] + (t_i *db1) / MAXBCOORD; - b[2] = b[2] + (t_i *db2) / MAXBCOORD; +Lfunc_call: - t[w] -= t_g; - *wptr = w; - return(nbr); - } - else - return(INVALID); - } - } + f.func(f.argptr,root,qt,n); + if(!QT_IS_EMPTY(qt)) + QT_LEAF_SET_FLAG(qt); + } -int -qtRoot_visit_tri_edgesi(qtptr,q0,q1,q2,tri,i_pt,wptr,func,arg1,arg2,arg3) - QUADTREE *qtptr; - FVECT q0,q1,q2; - FVECT tri[3],i_pt; - int *wptr; - int (*func)(); - int *arg1,arg2,*arg3; -{ - int x,y,z,nbr,w,i,j; - QUADTREE *child; - FVECT n,c,d,v[3]; - double pd,b[4][3],db[3][3],et[3],exit_pt; - BCOORD bi[3]; - TINT t[3]; - BDIR dbi[3][3]; - w = *wptr; - /* Project the origin onto the root node plane */ +/* 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; + + /* 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; - t[0] = t[1] = t[2] = 0; - /* Find the intersection point of the origin */ - tri_plane_equation(q0,q1,q2,n,&pd,FALSE); - /* map to 2d by dropping maximum magnitude component of normal */ - z = max_index(n,NULL); - x = (z+1)%3; - y = (z+2)%3; - /* Calculate barycentric coordinates for current vertex */ - if(w != -1) + 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(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) ) { - bary2d(q0[x],q0[y],q1[x],q1[y],q2[x],q2[y],i_pt[x],i_pt[y],b[3]); - intersect_vector_plane(tri[w],n,pd,&(et[w]),v[w]); - bary2d(q0[x],q0[y],q1[x],q1[y],q2[x],q2[y],v[w][x],v[w][y],b[w]); + db = t0[1] + dt10[1]*((double)(a-t0[0])/dt10[0]); + TEST_INT(testl,testh,db,q1[1],b,bl,bh) } - else - /* Just starting: b[0] is the origin point: guaranteed to be valid b*/ + 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)) { - w = 0; - intersect_vector_plane(tri[0],n,pd,&(et[0]),v[0]); - bary2d(q0[x],q0[y],q1[x],q1[y],q2[x],q2[y],v[0][x],v[0][y],b[0]); - VCOPY(b[3],b[0]); + 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; - - j = (w+1)%3; - intersect_vector_plane(tri[j],n,pd,&(et[j]),v[j]); - bary2d(q0[x],q0[y],q1[x],q1[y],q2[x],q2[y],v[j][x],v[j][y],b[j]); - if(et[j] < 0.0) + /* Only one/none got tested - pick either of other two/any two */ + /* Only need to test one edge */ + if(!test_t0t1 && (e0 & 2)) { - VSUB(db[w],b[3],b[j]); - t[w] = HUGET; + db = t0[1] + dt10[1]*((double)(a-t0[0]))/dt10[0]; + TEST_INT(testl,testh,db,q1[1],b,bl,bh) } - else - { - /* NOTE: for stability: do not increment with ipt- use full dir and - calculate t: but for wrap around case: could get same problem? - */ - VSUB(db[w],b[j],b[3]); - /* Check if the point is out side of the triangle: if so t[w] =HUGET */ - if((fabs(b[j][0])>(FTINY+1.0)) ||(fabs(b[j][1])>(FTINY+1.0)) || - (fabs(b[j][2])>(FTINY+1.0))) - t[w] = HUGET; - else - { - /* The next point is in the triangle- so db values will be in valid - range and t[w]= MAXT - */ - t[w] = MAXT; - if(j != 0) - for(;j < 3;j++) - { - i = (j+1)%3; - if(i!= w) - { - intersect_vector_plane(tri[i],n,pd,&(et[i]),v[i]); - bary2d(q0[x],q0[y],q1[x],q1[y],q2[x],q2[y],v[i][x], - v[i][y],b[i]); - } - if(et[i] < 0.0) - { - VSUB(db[j],b[j],b[i]); - t[j] = HUGET; - break; - } - else - { - VSUB(db[j],b[i],b[j]); - if((fabs(b[j][0])>(FTINY+1.0)) || - (fabs(b[j][1])>(FTINY+1.0)) || - (fabs(b[j][2])>(FTINY+1.0))) - { - t[j] = HUGET; - break; - } - else - t[j] = MAXT; - } - } + 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); } - *wptr = w; - bary_dtol(b[3],db,bi,dbi,t); - /* trace the ray starting with this node */ - nbr = qtVisit_tri_edgesi(qtptr,bi,dbi[w][0],dbi[w][1],dbi[w][2], - dbi,wptr,t,0,0,func,arg1,arg2,arg3); - if(nbr != INVALID && nbr != QT_DONE) + +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) { - b[3][0] = (double)bi[0]/(double)MAXBCOORD; - b[3][1] = (double)bi[1]/(double)MAXBCOORD; - b[3][2] = (double)bi[2]/(double)MAXBCOORD; - i_pt[x] = b[3][0]*q0[x] + b[3][1]*q1[x] + b[3][2]*q2[x]; - i_pt[y] = b[3][0]*q0[y] + b[3][1]*q1[y] + b[3][2]*q2[y]; - i_pt[z] = (-n[x]*i_pt[x] - n[y]*i_pt[y] -pd)/n[z]; + 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) + { + 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); } - return(nbr); - + 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) + { + 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); + } } +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); + } + } + else + { + qt = f.func(f.argptr,root,qt,parent,q0,q1,q2,bc,scale,1,f,n,doneptr); + return(qt); + } +}