--- ray/src/hd/sm_qtree.c 1998/09/16 18:16:29 3.6 +++ ray/src/hd/sm_qtree.c 1998/10/06 18:18:54 3.7 @@ -14,7 +14,7 @@ static char SCCSid[] = "$SunId$ SGI"; */ #include "standard.h" - +#include "sm_flag.h" #include "sm_geom.h" #include "sm_qtree.h" @@ -27,7 +27,10 @@ int4 *quad_flag= NULL; extern FVECT Pick_v0[500],Pick_v1[500],Pick_v2[500]; extern int Pick_cnt,Pick_tri,Pick_samp; extern FVECT Pick_point[500]; + + #endif +int Incnt=0; QUADTREE qtAlloc() /* allocate a quadtree */ @@ -46,12 +49,13 @@ qtAlloc() /* allocate a quadtree */ return(EMPTY); if ((quad_block[QT_BLOCK(freet)] = (QUADTREE *)malloc( QT_BLOCK_SIZE*4*sizeof(QUADTREE))) == NULL) - return(EMPTY); + error(SYSTEM,"qtAlloc(): Unable to allocate memory\n"); + /* Realloc the per/node flags */ quad_flag = (int4 *)realloc((char *)quad_flag, - (QT_BLOCK(freet)+1)*(QT_BLOCK_SIZE/8)); + (QT_BLOCK(freet)+1)*((QT_BLOCK_SIZE+7)>>3)); if (quad_flag == NULL) - return(EMPTY); + error(SYSTEM,"qtAlloc(): Unable to allocate memory\n"); } treetop += 4; return(freet); @@ -60,11 +64,15 @@ qtAlloc() /* allocate a quadtree */ qtClearAllFlags() /* clear all quadtree branch flags */ { - if (!treetop) return; - bzero((char *)quad_flag, (QT_BLOCK(treetop-4)+1)*(QT_BLOCK_SIZE/8)); + if (!treetop) + return; + + /* Clear the node flags*/ + bzero((char *)quad_flag, (QT_BLOCK(treetop-4)+1)*((QT_BLOCK_SIZE+7)>>3)); + /* Clear set flags */ + qtclearsetflags(); } - qtFree(qt) /* free a quadtree */ register QUADTREE qt; { @@ -85,7 +93,7 @@ qtFree(qt) /* free a quadtree */ qtDone() /* free EVERYTHING */ { register int i; - + qtfreeleaves(); for (i = 0; i < QT_MAX_BLK; i++) { @@ -94,6 +102,7 @@ qtDone() /* free EVERYTHING */ free((char *)quad_block[i]); quad_block[i] = NULL; } + /* Free the flags */ if (i) free((char *)quad_flag); quad_flag = NULL; quad_free_list = EMPTY; @@ -101,145 +110,67 @@ qtDone() /* free EVERYTHING */ } QUADTREE -*qtLocate_leaf(qtptr,bcoord,t0,t1,t2) -QUADTREE *qtptr; -double bcoord[3]; -FVECT t0,t1,t2; +qtLocate_leaf(qt,bcoord) +QUADTREE qt; +BCOORD bcoord[3]; { int i; - QUADTREE *child; - FVECT a,b,c; - if(QT_IS_TREE(*qtptr)) - { - i = bary_child(bcoord); -#ifdef DEBUG_TEST_DRIVER - qtSubdivide_tri(Pick_v0[Pick_cnt-1],Pick_v1[Pick_cnt-1], - Pick_v2[Pick_cnt-1],a,b,c); - qtNth_child_tri(Pick_v0[Pick_cnt-1],Pick_v1[Pick_cnt-1], - Pick_v2[Pick_cnt-1],a,b,c,i, - Pick_v0[Pick_cnt],Pick_v1[Pick_cnt], - Pick_v2[Pick_cnt]); - Pick_cnt++; -#endif + if(QT_IS_TREE(qt)) + { + i = baryi_child(bcoord); - 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); + 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 -*qtRoot_point_locate(qtptr,v0,v1,v2,pt,t0,t1,t2) -QUADTREE *qtptr; -FVECT v0,v1,v2; +qtRoot_point_locate(qt,q0,q1,q2,peq,pt) +QUADTREE qt; +FVECT q0,q1,q2; +FPEQ peq; 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]; + 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 = point_in_stri(v0,v1,v2,pt); - - - /* Not in this triangle */ - if(!d) - return(NULL); - - /* 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)) { /* Find the intersection point */ - tri_plane_equation(v0,v1,v2,n,&pd,FALSE); - intersect_vector_plane(pt,n,pd,NULL,i_pt); + intersect_vector_plane(pt,peq,NULL,i_pt); - i = max_index(n,NULL); - x = (i+1)%3; - y = (i+2)%3; + x = FP_X(peq); + y = FP_Y(peq); /* 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); + 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 = 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)); + i = baryi_child(bcoordi); + + return(qtLocate_leaf(QT_NTH_CHILD(qt,i),bcoordi)); } else - { - if(t0) - { - VCOPY(t0,v0); - VCOPY(t1,v1); - VCOPY(t2,v2); - } - return(qtptr); - } + return(qt); } -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 @@ -252,14 +183,6 @@ int n; 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; @@ -268,20 +191,49 @@ 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; + 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 @@ -292,177 +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 -qtRoot_add_tri(qtptr,q0,q1,q2,t0,t1,t2,id,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; -int n; +int id,n; { - int test; - int found; + if(stri_intersect(q0,q1,q2,t0,t1,t2)) + qt = qtAdd_tri(qt,q0,q1,q2,t0,t1,t2,id,n); - 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); + return(qt); } -int -qtAdd_tri(qtptr,q0,q1,q2,t0,t1,t2,id,n) -QUADTREE *qtptr; +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; int n; { - int i,index,test,found; FVECT a,b,c; - OBJECT os[QT_MAXSET+1],*optr; - QUADTREE qt; + OBJECT tset[QT_MAXSET+1],*optr,*tptr; FVECT r0,r1,r2; + int i; - found = 0; /* if this is tree: recurse */ - if(QT_IS_TREE(*qtptr)) + if(QT_IS_TREE(qt)) { + QT_SET_FLAG(qt); 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); + + 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(QT_IS_EMPTY(qt)) + qt = qtaddelem(qt,id); else { /* If the set is too large: subdivide */ - optr = qtqueryset(*qtptr); + optr = qtqueryset(qt); 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); + 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 + tptr = tset; + if(!tptr) + goto memerr; - n++; - qtfreeleaf(*qtptr); - qtSubdivide(qtptr); - found = qtAdd_tri(qtptr,q0,q1,q2,t0,t1,t2,id,n); + qtgetset(tptr,qt); + n++; + qtfreeleaf(qt); + qtSubdivide(qt); + qt = qtAdd_tri(qt,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,r0,r1,r2,NULL,NULL,NULL,NULL,NULL,NULL); - found=qtAdd_tri(qtptr,q0,q1,q2,r0,r1,r2,id,n); -#ifdef DEBUG - if(!found) - eputs("qtAdd_tri():Reinsert-in parent but not children\n"); -#endif - } + 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 - if(QT_SET_CNT(optr) < QT_MAXSET) - *qtptr = qtaddelem(*qtptr,id); - else - { -#ifdef DEBUG - eputs("qtAdd_tri():two many levels\n"); -#endif - return(FALSE); - } - } + qt = qtaddelem(qt,id); + } } } - return(TRUE); + return(qt); +memerr: + error(SYSTEM,"qtAdd_tri():Unable to allocate memory"); } -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; +QUADTREE +qtRemove_tri(qt,id,q0,q1,q2,t0,t1,t2) +QUADTREE qt; int id; +FVECT q0,q1,q2; 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(!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(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); + 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"); @@ -471,488 +394,73 @@ FVECT v0,v1,v2; /* remove id from set */ else { - if(!qtinset(*qtptr,id)) + if(!qtinset(qt,id)) { #ifdef DEBUG eputs("qtRemove_tri(): tri not in set\n"); #endif } else - { - *qtptr = qtdelelem(*qtptr,id); - } + qt = qtdelelem(qt,id); } } - return(TRUE); + return(qt); } -int -move_to_nbr(b,db0,db1,db2,tptr) -double b[3],db0,db1,db2; -double *tptr; -{ - double t,dt; - int nbr; - - nbr = -1; - /* Advance to next node */ - if(!ZERO(db0) && db0 < 0.0) - { - t = -b[0]/db0; - nbr = 0; - } - 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_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; +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 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,NULL); - x = (i+1)%3; - y = (i+2)%3; - /* Calculate barycentric coordinates of orig */ - bary2d(q0[x],q0[y],q1[x],q1[y],q2[x],q2[y],i_pt[x],i_pt[y],b); - /* Calculate barycentric coordinates of dir */ - bary2d(q0[x],q0[y],q1[x],q1[y],q2[x],q2[y],c[x],c[y],db); - if(t < 0.0) - VSUB(db,b,db); - 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); - -} - -qtVisit_tri_interior(qtptr,q0,q1,q2,t0,t1,t2,n,func,arg1,arg2,arg3) - QUADTREE *qtptr; - FVECT q0,q1,q2; FVECT t0,t1,t2; + FVECT n0,n1,n2; int n; - int (*func)(); - int *arg1,arg2,*arg3; + int (*func)(),*f; + int *argptr; { - int i,found,test; - QUADTREE *child; - FVECT c0,c1,c2,a,b,c; - OBJECT os[QT_MAXSET+1],*optr; - int w; + FVECT a,b,c; /* If qt Flag set, or qt vertices interior to t0t1t2-descend */ tree_modified: - if(QT_IS_TREE(*qtptr)) + if(QT_IS_TREE(qt)) { - if(QT_IS_FLAG(*qtptr) || point_in_stri(t0,t1,t2,q0)) + if(QT_IS_FLAG(qt) || point_in_stri_n(n0,n1,n2,q0)) { - QT_SET_FLAG(*qtptr); + QT_SET_FLAG(qt); qtSubdivide_tri(q0,q1,q2,a,b,c); /* descend to children */ - for(i=0;i < 4; i++) - { - child = QT_NTH_CHILD_PTR(*qtptr,i); - qtNth_child_tri(q0,q1,q2,a,b,c,i,c0,c1,c2); - qtVisit_tri_interior(child,c0,c1,c2,t0,t1,t2,n+1, - func,arg1,arg2,arg3); - } + 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 - { - /* NOTE THIS IN TRI TEST Could be replaced by a flag */ - if(!QT_IS_EMPTY(*qtptr)) + if((!QT_IS_EMPTY(qt) && QT_LEAF_IS_FLAG(qt)) || + point_in_stri_n(n0,n1,n2,q0)) { - if(qtinset(*qtptr,arg2)) - if(func(qtptr,q0,q1,q2,t0,t1,t2,n,arg1,arg2,arg3)==QT_MODIFIED) - goto tree_modified; - else - return; - } - if(point_in_stri(t0,t1,t2,q0) ) - if(func(qtptr,q0,q1,q2,t0,t1,t2,n,arg1,arg2,arg3)==QT_MODIFIED) - goto tree_modified; - } -} - - - - - - -/* NOTE: SINCE DIR could be unit: then we could use integer math */ -int -qtVisit_tri_edges(qtptr,b,db0,db1,db2, - db,wptr,t,sign,sfactor,func,arg1,arg2,arg3) - QUADTREE *qtptr; - double b[3],db0,db1,db2,db[3][3]; - int *wptr; - double t[3]; - int sign; - double sfactor; - int (*func)(); - int *arg1,arg2,*arg3; -{ - 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_edges(child,b,db0,db1,db2, - db,wptr,t,sign, - sfactor*2.0,func,arg1,arg2,arg3); - else - /* If the center cell- must flip direction signs */ - nbr = qtVisit_tri_edges(child,b,-db0,-db1,-db2, - db,wptr,t,1-sign, - sfactor*2.0,func,arg1,arg2,arg3); - - 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,arg3) == QT_DONE) - return(QT_DONE); - - /* Advance to next node */ - w = *wptr; - while(1) + func(&qt,f,argptr,q0,q1,q2,t0,t1,t2,n); + if(QT_FLAG_IS_MODIFIED(*f)) { - 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_edges():negative t\n"); -#endif - if(t_g >= t[w]) - { - if(w == 2) - return(QT_DONE); - - b[0] += (t[w])*sfactor*db0; - b[1] += (t[w])*sfactor*db1; - b[2] += (t[w])*sfactor*db2; - w++; - db0 = db[w][0]; - db1 = db[w][1]; - db2 = db[w][2]; - if(sign) - { db0 *= -1.0;db1 *= -1.0; db2 *= -1.0;} - } - else - if(nbr != INVALID) - { - b[0] += t_l * db0; - b[1] += t_l * db1; - b[2] += t_l * db2; - - t[w] -= t_g; - *wptr = w; - return(nbr); - } - else - return(INVALID); + 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 -qtRoot_visit_tri_edges(qtptr,q0,q1,q2,tri,i_pt,wptr,func,arg1,arg2,arg3) - QUADTREE *qtptr; - FVECT q0,q1,q2; - FVECT tri[3],i_pt; - int *wptr; - int (*func)(); - int *arg1,arg2,*arg3; -{ - 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,NULL); - 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_edges(qtptr,b[3],db[w][0],db[w][1],db[w][2], - db,wptr,t,0,1.0,func,arg1,arg2,arg3); - if(nbr != INVALID && nbr != QT_DONE) - { - 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); - -} - int move_to_nbri(b,db0,db1,db2,tptr) BCOORD b[3]; @@ -964,10 +472,18 @@ TINT *tptr; 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 = MAXBCOORD*b[0]; + bc = b[0]<> sfactor; if(t_g >= t[w]) { if(w == 2) - return(QT_DONE); - - /* The edge fits in the cell- therefore the result of shifting db by - sfactor is guaranteed to be less than MAXBCOORD - */ + { + 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 - */ - b[0] = b[0] + (t[w]*db0*(1 << sfactor))/MAXBCOORD; - b[1] = b[1] + (t[w]*db1*(1 << sfactor))/MAXBCOORD; - b[2] = b[2] + (t[w]*db2*(1 << sfactor))/MAXBCOORD; + */ + 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;} + { db0 *= -1;db1 *= -1; db2 *= -1;} } - else - if(nbr != INVALID) - { + else + { /* Caution: (t_i*db) must occur before divide by MAXBCOORD - since t_i will always be < MAXBCOORD - */ - b[0] = b[0] + (t_i *db0) / MAXBCOORD; - b[1] = b[1] + (t_i *db1) / MAXBCOORD; - b[2] = b[2] + (t_i *db2) / MAXBCOORD; - + 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; - return(nbr); + *nextptr = nbr; + return(qt); } - else - return(INVALID); } } } -int -qtRoot_visit_tri_edgesi(qtptr,q0,q1,q2,tri,i_pt,wptr,func,arg1,arg2,arg3) - QUADTREE *qtptr; +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; + int *wptr,*nextptr; int (*func)(); - int *arg1,arg2,*arg3; + int *f,*argptr; { - int x,y,z,nbr,w,i,j; - QUADTREE *child; - FVECT n,c,d,v[3]; - double pd,b[4][3],db[3][3],et[3],exit_pt; + 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 */ - tri_plane_equation(q0,q1,q2,n,&pd,FALSE); /* map to 2d by dropping maximum magnitude component of normal */ - z = max_index(n,NULL); - x = (z+1)%3; - y = (z+2)%3; + + x = FP_X(peq); + y = FP_Y(peq); + z = FP_Z(peq); /* Calculate barycentric coordinates for current vertex */ - if(w != -1) - { + if(!first) 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]); + 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],n,pd,&(et[j]),v[j]); + 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) { @@ -1188,8 +699,9 @@ qtRoot_visit_tri_edgesi(qtptr,q0,q1,q2,tri,i_pt,wptr,f VSUB(db[w],b[j],b[3]); /* Check if the point is out side of the triangle: if so t[w] =HUGET */ if((fabs(b[j][0])>(FTINY+1.0)) ||(fabs(b[j][1])>(FTINY+1.0)) || - (fabs(b[j][2])>(FTINY+1.0))) - t[w] = HUGET; + (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 @@ -1197,58 +709,124 @@ qtRoot_visit_tri_edgesi(qtptr,q0,q1,q2,tri,i_pt,wptr,f */ t[w] = MAXT; if(j != 0) - for(;j < 3;j++) - { - i = (j+1)%3; - if(i!= w) - { - intersect_vector_plane(tri[i],n,pd,&(et[i]),v[i]); - bary2d(q0[x],q0[y],q1[x],q1[y],q2[x],q2[y],v[i][x], - v[i][y],b[i]); - } - if(et[i] < 0.0) - { - VSUB(db[j],b[j],b[i]); - t[j] = HUGET; - break; - } - else - { - VSUB(db[j],b[i],b[j]); - if((fabs(b[j][0])>(FTINY+1.0)) || - (fabs(b[j][1])>(FTINY+1.0)) || - (fabs(b[j][2])>(FTINY+1.0))) - { - t[j] = HUGET; - break; - } - else - t[j] = MAXT; - } - } + 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; + } + } } -} - *wptr = w; - bary_dtol(b[3],db,bi,dbi,t); - + } + bary_dtol(b[3],db,bi,dbi,t,w); + /* trace the ray starting with this node */ - nbr = qtVisit_tri_edgesi(qtptr,bi,dbi[w][0],dbi[w][1],dbi[w][2], - dbi,wptr,t,0,0,func,arg1,arg2,arg3); - if(nbr != INVALID && nbr != QT_DONE) - { - b[3][0] = (double)bi[0]/(double)MAXBCOORD; - b[3][1] = (double)bi[1]/(double)MAXBCOORD; - b[3][2] = (double)bi[2]/(double)MAXBCOORD; - i_pt[x] = b[3][0]*q0[x] + b[3][1]*q1[x] + b[3][2]*q2[x]; - i_pt[y] = b[3][0]*q0[y] + b[3][1]*q1[y] + b[3][2]*q2[y]; - i_pt[z] = (-n[x]*i_pt[x] - n[y]*i_pt[y] -pd)/n[z]; - } - return(nbr); + 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); + +}