--- ray/src/hd/sm_qtree.c 1998/08/25 11:03:27 3.3 +++ ray/src/hd/sm_qtree.c 1998/09/11 11:52:26 3.4 @@ -17,13 +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; +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 */ { @@ -42,8 +47,9 @@ qtAlloc() /* allocate a quadtree */ if ((quad_block[QT_BLOCK(freet)] = (QUADTREE *)malloc( QT_BLOCK_SIZE*4*sizeof(QUADTREE))) == NULL) return(EMPTY); + quad_flag = (int4 *)realloc((char *)quad_flag, - (QT_BLOCK(freet)+1)*QT_BLOCK_SIZE/(8*sizeof(int4))); + (QT_BLOCK(freet)+1)*(QT_BLOCK_SIZE/8)); if (quad_flag == NULL) return(EMPTY); } @@ -55,8 +61,7 @@ qtAlloc() /* allocate a quadtree */ qtClearAllFlags() /* clear all quadtree branch flags */ { if (!treetop) return; - bzero((char *)quad_flag, - (QT_BLOCK(treetop-1)+1)*QT_BLOCK_SIZE/(8*sizeof(int4))); + bzero((char *)quad_flag, (QT_BLOCK(treetop-4)+1)*(QT_BLOCK_SIZE/8)); } @@ -81,6 +86,7 @@ qtDone() /* free EVERYTHING */ { register int i; + qtfreeleaves(); for (i = 0; i < QT_MAX_BLK; i++) { if (quad_block[i] == NULL) @@ -94,30 +100,7 @@ qtDone() /* free EVERYTHING */ treetop = 0; } - QUADTREE -qtCompress(qt) /* recursively combine nodes */ -register QUADTREE qt; -{ - register int i; - register QUADTREE qres; - - 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; - } - return(qres); -} - - -QUADTREE *qtLocate_leaf(qtptr,bcoord,t0,t1,t2) QUADTREE *qtptr; double bcoord[3]; @@ -129,7 +112,17 @@ FVECT t0,t1,t2; if(QT_IS_TREE(*qtptr)) { - i = bary2d_child(bcoord); + 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) { @@ -143,191 +136,6 @@ FVECT t0,t1,t2; } - -int -qtLeaf_add_tri_from_pt(qtptr,bcoord,id,v0,v1,v2,n) -QUADTREE *qtptr; -double bcoord[3]; -int id; -FVECT v0,v1,v2; -int n; -{ - int i; - QUADTREE *child; - OBJECT os[MAXSET+1],*optr; - int found; - FVECT r0,r1,r2; - - if(QT_IS_TREE(*qtptr)) - { - QT_SET_FLAG(*qtptr); - i = bary2d_child(bcoord); - child = QT_NTH_CHILD_PTR(*qtptr,i); - return(qtLeaf_add_tri_from_pt(child,bcoord,id,v0,v1,v2,++n)); - } - else - { - /* If this leave node emptry- create a new set */ - if(QT_IS_EMPTY(*qtptr)) - *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); - else - { - if (n < QT_MAX_LEVELS) - { - /* If set size exceeds threshold: subdivide cell and - reinsert set tris into cell - */ - n++; - qtfreeleaf(*qtptr); - qtSubdivide(qtptr); - QT_SET_FLAG(*qtptr); - found = qtLeaf_add_tri_from_pt(qtptr,bcoord,id,v0,v1,v2,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,r0,r1,r2); - found= qtLeaf_add_tri_from_pt(qtptr,bcoord,id,v0,v1,v2,++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); - } - else - { -#ifdef DEBUG - eputs("qtAdd_tri():two many levels\n"); -#endif - return(FALSE); - } - } - } - } - return(TRUE); -} - - -int -qtAdd_tri_from_point(qtptr,v0,v1,v2,pt,id) -QUADTREE *qtptr; -FVECT v0,v1,v2; -FVECT pt; -int id; -{ - char d,w; - int i,x,y; - QUADTREE *child; - QUADTREE qt; - FVECT i_pt,n,a,b,c,r0,r1,r2; - double pd,bcoord[3]; - OBJECT os[MAXSET+1],*optr; - int found; - - /* 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(v0,v1,v2,pt,&w); - - /* Not in this triangle */ - if(!d) - return(FALSE); - - /* 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)) - { - QT_SET_FLAG(*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); - 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 = bary2d_child(bcoord); - child = QT_NTH_CHILD_PTR(*qtptr,i); - /* NOTE: Optimize: only subdivide for specified child */ - qtSubdivide_tri(v0,v1,v2,a,b,c); - qtNth_child_tri(v0,v1,v2,a,b,c,i,v0,v1,v2); - return(qtLeaf_add_tri_from_pt(child,bcoord,id,v0,v1,v2,1)); - } - else - { - /* If this leave node emptry- create a new set */ - if(QT_IS_EMPTY(*qtptr)) - *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); - else - { - /* If set size exceeds threshold: subdivide cell and - reinsert set tris into cell - */ - qtfreeleaf(*qtptr); - qtSubdivide(qtptr); - found = qtLeaf_add_tri_from_pt(qtptr,bcoord,id,v0,v1,v2,1); -#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,r0,r1,r2); - found=qtAdd_tri(qtptr,id,r0,r1,r2,v0,v1,v2,1); -#ifdef DEBUG - if(!found) - eputs("qtAdd_tri():Reinsert-in parent but not children\n"); -#endif - } - } - } - } - return(TRUE); -} - - QUADTREE *qtRoot_point_locate(qtptr,v0,v1,v2,pt,t0,t1,t2) QUADTREE *qtptr; @@ -335,10 +143,9 @@ FVECT v0,v1,v2; FVECT pt; FVECT t0,t1,t2; { - char d,w; + int d; int i,x,y; QUADTREE *child; - QUADTREE qt; FVECT n,i_pt,a,b,c; double pd,bcoord[3]; @@ -349,13 +156,12 @@ FVECT t0,t1,t2; 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(v0,v1,v2,pt,&w); + d = point_in_stri(v0,v1,v2,pt); + /* 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 @@ -374,8 +180,22 @@ FVECT t0,t1,t2; /* 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 = bary2d_child(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) { qtSubdivide_tri(v0,v1,v2,a,b,c); @@ -384,46 +204,18 @@ FVECT t0,t1,t2; return(qtLocate_leaf(child,bcoord,t0,t1,t2)); } else - return(qtptr); -} - -int -qtPoint_in_tri(qtptr,v0,v1,v2,pt,type,which) -QUADTREE *qtptr; -FVECT v0,v1,v2; -FVECT pt; -char *type,*which; -{ - OBJECT os[MAXSET+1],*optr; - char d,w; - int i,id; - FVECT p0,p1,p2; - - qtptr = qtRoot_point_locate(qtptr,v0,v1,v2,pt,NULL,NULL,NULL); - - if(!qtptr || QT_IS_EMPTY(*qtptr)) - return(EMPTY); - - /* Get the set */ - qtgetset(os,*qtptr); - 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; @@ -475,6 +267,7 @@ FVECT a,b,c; int i; FVECT r0,r1,r2; { + switch(i){ case 0: VCOPY(r0,v0); VCOPY(r1,a); VCOPY(r2,c); @@ -500,58 +293,37 @@ into the new child cells: it is assumed that "v1,v2,v3 */ int -qtRoot_add_tri(qtptr,id,t0,t1,t2,v0,v1,v2,n) +qtRoot_add_tri(qtptr,q0,q1,q2,t0,t1,t2,id,n) QUADTREE *qtptr; -int id; +FVECT q0,q1,q2; FVECT t0,t1,t2; -FVECT v0,v1,v2; -int n; -{ - char test; - int found; - - test = spherical_tri_intersect(t0,t1,t2,v0,v1,v2); - if(!test) - return(FALSE); - - found = qtAdd_tri(qtptr,id,t0,t1,t2,v0,v1,v2,n); - return(found); -} - -int -qtRoot_add_tri_from_point(qtptr,id,t0,t1,t2,v0,v1,v2,n) -QUADTREE *qtptr; int id; -FVECT t0,t1,t2; -FVECT v0,v1,v2; int n; { - char test; + int test; int found; - test = spherical_tri_intersect(t0,t1,t2,v0,v1,v2); + test = stri_intersect(q0,q1,q2,t0,t1,t2); if(!test) return(FALSE); - found = qtAdd_tri_from_point(qtptr,id,t0,t1,t2,v0,v1,v2,n); + found = qtAdd_tri(qtptr,q0,q1,q2,t0,t1,t2,id,n); + return(found); } int -qtAdd_tri(qtptr,id,t0,t1,t2,v0,v1,v2,n) +qtAdd_tri(qtptr,q0,q1,q2,t0,t1,t2,id,n) QUADTREE *qtptr; -int id; +FVECT q0,q1,q2; FVECT t0,t1,t2; -FVECT v0,v1,v2; +int id; int n; { - - char test; - int i,index; + int i,index,test,found; FVECT a,b,c; - OBJECT os[MAXSET+1],*optr; + OBJECT os[QT_MAXSET+1],*optr; QUADTREE qt; - int found; FVECT r0,r1,r2; found = 0; @@ -559,30 +331,19 @@ int n; if(QT_IS_TREE(*qtptr)) { n++; - qtSubdivide_tri(v0,v1,v2,a,b,c); - test = spherical_tri_intersect(t0,t1,t2,v0,a,c); + 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),id,t0,t1,t2,v0,a,c,n); - test = spherical_tri_intersect(t0,t1,t2,a,v1,b); + 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),id,t0,t1,t2,a,v1,b,n); - test = spherical_tri_intersect(t0,t1,t2,c,b,v2); + 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),id,t0,t1,t2,c,b,v2,n); - test = spherical_tri_intersect(t0,t1,t2,b,c,a); + 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),id,t0,t1,t2,b,c,a,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 + found |= qtAdd_tri(QT_NTH_CHILD_PTR(*qtptr,3),b,c,a,t0,t1,t2,id,n); } else { @@ -591,10 +352,11 @@ int n; *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); + optr = qtqueryset(*qtptr); + + if(QT_SET_CNT(optr) < QT_SET_THRESHOLD) + *qtptr = qtaddelem(*qtptr,id); else { if (n < QT_MAX_LEVELS) @@ -602,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,t0,t1,t2,v0,v1,v2,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,r0,r1,r2); - found=qtAdd_tri(qtptr,id,r0,r1,r2,v0,v1,v2,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 @@ -666,32 +405,32 @@ QUADTREE *qtptr; 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 (t0,t1,t2) overlaps cell triangle (v0,v1,v2) */ - test = spherical_tri_intersect(t0,t1,t2,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(v0,v1,v2,a,b,c); + 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,t0,t1,t2,v0,v1,v2) QUADTREE *qtptr; @@ -700,13 +439,13 @@ 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 (t0,t1,t2) overlaps cell triangle (v0,v1,v2) */ - test = spherical_tri_intersect(t0,t1,t2,v0,v1,v2); + test = stri_intersect(t0,t1,t2,v0,v1,v2); /* If triangles do not overlap: done */ if(!test) @@ -732,8 +471,7 @@ FVECT v0,v1,v2; /* 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"); @@ -747,6 +485,818 @@ FVECT v0,v1,v2; } 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]); + } +#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 + { + 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); + +} + + + + + +