/* Copyright (c) 1998 Silicon Graphics, Inc. */ #ifndef lint static char SCCSid[] = "$SunId$ SGI"; #endif /* * sm_qtree.c: adapted from octree.c from radiance code */ /* * octree.c - routines dealing with octrees and cubes. * * 7/28/85 */ #include "standard.h" #include "sm_geom.h" #include "sm_qtree.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 */ { register QUADTREE freet; if ((freet = quad_free_list) != EMPTY) { quad_free_list = QT_NTH_CHILD(freet, 0); return(freet); } freet = treetop; if (QT_BLOCK_INDEX(freet) == 0) { if (QT_BLOCK(freet) >= QT_MAX_BLK) return(EMPTY); 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)); 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; { register int i; if (!QT_IS_TREE(qt)) { qtfreeleaf(qt); return; } for (i = 0; i < 4; i++) qtFree(QT_NTH_CHILD(qt, i)); QT_NTH_CHILD(qt, 0) = quad_free_list; quad_free_list = qt; } qtDone() /* free EVERYTHING */ { register int i; qtfreeleaves(); for (i = 0; i < QT_MAX_BLK; i++) { 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 *qtLocate_leaf(qtptr,bcoord,t0,t1,t2) QUADTREE *qtptr; double bcoord[3]; FVECT t0,t1,t2; { 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 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)); } else return(qtptr); } QUADTREE *qtRoot_point_locate(qtptr,v0,v1,v2,pt,t0,t1,t2) QUADTREE *qtptr; FVECT v0,v1,v2; FVECT pt; FVECT t0,t1,t2; { int d; int i,x,y; QUADTREE *child; 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 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); /* 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); 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) { qtSubdivide_tri(v0,v1,v2,a,b,c); qtNth_child_tri(v0,v1,v2,a,b,c,i,t0,t1,t2); } return(qtLocate_leaf(child,bcoord,t0,t1,t2)); } else { if(t0) { VCOPY(t0,v0); VCOPY(t1,v1); VCOPY(t2,v2); } return(qtptr); } } 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; { 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 quadtree pointed to by "qtptr" with cell vertices "t1,t2,t3" that it overlaps (vertex and edge adjacencies do not count as overlapping). If the addition of the triangle causes the cell to go over threshold- the cell is split- and the triangle must be recursively inserted into the new child cells: it is assumed that "v1,v2,v3" are normalized */ int qtRoot_add_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 test; int found; test = stri_intersect(q0,q1,q2,t0,t1,t2); if(!test) return(FALSE); 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(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); else { /* If the set is too large: subdivide */ optr = qtqueryset(*qtptr); 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); 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) 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)) { 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); } } int qtRemove_tri(qtptr,id,t0,t1,t2,v0,v1,v2) QUADTREE *qtptr; int id; FVECT t0,t1,t2; FVECT v0,v1,v2; { 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)) { 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 { 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); } } } 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); }