--- ray/src/hd/sm_qtree.c 1998/08/19 17:45:24 3.1 +++ ray/src/hd/sm_qtree.c 1998/11/11 12:05:39 3.8 @@ -14,16 +14,23 @@ static char SCCSid[] = "$SunId$ SGI"; */ #include "standard.h" - +#include "sm_flag.h" #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]; +extern int Pick_q[500]; +#endif +int Incnt=0; QUADTREE qtAlloc() /* allocate a quadtree */ @@ -41,14 +48,31 @@ qtAlloc() /* allocate a quadtree */ if (QT_BLOCK(freet) >= QT_MAX_BLK) return(EMPTY); if ((quad_block[QT_BLOCK(freet)] = (QUADTREE *)malloc( - (unsigned)QT_BLOCK_SIZE*4*sizeof(QUADTREE))) == NULL) - return(EMPTY); + QT_BLOCK_SIZE*4*sizeof(QUADTREE))) == NULL) + error(SYSTEM,"qtAlloc(): Unable to allocate memory\n"); + + /* Realloc the per/node flags */ + quad_flag = (int4 *)realloc((char *)quad_flag, + (QT_BLOCK(freet)+1)*((QT_BLOCK_SIZE+7)>>3)); + if (quad_flag == NULL) + error(SYSTEM,"qtAlloc(): Unable to allocate memory\n"); } treetop += 4; return(freet); } +qtClearAllFlags() /* clear all quadtree branch flags */ +{ + if (!treetop) + return; + + /* Clear the node flags*/ + bzero((char *)quad_flag, (QT_BLOCK(treetop-4)+1)*((QT_BLOCK_SIZE+7)>>3)); + /* Clear set flags */ + qtclearsetflags(); +} + qtFree(qt) /* free a quadtree */ register QUADTREE qt; { @@ -69,234 +93,147 @@ qtFree(qt) /* free a quadtree */ qtDone() /* free EVERYTHING */ { register int i; - + + qtfreeleaves(); for (i = 0; i < QT_MAX_BLK; i++) { - free((char *)quad_block[i], - (unsigned)QT_BLOCK_SIZE*4*sizeof(QUADTREE)); + if (quad_block[i] == NULL) + break; + free((char *)quad_block[i]); quad_block[i] = NULL; } + /* Free the flags */ + 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(qt,bcoord) +QUADTREE qt; +BCOORD bcoord[3]; { - register int i; - register QUADTREE qres; + int i; - 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); + if(QT_IS_TREE(qt)) + { + i = baryi_child(bcoord); + + return(qtLocate_leaf(QT_NTH_CHILD(qt,i),bcoord)); + } + else + return(qt); } +/* + Return the quadtree node containing pt. It is assumed that pt is in + the root node qt with ws vertices q0,q1,q2 and plane equation peq. +*/ QUADTREE -qtPoint_locate(qtptr,v1,v2,v3,pt,type,which,p0,p1,p2) -QUADTREE *qtptr; -FVECT v1,v2,v3; +qtRoot_point_locate(qt,q0,q1,q2,peq,pt) +QUADTREE qt; +FVECT q0,q1,q2; +FPEQ peq; FVECT pt; -char *type,*which; -FVECT p0,p1,p2; { - char d,w; - int i; - QUADTREE *child; - QUADTREE qt; - FVECT a,b,c; - FVECT t1,t2,t3; + int i,x,y; + FVECT i_pt; + double bcoord[3]; + BCOORD bcoordi[3]; - /* Determine if point lies within pyramid (and therefore - inside a spherical quadtree cell):GT_INTERIOR, on one of the - pyramid sides (and on cell edge):GT_EDGE(1,2 or 3), - or on pyramid vertex (and on cell vertex):GT_VERTEX(1,2, or 3). - For each triangle edge: compare the - point against the plane formed by the edge and the view center - */ - d = test_single_point_against_spherical_tri(v1,v2,v3,pt,&w); - - /* Not in this triangle */ - if(!d) - { - if(which) - *which = 0; - return(EMPTY); - } - - /* Will return lowest level triangle containing point: It the + /* 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)) + if(QT_IS_TREE(qt)) { - qtSubdivide_tri(v1,v2,v3,a,b,c); - child = QT_NTH_CHILD_PTR(*qtptr,0); - if(!QT_IS_EMPTY(*child)) - { - qt = qtPoint_locate(child,v1,a,c,pt,type,which,p0,p1,p2); - if(!QT_IS_EMPTY(qt)) - return(qt); - } - 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); - } + /* Find the intersection point */ + intersect_vector_plane(pt,peq,NULL,i_pt); + + x = FP_X(peq); + y = FP_Y(peq); + /* Calculate barycentric coordinates of i_pt */ + bary2d(q0[x],q0[y],q1[x],q1[y],q2[x],q2[y],i_pt[x],i_pt[y],bcoord); + + /* convert to integer coordinate */ + convert_dtol(bcoord,bcoordi); + + i = baryi_child(bcoordi); + + return(qtLocate_leaf(QT_NTH_CHILD(qt,i),bcoordi)); } 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); + return(qt); } -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); - } - } - return(EMPTY); -} -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; +/* for triangle v0-v1-v2- returns a,b,c: children are: - node = qtSubdivide(&(QT_NTH_CHILD(qt,n))); - - return(node); -} - -/* for triangle v1-v2-v3- 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; -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); -} -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); - break; - case 1: - break; - case 2: - VCOPY(r3,r2); - VCOPY(r2,v2); - break; - case 3: - VCOPY(r1,r3); - VCOPY(r3,v3); - break; + + if(!a) + { + /* Caution: r's must not be equal to v's:will be incorrect */ + switch(i){ + case 0: + VCOPY(r0,v0); + EDGE_MIDPOINT_VEC3(r1,v0,v1); + EDGE_MIDPOINT_VEC3(r2,v2,v0); + break; + case 1: + EDGE_MIDPOINT_VEC3(r0,v0,v1); + VCOPY(r1,v1); + EDGE_MIDPOINT_VEC3(r2,v1,v2); + break; + case 2: + EDGE_MIDPOINT_VEC3(r0,v2,v0); + EDGE_MIDPOINT_VEC3(r1,v1,v2); + VCOPY(r2,v2); + break; + case 3: + EDGE_MIDPOINT_VEC3(r0,v1,v2); + EDGE_MIDPOINT_VEC3(r1,v2,v0); + EDGE_MIDPOINT_VEC3(r2,v0,v1); + break; + } } + else + { + switch(i){ + case 0: + 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(r0,c); VCOPY(r1,b); VCOPY(r2,v2); + break; + case 3: + VCOPY(r0,b); VCOPY(r1,c); VCOPY(r2,a); + break; + } + } } /* Add triangle "id" to all leaf level cells that are children of @@ -307,226 +244,148 @@ threshold- the cell is split- and the triangle must be into the new child cells: it is assumed that "v1,v2,v3" are normalized */ -int -qtAdd_tri(qtptr,id,t1,t2,t3,v1,v2,v3,n) -QUADTREE *qtptr; +QUADTREE +qtRoot_add_tri(qt,q0,q1,q2,t0,t1,t2,id,n) +QUADTREE qt; +FVECT q0,q1,q2; +FVECT t0,t1,t2; +int id,n; +{ + if(stri_intersect(q0,q1,q2,t0,t1,t2)) + qt = qtAdd_tri(qt,q0,q1,q2,t0,t1,t2,id,n); + + return(qt); +} + +QUADTREE +qtRoot_remove_tri(qt,q0,q1,q2,t0,t1,t2,id,n) +QUADTREE qt; +FVECT q0,q1,q2; +FVECT t0,t1,t2; +int id,n; +{ + + if(stri_intersect(q0,q1,q2,t0,t1,t2)) + qt = qtRemove_tri(qt,q0,q1,q2,t0,t1,t2,id,n); + return(qt); +} + + +QUADTREE +qtAdd_tri(qt,q0,q1,q2,t0,t1,t2,id,n) +QUADTREE qt; +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 found; - FVECT r1,r2,r3; + OBJECT tset[QT_MAXSET+1],*optr,*tptr; + FVECT r0,r1,r2; + int i; - /* 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 */ - if(!test) - return(FALSE); - found = 0; - /* if this is tree: recurse */ - if(QT_IS_TREE(*qtptr)) + if(QT_IS_TREE(qt)) { + QT_SET_FLAG(qt); 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); + qtSubdivide_tri(q0,q1,q2,a,b,c); -#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 + if(stri_intersect(t0,t1,t2,q0,a,c)) + QT_NTH_CHILD(qt,0) = qtAdd_tri(QT_NTH_CHILD(qt,0),q0,a,c,t0,t1,t2,id,n); + if(stri_intersect(t0,t1,t2,a,q1,b)) + QT_NTH_CHILD(qt,1) = qtAdd_tri(QT_NTH_CHILD(qt,1),a,q1,b,t0,t1,t2,id,n); + if(stri_intersect(t0,t1,t2,c,b,q2)) + QT_NTH_CHILD(qt,2) = qtAdd_tri(QT_NTH_CHILD(qt,2),c,b,q2,t0,t1,t2,id,n); + if(stri_intersect(t0,t1,t2,b,c,a)) + QT_NTH_CHILD(qt,3) = qtAdd_tri(QT_NTH_CHILD(qt,3),b,c,a,t0,t1,t2,id,n); + return(qt); } 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; - */ - } + if(QT_IS_EMPTY(qt)) + qt = qtaddelem(qt,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); - */ - } - else - { - if (n < QT_MAX_LEVELS) - { - /* If set size exceeds threshold: subdivide cell and - reinsert set tris into cell - */ - n++; - qtfreeleaf(*qtptr); - qtSubdivide(qtptr); - found = qtAdd_tri(qtptr,id,t1,t2,t3,v1,v2,v3,n); -#if 0 - if(!found) - { -#ifdef TEST_DRIVER - HANDLE_ERROR("qtAdd_tri():Found in parent but not children\n"); -#else - eputs("qtAdd_tri():Found in parent but not children\n"); -#endif - } -#endif - for(optr = &(os[1]),i = QT_SET_CNT(os); i > 0; i--) - { - id = QT_SET_NEXT_ELEM(optr); - qtTri_verts_from_id(id,r1,r2,r3); - found=qtAdd_tri(qtptr,id,r1,r2,r3,v1,v2,v3,n); -#ifdef DEBUG - if(!found) - eputs("qtAdd_tri():Reinsert-in parent but not children\n"); -#endif - } - } + optr = qtqueryset(qt); + + if(QT_SET_CNT(optr) < QT_SET_THRESHOLD) + qt = qtaddelem(qt,id); + else + { + if (n < QT_MAX_LEVELS) + { + /* If set size exceeds threshold: subdivide cell and + reinsert set tris into cell + */ + /* CAUTION:If QT_SET_THRESHOLD << QT_MAXSET, and dont add + more than a few triangles before expanding: then are safe here + otherwise must check to make sure set size is < MAXSET, + or qtgetset could overflow os. + */ + tptr = qtqueryset(qt); + if(QT_SET_CNT(tptr) > QT_MAXSET) + tptr = (OBJECT *)malloc((QT_SET_CNT(tptr)+1)*sizeof(OBJECT)); else - if(QT_SET_CNT(os) < QT_MAX_SET) - { - *qtptr = qtaddelem(*qtptr,id); -#if 0 - { - int k; - qtgetset(os,*qtptr); - printf("\n%d:\n",os[0]); - for(k=1; k <= os[0];k++) - printf("%d ",os[k]); - printf("\n"); - } -#endif - /* - insertelem(os,id); - *qtptr = fullnode(os); - */ - } - else - { -#ifdef DEBUG - eputs("qtAdd_tri():two many levels\n"); -#endif - return(FALSE); - } - } - } - } - return(TRUE); -} + tptr = tset; + if(!tptr) + goto memerr; + qtgetset(tptr,qt); + n++; + qtfreeleaf(qt); + qtSubdivide(qt); + qt = qtAdd_tri(qt,q0,q1,q2,t0,t1,t2,id,n); -int -qtApply_to_tri_cells(qtptr,t1,t2,t3,v1,v2,v3,func,arg) -QUADTREE *qtptr; -FVECT t1,t2,t3; -FVECT v1,v2,v3; -int (*func)(); -char *arg; -{ - char test; - FVECT a,b,c; - - /* test if triangle (t1,t2,t3) overlaps cell triangle (v1,v2,v3) */ - test = spherical_tri_intersect(t1,t2,t3,v1,v2,v3); - - /* If triangles do not overlap: done */ - if(!test) - return(FALSE); - - /* if this is tree: recurse */ - 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); + for(optr = QT_SET_PTR(tptr),i = QT_SET_CNT(tptr); i > 0; i--) + { + id = QT_SET_NEXT_ELEM(optr); + if(!qtTri_from_id(id,r0,r1,r2)) + continue; + qt = qtAdd_tri(qt,q0,q1,q2,r0,r1,r2,id,n); + } + if(tptr != tset) + free(tptr); + } + else + qt = qtaddelem(qt,id); + } + } } - else - return(func(qtptr,arg)); + return(qt); +memerr: + error(SYSTEM,"qtAdd_tri():Unable to allocate memory"); } -int -qtRemove_tri(qtptr,id,t1,t2,t3,v1,v2,v3) -QUADTREE *qtptr; +QUADTREE +qtRemove_tri(qt,id,q0,q1,q2,t0,t1,t2) +QUADTREE qt; int id; -FVECT t1,t2,t3; -FVECT v1,v2,v3; +FVECT q0,q1,q2; +FVECT t0,t1,t2; { - - char test; - int i; FVECT a,b,c; - OBJECT os[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) */ + if(!stri_intersect(t0,t1,t2,q0,q1,q2)) + return(qt); - /* If triangles do not overlap: done */ - if(!test) - return(FALSE); - /* if this is tree: recurse */ - if(QT_IS_TREE(*qtptr)) + if(QT_IS_TREE(qt)) { - 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(q0,q1,q2,a,b,c); + QT_NTH_CHILD(qt,0) = qtRemove_tri(QT_NTH_CHILD(qt,0),id,t0,t1,t2,q0,a,c); + QT_NTH_CHILD(qt,1) = qtRemove_tri(QT_NTH_CHILD(qt,1),id,t0,t1,t2,a,q1,b); + QT_NTH_CHILD(qt,2) = qtRemove_tri(QT_NTH_CHILD(qt,2),id,t0,t1,t2,c,b,q2); + QT_NTH_CHILD(qt,3) = qtRemove_tri(QT_NTH_CHILD(qt,3),id,t0,t1,t2,b,c,a); + return(qt); } else { - if(QT_IS_EMPTY(*qtptr)) + if(QT_IS_EMPTY(qt)) { #ifdef DEBUG eputs("qtRemove_tri(): triangle not found\n"); @@ -535,31 +394,453 @@ FVECT v1,v2,v3; /* remove id from set */ else { - qtgetset(os,*qtptr); - if(!inset(os,id)) + if(!qtinset(qt,id)) { #ifdef DEBUG eputs("qtRemove_tri(): tri not in set\n"); #endif } else - { - *qtptr = qtdelelem(*qtptr,id); -#if 0 - { - int k; - if(!QT_IS_EMPTY(*qtptr)) - {qtgetset(os,*qtptr); - printf("\n%d:\n",os[0]); - for(k=1; k <= os[0];k++) - printf("%d ",os[k]); - printf("\n"); - } + qt = qtdelelem(qt,id); + } + } + return(qt); +} - } -#endif + +QUADTREE +qtVisit_tri_interior(qt,q0,q1,q2,t0,t1,t2,n0,n1,n2,n,func,f,argptr) + QUADTREE qt; + FVECT q0,q1,q2; + FVECT t0,t1,t2; + FVECT n0,n1,n2; + int n; + int (*func)(),*f; + int *argptr; +{ + FVECT a,b,c; + + /* If qt Flag set, or qt vertices interior to t0t1t2-descend */ +tree_modified: + + if(QT_IS_TREE(qt)) + { + if(QT_IS_FLAG(qt) || point_in_stri_n(n0,n1,n2,q0)) + { + QT_SET_FLAG(qt); + qtSubdivide_tri(q0,q1,q2,a,b,c); + /* descend to children */ + QT_NTH_CHILD(qt,0) = qtVisit_tri_interior(QT_NTH_CHILD(qt,0), + q0,a,c,t0,t1,t2,n0,n1,n2,n+1,func,f,argptr); + QT_NTH_CHILD(qt,1) = qtVisit_tri_interior(QT_NTH_CHILD(qt,1), + a,q1,b,t0,t1,t2,n0,n1,n2,n+1,func,f,argptr); + QT_NTH_CHILD(qt,2) = qtVisit_tri_interior(QT_NTH_CHILD(qt,2), + c,b,q2,t0,t1,t2,n0,n1,n2,n+1,func,f,argptr); + QT_NTH_CHILD(qt,3) = qtVisit_tri_interior(QT_NTH_CHILD(qt,3), + b,c,a,t0,t1,t2,n0,n1,n2,n+1,func,f,argptr); } + } + else + if((!QT_IS_EMPTY(qt) && QT_LEAF_IS_FLAG(qt)) || + point_in_stri_n(n0,n1,n2,q0)) + { + func(&qt,f,argptr,q0,q1,q2,t0,t1,t2,n); + if(QT_FLAG_IS_MODIFIED(*f)) + { + QT_SET_FLAG(qt); + goto tree_modified; + } + if(QT_IS_LEAF(qt)) + QT_LEAF_SET_FLAG(qt); + else + if(QT_IS_TREE(qt)) + QT_SET_FLAG(qt); } + return(qt); +} + + + +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; + *tptr = 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) + { + bc = b[0]<> sfactor; + + if(t_g >= t[w]) + { + if(w == 2) + { + QT_FLAG_SET_DONE(*f); + return(qt); + } + /* 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 + */ + l = t[w] << sfactor; + /* NOTE: Change divides to Shift and multiply by sign*/ + b[0] += (l*db0)/MAXBCOORD; + b[1] += (l*db1)/MAXBCOORD; + b[2] += (l*db2)/MAXBCOORD; + w++; + db0 = db[w][0]; db1 = db[w][1]; db2 = db[w][2]; + if(sign) + { db0 *= -1;db1 *= -1; db2 *= -1;} + } + else + { + /* Caution: (t_i*db) must occur before divide by MAXBCOORD + since t_i will always be < MAXBCOORD*/ + /* NOTE: Change divides to Shift and by sign*/ + b[0] += (t_i *db0) / MAXBCOORD; + b[1] += (t_i *db1) / MAXBCOORD; + b[2] += (t_i *db2) / MAXBCOORD; + + t[w] -= t_g; + *wptr = w; + *nextptr = nbr; + return(qt); + } + } + } +} + + +QUADTREE +qtRoot_visit_tri_edges(qt,q0,q1,q2,peq,tri,i_pt,wptr,nextptr,func,f,argptr) + QUADTREE qt; + FVECT q0,q1,q2; + FPEQ peq; + FVECT tri[3],i_pt; + int *wptr,*nextptr; + int (*func)(); + int *f,*argptr; +{ + int x,y,z,w,i,j,first; + QUADTREE child; + FVECT c,d,v[3]; + double b[4][3],db[3][3],et[3],exit_pt; + BCOORD bi[3]; + TINT t[3]; + BDIR dbi[3][3]; + + first =0; + w = *wptr; + if(w==-1) + { + first = 1; + *wptr = w = 0; + } + /* Project the origin onto the root node plane */ + + t[0] = t[1] = t[2] = 0; + /* Find the intersection point of the origin */ + /* map to 2d by dropping maximum magnitude component of normal */ + + x = FP_X(peq); + y = FP_Y(peq); + z = FP_Z(peq); + /* Calculate barycentric coordinates for current vertex */ + if(!first) + bary2d(q0[x],q0[y],q1[x],q1[y],q2[x],q2[y],i_pt[x],i_pt[y],b[3]); + else + /* Just starting: b[0] is the origin point: guaranteed to be valid b*/ + { + intersect_vector_plane(tri[0],peq,&(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],peq,&(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] = HUGET; + } + 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))||(b[j][0] <-FTINY) || + (b[j][1]<-FTINY) ||(b[j][2]<-FTINY)) + 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(!first || i != 0) + { + intersect_vector_plane(tri[i],peq,&(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))||(b[i][0] <-FTINY) || + (b[i][1]<-FTINY) ||(b[i][2]<-FTINY)) + { + t[j] = HUGET; + break; + } + else + t[j] = MAXT; + } + } + } + } + bary_dtol(b[3],db,bi,dbi,t,w); + + /* trace the ray starting with this node */ + qt = qtVisit_tri_edges(qt,bi,dbi[w][0],dbi[w][1],dbi[w][2], + dbi,wptr,nextptr,t,0,0,func,f,argptr); + if(!QT_FLAG_IS_DONE(*f)) + { + 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] = (-FP_N(peq)[x]*i_pt[x] - FP_N(peq)[y]*i_pt[y]-FP_D(peq))/FP_N(peq)[z]; + } + return(qt); + +} + + +QUADTREE +qtRoot_trace_ray(qt,q0,q1,q2,peq,orig,dir,nextptr,func,f,argptr) + QUADTREE qt; + FVECT q0,q1,q2; + FPEQ peq; + FVECT orig,dir; + int *nextptr; + int (*func)(); + int *f,*argptr; +{ + int x,y,z,nbr,w,i; + QUADTREE child; + FVECT v,i_pt; + double b[2][3],db[3],et[2],d,br[3]; + BCOORD bi[3]; + TINT t[3]; + BDIR dbi[3][3]; + + /* Project the origin onto the root node plane */ + t[0] = t[1] = t[2] = 0; + + VADD(v,orig,dir); + /* Find the intersection point of the origin */ + /* map to 2d by dropping maximum magnitude component of normal */ + x = FP_X(peq); + y = FP_Y(peq); + z = FP_Z(peq); + + /* Calculate barycentric coordinates for current vertex */ + intersect_vector_plane(orig,peq,&(et[0]),i_pt); + bary2d(q0[x],q0[y],q1[x],q1[y],q2[x],q2[y],i_pt[x],i_pt[y],b[0]); + + intersect_vector_plane(v,peq,&(et[1]),i_pt); + bary2d(q0[x],q0[y],q1[x],q1[y],q2[x],q2[y],i_pt[x],i_pt[y],b[1]); + if(et[1] < 0.0) + VSUB(db,b[0],b[1]); + else + VSUB(db,b[1],b[0]); + t[0] = HUGET; + convert_dtol(b[0],bi); + if(et[1]<0.0 ||(fabs(b[1][0])>(FTINY+1.0))||(fabs(b[1][1])>(FTINY+1.0)) || + (fabs(b[1][2])>(FTINY+1.0))||(b[1][0] <-FTINY) || + (b[1][1]<-FTINY) ||(b[1][2]<-FTINY)) + { + max_index(db,&d); + for(i=0; i< 3; i++) + dbi[0][i] = (BDIR)(db[i]/d*MAXBDIR); + } + else + for(i=0; i< 3; i++) + dbi[0][i] = (BDIR)(db[i]*MAXBDIR); + w=0; + /* trace the ray starting with this node */ + qt = qtVisit_tri_edges(qt,bi,dbi[0][0],dbi[0][1],dbi[0][2], dbi,&w, + nextptr,t,0,0,func,f,argptr); + if(!QT_FLAG_IS_DONE(*f)) + { + br[0] = (double)bi[0]/(double)MAXBCOORD; + br[1] = (double)bi[1]/(double)MAXBCOORD; + br[2] = (double)bi[2]/(double)MAXBCOORD; + orig[x] = br[0]*q0[x] + br[1]*q1[x] + br[2]*q2[x]; + orig[y] = br[0]*q0[y] + br[1]*q1[y] + br[2]*q2[y]; + orig[z]=(-FP_N(peq)[x]*orig[x] - + FP_N(peq)[y]*orig[y]-FP_D(peq))/FP_N(peq)[z]; + } + return(qt); + +} + + + + + + + + + + + + +