src/hd/sm_stree.c

/* Copyright (c) 1998 Silicon Graphics, Inc. */

#ifndef lint
static char SCCSid[] = "$SunId$ SGI";
#endif

/*
 * sm_stree.c
 */
#include "standard.h"
#include "sm_geom.h"
#include "sm_stree.h"

/* Define 4 vertices on the sphere to create a tetrahedralization on 
   the sphere: triangles are as follows:
        (2,1,0),(3,2,0), (1,3,0), (2,3,1)
*/

#ifdef TEST_DRIVER
extern FVECT Pick_point[500],Pick_v0[500],Pick_v1[500],Pick_v2[500];
extern int Pick_cnt;
#endif
FVECT stDefault_base[4] = { {SQRT3_INV, SQRT3_INV, SQRT3_INV},
                          {-SQRT3_INV, -SQRT3_INV, SQRT3_INV},
                          {-SQRT3_INV, SQRT3_INV, -SQRT3_INV},
                          {SQRT3_INV, -SQRT3_INV, -SQRT3_INV}};
int stTri_verts[4][3] = { {2,1,0},{3,2,0},{1,3,0},{2,3,1}};
int stTri_nbrs[4][3] =  { {2,1,3},{0,2,3},{1,0,3},{2,0,1}};

stNth_base_verts(st,i,v1,v2,v3)
STREE *st;
int i;
FVECT v1,v2,v3;
{
  VCOPY(v1,ST_NTH_BASE(st,stTri_verts[i][0]));
  VCOPY(v2,ST_NTH_BASE(st,stTri_verts[i][1]));
  VCOPY(v3,ST_NTH_BASE(st,stTri_verts[i][2]));
}

/* Frees the 4 quadtrees rooted at st */
stClear(st)
STREE *st;
{
  int i;

  /* stree always has 4 children corresponding to the base tris
  */
  for (i = 0; i < 4; i++)
    qtFree(ST_NTH_ROOT(st, i));

  QT_CLEAR_CHILDREN(ST_ROOT(st));

}


STREE
*stInit(st,center,base)
STREE *st;
FVECT  center,base[4];
{

  if(base)
    ST_SET_BASE(st,base);
  else
    ST_SET_BASE(st,stDefault_base);

  ST_SET_CENTER(st,center);
  stClear(st);

  return(st);
}


/* "base" defines 4 vertices on the sphere to create a tetrahedralization on 
     the sphere: triangles are as follows:(0,1,2),(0,2,3), (0,3,1), (1,3,2)
     if base is null: does default.

 */
STREE
*stAlloc(st)
STREE *st;
{
  int i;

  if(!st)
    st = (STREE *)malloc(sizeof(STREE));

  ST_ROOT(st) = qtAlloc();
    
  QT_CLEAR_CHILDREN(ST_ROOT(st));

  return(st);
}


/* Find location of sample point in the DAG and return lowest level
   containing triangle. "type" indicates whether the point was found
   to be in interior to the triangle: GT_FACE, on one of its
   edges GT_EDGE or coinciding with one of its vertices GT_VERTEX.
   "which" specifies which vertex (0,1,2) or edge (0=v0v1, 1 = v1v2, 2 = v21) 
*/
int
stPoint_locate(st,npt)
    STREE *st;
    FVECT npt;
{
    int i,d,j,id;
    QUADTREE *rootptr,*qtptr;
    FVECT v1,v2,v3;
    OBJECT os[QT_MAXSET+1],*optr;
    FVECT p0,p1,p2;

    /* Test each of the root triangles against point id */
    for(i=0; i < 4; i++)
     {
         rootptr = ST_NTH_ROOT_PTR(st,i);
         stNth_base_verts(st,i,v1,v2,v3);
         /* Return tri that p falls in */
         qtptr = qtRoot_point_locate(rootptr,v1,v2,v3,npt,NULL,NULL,NULL);
         if(!qtptr || QT_IS_EMPTY(*qtptr))
            continue;
         /* Get the set */
         optr = qtqueryset(*qtptr);
         for (j = QT_SET_CNT(optr),optr = QT_SET_PTR(optr);j > 0; j--)
         {
             /* Find the first triangle that pt falls */
             id = QT_SET_NEXT_ELEM(optr);
             qtTri_from_id(id,NULL,NULL,NULL,p0,p1,p2,NULL,NULL,NULL);
             d = point_in_stri(p0,p1,p2,npt);  
             if(d)
               return(id);
         }
     }
    return(EMPTY);
}

QUADTREE
*stPoint_locate_cell(st,p,t0,t1,t2)
    STREE *st;
    FVECT p;
    FVECT t0,t1,t2;
{
    int i,d;
    QUADTREE *rootptr,*qtptr;
    FVECT v0,v1,v2;

    
    /* Test each of the root triangles against point id */
    for(i=0; i < 4; i++)
     {
         rootptr = ST_NTH_ROOT_PTR(st,i);
         stNth_base_verts(st,i,v0,v1,v2);
         /* Return quadtree tri that p falls in */
         qtptr = qtRoot_point_locate(rootptr,v0,v1,v2,p,t0,t1,t2);
         if(qtptr)
            return(qtptr);
     }    /* Point not found */
    return(NULL);
}


int
stAdd_tri(st,id,v0,v1,v2)
STREE *st;
int id;
FVECT v0,v1,v2;
{
  
  int i,found;
  QUADTREE *rootptr;
  FVECT t0,t1,t2;

  found = 0;
  for(i=0; i < 4; i++)
  {
    rootptr = ST_NTH_ROOT_PTR(st,i);
    stNth_base_verts(st,i,t0,t1,t2);
    found |= qtRoot_add_tri(rootptr,t0,t1,t2,v0,v1,v2,id,0);
  }
  return(found);
}

int
stApply_to_tri_cells(st,v0,v1,v2,func,arg)
STREE *st;
FVECT v0,v1,v2;
int (*func)();
int *arg;
{
  int i,found;
  QUADTREE *rootptr;
  FVECT t0,t1,t2;

  found = 0;
  func(ST_ROOT_PTR(st),arg);
  QT_SET_FLAG(ST_ROOT(st));
  for(i=0; i < 4; i++)
  {
    rootptr = ST_NTH_ROOT_PTR(st,i);
    stNth_base_verts(st,i,t0,t1,t2); 
    found |= qtApply_to_tri_cells(rootptr,v0,v1,v2,t0,t1,t2,func,arg);
  }
  return(found);
}


int
stRemove_tri(st,id,v0,v1,v2)
STREE *st;
int id;
FVECT v0,v1,v2;
{
  
  int i,found;
  QUADTREE *rootptr;
  FVECT t0,t1,t2;

  found = 0;
  for(i=0; i < 4; i++)
  {
    rootptr = ST_NTH_ROOT_PTR(st,i);
    stNth_base_verts(st,i,t0,t1,t2); 
   found |= qtRemove_tri(rootptr,id,v0,v1,v2,t0,t1,t2);
  }
  return(found);
}

int
stVisit_tri_edges(st,t0,t1,t2,func,arg1,arg2)
   STREE *st;
   FVECT t0,t1,t2;
   int (*func)();
   int *arg1,arg2;
{
    int id,i,w;
    QUADTREE *rootptr;
    FVECT q0,q1,q2,n,v[3],sdir[3],dir[3],tv,d;
    double pd,t;

    VCOPY(v[0],t0); VCOPY(v[1],t1); VCOPY(v[2],t2);
    VSUB(dir[0],t1,t0); VSUB(dir[1],t2,t1);VSUB(dir[2],t0,t2);
    VCOPY(sdir[0],dir[0]);VCOPY(sdir[1],dir[1]);VCOPY(sdir[2],dir[2]);
    w = 0;
    for(i=0; i < 4; i++)
     {
#ifdef TEST_DRIVER 
Pick_cnt = 0;
#endif
         rootptr = ST_NTH_ROOT_PTR(st,i);
         stNth_base_verts(st,i,q0,q1,q2);
         /* Return quadtree tri that p falls in */
         if(!point_in_stri(q0,q1,q2,v[w]))  
           continue;
         id = qtRoot_visit_tri_edges(rootptr,q0,q1,q2,v,dir,&w,func,arg1,arg2);
         if(id == INVALID)
         {
#ifdef DEBUG
           eputs("stVisit_tri_edges(): Unable to trace edges\n");
#endif
           return(INVALID);
         }
         if(id == QT_DONE)
            return(*arg1);
        
         /* Crossed over to next cell: id = nbr */
         while(1) 
         {
             /* test if ray crosses plane between this quadtree triangle and
                its neighbor- if it does then find intersection point with 
                ray and plane- this is the new origin
                */
           if(id==0)
             VCROSS(n,q1,q2);
           else
             if(id==1)
               VCROSS(n,q2,q0);
           else
             VCROSS(n,q0,q1);

           if(w==0)
             VCOPY(tv,t0);
           else
             if(w==1)
               VCOPY(tv,t1);
           else
             VCOPY(tv,t2);
           if(!intersect_ray_plane(tv,sdir[w],n,0.0,&t,v[w]))
             return(INVALID);

           VSUM(v[w],v[w],sdir[w],10.0*FTINY);

           t = (1.0-t-10.0*FTINY);
           if(t <= 0.0)
           {
             t = FTINY;
#if 0
             eputs("stVisit_tri_edges(): edge end on plane\n");
#endif
           }
           dir[w][0] = sdir[w][0] * t;
           dir[w][1] = sdir[w][1] * t;
           dir[w][2] = sdir[w][2] * t;
           i = stTri_nbrs[i][id];
           rootptr = ST_NTH_ROOT_PTR(st,i);
           stNth_base_verts(st,i,q0,q1,q2);
           id=qtRoot_visit_tri_edges(rootptr,q0,q1,q2,v,dir,&w,func,arg1,arg2);
           if(id == QT_DONE)
             return(*arg1);
           if(id == INVALID)
             {
#if 0
             eputs("stVisit_tri_edges(): point not found\n");
#endif  
             return(INVALID);
             }
           
         }
     }    /* Point not found */
    return(INVALID); 
}


int
stVisit_tri_edges2(st,t0,t1,t2,func,arg1,arg2)
   STREE *st;
   FVECT t0,t1,t2;
   int (*func)();
   int *arg1,arg2;
{
    int id,i,w;
    QUADTREE *rootptr;
    FVECT q0,q1,q2,v[3],i_pt;

    VCOPY(v[0],t0); VCOPY(v[1],t1); VCOPY(v[2],t2);
    w = -1;
    for(i=0; i < 4; i++)
     {
#ifdef TEST_DRIVER 
Pick_cnt = 0;
#endif
         rootptr = ST_NTH_ROOT_PTR(st,i);
         stNth_base_verts(st,i,q0,q1,q2);
         /* Return quadtree tri that p falls in */
         if(!point_in_stri(q0,q1,q2,v[0]))  
           continue;
         id = qtRoot_visit_tri_edges2(rootptr,q0,q1,q2,v,i_pt,&w,
                                      func,arg1,arg2);
         if(id == INVALID)
         {
#ifdef DEBUG
           eputs("stVisit_tri_edges(): Unable to trace edges\n");
#endif
           return(INVALID);
         }
         if(id == QT_DONE)
            return(*arg1);
        
         /* Crossed over to next cell: id = nbr */
         while(1) 
         {
             /* test if ray crosses plane between this quadtree triangle and
                its neighbor- if it does then find intersection point with 
                ray and plane- this is the new origin
                */
           i = stTri_nbrs[i][id];
           rootptr = ST_NTH_ROOT_PTR(st,i);
           stNth_base_verts(st,i,q0,q1,q2);
           id=qtRoot_visit_tri_edges2(rootptr,q0,q1,q2,v,i_pt,&w,
                                      func,arg1,arg2);
           if(id == QT_DONE)
             return(*arg1);
           if(id == INVALID)
             {
#ifdef DEBUG
             eputs("stVisit_tri_edges(): point not found\n");
#endif  
             return(INVALID);
             }
           
         }
     }    /* Point not found */
    return(INVALID); 
}

int
stTrace_edge(st,orig,dir,max_t,func,arg1,arg2)
   STREE *st;
   FVECT orig,dir;
   double max_t;
   int (*func)();
   int *arg1,arg2;
{
    int id,i;
    QUADTREE *rootptr;
    FVECT q0,q1,q2,o,n,d;
    double pd,t;

#if DEBUG
    if(max_t > 1.0 || max_t < 0.0)
    {
      eputs("stTrace_edge(): max_t must be in [0,1]:adjusting\n");
      max_t = 1.0;
    }
#endif

    VCOPY(o,orig);
    for(i=0; i < 4; i++)
     {
#ifdef TEST_DRIVER 
Pick_cnt = 0;
#endif
         rootptr = ST_NTH_ROOT_PTR(st,i);
         stNth_base_verts(st,i,q0,q1,q2);
         /* Return quadtree tri that p falls in */
         id= qtRoot_trace_edge(rootptr,q0,q1,q2,o,dir,max_t,func,arg1,arg2);
         if(id == INVALID)
           continue;
         if(id == QT_DONE)
            return(*arg1);
        
         /* Crossed over to next cell: id = nbr */
         while(1) 
         {
             /* test if ray crosses plane between this quadtree triangle and
                its neighbor- if it does then find intersection point with 
                ray and plane- this is the new origin
                */
           if(id==0)
             VCROSS(n,q1,q2);
           else
             if(id==1)
               VCROSS(n,q2,q0);
           else
             VCROSS(n,q0,q1);

           /* Ray does not cross into next cell: done and tri not found*/
           if(!intersect_ray_plane(orig,dir,n,0.0,&t,o))
             return(INVALID);

           VSUM(o,o,dir,10*FTINY);

           d[0] = dir[0]*(1-t-10*FTINY);
           d[1] = dir[1]*(1-t-10*FTINY);
           d[2] = dir[2]*(1-t-10*FTINY);
           i = stTri_nbrs[i][id];
           rootptr = ST_NTH_ROOT_PTR(st,i);
           stNth_base_verts(st,i,q0,q1,q2);
           id = qtRoot_trace_edge(rootptr,q0,q1,q2,o,d,max_t,func,arg1,arg2);
           if(id == QT_DONE)
             return(*arg1);
           if(id == INVALID)
             {
#if 0
             eputs("stTrace_edges(): point not found\n");
#endif  
             return(INVALID);
             }
           
         }
     }    /* Point not found */
    return(INVALID);
}


int
stTrace_ray(st,orig,dir,func,arg1,arg2)
   STREE *st;
   FVECT orig,dir;
   int (*func)();
   int *arg1,arg2;
{
    int id,i;
    QUADTREE *rootptr;
    FVECT q0,q1,q2,o,n;
    double pd,t;

    VCOPY(o,orig);
    for(i=0; i < 4; i++)
     {
#ifdef TEST_DRIVER 
Pick_cnt = 0;
#endif
         rootptr = ST_NTH_ROOT_PTR(st,i);
         stNth_base_verts(st,i,q0,q1,q2);
         /* Return quadtree tri that p falls in */
         id= qtRoot_trace_ray(rootptr,q0,q1,q2,o,dir,func,arg1,arg2);
         if(id == INVALID)
           continue;
         if(id == QT_DONE)
            return(*arg1);
        
         /* Crossed over to next cell: id = nbr */
         while(1) 
         {
             /* test if ray crosses plane between this quadtree triangle and
                its neighbor- if it does then find intersection point with 
                ray and plane- this is the new origin
                */
           if(id==0)
             VCROSS(n,q1,q2);
           else
             if(id==1)
               VCROSS(n,q2,q0);
           else
             VCROSS(n,q0,q1);

           /* Ray does not cross into next cell: done and tri not found*/
           if(!intersect_ray_plane(orig,dir,n,0.0,NULL,o))
             return(INVALID);

           VSUM(o,o,dir,10*FTINY);
           i = stTri_nbrs[i][id];
           rootptr = ST_NTH_ROOT_PTR(st,i);
           stNth_base_verts(st,i,q0,q1,q2);
           id = qtRoot_trace_ray(rootptr,q0,q1,q2,o,dir,func,arg1,arg2);
           if(id == QT_DONE)
             return(*arg1);
           if(id == INVALID)
             return(INVALID);
           
         }
     }    /* Point not found */
    return(INVALID);
}


stVisit_tri_interior(st,t0,t1,t2,func,arg1,arg2)
   STREE *st;
   FVECT t0,t1,t2;
   int (*func)();
   int *arg1,arg2;
{
  int i;
  QUADTREE *rootptr;
  FVECT q0,q1,q2;

  for(i=0; i < 4; i++)
  {
    rootptr = ST_NTH_ROOT_PTR(st,i);
    stNth_base_verts(st,i,q0,q1,q2); 
    qtVisit_tri_interior(rootptr,q0,q1,q2,t0,t1,t2,0,func,arg1,arg2);
  }
}


int
stApply_to_tri(st,t0,t1,t2,func,arg1,arg2)
   STREE *st;
   FVECT t0,t1,t2;
   int (*func)();
   int *arg1,arg2;
{
    int f;
    FVECT dir;
    
  /* First add all of the leaf cells lying on the triangle perimeter:
     mark all cells seen on the way
   */
    qtClearAllFlags();          /* clear all quadtree branch flags */
    f = 0;
    VSUB(dir,t1,t0);
    stTrace_edge(st,t0,dir,1.0,func,arg1,arg2);
    VSUB(dir,t2,t1);
    stTrace_edge(st,t1,dir,1.0,func,arg1,arg2);
    VSUB(dir,t0,t2);
    stTrace_edge(st,t2,dir,1.0,func,arg1,arg2);
    /* Now visit interior */
    stVisit_tri_interior(st,t0,t1,t2,func,arg1,arg2);
}


int
stUpdate_tri(st,t_id,t0,t1,t2,edge_func,interior_func)
   STREE *st;
   int t_id;
   FVECT t0,t1,t2;
   int (*edge_func)(),(*interior_func)();
{
    int f;
    FVECT dir;
    
  /* First add all of the leaf cells lying on the triangle perimeter:
     mark all cells seen on the way
   */
    ST_CLEAR_FLAGS(st);
    f = 0;
    /* Visit cells along edges of the tri */

    stVisit_tri_edges2(st,t0,t1,t2,edge_func,&f,t_id);

    /* Now visit interior */
    if(QT_FLAG_FILL_TRI(f) || QT_FLAG_UPDATE(f))
       stVisit_tri_interior(st,t0,t1,t2,interior_func,&f,t_id);
}


Revision:	3.4
Committed:	Fri Sep 11 11:52:27 1998 UTC (25 years, 7 months ago) by gwlarson
Content type:	text/plain
Branch:	MAIN
Changes since 3.3:	+380 -92 lines
Log Message:	fixed triangle insertion using edge tracing
#	User	Rev	Content
1	gwlarson	3.1	/* Copyright (c) 1998 Silicon Graphics, Inc. */
2
3			#ifndef lint
4			static char SCCSid[] = "$SunId$ SGI";
5			#endif
6
7			/*
8			* sm_stree.c
9			*/
10			#include "standard.h"
11			#include "sm_geom.h"
12			#include "sm_stree.h"
13
14			/* Define 4 vertices on the sphere to create a tetrahedralization on
15			the sphere: triangles are as follows:
16	gwlarson	3.4	(2,1,0),(3,2,0), (1,3,0), (2,3,1)
17	gwlarson	3.1	*/
18
19	gwlarson	3.4	#ifdef TEST_DRIVER
20			extern FVECT Pick_point[500],Pick_v0[500],Pick_v1[500],Pick_v2[500];
21			extern int Pick_cnt;
22			#endif
23	gwlarson	3.1	FVECT stDefault_base[4] = { {SQRT3_INV, SQRT3_INV, SQRT3_INV},
24			{-SQRT3_INV, -SQRT3_INV, SQRT3_INV},
25			{-SQRT3_INV, SQRT3_INV, -SQRT3_INV},
26			{SQRT3_INV, -SQRT3_INV, -SQRT3_INV}};
27	gwlarson	3.4	int stTri_verts[4][3] = { {2,1,0},{3,2,0},{1,3,0},{2,3,1}};
28			int stTri_nbrs[4][3] = { {2,1,3},{0,2,3},{1,0,3},{2,0,1}};
29	gwlarson	3.1
30			stNth_base_verts(st,i,v1,v2,v3)
31			STREE *st;
32			int i;
33			FVECT v1,v2,v3;
34			{
35			VCOPY(v1,ST_NTH_BASE(st,stTri_verts[i][0]));
36			VCOPY(v2,ST_NTH_BASE(st,stTri_verts[i][1]));
37			VCOPY(v3,ST_NTH_BASE(st,stTri_verts[i][2]));
38			}
39
40			/* Frees the 4 quadtrees rooted at st */
41			stClear(st)
42			STREE *st;
43			{
44			int i;
45
46			/* stree always has 4 children corresponding to the base tris
47			*/
48			for (i = 0; i < 4; i++)
49			qtFree(ST_NTH_ROOT(st, i));
50
51			QT_CLEAR_CHILDREN(ST_ROOT(st));
52
53			}
54
55
56			STREE
57			*stInit(st,center,base)
58			STREE *st;
59			FVECT center,base[4];
60			{
61
62			if(base)
63			ST_SET_BASE(st,base);
64			else
65			ST_SET_BASE(st,stDefault_base);
66
67			ST_SET_CENTER(st,center);
68			stClear(st);
69
70			return(st);
71			}
72
73
74			/* "base" defines 4 vertices on the sphere to create a tetrahedralization on
75			the sphere: triangles are as follows:(0,1,2),(0,2,3), (0,3,1), (1,3,2)
76			if base is null: does default.
77
78			*/
79			STREE
80			*stAlloc(st)
81			STREE *st;
82			{
83			int i;
84
85			if(!st)
86			st = (STREE *)malloc(sizeof(STREE));
87
88			ST_ROOT(st) = qtAlloc();
89
90			QT_CLEAR_CHILDREN(ST_ROOT(st));
91
92			return(st);
93			}
94
95
96			/* Find location of sample point in the DAG and return lowest level
97			containing triangle. "type" indicates whether the point was found
98			to be in interior to the triangle: GT_FACE, on one of its
99			edges GT_EDGE or coinciding with one of its vertices GT_VERTEX.
100			"which" specifies which vertex (0,1,2) or edge (0=v0v1, 1 = v1v2, 2 = v21)
101			*/
102			int
103	gwlarson	3.4	stPoint_locate(st,npt)
104	gwlarson	3.1	STREE *st;
105			FVECT npt;
106			{
107			int i,d,j,id;
108	gwlarson	3.3	QUADTREE rootptr,qtptr;
109	gwlarson	3.1	FVECT v1,v2,v3;
110	gwlarson	3.4	OBJECT os[QT_MAXSET+1],*optr;
111	gwlarson	3.1	FVECT p0,p1,p2;
112	gwlarson	3.2
113	gwlarson	3.1	/* Test each of the root triangles against point id */
114			for(i=0; i < 4; i++)
115			{
116			rootptr = ST_NTH_ROOT_PTR(st,i);
117			stNth_base_verts(st,i,v1,v2,v3);
118			/* Return tri that p falls in */
119	gwlarson	3.3	qtptr = qtRoot_point_locate(rootptr,v1,v2,v3,npt,NULL,NULL,NULL);
120	gwlarson	3.4	if(!qtptr \|\| QT_IS_EMPTY(*qtptr))
121	gwlarson	3.1	continue;
122			/* Get the set */
123	gwlarson	3.4	optr = qtqueryset(*qtptr);
124			for (j = QT_SET_CNT(optr),optr = QT_SET_PTR(optr);j > 0; j--)
125	gwlarson	3.1	{
126			/* Find the first triangle that pt falls */
127			id = QT_SET_NEXT_ELEM(optr);
128	gwlarson	3.4	qtTri_from_id(id,NULL,NULL,NULL,p0,p1,p2,NULL,NULL,NULL);
129			d = point_in_stri(p0,p1,p2,npt);
130	gwlarson	3.1	if(d)
131	gwlarson	3.4	return(id);
132	gwlarson	3.1	}
133			}
134			return(EMPTY);
135			}
136
137	gwlarson	3.3	QUADTREE
138			*stPoint_locate_cell(st,p,t0,t1,t2)
139	gwlarson	3.1	STREE *st;
140	gwlarson	3.2	FVECT p;
141	gwlarson	3.3	FVECT t0,t1,t2;
142	gwlarson	3.1	{
143			int i,d;
144	gwlarson	3.3	QUADTREE rootptr,qtptr;
145	gwlarson	3.1	FVECT v0,v1,v2;
146
147
148			/* Test each of the root triangles against point id */
149			for(i=0; i < 4; i++)
150			{
151			rootptr = ST_NTH_ROOT_PTR(st,i);
152			stNth_base_verts(st,i,v0,v1,v2);
153	gwlarson	3.4	/* Return quadtree tri that p falls in */
154	gwlarson	3.3	qtptr = qtRoot_point_locate(rootptr,v0,v1,v2,p,t0,t1,t2);
155			if(qtptr)
156			return(qtptr);
157	gwlarson	3.1	} /* Point not found */
158	gwlarson	3.3	return(NULL);
159	gwlarson	3.1	}
160
161	gwlarson	3.3
162	gwlarson	3.1	int
163	gwlarson	3.2	stAdd_tri(st,id,v0,v1,v2)
164	gwlarson	3.1	STREE *st;
165			int id;
166	gwlarson	3.2	FVECT v0,v1,v2;
167	gwlarson	3.1	{
168
169			int i,found;
170			QUADTREE *rootptr;
171	gwlarson	3.2	FVECT t0,t1,t2;
172
173	gwlarson	3.1	found = 0;
174			for(i=0; i < 4; i++)
175			{
176			rootptr = ST_NTH_ROOT_PTR(st,i);
177	gwlarson	3.2	stNth_base_verts(st,i,t0,t1,t2);
178	gwlarson	3.4	found \|= qtRoot_add_tri(rootptr,t0,t1,t2,v0,v1,v2,id,0);
179	gwlarson	3.1	}
180			return(found);
181			}
182
183			int
184			stApply_to_tri_cells(st,v0,v1,v2,func,arg)
185			STREE *st;
186			FVECT v0,v1,v2;
187			int (*func)();
188	gwlarson	3.4	int *arg;
189	gwlarson	3.1	{
190			int i,found;
191			QUADTREE *rootptr;
192			FVECT t0,t1,t2;
193
194			found = 0;
195	gwlarson	3.4	func(ST_ROOT_PTR(st),arg);
196			QT_SET_FLAG(ST_ROOT(st));
197	gwlarson	3.1	for(i=0; i < 4; i++)
198			{
199			rootptr = ST_NTH_ROOT_PTR(st,i);
200			stNth_base_verts(st,i,t0,t1,t2);
201			found \|= qtApply_to_tri_cells(rootptr,v0,v1,v2,t0,t1,t2,func,arg);
202			}
203			return(found);
204			}
205
206
207
208
209			int
210			stRemove_tri(st,id,v0,v1,v2)
211			STREE *st;
212			int id;
213			FVECT v0,v1,v2;
214			{
215
216			int i,found;
217			QUADTREE *rootptr;
218			FVECT t0,t1,t2;
219
220			found = 0;
221			for(i=0; i < 4; i++)
222			{
223			rootptr = ST_NTH_ROOT_PTR(st,i);
224			stNth_base_verts(st,i,t0,t1,t2);
225			found \|= qtRemove_tri(rootptr,id,v0,v1,v2,t0,t1,t2);
226			}
227			return(found);
228			}
229
230	gwlarson	3.4	int
231			stVisit_tri_edges(st,t0,t1,t2,func,arg1,arg2)
232			STREE *st;
233			FVECT t0,t1,t2;
234			int (*func)();
235			int *arg1,arg2;
236			{
237			int id,i,w;
238			QUADTREE *rootptr;
239			FVECT q0,q1,q2,n,v[3],sdir[3],dir[3],tv,d;
240			double pd,t;
241	gwlarson	3.1
242	gwlarson	3.4	VCOPY(v[0],t0); VCOPY(v[1],t1); VCOPY(v[2],t2);
243			VSUB(dir[0],t1,t0); VSUB(dir[1],t2,t1);VSUB(dir[2],t0,t2);
244			VCOPY(sdir[0],dir[0]);VCOPY(sdir[1],dir[1]);VCOPY(sdir[2],dir[2]);
245			w = 0;
246			for(i=0; i < 4; i++)
247			{
248			#ifdef TEST_DRIVER
249			Pick_cnt = 0;
250			#endif
251			rootptr = ST_NTH_ROOT_PTR(st,i);
252			stNth_base_verts(st,i,q0,q1,q2);
253			/* Return quadtree tri that p falls in */
254			if(!point_in_stri(q0,q1,q2,v[w]))
255			continue;
256			id = qtRoot_visit_tri_edges(rootptr,q0,q1,q2,v,dir,&w,func,arg1,arg2);
257			if(id == INVALID)
258			{
259			#ifdef DEBUG
260			eputs("stVisit_tri_edges(): Unable to trace edges\n");
261			#endif
262			return(INVALID);
263			}
264			if(id == QT_DONE)
265			return(*arg1);
266
267			/* Crossed over to next cell: id = nbr */
268			while(1)
269			{
270			/* test if ray crosses plane between this quadtree triangle and
271			its neighbor- if it does then find intersection point with
272			ray and plane- this is the new origin
273			*/
274			if(id==0)
275			VCROSS(n,q1,q2);
276			else
277			if(id==1)
278			VCROSS(n,q2,q0);
279			else
280			VCROSS(n,q0,q1);
281	gwlarson	3.1
282	gwlarson	3.4	if(w==0)
283			VCOPY(tv,t0);
284			else
285			if(w==1)
286			VCOPY(tv,t1);
287			else
288			VCOPY(tv,t2);
289			if(!intersect_ray_plane(tv,sdir[w],n,0.0,&t,v[w]))
290			return(INVALID);
291	gwlarson	3.1
292	gwlarson	3.4	VSUM(v[w],v[w],sdir[w],10.0*FTINY);
293	gwlarson	3.1
294	gwlarson	3.4	t = (1.0-t-10.0*FTINY);
295			if(t <= 0.0)
296			{
297			t = FTINY;
298	gwlarson	3.3	#if 0
299	gwlarson	3.4	eputs("stVisit_tri_edges(): edge end on plane\n");
300			#endif
301			}
302			dir[w][0] = sdir[w][0] * t;
303			dir[w][1] = sdir[w][1] * t;
304			dir[w][2] = sdir[w][2] * t;
305			i = stTri_nbrs[i][id];
306			rootptr = ST_NTH_ROOT_PTR(st,i);
307			stNth_base_verts(st,i,q0,q1,q2);
308			id=qtRoot_visit_tri_edges(rootptr,q0,q1,q2,v,dir,&w,func,arg1,arg2);
309			if(id == QT_DONE)
310			return(*arg1);
311			if(id == INVALID)
312			{
313			#if 0
314			eputs("stVisit_tri_edges(): point not found\n");
315			#endif
316			return(INVALID);
317			}
318
319			}
320			} /* Point not found */
321			return(INVALID);
322			}
323
324
325	gwlarson	3.3	int
326	gwlarson	3.4	stVisit_tri_edges2(st,t0,t1,t2,func,arg1,arg2)
327			STREE *st;
328			FVECT t0,t1,t2;
329			int (*func)();
330			int *arg1,arg2;
331	gwlarson	3.3	{
332	gwlarson	3.4	int id,i,w;
333			QUADTREE *rootptr;
334			FVECT q0,q1,q2,v[3],i_pt;
335	gwlarson	3.3
336	gwlarson	3.4	VCOPY(v[0],t0); VCOPY(v[1],t1); VCOPY(v[2],t2);
337			w = -1;
338			for(i=0; i < 4; i++)
339			{
340			#ifdef TEST_DRIVER
341			Pick_cnt = 0;
342			#endif
343			rootptr = ST_NTH_ROOT_PTR(st,i);
344			stNth_base_verts(st,i,q0,q1,q2);
345			/* Return quadtree tri that p falls in */
346			if(!point_in_stri(q0,q1,q2,v[0]))
347			continue;
348			id = qtRoot_visit_tri_edges2(rootptr,q0,q1,q2,v,i_pt,&w,
349			func,arg1,arg2);
350			if(id == INVALID)
351			{
352			#ifdef DEBUG
353			eputs("stVisit_tri_edges(): Unable to trace edges\n");
354			#endif
355			return(INVALID);
356			}
357			if(id == QT_DONE)
358			return(*arg1);
359
360			/* Crossed over to next cell: id = nbr */
361			while(1)
362			{
363			/* test if ray crosses plane between this quadtree triangle and
364			its neighbor- if it does then find intersection point with
365			ray and plane- this is the new origin
366			*/
367			i = stTri_nbrs[i][id];
368			rootptr = ST_NTH_ROOT_PTR(st,i);
369			stNth_base_verts(st,i,q0,q1,q2);
370			id=qtRoot_visit_tri_edges2(rootptr,q0,q1,q2,v,i_pt,&w,
371			func,arg1,arg2);
372			if(id == QT_DONE)
373			return(*arg1);
374			if(id == INVALID)
375			{
376			#ifdef DEBUG
377			eputs("stVisit_tri_edges(): point not found\n");
378			#endif
379			return(INVALID);
380			}
381
382			}
383			} /* Point not found */
384			return(INVALID);
385			}
386
387			int
388			stTrace_edge(st,orig,dir,max_t,func,arg1,arg2)
389			STREE *st;
390			FVECT orig,dir;
391			double max_t;
392			int (*func)();
393			int *arg1,arg2;
394			{
395			int id,i;
396			QUADTREE *rootptr;
397			FVECT q0,q1,q2,o,n,d;
398			double pd,t;
399
400			#if DEBUG
401			if(max_t > 1.0 \|\| max_t < 0.0)
402			{
403			eputs("stTrace_edge(): max_t must be in [0,1]:adjusting\n");
404			max_t = 1.0;
405			}
406			#endif
407
408			VCOPY(o,orig);
409			for(i=0; i < 4; i++)
410			{
411			#ifdef TEST_DRIVER
412			Pick_cnt = 0;
413			#endif
414			rootptr = ST_NTH_ROOT_PTR(st,i);
415			stNth_base_verts(st,i,q0,q1,q2);
416			/* Return quadtree tri that p falls in */
417			id= qtRoot_trace_edge(rootptr,q0,q1,q2,o,dir,max_t,func,arg1,arg2);
418			if(id == INVALID)
419			continue;
420			if(id == QT_DONE)
421			return(*arg1);
422
423			/* Crossed over to next cell: id = nbr */
424			while(1)
425			{
426			/* test if ray crosses plane between this quadtree triangle and
427			its neighbor- if it does then find intersection point with
428			ray and plane- this is the new origin
429			*/
430			if(id==0)
431			VCROSS(n,q1,q2);
432			else
433			if(id==1)
434			VCROSS(n,q2,q0);
435			else
436			VCROSS(n,q0,q1);
437
438			/* Ray does not cross into next cell: done and tri not found*/
439			if(!intersect_ray_plane(orig,dir,n,0.0,&t,o))
440			return(INVALID);
441
442			VSUM(o,o,dir,10*FTINY);
443
444			d[0] = dir[0](1-t-10FTINY);
445			d[1] = dir[1](1-t-10FTINY);
446			d[2] = dir[2](1-t-10FTINY);
447			i = stTri_nbrs[i][id];
448			rootptr = ST_NTH_ROOT_PTR(st,i);
449			stNth_base_verts(st,i,q0,q1,q2);
450			id = qtRoot_trace_edge(rootptr,q0,q1,q2,o,d,max_t,func,arg1,arg2);
451			if(id == QT_DONE)
452			return(*arg1);
453			if(id == INVALID)
454			{
455			#if 0
456			eputs("stTrace_edges(): point not found\n");
457			#endif
458			return(INVALID);
459			}
460
461			}
462			} /* Point not found */
463			return(INVALID);
464			}
465
466
467
468			int
469			stTrace_ray(st,orig,dir,func,arg1,arg2)
470			STREE *st;
471			FVECT orig,dir;
472			int (*func)();
473			int *arg1,arg2;
474			{
475			int id,i;
476			QUADTREE *rootptr;
477			FVECT q0,q1,q2,o,n;
478			double pd,t;
479
480			VCOPY(o,orig);
481			for(i=0; i < 4; i++)
482			{
483			#ifdef TEST_DRIVER
484			Pick_cnt = 0;
485			#endif
486			rootptr = ST_NTH_ROOT_PTR(st,i);
487			stNth_base_verts(st,i,q0,q1,q2);
488			/* Return quadtree tri that p falls in */
489			id= qtRoot_trace_ray(rootptr,q0,q1,q2,o,dir,func,arg1,arg2);
490			if(id == INVALID)
491			continue;
492			if(id == QT_DONE)
493			return(*arg1);
494
495			/* Crossed over to next cell: id = nbr */
496			while(1)
497			{
498			/* test if ray crosses plane between this quadtree triangle and
499			its neighbor- if it does then find intersection point with
500			ray and plane- this is the new origin
501			*/
502			if(id==0)
503			VCROSS(n,q1,q2);
504			else
505			if(id==1)
506			VCROSS(n,q2,q0);
507			else
508			VCROSS(n,q0,q1);
509
510			/* Ray does not cross into next cell: done and tri not found*/
511			if(!intersect_ray_plane(orig,dir,n,0.0,NULL,o))
512			return(INVALID);
513
514			VSUM(o,o,dir,10*FTINY);
515			i = stTri_nbrs[i][id];
516			rootptr = ST_NTH_ROOT_PTR(st,i);
517			stNth_base_verts(st,i,q0,q1,q2);
518			id = qtRoot_trace_ray(rootptr,q0,q1,q2,o,dir,func,arg1,arg2);
519			if(id == QT_DONE)
520			return(*arg1);
521			if(id == INVALID)
522			return(INVALID);
523
524			}
525			} /* Point not found */
526			return(INVALID);
527			}
528
529
530
531			stVisit_tri_interior(st,t0,t1,t2,func,arg1,arg2)
532			STREE *st;
533			FVECT t0,t1,t2;
534			int (*func)();
535			int *arg1,arg2;
536			{
537			int i;
538			QUADTREE *rootptr;
539			FVECT q0,q1,q2;
540
541			for(i=0; i < 4; i++)
542			{
543			rootptr = ST_NTH_ROOT_PTR(st,i);
544			stNth_base_verts(st,i,q0,q1,q2);
545			qtVisit_tri_interior(rootptr,q0,q1,q2,t0,t1,t2,0,func,arg1,arg2);
546			}
547			}
548
549
550			int
551			stApply_to_tri(st,t0,t1,t2,func,arg1,arg2)
552			STREE *st;
553			FVECT t0,t1,t2;
554			int (*func)();
555			int *arg1,arg2;
556			{
557			int f;
558			FVECT dir;
559
560	gwlarson	3.3	/* First add all of the leaf cells lying on the triangle perimeter:
561			mark all cells seen on the way
562			*/
563	gwlarson	3.4	qtClearAllFlags(); /* clear all quadtree branch flags */
564			f = 0;
565			VSUB(dir,t1,t0);
566			stTrace_edge(st,t0,dir,1.0,func,arg1,arg2);
567			VSUB(dir,t2,t1);
568			stTrace_edge(st,t1,dir,1.0,func,arg1,arg2);
569			VSUB(dir,t0,t2);
570			stTrace_edge(st,t2,dir,1.0,func,arg1,arg2);
571			/* Now visit interior */
572			stVisit_tri_interior(st,t0,t1,t2,func,arg1,arg2);
573			}
574	gwlarson	3.3
575	gwlarson	3.4
576
577
578
579			int
580			stUpdate_tri(st,t_id,t0,t1,t2,edge_func,interior_func)
581			STREE *st;
582			int t_id;
583			FVECT t0,t1,t2;
584			int (edge_func)(),(interior_func)();
585			{
586			int f;
587			FVECT dir;
588
589			/* First add all of the leaf cells lying on the triangle perimeter:
590			mark all cells seen on the way
591	gwlarson	3.3	*/
592	gwlarson	3.4	ST_CLEAR_FLAGS(st);
593			f = 0;
594			/* Visit cells along edges of the tri */
595	gwlarson	3.3
596	gwlarson	3.4	stVisit_tri_edges2(st,t0,t1,t2,edge_func,&f,t_id);
597
598			/* Now visit interior */
599			if(QT_FLAG_FILL_TRI(f) \|\| QT_FLAG_UPDATE(f))
600			stVisit_tri_interior(st,t0,t1,t2,interior_func,&f,t_id);
601	gwlarson	3.3	}
602
603	gwlarson	3.4
604
605