ViewVC Help
View File | Revision Log | Show Annotations | Download File | Root Listing
root/radiance/ray/src/hd/sm_qtree.c
(Generate patch)

Comparing ray/src/hd/sm_qtree.c (file contents):
Revision 3.1 by gwlarson, Wed Aug 19 17:45:24 1998 UTC vs.
Revision 3.8 by gwlarson, Wed Nov 11 12:05:39 1998 UTC

# Line 14 | Line 14 | static char SCCSid[] = "$SunId$ SGI";
14   */
15  
16   #include "standard.h"
17 <
17 > #include "sm_flag.h"
18   #include "sm_geom.h"
19   #include "sm_qtree.h"
20 #include "object.h"
20  
21   QUADTREE  *quad_block[QT_MAX_BLK];              /* our quadtree */
22   static QUADTREE  quad_free_list = EMPTY;  /* freed octree nodes */
23   static QUADTREE  treetop = 0;           /* next free node */
24 + int4 *quad_flag= NULL;
25  
26 + #ifdef TEST_DRIVER
27 + extern FVECT Pick_v0[500],Pick_v1[500],Pick_v2[500];
28 + extern int Pick_cnt,Pick_tri,Pick_samp;
29 + extern FVECT Pick_point[500];
30 + extern int Pick_q[500];
31  
32 + #endif
33 + int Incnt=0;
34  
35   QUADTREE
36   qtAlloc()                       /* allocate a quadtree */
# Line 41 | Line 48 | qtAlloc()                      /* allocate a quadtree */
48          if (QT_BLOCK(freet) >= QT_MAX_BLK)
49             return(EMPTY);
50          if ((quad_block[QT_BLOCK(freet)] = (QUADTREE *)malloc(
51 <                   (unsigned)QT_BLOCK_SIZE*4*sizeof(QUADTREE))) == NULL)
52 <           return(EMPTY);
51 >                        QT_BLOCK_SIZE*4*sizeof(QUADTREE))) == NULL)
52 >           error(SYSTEM,"qtAlloc(): Unable to allocate memory\n");
53 >
54 >        /* Realloc the per/node flags */
55 >        quad_flag = (int4 *)realloc((char *)quad_flag,
56 >                        (QT_BLOCK(freet)+1)*((QT_BLOCK_SIZE+7)>>3));
57 >        if (quad_flag == NULL)
58 >           error(SYSTEM,"qtAlloc(): Unable to allocate memory\n");
59      }
60      treetop += 4;
61      return(freet);
62   }
63  
64  
65 + qtClearAllFlags()               /* clear all quadtree branch flags */
66 + {
67 +  if (!treetop)
68 +    return;
69 +  
70 +  /* Clear the node flags*/
71 +  bzero((char *)quad_flag, (QT_BLOCK(treetop-4)+1)*((QT_BLOCK_SIZE+7)>>3));
72 +  /* Clear set flags */
73 +  qtclearsetflags();
74 + }
75 +
76   qtFree(qt)                      /* free a quadtree */
77     register QUADTREE  qt;
78   {
# Line 69 | Line 93 | qtFree(qt)                     /* free a quadtree */
93   qtDone()                        /* free EVERYTHING */
94   {
95          register int    i;
96 <
96 >        
97 >        qtfreeleaves();
98          for (i = 0; i < QT_MAX_BLK; i++)
99          {
100 <            free((char *)quad_block[i],
101 <                  (unsigned)QT_BLOCK_SIZE*4*sizeof(QUADTREE));
100 >            if (quad_block[i] == NULL)
101 >                break;
102 >            free((char *)quad_block[i]);
103              quad_block[i] = NULL;
104          }
105 +        /* Free the flags */
106 +        if (i) free((char *)quad_flag);
107 +        quad_flag = NULL;
108          quad_free_list = EMPTY;
109          treetop = 0;
110   }
111  
112   QUADTREE
113 < qtCompress(qt)                  /* recursively combine nodes */
114 < register QUADTREE  qt;
113 > qtLocate_leaf(qt,bcoord)
114 > QUADTREE qt;
115 > BCOORD bcoord[3];
116   {
117 <    register int  i;
88 <    register QUADTREE  qres;
117 >  int i;
118  
119 <    if (!QT_IS_TREE(qt))        /* not a tree */
120 <       return(qt);
121 <    qres = QT_NTH_CHILD(qt,0) = qtCompress(QT_NTH_CHILD(qt,0));
122 <    for (i = 1; i < 4; i++)
123 <       if((QT_NTH_CHILD(qt,i) = qtCompress(QT_NTH_CHILD(qt,i))) != qres)
124 <          qres = qt;
125 <    if(!QT_IS_TREE(qres))
126 <    {   /* all were identical leaves */
98 <        QT_NTH_CHILD(qt,0) = quad_free_list;
99 <        quad_free_list = qt;
100 <    }
101 <    return(qres);
119 >  if(QT_IS_TREE(qt))
120 >  {  
121 >    i = baryi_child(bcoord);
122 >
123 >    return(qtLocate_leaf(QT_NTH_CHILD(qt,i),bcoord));
124 >  }
125 >  else
126 >    return(qt);
127   }
128  
129 + /*
130 +   Return the quadtree node containing pt. It is assumed that pt is in
131 +   the root node qt with ws vertices q0,q1,q2 and plane equation peq.
132 + */
133   QUADTREE
134 < qtPoint_locate(qtptr,v1,v2,v3,pt,type,which,p0,p1,p2)
135 < QUADTREE *qtptr;
136 < FVECT v1,v2,v3;
134 > qtRoot_point_locate(qt,q0,q1,q2,peq,pt)
135 > QUADTREE qt;
136 > FVECT q0,q1,q2;
137 > FPEQ peq;
138   FVECT pt;
109 char *type,*which;
110 FVECT p0,p1,p2;
139   {
140 <    char d,w;
141 <    int i;
142 <    QUADTREE *child;
143 <    QUADTREE qt;
116 <    FVECT a,b,c;
117 <    FVECT t1,t2,t3;
140 >    int i,x,y;
141 >    FVECT i_pt;
142 >    double bcoord[3];
143 >    BCOORD bcoordi[3];
144  
145 <    /* Determine if point lies within pyramid (and therefore
120 <       inside a spherical quadtree cell):GT_INTERIOR, on one of the
121 <       pyramid sides (and on cell edge):GT_EDGE(1,2 or 3),
122 <       or on pyramid vertex (and on cell vertex):GT_VERTEX(1,2, or 3).
123 <       For each triangle edge: compare the
124 <       point against the plane formed by the edge and the view center
125 <    */
126 <    d = test_single_point_against_spherical_tri(v1,v2,v3,pt,&w);  
127 <
128 <    /* Not in this triangle */
129 <    if(!d)
130 <    {
131 <      if(which)
132 <        *which = 0;
133 <      return(EMPTY);
134 <    }
135 <
136 <    /* Will return lowest level triangle containing point: It the
145 >     /* Will return lowest level triangle containing point: It the
146         point is on an edge or vertex: will return first associated
147         triangle encountered in the child traversal- the others can
148         be derived using triangle adjacency information
149      */
150 <    
142 <    if(QT_IS_TREE(*qtptr))
150 >    if(QT_IS_TREE(qt))
151      {  
152 <      qtSubdivide_tri(v1,v2,v3,a,b,c);
153 <      child = QT_NTH_CHILD_PTR(*qtptr,0);
154 <      if(!QT_IS_EMPTY(*child))
155 <      {
156 <        qt = qtPoint_locate(child,v1,a,c,pt,type,which,p0,p1,p2);
157 <        if(!QT_IS_EMPTY(qt))
158 <          return(qt);
159 <      }
160 <      child = QT_NTH_CHILD_PTR(*qtptr,1);
161 <      if(!QT_IS_EMPTY(*child))
162 <      {
163 <        qt = qtPoint_locate(child,a,b,c,pt,type,which,p0,p1,p2);
164 <        if(!QT_IS_EMPTY(qt))
165 <          return(qt);
158 <      }
159 <      child = QT_NTH_CHILD_PTR(*qtptr,2);
160 <      if(!QT_IS_EMPTY(*child))
161 <      {
162 <        qt = qtPoint_locate(child,a,v2,b,pt,type,which,p0,p1,p2);
163 <        if(!QT_IS_EMPTY(qt))
164 <          return(qt);
165 <      }
166 <      child = QT_NTH_CHILD_PTR(*qtptr,3);
167 <      if(!QT_IS_EMPTY(*child))
168 <      {
169 <        qt = qtPoint_locate(child,c,b,v3,pt,type,which,p0,p1,p2);
170 <        if(!QT_IS_EMPTY(qt))
171 <          return(qt);
172 <      }
152 >      /* Find the intersection point */
153 >      intersect_vector_plane(pt,peq,NULL,i_pt);
154 >
155 >      x = FP_X(peq);
156 >      y = FP_Y(peq);
157 >      /* Calculate barycentric coordinates of i_pt */
158 >      bary2d(q0[x],q0[y],q1[x],q1[y],q2[x],q2[y],i_pt[x],i_pt[y],bcoord);
159 >      
160 >      /* convert to integer coordinate */
161 >      convert_dtol(bcoord,bcoordi);
162 >
163 >      i = baryi_child(bcoordi);
164 >
165 >      return(qtLocate_leaf(QT_NTH_CHILD(qt,i),bcoordi));
166      }
167      else
168 <       if(!QT_IS_EMPTY(*qtptr))
176 <       {  
177 <           /* map GT_VERTEX,GT_EDGE,GT_FACE GT_INTERIOR of pyramid to
178 <              spherical triangle primitives
179 <              */
180 <         if(type)
181 <           *type = d;
182 <         if(which)
183 <           *which = w;
184 <         VCOPY(p0,v1);
185 <         VCOPY(p1,v2);
186 <         VCOPY(p2,v3);
187 <         return(*qtptr);
188 <       }
189 <    return(EMPTY);
168 >      return(qt);
169   }
170  
192 int
193 qtPoint_in_tri(qtptr,v0,v1,v2,pt,type,which)
194 QUADTREE *qtptr;
195 FVECT v0,v1,v2;
196 FVECT pt;
197 char *type,*which;
198 {
199    QUADTREE qt;
200    OBJECT os[MAXSET+1],*optr;
201    char d,w;
202    int i,id;
203    FVECT p0,p1,p2;
204  
205    qt = qtPoint_locate(qtptr,v0,v1,v2,pt,type,which,p0,p1,p2);
206    if(QT_IS_EMPTY(qt))
207       return(EMPTY);
208    
209    /* Get the set */
210    qtgetset(os,qt);
211    for (i = QT_SET_CNT(os),optr = QT_SET_PTR(os); i > 0; i--)
212    {
213        /* Find the triangle that pt falls in (NOTE:FOR now return first 1) */
214        id = QT_SET_NEXT_ELEM(optr);
215        qtTri_verts_from_id(id,p0,p1,p2);
216        d = test_single_point_against_spherical_tri(p0,p1,p2,pt,&w);  
217        if(d)
218        {
219          if(type)
220            *type = d;
221          if(which)
222            *which = w;
223          return(id);
224        }
225    }
226    return(EMPTY);
227 }
171  
229 QUADTREE
230 qtSubdivide(qtptr)
231 QUADTREE *qtptr;
232 {
233  QUADTREE node;
234  node = qtAlloc();
235  QT_CLEAR_CHILDREN(node);
236  *qtptr = node;
237  return(node);
238 }
172  
173  
174 < QUADTREE
242 < qtSubdivide_nth_child(qt,n)
243 < QUADTREE qt;
244 < int n;
245 < {
246 <  QUADTREE node;
174 > /* for triangle v0-v1-v2- returns a,b,c: children are:
175  
176 <  node = qtSubdivide(&(QT_NTH_CHILD(qt,n)));
177 <  
178 <  return(node);
179 < }
252 <
253 < /* for triangle v1-v2-v3- returns a,b,c: children are:
254 <
255 <        v3                     0: v1,a,c
256 <        /\                     1: a,b,c
257 <       /3 \                    2: a,v2,b
258 <     c/____\b                  3: c,b,v3
176 >        v2                     0: v0,a,c
177 >        /\                     1: a,v1,b                  
178 >       /2 \                    2: c,b,v2
179 >     c/____\b                  3: b,c,a
180       /\    /\  
181 <    /0 \1 /2 \
182 < v1/____\/____\v2
181 >    /0 \3 /1 \
182 >  v0____\/____\v1
183          a
184   */
185  
265 qtSubdivide_tri(v1,v2,v3,a,b,c)
266 FVECT v1,v2,v3;
267 FVECT a,b,c;
268 {
269    EDGE_MIDPOINT_VEC3(a,v1,v2);
270    normalize(a);
271    EDGE_MIDPOINT_VEC3(b,v2,v3);
272    normalize(b);
273    EDGE_MIDPOINT_VEC3(c,v3,v1);
274    normalize(c);
275 }
186  
187 < qtNth_child_tri(v1,v2,v3,a,b,c,i,r1,r2,r3)
188 < FVECT v1,v2,v3;
187 > qtNth_child_tri(v0,v1,v2,a,b,c,i,r0,r1,r2)
188 > FVECT v0,v1,v2;
189   FVECT a,b,c;
190   int i;
191 < FVECT r1,r2,r3;
191 > FVECT r0,r1,r2;
192   {
193 <  VCOPY(r1,a); VCOPY(r2,b);    VCOPY(r3,c);
194 <  switch(i){
195 <  case 0:  
196 <    VCOPY(r2,r1);
197 <    VCOPY(r1,v1);
198 <    break;
199 <  case 1:  
200 <    break;
201 <  case 2:  
202 <    VCOPY(r3,r2);
203 <    VCOPY(r2,v2);
204 <    break;
205 <  case 3:  
206 <    VCOPY(r1,r3);
207 <    VCOPY(r3,v3);
208 <    break;
193 >
194 >  if(!a)
195 >  {
196 >    /* Caution: r's must not be equal to v's:will be incorrect */
197 >    switch(i){
198 >    case 0:  
199 >      VCOPY(r0,v0);
200 >      EDGE_MIDPOINT_VEC3(r1,v0,v1);
201 >      EDGE_MIDPOINT_VEC3(r2,v2,v0);
202 >      break;
203 >    case 1:  
204 >      EDGE_MIDPOINT_VEC3(r0,v0,v1);
205 >      VCOPY(r1,v1);    
206 >      EDGE_MIDPOINT_VEC3(r2,v1,v2);
207 >      break;
208 >    case 2:  
209 >      EDGE_MIDPOINT_VEC3(r0,v2,v0);
210 >      EDGE_MIDPOINT_VEC3(r1,v1,v2);
211 >      VCOPY(r2,v2);
212 >      break;
213 >    case 3:  
214 >      EDGE_MIDPOINT_VEC3(r0,v1,v2);
215 >      EDGE_MIDPOINT_VEC3(r1,v2,v0);
216 >      EDGE_MIDPOINT_VEC3(r2,v0,v1);
217 >      break;
218 >    }
219    }
220 +  else
221 +  {
222 +    switch(i){
223 +    case 0:  
224 +      VCOPY(r0,v0); VCOPY(r1,a);    VCOPY(r2,c);
225 +      break;
226 +    case 1:  
227 +      VCOPY(r0,a); VCOPY(r1,v1);    VCOPY(r2,b);
228 +      break;
229 +    case 2:  
230 +      VCOPY(r0,c); VCOPY(r1,b);    VCOPY(r2,v2);
231 +      break;
232 +    case 3:  
233 +      VCOPY(r0,b); VCOPY(r1,c);    VCOPY(r2,a);
234 +      break;
235 +    }
236 +  }
237   }
238  
239   /* Add triangle "id" to all leaf level cells that are children of
# Line 307 | Line 244 | threshold- the cell is split- and the triangle must be
244   into the new child cells: it is assumed that "v1,v2,v3" are normalized
245   */
246  
247 < int
248 < qtAdd_tri(qtptr,id,t1,t2,t3,v1,v2,v3,n)
249 < QUADTREE *qtptr;
247 > QUADTREE
248 > qtRoot_add_tri(qt,q0,q1,q2,t0,t1,t2,id,n)
249 > QUADTREE qt;
250 > FVECT q0,q1,q2;
251 > FVECT t0,t1,t2;
252 > int id,n;
253 > {
254 >  if(stri_intersect(q0,q1,q2,t0,t1,t2))
255 >    qt = qtAdd_tri(qt,q0,q1,q2,t0,t1,t2,id,n);
256 >
257 >  return(qt);
258 > }
259 >
260 > QUADTREE
261 > qtRoot_remove_tri(qt,q0,q1,q2,t0,t1,t2,id,n)
262 > QUADTREE qt;
263 > FVECT q0,q1,q2;
264 > FVECT t0,t1,t2;
265 > int id,n;
266 > {
267 >
268 >  if(stri_intersect(q0,q1,q2,t0,t1,t2))
269 >    qt = qtRemove_tri(qt,q0,q1,q2,t0,t1,t2,id,n);
270 >  return(qt);
271 > }
272 >
273 >
274 > QUADTREE
275 > qtAdd_tri(qt,q0,q1,q2,t0,t1,t2,id,n)
276 > QUADTREE qt;
277 > FVECT q0,q1,q2;
278 > FVECT t0,t1,t2;
279   int id;
314 FVECT t1,t2,t3;
315 FVECT v1,v2,v3;
280   int n;
281   {
318  
319  char test;
320  int i,index;
282    FVECT a,b,c;
283 <  OBJECT os[MAXSET+1],*optr;
284 <  QUADTREE qt;
285 <  int found;
325 <  FVECT r1,r2,r3;
283 >  OBJECT tset[QT_MAXSET+1],*optr,*tptr;
284 >  FVECT r0,r1,r2;
285 >  int i;
286  
327  /* test if triangle (t1,t2,t3) overlaps cell triangle (v1,v2,v3) */
328  test = spherical_tri_intersect(t1,t2,t3,v1,v2,v3);
329
330  /* If triangles do not overlap: done */
331  if(!test)
332    return(FALSE);
333  found = 0;
334
287    /* if this is tree: recurse */
288 <  if(QT_IS_TREE(*qtptr))
288 >  if(QT_IS_TREE(qt))
289    {
290 +    QT_SET_FLAG(qt);
291      n++;
292 <    qtSubdivide_tri(v1,v2,v3,a,b,c);
340 <    found |= qtAdd_tri(QT_NTH_CHILD_PTR(*qtptr,0),id,t1,t2,t3,v1,a,c,n);
341 <    found |= qtAdd_tri(QT_NTH_CHILD_PTR(*qtptr,1),id,t1,t2,t3,a,b,c,n);
342 <    found |= qtAdd_tri(QT_NTH_CHILD_PTR(*qtptr,2),id,t1,t2,t3,a,v2,b,n);
343 <    found |= qtAdd_tri(QT_NTH_CHILD_PTR(*qtptr,3),id,t1,t2,t3,c,b,v3,n);
292 >    qtSubdivide_tri(q0,q1,q2,a,b,c);
293  
294 < #if 0
295 <    if(!found)
296 <    {
297 < #ifdef TEST_DRIVER    
298 <      HANDLE_ERROR("qtAdd_tri():Found in parent but not children\n");
299 < #else
300 <      eputs("qtAdd_tri():Found in parent but not children\n");
301 < #endif
302 <    }
354 < #endif
294 >    if(stri_intersect(t0,t1,t2,q0,a,c))
295 >      QT_NTH_CHILD(qt,0) = qtAdd_tri(QT_NTH_CHILD(qt,0),q0,a,c,t0,t1,t2,id,n);
296 >    if(stri_intersect(t0,t1,t2,a,q1,b))
297 >       QT_NTH_CHILD(qt,1) = qtAdd_tri(QT_NTH_CHILD(qt,1),a,q1,b,t0,t1,t2,id,n);
298 >    if(stri_intersect(t0,t1,t2,c,b,q2))
299 >      QT_NTH_CHILD(qt,2) = qtAdd_tri(QT_NTH_CHILD(qt,2),c,b,q2,t0,t1,t2,id,n);
300 >    if(stri_intersect(t0,t1,t2,b,c,a))
301 >      QT_NTH_CHILD(qt,3) = qtAdd_tri(QT_NTH_CHILD(qt,3),b,c,a,t0,t1,t2,id,n);
302 >    return(qt);
303    }
304    else
305    {
306        /* If this leave node emptry- create a new set */
307 <      if(QT_IS_EMPTY(*qtptr))
308 <      {
361 <          *qtptr = qtaddelem(*qtptr,id);
362 < #if 0
363 <          {
364 <              int k;
365 <              qtgetset(os,*qtptr);
366 <              printf("\n%d:\n",os[0]);
367 <              for(k=1; k <= os[0];k++)
368 <                 printf("%d ",os[k]);
369 <              printf("\n");
370 <          }
371 < #endif
372 <          /*
373 <          os[0] = 0;
374 <          insertelem(os,id);
375 <          qt = fullnode(os);
376 <          *qtptr = qt;
377 <          */
378 <      }
307 >      if(QT_IS_EMPTY(qt))
308 >        qt = qtaddelem(qt,id);
309        else
310        {
381          qtgetset(os,*qtptr);
311            /* If the set is too large: subdivide */
312 <          if(QT_SET_CNT(os) < QT_SET_THRESHOLD)
313 <          {
314 <            *qtptr = qtaddelem(*qtptr,id);
315 < #if 0
316 <            {
317 <              int k;
318 <              qtgetset(os,*qtptr);
319 <              printf("\n%d:\n",os[0]);
320 <              for(k=1; k <= os[0];k++)
321 <                 printf("%d ",os[k]);
322 <              printf("\n");
323 <          }
324 < #endif      
325 <            /*
326 <             insertelem(os,id);
327 <             *qtptr = fullnode(os);
328 <             */
329 <          }
330 <          else
402 <          {
403 <            if (n < QT_MAX_LEVELS)
404 <            {
405 <                 /* If set size exceeds threshold: subdivide cell and
406 <                    reinsert set tris into cell
407 <                    */
408 <                 n++;
409 <                 qtfreeleaf(*qtptr);
410 <                 qtSubdivide(qtptr);
411 <                 found = qtAdd_tri(qtptr,id,t1,t2,t3,v1,v2,v3,n);
412 < #if 0
413 <                 if(!found)
414 <                 {
415 < #ifdef TEST_DRIVER    
416 <      HANDLE_ERROR("qtAdd_tri():Found in parent but not children\n");
417 < #else
418 <                   eputs("qtAdd_tri():Found in parent but not children\n");
419 < #endif
420 <                 }
421 < #endif
422 <                 for(optr = &(os[1]),i = QT_SET_CNT(os); i > 0; i--)
423 <                 {
424 <                   id = QT_SET_NEXT_ELEM(optr);
425 <                   qtTri_verts_from_id(id,r1,r2,r3);
426 <                   found=qtAdd_tri(qtptr,id,r1,r2,r3,v1,v2,v3,n);
427 < #ifdef DEBUG
428 <                 if(!found)
429 <                    eputs("qtAdd_tri():Reinsert-in parent but not children\n");
430 < #endif
431 <               }
432 <             }
312 >        optr = qtqueryset(qt);
313 >
314 >        if(QT_SET_CNT(optr) < QT_SET_THRESHOLD)
315 >          qt = qtaddelem(qt,id);
316 >        else
317 >        {
318 >          if (n < QT_MAX_LEVELS)
319 >          {
320 >            /* If set size exceeds threshold: subdivide cell and
321 >               reinsert set tris into cell
322 >               */
323 >            /* CAUTION:If QT_SET_THRESHOLD << QT_MAXSET, and dont add
324 >               more than a few triangles before expanding: then are safe here
325 >               otherwise must check to make sure set size is < MAXSET,
326 >               or  qtgetset could overflow os.
327 >               */
328 >            tptr = qtqueryset(qt);
329 >            if(QT_SET_CNT(tptr) > QT_MAXSET)
330 >              tptr = (OBJECT *)malloc((QT_SET_CNT(tptr)+1)*sizeof(OBJECT));
331              else
332 <              if(QT_SET_CNT(os) < QT_MAX_SET)
333 <                {
334 <                  *qtptr = qtaddelem(*qtptr,id);
437 < #if 0
438 <                  {
439 <              int k;
440 <              qtgetset(os,*qtptr);
441 <              printf("\n%d:\n",os[0]);
442 <              for(k=1; k <= os[0];k++)
443 <                 printf("%d ",os[k]);
444 <              printf("\n");
445 <          }
446 < #endif            
447 <                  /*
448 <                    insertelem(os,id);
449 <                    *qtptr = fullnode(os);
450 <                  */
451 <                }
452 <              else
453 <                {
454 < #ifdef DEBUG
455 <                    eputs("qtAdd_tri():two many levels\n");
456 < #endif
457 <                    return(FALSE);
458 <                }
459 <          }
460 <      }
461 <  }
462 <  return(TRUE);
463 < }
332 >              tptr = tset;
333 >            if(!tptr)
334 >              goto memerr;
335  
336 +            qtgetset(tptr,qt);  
337 +            n++;
338 +            qtfreeleaf(qt);
339 +            qtSubdivide(qt);
340 +            qt = qtAdd_tri(qt,q0,q1,q2,t0,t1,t2,id,n);
341  
342 < int
343 < qtApply_to_tri_cells(qtptr,t1,t2,t3,v1,v2,v3,func,arg)
344 < QUADTREE *qtptr;
345 < FVECT t1,t2,t3;
346 < FVECT v1,v2,v3;
347 < int (*func)();
348 < char *arg;
349 < {
350 <  char test;
351 <  FVECT a,b,c;
352 <
353 <  /* test if triangle (t1,t2,t3) overlaps cell triangle (v1,v2,v3) */
354 <  test = spherical_tri_intersect(t1,t2,t3,v1,v2,v3);
355 <
480 <  /* If triangles do not overlap: done */
481 <  if(!test)
482 <    return(FALSE);
483 <
484 <  /* if this is tree: recurse */
485 <  if(QT_IS_TREE(*qtptr))
486 <  {
487 <    qtSubdivide_tri(v1,v2,v3,a,b,c);
488 <    qtApply_to_tri_cells(QT_NTH_CHILD_PTR(*qtptr,0),t1,t2,t3,v1,a,c,func,arg);
489 <    qtApply_to_tri_cells(QT_NTH_CHILD_PTR(*qtptr,1),t1,t2,t3,a,b,c,func,arg);
490 <    qtApply_to_tri_cells(QT_NTH_CHILD_PTR(*qtptr,2),t1,t2,t3,a,v2,b,func,arg);
491 <    qtApply_to_tri_cells(QT_NTH_CHILD_PTR(*qtptr,3),t1,t2,t3,c,b,v3,func,arg);
342 >            for(optr = QT_SET_PTR(tptr),i = QT_SET_CNT(tptr); i > 0; i--)
343 >              {
344 >                id = QT_SET_NEXT_ELEM(optr);
345 >                if(!qtTri_from_id(id,r0,r1,r2))
346 >                  continue;
347 >                qt  = qtAdd_tri(qt,q0,q1,q2,r0,r1,r2,id,n);
348 >              }
349 >            if(tptr != tset)
350 >              free(tptr);
351 >            }
352 >            else
353 >                qt = qtaddelem(qt,id);
354 >        }
355 >      }
356    }
357 <  else
358 <    return(func(qtptr,arg));
357 >  return(qt);
358 > memerr:
359 >    error(SYSTEM,"qtAdd_tri():Unable to allocate memory");
360   }
361  
362  
363 < int
364 < qtRemove_tri(qtptr,id,t1,t2,t3,v1,v2,v3)
365 < QUADTREE *qtptr;
363 > QUADTREE
364 > qtRemove_tri(qt,id,q0,q1,q2,t0,t1,t2)
365 > QUADTREE qt;
366   int id;
367 < FVECT t1,t2,t3;
368 < FVECT v1,v2,v3;
367 > FVECT q0,q1,q2;
368 > FVECT t0,t1,t2;
369   {
505  
506  char test;
507  int i;
370    FVECT a,b,c;
509  OBJECT os[MAXSET+1];
371    
372 <  /* test if triangle (t1,t2,t3) overlaps cell triangle (v1,v2,v3) */
373 <  test = spherical_tri_intersect(t1,t2,t3,v1,v2,v3);
372 >  /* test if triangle (t0,t1,t2) overlaps cell triangle (v0,v1,v2) */
373 >  if(!stri_intersect(t0,t1,t2,q0,q1,q2))
374 >    return(qt);
375  
514  /* If triangles do not overlap: done */
515  if(!test)
516    return(FALSE);
517
376    /* if this is tree: recurse */
377 <  if(QT_IS_TREE(*qtptr))
377 >  if(QT_IS_TREE(qt))
378    {
379 <    qtSubdivide_tri(v1,v2,v3,a,b,c);
380 <    qtRemove_tri(QT_NTH_CHILD_PTR(*qtptr,0),id,t1,t2,t3,v1,a,c);
381 <    qtRemove_tri(QT_NTH_CHILD_PTR(*qtptr,1),id,t1,t2,t3,a,b,c);
382 <    qtRemove_tri(QT_NTH_CHILD_PTR(*qtptr,2),id,t1,t2,t3,a,v2,b);
383 <    qtRemove_tri(QT_NTH_CHILD_PTR(*qtptr,3),id,t1,t2,t3,c,b,v3);
379 >    qtSubdivide_tri(q0,q1,q2,a,b,c);
380 >    QT_NTH_CHILD(qt,0) = qtRemove_tri(QT_NTH_CHILD(qt,0),id,t0,t1,t2,q0,a,c);
381 >    QT_NTH_CHILD(qt,1) = qtRemove_tri(QT_NTH_CHILD(qt,1),id,t0,t1,t2,a,q1,b);
382 >    QT_NTH_CHILD(qt,2) = qtRemove_tri(QT_NTH_CHILD(qt,2),id,t0,t1,t2,c,b,q2);
383 >    QT_NTH_CHILD(qt,3) = qtRemove_tri(QT_NTH_CHILD(qt,3),id,t0,t1,t2,b,c,a);
384 >    return(qt);
385    }
386    else
387    {
388 <      if(QT_IS_EMPTY(*qtptr))
388 >      if(QT_IS_EMPTY(qt))
389        {
390   #ifdef DEBUG    
391            eputs("qtRemove_tri(): triangle not found\n");
# Line 535 | Line 394 | FVECT v1,v2,v3;
394        /* remove id from set */
395        else
396        {
397 <          qtgetset(os,*qtptr);
539 <          if(!inset(os,id))
397 >          if(!qtinset(qt,id))
398            {
399   #ifdef DEBUG          
400                eputs("qtRemove_tri(): tri not in set\n");
401   #endif
402            }
403            else
404 <          {
405 <            *qtptr = qtdelelem(*qtptr,id);
406 < #if 0
407 <            {
408 <              int k;
551 <              if(!QT_IS_EMPTY(*qtptr))
552 <              {qtgetset(os,*qtptr);
553 <              printf("\n%d:\n",os[0]);
554 <              for(k=1; k <= os[0];k++)
555 <                 printf("%d ",os[k]);
556 <              printf("\n");
557 <           }
404 >            qt = qtdelelem(qt,id);
405 >      }
406 >  }
407 >  return(qt);
408 > }
409  
410 <          }
411 < #endif
410 >
411 > QUADTREE
412 > qtVisit_tri_interior(qt,q0,q1,q2,t0,t1,t2,n0,n1,n2,n,func,f,argptr)
413 >   QUADTREE qt;
414 >   FVECT q0,q1,q2;
415 >   FVECT t0,t1,t2;
416 >   FVECT n0,n1,n2;
417 >   int n;
418 >   int (*func)(),*f;
419 >   int *argptr;
420 > {
421 >    FVECT a,b,c;
422 >    
423 >    /* If qt Flag set, or qt vertices interior to t0t1t2-descend */
424 > tree_modified:
425 >
426 >    if(QT_IS_TREE(qt))
427 >    {  
428 >        if(QT_IS_FLAG(qt) ||  point_in_stri_n(n0,n1,n2,q0))
429 >        {
430 >            QT_SET_FLAG(qt);
431 >            qtSubdivide_tri(q0,q1,q2,a,b,c);
432 >            /* descend to children */
433 >            QT_NTH_CHILD(qt,0) = qtVisit_tri_interior(QT_NTH_CHILD(qt,0),
434 >                                  q0,a,c,t0,t1,t2,n0,n1,n2,n+1,func,f,argptr);
435 >            QT_NTH_CHILD(qt,1) = qtVisit_tri_interior(QT_NTH_CHILD(qt,1),
436 >                                  a,q1,b,t0,t1,t2,n0,n1,n2,n+1,func,f,argptr);
437 >            QT_NTH_CHILD(qt,2) = qtVisit_tri_interior(QT_NTH_CHILD(qt,2),
438 >                                  c,b,q2,t0,t1,t2,n0,n1,n2,n+1,func,f,argptr);
439 >            QT_NTH_CHILD(qt,3) = qtVisit_tri_interior(QT_NTH_CHILD(qt,3),
440 >                                  b,c,a,t0,t1,t2,n0,n1,n2,n+1,func,f,argptr);
441          }
442 +    }
443 +    else
444 +      if((!QT_IS_EMPTY(qt) && QT_LEAF_IS_FLAG(qt)) ||
445 +         point_in_stri_n(n0,n1,n2,q0))
446 +      {
447 +        func(&qt,f,argptr,q0,q1,q2,t0,t1,t2,n);
448 +        if(QT_FLAG_IS_MODIFIED(*f))
449 +       {
450 +         QT_SET_FLAG(qt);
451 +         goto tree_modified;
452 +       }
453 +        if(QT_IS_LEAF(qt))
454 +           QT_LEAF_SET_FLAG(qt);
455 +        else
456 +          if(QT_IS_TREE(qt))
457 +            QT_SET_FLAG(qt);
458        }
459 +    return(qt);
460 + }
461 +
462 +
463 +
464 + int
465 + move_to_nbri(b,db0,db1,db2,tptr)
466 + BCOORD b[3];
467 + BDIR db0,db1,db2;
468 + TINT *tptr;
469 + {
470 +  TINT t,dt;
471 +  BCOORD bc;
472 +  int nbr;
473 +  
474 +  nbr = -1;
475 +  *tptr = 0;
476 +  /* Advance to next node */
477 +  if(b[0]==0 && db0 < 0)
478 +    return(0);
479 +  if(b[1]==0 && db1 < 0)
480 +    return(1);
481 +  if(b[2]==0 && db2 < 0)
482 +    return(2);
483 +
484 +  if(db0 < 0)
485 +   {
486 +     bc = b[0]<<SHIFT_MAXBCOORD;
487 +     t = bc/-db0;
488 +     nbr = 0;
489 +   }
490 +  else
491 +    t = HUGET;
492 +  if(db1 < 0)
493 +  {
494 +     bc = b[1] <<SHIFT_MAXBCOORD;
495 +     dt = bc/-db1;
496 +    if( dt < t)
497 +    {
498 +      t = dt;
499 +      nbr = 1;
500 +    }
501    }
502 <  return(TRUE);
502 >  if(db2 < 0)
503 >  {
504 >     bc = b[2] << SHIFT_MAXBCOORD;
505 >     dt = bc/-db2;
506 >       if( dt < t)
507 >      {
508 >        t = dt;
509 >        nbr = 2;
510 >      }
511 >    }
512 >  *tptr = t;
513 >  return(nbr);
514   }
515 +
516 + QUADTREE
517 + qtVisit_tri_edges(qt,b,db0,db1,db2,db,wptr,nextptr,t,sign,sfactor,func,f,argptr)
518 +   QUADTREE qt;
519 +   BCOORD b[3];
520 +   BDIR db0,db1,db2,db[3][3];
521 +   int *wptr,*nextptr;
522 +   TINT t[3];
523 +   int sign,sfactor;
524 +   int (*func)();
525 +   int *f,*argptr;
526 + {
527 +    int i,found;
528 +    QUADTREE child;
529 +    int nbr,next,w;
530 +    TINT t_g,t_l,t_i,l;
531 +
532 +    if(QT_IS_TREE(qt))
533 +    {
534 +      /* Find the appropriate child and reset the coord */
535 +      i = baryi_child(b);
536 +
537 +      QT_SET_FLAG(qt);
538 +
539 +      for(;;)
540 +      {
541 +        w = *wptr;
542 +        child = QT_NTH_CHILD(qt,i);
543 +        if(i != 3)
544 +          QT_NTH_CHILD(qt,i) =
545 +            qtVisit_tri_edges(child,b,db0,db1,db2,db,wptr,nextptr,t,sign,
546 +                                sfactor+1,func,f,argptr);
547 +        else
548 +          /* If the center cell- must flip direction signs */
549 +          QT_NTH_CHILD(qt,i) =
550 +            qtVisit_tri_edges(child,b,-db0,-db1,-db2,db,wptr,nextptr,t,1-sign,
551 +                                  sfactor+1,func,f,argptr);
552 +
553 +        if(QT_FLAG_IS_DONE(*f))
554 +          return(qt);
555 +        if(*wptr != w)
556 +        {
557 +          w = *wptr;
558 +          db0 = db[w][0];db1 = db[w][1];db2 = db[w][2];
559 +          if(sign)
560 +            {  db0 *= -1;db1 *= -1; db2 *= -1;}
561 +        }
562 +        /* If in same block: traverse */
563 +        if(i==3)
564 +          next = *nextptr;
565 +        else
566 +          if(*nextptr == i)
567 +            next = 3;
568 +          else
569 +         {
570 +           /* reset the barycentric coordinates in the parents*/
571 +           baryi_parent(b,i);
572 +           /* Else pop up to parent and traverse from there */
573 +           return(qt);
574 +         }
575 +        baryi_from_child(b,i,next);
576 +        i = next;
577 +      }
578 +    }
579 +    else
580 +   {
581 + #ifdef TEST_DRIVER
582 +       if(Pick_cnt < 500)
583 +          Pick_q[Pick_cnt++]=qt;
584 + #endif;
585 +       func(&qt,f,argptr);
586 +     if(QT_FLAG_IS_DONE(*f))
587 +     {
588 +       if(!QT_IS_EMPTY(qt))
589 +         QT_LEAF_SET_FLAG(qt);
590 +       return(qt);
591 +     }
592 +    
593 +     if(!QT_IS_EMPTY(qt))
594 +       QT_LEAF_SET_FLAG(qt);
595 +     /* Advance to next node */
596 +     w = *wptr;
597 +     while(1)
598 +       {
599 +         nbr = move_to_nbri(b,db0,db1,db2,&t_i);
600 +        
601 +         t_g = t_i >> sfactor;
602 +                
603 +         if(t_g >= t[w])
604 +         {
605 +           if(w == 2)
606 +           {
607 +             QT_FLAG_SET_DONE(*f);
608 +             return(qt);
609 +           }
610 +           /* The edge fits in the cell- therefore the result of shifting
611 +              db by sfactor is guaranteed to be less than MAXBCOORD
612 +              */
613 +           /* Caution: (t[w]*db) must occur before divide by MAXBCOORD
614 +              since t[w] will always be < MAXBCOORD
615 +              */
616 +           l = t[w] << sfactor;
617 +           /* NOTE: Change divides to Shift and multiply by sign*/
618 +           b[0] += (l*db0)/MAXBCOORD;
619 +           b[1] += (l*db1)/MAXBCOORD;
620 +           b[2] += (l*db2)/MAXBCOORD;
621 +           w++;
622 +           db0 = db[w][0]; db1 = db[w][1]; db2 = db[w][2];
623 +           if(sign)
624 +             {  db0 *= -1;db1 *= -1; db2 *= -1;}
625 +         }
626 +         else
627 +         {
628 +           /* Caution: (t_i*db) must occur before divide by MAXBCOORD
629 +              since t_i will always be < MAXBCOORD*/
630 +           /* NOTE: Change divides to Shift and  by sign*/
631 +           b[0] += (t_i *db0) / MAXBCOORD;
632 +           b[1] += (t_i *db1) / MAXBCOORD;
633 +           b[2] += (t_i *db2) / MAXBCOORD;
634 +          
635 +           t[w] -= t_g;
636 +           *wptr = w;
637 +           *nextptr = nbr;
638 +           return(qt);
639 +         }
640 +       }
641 +   }    
642 + }
643 +
644 +
645 + QUADTREE
646 + qtRoot_visit_tri_edges(qt,q0,q1,q2,peq,tri,i_pt,wptr,nextptr,func,f,argptr)
647 +   QUADTREE qt;
648 +   FVECT q0,q1,q2;
649 +   FPEQ  peq;
650 +   FVECT tri[3],i_pt;
651 +   int *wptr,*nextptr;
652 +   int (*func)();
653 +   int *f,*argptr;
654 + {
655 +  int x,y,z,w,i,j,first;
656 +  QUADTREE child;
657 +  FVECT c,d,v[3];
658 +  double b[4][3],db[3][3],et[3],exit_pt;
659 +  BCOORD bi[3];
660 +  TINT t[3];
661 +  BDIR dbi[3][3];
662 +  
663 + first =0;
664 +  w = *wptr;
665 +  if(w==-1)
666 +  {
667 +    first = 1;
668 +    *wptr = w = 0;
669 +  }
670 +  /* Project the origin onto the root node plane */
671 +
672 +  t[0] = t[1] = t[2] = 0;
673 +  /* Find the intersection point of the origin */
674 +  /* map to 2d by dropping maximum magnitude component of normal */
675 +
676 +  x = FP_X(peq);
677 +  y = FP_Y(peq);
678 +  z = FP_Z(peq);
679 +  /* Calculate barycentric coordinates for current vertex */
680 +  if(!first)
681 +    bary2d(q0[x],q0[y],q1[x],q1[y],q2[x],q2[y],i_pt[x],i_pt[y],b[3]);  
682 +  else
683 +  /* Just starting: b[0] is the origin point: guaranteed to be valid b*/
684 +  {
685 +    intersect_vector_plane(tri[0],peq,&(et[0]),v[0]);
686 +    bary2d(q0[x],q0[y],q1[x],q1[y],q2[x],q2[y],v[0][x],v[0][y],b[0]);  
687 +    VCOPY(b[3],b[0]);
688 +  }
689 +
690 +  j = (w+1)%3;
691 +  intersect_vector_plane(tri[j],peq,&(et[j]),v[j]);
692 +  bary2d(q0[x],q0[y],q1[x],q1[y],q2[x],q2[y],v[j][x],v[j][y],b[j]);  
693 +  if(et[j] < 0.0)
694 +  {
695 +      VSUB(db[w],b[3],b[j]);
696 +      t[w] = HUGET;
697 +  }
698 +  else
699 + {
700 +   /* NOTE: for stability: do not increment with ipt- use full dir and
701 +      calculate t: but for wrap around case: could get same problem?
702 +      */
703 +   VSUB(db[w],b[j],b[3]);
704 +   /* Check if the point is out side of the triangle: if so t[w] =HUGET */
705 +   if((fabs(b[j][0])>(FTINY+1.0)) ||(fabs(b[j][1])>(FTINY+1.0)) ||
706 +      (fabs(b[j][2])>(FTINY+1.0))||(b[j][0] <-FTINY) ||
707 +      (b[j][1]<-FTINY) ||(b[j][2]<-FTINY))
708 +     t[w] = HUGET;
709 +   else
710 +   {
711 +       /* The next point is in the triangle- so db values will be in valid
712 +          range and t[w]= MAXT
713 +        */  
714 +       t[w] = MAXT;
715 +       if(j != 0)
716 +         for(;j < 3;j++)
717 +         {
718 +           i = (j+1)%3;
719 +           if(!first || i != 0)
720 +           {
721 +             intersect_vector_plane(tri[i],peq,&(et[i]),v[i]);
722 +             bary2d(q0[x],q0[y],q1[x],q1[y],q2[x],q2[y],v[i][x],
723 +                    v[i][y],b[i]);
724 +           }
725 +           if(et[i] < 0.0)
726 +           {
727 +             VSUB(db[j],b[j],b[i]);
728 +             t[j] = HUGET;
729 +             break;
730 +           }
731 +           else
732 +           {
733 +             VSUB(db[j],b[i],b[j]);
734 +             if((fabs(b[j][0])>(FTINY+1.0))||(fabs(b[j][1])>(FTINY+1.0)) ||
735 +                (fabs(b[j][2])>(FTINY+1.0))||(b[i][0] <-FTINY) ||
736 +                (b[i][1]<-FTINY) ||(b[i][2]<-FTINY))
737 +               {
738 +                 t[j] = HUGET;
739 +                 break;
740 +               }
741 +             else
742 +               t[j] = MAXT;
743 +           }
744 +         }
745 +   }
746 + }
747 +  bary_dtol(b[3],db,bi,dbi,t,w);
748 +  
749 +  /* trace the ray starting with this node */
750 +  qt = qtVisit_tri_edges(qt,bi,dbi[w][0],dbi[w][1],dbi[w][2],
751 +                     dbi,wptr,nextptr,t,0,0,func,f,argptr);
752 +  if(!QT_FLAG_IS_DONE(*f))
753 +  {
754 +    b[3][0] = (double)bi[0]/(double)MAXBCOORD;
755 +    b[3][1] = (double)bi[1]/(double)MAXBCOORD;
756 +    b[3][2] = (double)bi[2]/(double)MAXBCOORD;
757 +    i_pt[x] = b[3][0]*q0[x] + b[3][1]*q1[x] + b[3][2]*q2[x];
758 +    i_pt[y] = b[3][0]*q0[y] + b[3][1]*q1[y] + b[3][2]*q2[y];
759 +    i_pt[z] = (-FP_N(peq)[x]*i_pt[x] - FP_N(peq)[y]*i_pt[y]-FP_D(peq))/FP_N(peq)[z];
760 +  }
761 +  return(qt);
762 +    
763 + }
764 +
765 +
766 + QUADTREE
767 + qtRoot_trace_ray(qt,q0,q1,q2,peq,orig,dir,nextptr,func,f,argptr)
768 +   QUADTREE qt;
769 +   FVECT q0,q1,q2;
770 +   FPEQ  peq;
771 +   FVECT orig,dir;
772 +   int *nextptr;
773 +   int (*func)();
774 +   int *f,*argptr;
775 + {
776 +  int x,y,z,nbr,w,i;
777 +  QUADTREE child;
778 +  FVECT v,i_pt;
779 +  double b[2][3],db[3],et[2],d,br[3];
780 +  BCOORD bi[3];
781 +  TINT t[3];
782 +  BDIR dbi[3][3];
783 +  
784 +  /* Project the origin onto the root node plane */
785 +  t[0] = t[1] = t[2] = 0;
786 +
787 +  VADD(v,orig,dir);
788 +  /* Find the intersection point of the origin */
789 +  /* map to 2d by dropping maximum magnitude component of normal */
790 +  x = FP_X(peq);
791 +  y = FP_Y(peq);
792 +  z = FP_Z(peq);
793 +
794 +  /* Calculate barycentric coordinates for current vertex */
795 +  intersect_vector_plane(orig,peq,&(et[0]),i_pt);
796 +  bary2d(q0[x],q0[y],q1[x],q1[y],q2[x],q2[y],i_pt[x],i_pt[y],b[0]);  
797 +
798 +  intersect_vector_plane(v,peq,&(et[1]),i_pt);
799 +  bary2d(q0[x],q0[y],q1[x],q1[y],q2[x],q2[y],i_pt[x],i_pt[y],b[1]);  
800 +  if(et[1] < 0.0)
801 +    VSUB(db,b[0],b[1]);
802 +  else
803 +   VSUB(db,b[1],b[0]);
804 +  t[0] = HUGET;
805 +  convert_dtol(b[0],bi);
806 +   if(et[1]<0.0 ||(fabs(b[1][0])>(FTINY+1.0))||(fabs(b[1][1])>(FTINY+1.0)) ||
807 +      (fabs(b[1][2])>(FTINY+1.0))||(b[1][0] <-FTINY) ||
808 +      (b[1][1]<-FTINY) ||(b[1][2]<-FTINY))
809 +     {
810 +       max_index(db,&d);
811 +       for(i=0; i< 3; i++)
812 +         dbi[0][i] = (BDIR)(db[i]/d*MAXBDIR);
813 +     }
814 +   else
815 +       for(i=0; i< 3; i++)
816 +         dbi[0][i] = (BDIR)(db[i]*MAXBDIR);
817 +  w=0;
818 +  /* trace the ray starting with this node */
819 +  qt = qtVisit_tri_edges(qt,bi,dbi[0][0],dbi[0][1],dbi[0][2], dbi,&w,
820 +                         nextptr,t,0,0,func,f,argptr);
821 +  if(!QT_FLAG_IS_DONE(*f))
822 +  {
823 +    br[0] = (double)bi[0]/(double)MAXBCOORD;
824 +    br[1] = (double)bi[1]/(double)MAXBCOORD;
825 +    br[2] = (double)bi[2]/(double)MAXBCOORD;
826 +    orig[x] = br[0]*q0[x] + br[1]*q1[x] + br[2]*q2[x];
827 +    orig[y] = br[0]*q0[y] + br[1]*q1[y] + br[2]*q2[y];
828 +    orig[z]=(-FP_N(peq)[x]*orig[x] -
829 +             FP_N(peq)[y]*orig[y]-FP_D(peq))/FP_N(peq)[z];
830 +  }
831 +  return(qt);
832 +    
833 + }
834 +
835 +
836 +
837 +
838 +
839 +
840 +
841 +
842 +
843 +
844 +
845 +
846 +

Diff Legend

Removed lines
+ Added lines
< Changed lines
> Changed lines