--- ray/src/hd/sm_qtree.c 1998/08/19 17:45:24 3.1 +++ ray/src/hd/sm_qtree.c 1998/09/14 10:33:47 3.5 @@ -17,14 +17,18 @@ static char SCCSid[] = "$SunId$ SGI"; #include "sm_geom.h" #include "sm_qtree.h" -#include "object.h" QUADTREE *quad_block[QT_MAX_BLK]; /* our quadtree */ static QUADTREE quad_free_list = EMPTY; /* freed octree nodes */ static QUADTREE treetop = 0; /* next free node */ +int4 *quad_flag= NULL; +#ifdef TEST_DRIVER +extern FVECT Pick_v0[500],Pick_v1[500],Pick_v2[500]; +extern int Pick_cnt,Pick_tri,Pick_samp; +extern FVECT Pick_point[500]; +#endif - QUADTREE qtAlloc() /* allocate a quadtree */ { @@ -41,14 +45,26 @@ 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) + QT_BLOCK_SIZE*4*sizeof(QUADTREE))) == NULL) return(EMPTY); + + quad_flag = (int4 *)realloc((char *)quad_flag, + (QT_BLOCK(freet)+1)*(QT_BLOCK_SIZE/8)); + if (quad_flag == NULL) + return(EMPTY); } treetop += 4; return(freet); } +qtClearAllFlags() /* clear all quadtree branch flags */ +{ + if (!treetop) return; + bzero((char *)quad_flag, (QT_BLOCK(treetop-4)+1)*(QT_BLOCK_SIZE/8)); +} + + qtFree(qt) /* free a quadtree */ register QUADTREE qt; { @@ -70,51 +86,68 @@ 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((char *)quad_block[i]); quad_block[i] = NULL; } + if (i) free((char *)quad_flag); + quad_flag = NULL; quad_free_list = EMPTY; treetop = 0; } QUADTREE -qtCompress(qt) /* recursively combine nodes */ -register QUADTREE qt; +*qtLocate_leaf(qtptr,bcoord,t0,t1,t2) +QUADTREE *qtptr; +double bcoord[3]; +FVECT t0,t1,t2; { - register int i; - register QUADTREE qres; + int i; + QUADTREE *child; + FVECT a,b,c; - 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; + 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 + + child = QT_NTH_CHILD_PTR(*qtptr,i); + if(t0) + { + qtSubdivide_tri(t0,t1,t2,a,b,c); + qtNth_child_tri(t0,t1,t2,a,b,c,i,t0,t1,t2); + } + return(qtLocate_leaf(child,bcoord,t0,t1,t2)); } - return(qres); + else + return(qtptr); } + QUADTREE -qtPoint_locate(qtptr,v1,v2,v3,pt,type,which,p0,p1,p2) +*qtRoot_point_locate(qtptr,v0,v1,v2,pt,t0,t1,t2) QUADTREE *qtptr; -FVECT v1,v2,v3; +FVECT v0,v1,v2; FVECT pt; -char *type,*which; -FVECT p0,p1,p2; +FVECT t0,t1,t2; { - char d,w; - int i; + int d; + int i,x,y; QUADTREE *child; - QUADTREE qt; - FVECT a,b,c; - FVECT t1,t2,t3; + FVECT n,i_pt,a,b,c; + double pd,bcoord[3]; /* Determine if point lies within pyramid (and therefore inside a spherical quadtree cell):GT_INTERIOR, on one of the @@ -123,109 +156,66 @@ FVECT p0,p1,p2; 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); + d = point_in_stri(v0,v1,v2,pt); + /* Not in this triangle */ if(!d) - { - if(which) - *which = 0; - return(EMPTY); - } + 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)) { - qtSubdivide_tri(v1,v2,v3,a,b,c); - child = QT_NTH_CHILD_PTR(*qtptr,0); - if(!QT_IS_EMPTY(*child)) + /* 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); + 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) { - qt = qtPoint_locate(child,v1,a,c,pt,type,which,p0,p1,p2); - if(!QT_IS_EMPTY(qt)) - return(qt); + 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_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); - } + return(qtLocate_leaf(child,bcoord,t0,t1,t2)); } 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); -} - -int -qtPoint_in_tri(qtptr,v0,v1,v2,pt,type,which) -QUADTREE *qtptr; -FVECT v0,v1,v2; -FVECT pt; -char *type,*which; -{ - QUADTREE qt; - OBJECT os[MAXSET+1],*optr; - char d,w; - int i,id; - FVECT p0,p1,p2; - - 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--) { - /* 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); + if(t0) + { + VCOPY(t0,v0); + VCOPY(t1,v1); + VCOPY(t2,v2); } + return(qtptr); } - return(EMPTY); } + QUADTREE qtSubdivide(qtptr) QUADTREE *qtptr; @@ -250,51 +240,46 @@ int n; return(node); } -/* for triangle v1-v2-v3- returns a,b,c: children are: +/* for triangle v0-v1-v2- returns a,b,c: children are: - v3 0: v1,a,c - /\ 1: a,b,c - /3 \ 2: a,v2,b - c/____\b 3: c,b,v3 + v2 0: v0,a,c + /\ 1: a,v1,b + /2 \ 2: c,b,v2 + c/____\b 3: b,c,a /\ /\ - /0 \1 /2 \ - v1/____\/____\v2 + /0 \3 /1 \ + v0____\/____\v1 a */ -qtSubdivide_tri(v1,v2,v3,a,b,c) -FVECT v1,v2,v3; +qtSubdivide_tri(v0,v1,v2,a,b,c) +FVECT v0,v1,v2; FVECT a,b,c; { - EDGE_MIDPOINT_VEC3(a,v1,v2); - normalize(a); - EDGE_MIDPOINT_VEC3(b,v2,v3); - normalize(b); - EDGE_MIDPOINT_VEC3(c,v3,v1); - normalize(c); + EDGE_MIDPOINT_VEC3(a,v0,v1); + EDGE_MIDPOINT_VEC3(b,v1,v2); + EDGE_MIDPOINT_VEC3(c,v2,v0); } -qtNth_child_tri(v1,v2,v3,a,b,c,i,r1,r2,r3) -FVECT v1,v2,v3; +qtNth_child_tri(v0,v1,v2,a,b,c,i,r0,r1,r2) +FVECT v0,v1,v2; FVECT a,b,c; int i; -FVECT r1,r2,r3; +FVECT r0,r1,r2; { - VCOPY(r1,a); VCOPY(r2,b); VCOPY(r3,c); + switch(i){ case 0: - VCOPY(r2,r1); - VCOPY(r1,v1); + VCOPY(r0,v0); VCOPY(r1,a); VCOPY(r2,c); break; case 1: + VCOPY(r0,a); VCOPY(r1,v1); VCOPY(r2,b); break; case 2: - VCOPY(r3,r2); - VCOPY(r2,v2); + VCOPY(r0,c); VCOPY(r1,b); VCOPY(r2,v2); break; case 3: - VCOPY(r1,r3); - VCOPY(r3,v3); + VCOPY(r0,b); VCOPY(r1,c); VCOPY(r2,a); break; } } @@ -308,96 +293,70 @@ into the new child cells: it is assumed that "v1,v2,v3 */ int -qtAdd_tri(qtptr,id,t1,t2,t3,v1,v2,v3,n) +qtRoot_add_tri(qtptr,q0,q1,q2,t0,t1,t2,id,n) QUADTREE *qtptr; +FVECT q0,q1,q2; +FVECT t0,t1,t2; int id; -FVECT t1,t2,t3; -FVECT v1,v2,v3; int n; { - - char test; - int i,index; - FVECT a,b,c; - OBJECT os[MAXSET+1],*optr; - QUADTREE qt; + int test; int found; - FVECT r1,r2,r3; - /* test if triangle (t1,t2,t3) overlaps cell triangle (v1,v2,v3) */ - test = spherical_tri_intersect(t1,t2,t3,v1,v2,v3); - - /* If triangles do not overlap: done */ + test = stri_intersect(q0,q1,q2,t0,t1,t2); if(!test) return(FALSE); - found = 0; + + found = qtAdd_tri(qtptr,q0,q1,q2,t0,t1,t2,id,n); + + return(found); +} +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)) { 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); - -#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 + 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); } else { /* 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; - */ - } + *qtptr = qtaddelem(*qtptr,id); 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); - */ - } + optr = qtqueryset(*qtptr); + + if(QT_SET_CNT(optr) < QT_SET_THRESHOLD) + *qtptr = qtaddelem(*qtptr,id); else { if (n < QT_MAX_LEVELS) @@ -405,50 +364,27 @@ int n; /* 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); + qtgetset(os,*qtptr); + + n++; + qtfreeleaf(*qtptr); + qtSubdivide(qtptr); + found = qtAdd_tri(qtptr,q0,q1,q2,t0,t1,t2,id,n); + + for(optr = QT_SET_PTR(os),i = QT_SET_CNT(os); i > 0; i--) + { + id = QT_SET_NEXT_ELEM(optr); + qtTri_from_id(id,NULL,NULL,NULL,r0,r1,r2,NULL,NULL,NULL); + found=qtAdd_tri(qtptr,q0,q1,q2,r0,r1,r2,id,n); #ifdef DEBUG - if(!found) + if(!found) eputs("qtAdd_tri():Reinsert-in parent but not children\n"); #endif - } - } + } + } else - if(QT_SET_CNT(os) < QT_MAX_SET) - { + if(QT_SET_CNT(optr) < QT_MAXSET) *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 @@ -464,52 +400,52 @@ int n; int -qtApply_to_tri_cells(qtptr,t1,t2,t3,v1,v2,v3,func,arg) +qtApply_to_tri_cells(qtptr,t0,t1,t2,v0,v1,v2,func,arg) QUADTREE *qtptr; -FVECT t1,t2,t3; -FVECT v1,v2,v3; +FVECT t0,t1,t2; +FVECT v0,v1,v2; int (*func)(); -char *arg; +int *arg; { - char test; + int 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); + /* 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)) { - 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); + 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); } - else - return(func(qtptr,arg)); } - int -qtRemove_tri(qtptr,id,t1,t2,t3,v1,v2,v3) +qtRemove_tri(qtptr,id,t0,t1,t2,v0,v1,v2) QUADTREE *qtptr; int id; -FVECT t1,t2,t3; -FVECT v1,v2,v3; +FVECT t0,t1,t2; +FVECT v0,v1,v2; { - char test; + int test; int i; FVECT a,b,c; - OBJECT os[MAXSET+1]; + OBJECT os[QT_MAXSET+1]; - /* test if triangle (t1,t2,t3) overlaps cell triangle (v1,v2,v3) */ - test = spherical_tri_intersect(t1,t2,t3,v1,v2,v3); + /* 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) @@ -518,11 +454,11 @@ FVECT v1,v2,v3; /* if this is tree: recurse */ if(QT_IS_TREE(*qtptr)) { - 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); + 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); } else { @@ -535,8 +471,7 @@ FVECT v1,v2,v3; /* remove id from set */ else { - qtgetset(os,*qtptr); - if(!inset(os,id)) + if(!qtinset(*qtptr,id)) { #ifdef DEBUG eputs("qtRemove_tri(): tri not in set\n"); @@ -545,21 +480,835 @@ FVECT v1,v2,v3; 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"); - } + } + } + } + return(TRUE); +} - } + +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; + } + else + t = FHUGE; + if(!ZERO(db1) && db1 < 0.0 ) + { + dt = -b[1]/db1; + if( dt < t) + { + t = dt; + nbr = 1; + } + } + if(!ZERO(db2) && db2 < 0.0 ) + { + dt = -b[2]/db2; + if( dt < t) + { + t = dt; + nbr = 2; + } + } + *tptr = t; + return(nbr); +} + +int +qtTrace_edge(qtptr,b,db,orig,dir,max_t,func,arg1,arg2) + QUADTREE *qtptr; + double b[3],db[3]; + FVECT orig,dir; + double max_t; + int (*func)(); + int *arg1,arg2; +{ + + 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); + + for(;;) + { + child = QT_NTH_CHILD_PTR(*qtptr,i); + + if(i != 3) + { + + db[0] *= 2.0;db[1] *= 2.0; db[2] *= 2.0; + nbr = qtTrace_edge(child,b,db,orig,dir,max_t,func,arg1,arg2); + db[0] *= 0.5;db[1] *= 0.5; db[2] *= 0.5; + } + else + { + db[0] *=-2.0;db[1] *= -2.0; db[2] *= -2.0; + /* If the center cell- must flip direction signs */ + nbr = qtTrace_edge(child,b,db,orig,dir,max_t,func,arg1,arg2); + db[0] *=-0.5;db[1] *= -0.5; db[2] *= -0.5; + } + if(nbr == QT_DONE) + return(nbr); + + /* 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 + + if(func(qtptr,arg1,arg2) == QT_DONE) + return(QT_DONE); + + /* Advance to next node */ + /* NOTE: Optimize: should only have to check 1/2 */ + nbr = move_to_nbr(b,db[0],db[1],db[2],&t); + + if(t >= max_t) + return(QT_DONE); + if(nbr != -1) + { + b[0] += t * db[0]; + b[1] += t * db[1]; + b[2] += t * db[2]; + db[0] *= (1.0 - t); + db[1] *= (1.0 - t); + db[2] *= (1.0 - 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; +{ + + 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); + + for(;;) + { + child = QT_NTH_CHILD_PTR(*qtptr,i); + + 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); + + /* 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 + + if(func(qtptr,orig,dir,arg1,arg2) == QT_DONE) + return(QT_DONE); + + /* Advance to next node */ + /* NOTE: Optimize: should only have to check 1/2 */ + nbr = move_to_nbr(b,db0,db1,db2,&t); + + if(nbr != -1) + { + b[0] += t * db0; + b[1] += t * db1; + b[2] += t * db2; + } + return(nbr); + } + +} + +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 */ + + /* 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); + 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); + else + VSUB(db,db,b); + + +#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 + + /* 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); + +} + + +int +qtRoot_trace_edge(qtptr,q0,q1,q2,orig,dir,max_t,func,arg1,arg2) + QUADTREE *qtptr; + FVECT q0,q1,q2; + FVECT orig,dir; + double max_t; + 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(-1); + /* Project the origin onto the root node plane */ + + /* 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); + 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); + else + VSUB(db,db,b); + + +#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 + /* trace the ray starting with this node */ + nbr = qtTrace_edge(qtptr,b,db,orig,d,max_t,func,arg1,arg2); + return(nbr); + +} + + +qtVisit_tri_interior(qtptr,q0,q1,q2,t0,t1,t2,n,func,arg1,arg2) + QUADTREE *qtptr; + FVECT q0,q1,q2; + FVECT t0,t1,t2; + int n; + int (*func)(); + int *arg1,arg2; +{ + 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: + + 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); + } } + } + else + { + /* 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)==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)==QT_MODIFIED) + goto tree_modified; + } +} + + + + + + +int +qtVisit_tri_edges(qtptr,b,db,wptr,sfactor,func,arg1,arg2) + QUADTREE *qtptr; + double b[3],db[3][3]; + int *wptr; + double sfactor; + int (*func)(); + int *arg1,arg2; +{ + int i,found; + QUADTREE *child; + int nbr,next,w; + 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); + + for(;;) + { + w = *wptr; + child = QT_NTH_CHILD_PTR(*qtptr,i); + + if(i != 3) + { + + db[w][0] *= 2.0;db[w][1] *= 2.0; db[w][2] *= 2.0; + nbr = qtVisit_tri_edges(child,b,db,wptr,sfactor*2.0, + func,arg1,arg2); + w = *wptr; + db[w][0] *= 0.5;db[w][1] *= 0.5; db[w][2] *= 0.5; + } + else + { + db[w][0] *=-2.0;db[w][1] *= -2.0; db[w][2] *= -2.0; + /* If the center cell- must flip direction signs */ + nbr = qtVisit_tri_edges(child,b,db,wptr,sfactor*(-2.0), + func,arg1,arg2); + w = *wptr; + db[w][0] *=-0.5;db[w][1] *= -0.5; db[w][2] *= -0.5; + } + if(nbr == QT_DONE) + return(nbr); + + /* 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 + + if(func(qtptr,arg1,arg2) == QT_DONE) + return(QT_DONE); + + /* Advance to next node */ + /* NOTE: Optimize: should only have to check 1/2 */ + w = *wptr; + while(1) + { + nbr = move_to_nbr(b,db[w][0],db[w][1],db[w][2],&t); + + if(t >= 1.0) + { + if(w == 2) + return(QT_DONE); + b[0] += db[w][0]; + b[1] += db[w][1]; + b[2] += db[w][2]; + w++; + db[w][0] *= sfactor; + db[w][1] *= sfactor; + db[w][2] *= sfactor; + } + else + if(nbr != INVALID) + { + b[0] += t * db[w][0]; + b[1] += t * db[w][1]; + b[2] += t * db[w][2]; + db[w][0] *= (1.0 - t); + db[w][1] *= (1.0 - t); + db[w][2] *= (1.0 - t); + *wptr = w; + return(nbr); + } + else + return(INVALID); + } + } + +} + + +int +qtRoot_visit_tri_edges(qtptr,q0,q1,q2,tri,dir,wptr,func,arg1,arg2) + QUADTREE *qtptr; + FVECT q0,q1,q2; + FVECT tri[3],dir[3]; + int *wptr; + int (*func)(); + int *arg1,arg2; +{ + int i,x,y,nbr,w; + QUADTREE *child; + FVECT n,c,i_pt,d; + double pd,b[3][3],db[3][3],t; + + w = *wptr; + + /* 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 */ + i = max_index(n); + x = (i+1)%3; + y = (i+2)%3; + /* Calculate barycentric coordinates for current vertex */ + + for(i=0;i < 3; i++) + { + /* If processing 3rd edge-dont need info for t1 */ + if(i==1 && w==2) + continue; + /* project the dir as well */ + intersect_vector_plane(tri[i],n,pd,NULL,i_pt); + bary2d(q0[x],q0[y],q1[x],q1[y],q2[x],q2[y],i_pt[x],i_pt[y],b[i]); + VADD(c,tri[i],dir[i]); + intersect_vector_plane(c,n,pd,&t,c); + /* Calculate barycentric coordinates of dir */ + bary2d(q0[x],q0[y],q1[x],q1[y],q2[x],q2[y],c[x],c[y],db[i]); + if(t < 0.0) + VSUB(db[i],b[i],db[i]); + else + VSUB(db[i],db[i],b[i]); } - return(TRUE); +#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 + /* trace the ray starting with this node */ + nbr = qtVisit_tri_edges(qtptr,b[w],db,wptr,1.0,func,arg1,arg2); + return(nbr); + } + + + + +/* NOTE: SINCE DIR could be unit: then we could use integer math */ +int +qtVisit_tri_edges2(qtptr,b,db0,db1,db2, + db,wptr,t,sign,sfactor,func,arg1,arg2) + 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; +{ + 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); + + QT_SET_FLAG(*qtptr); + + for(;;) + { + w = *wptr; + child = QT_NTH_CHILD_PTR(*qtptr,i); + + if(i != 3) + nbr = qtVisit_tri_edges2(child,b,db0,db1,db2, + db,wptr,t,sign, + sfactor*2.0,func,arg1,arg2); + else + /* If the center cell- must flip direction signs */ + nbr = qtVisit_tri_edges2(child,b,-db0,-db1,-db2, + db,wptr,t,1-sign, + sfactor*2.0,func,arg1,arg2); + + 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; + } + } + 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 + + if(func(qtptr,arg1,arg2) == QT_DONE) + return(QT_DONE); + + /* Advance to next node */ + w = *wptr; + while(1) + { + nbr = move_to_nbr(b,db0,db1,db2,&t_l); + + t_g = t_l/sfactor; +#ifdef DEBUG + if(t[w] <= 0.0) + eputs("qtVisit_tri_edges2():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_edges2(qtptr,q0,q1,q2,tri,i_pt,wptr,func,arg1,arg2) + QUADTREE *qtptr; + FVECT q0,q1,q2; + FVECT tri[3],i_pt; + int *wptr; + int (*func)(); + int *arg1,arg2; +{ + 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; + + /* 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); + x = (z+1)%3; + y = (z+2)%3; + /* Calculate barycentric coordinates for current vertex */ + if(w != -1) + { + 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]); + } + + + 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_edges2(qtptr,b[3],db[w][0],db[w][1],db[w][2], + db,wptr,t,0,1.0,func,arg1,arg2); + if(nbr != INVALID && nbr != QT_DONE) + { + 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]; + } + return(nbr); + +} + + + + + + + + + + + + + + + + + +