--- ray/src/hd/sm.c	1998/09/14 10:33:46	3.6
+++ ray/src/hd/sm.c	1998/11/11 12:05:37	3.9
@@ -8,23 +8,34 @@ static char SCCSid[] = "$SunId$ SGI";
  *  sm.c
  */
 #include "standard.h"
+#include "sm_flag.h"
 #include "sm_list.h"
 #include "sm_geom.h"
+#include "sm_qtree.h"
+#include "sm_stree.h"
 #include "sm.h"
 
 
 SM *smMesh = NULL;
 double smDist_sum=0;
 int smNew_tri_cnt=0;
+double smMinSampDiff = 1.7e-3;	/* min edge length in radians */
 
-static int smBase_nbrs[4][3] =  { {3,2,1},{3,0,2},{3,1,0},{1,2,0}};
+static FVECT smDefault_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}};
+static int smTri_verts[4][3] = { {0,1,2},{0,2,3},{0,3,1},{1,3,2}};
 
+static int smBase_nbrs[4][3] =  { {3,1,2},{3,2,0},{3,0,1},{1,0,2}};
+
 #ifdef TEST_DRIVER
 VIEW  Current_View = {0,{0,0,0},{0,0,-1},{0,1,0},60,60,0};
 int Pick_cnt =0;
 int Pick_tri = -1,Picking = FALSE,Pick_samp=-1;
 FVECT Pick_point[500],Pick_origin,Pick_dir;
 FVECT  Pick_v0[500], Pick_v1[500], Pick_v2[500];
+int    Pick_q[500];
 FVECT P0,P1,P2;
 FVECT FrustumNear[4],FrustumFar[4];
 double dev_zmin=.01,dev_zmax=1000;
@@ -35,19 +46,18 @@ smDir(sm,ps,id)
    FVECT ps;
    int id;
 {
-    FVECT p;
-    
-    VCOPY(p,SM_NTH_WV(sm,id));
-    point_on_sphere(ps,p,SM_VIEW_CENTER(sm)); 
+    VSUB(ps,SM_NTH_WV(sm,id),SM_VIEW_CENTER(sm)); 
+    normalize(ps);
 }
 
-smClear_mesh(sm)
-    SM *sm;
+smDir_in_cone(sm,ps,id)
+   SM *sm;
+   FVECT ps;
+   int id;
 {
-    /* Reset the triangle counters */
-    SM_TRI_CNT(sm) = 0;
-    SM_NUM_TRIS(sm) = 0;
-    SM_FREE_TRIS(sm) = -1;
+    
+    VSUB(ps,SM_NTH_WV(sm,id),SM_VIEW_CENTER(sm)); 
+    normalize(ps);
 }
 
 smClear_flags(sm,which)
@@ -58,95 +68,132 @@ int which;
 
   if(which== -1)
     for(i=0; i < T_FLAGS;i++)
-      bzero(SM_NTH_FLAGS(sm,i),T_TOTAL_FLAG_BYTES(SM_MAX_TRIS(sm)));
+      bzero(SM_NTH_FLAGS(sm,i),FLAG_BYTES(SM_MAX_TRIS(sm)));
   else
-    bzero(SM_NTH_FLAGS(sm,which),T_TOTAL_FLAG_BYTES(SM_MAX_TRIS(sm)));
+    bzero(SM_NTH_FLAGS(sm,which),FLAG_BYTES(SM_MAX_TRIS(sm)));
 }
 
-smClear_locator(sm)
-SM *sm;
+/* Given an allocated mesh- initialize */
+smInit_mesh(sm)
+    SM *sm;
 {
-  STREE  *st;
-  
-  st = SM_LOCATOR(sm);
- 
-  stClear(st);
+    /* Reset the triangle counters */
+    SM_NUM_TRI(sm) = 0;
+    SM_SAMPLE_TRIS(sm) = 0;
+    SM_FREE_TRIS(sm) = -1;
+    smClear_flags(sm,-1);
 }
 
-smInit_locator(sm,center,base)
-SM *sm;
-FVECT  center,base[4];
-{
-  STREE  *st;
-  
-  st = SM_LOCATOR(sm);
- 
-  stInit(st,center,base);
 
-}
-
+/* Clear the SM for reuse: free any extra allocated memory:does initialization
+   as well
+*/
 smClear(sm)
 SM *sm;
 {
   smClear_samples(sm);
-  smClear_mesh(sm);
   smClear_locator(sm);
+  smInit_mesh(sm);
 }
 
+static int realloc_cnt=0;
+
 int
-smAlloc_tris(sm,max_verts,max_tris)
+smRealloc_mesh(sm)
 SM *sm;
+{
+  int fbytes,i,max_tris,max_verts;
+  
+  if(realloc_cnt <=0)
+    realloc_cnt = SM_MAX_TRIS(sm);
+
+  max_tris = SM_MAX_TRIS(sm) + realloc_cnt;
+  fbytes = FLAG_BYTES(max_tris);
+
+  if(!(SM_TRIS(sm) = (TRI *)realloc(SM_TRIS(sm),max_tris*sizeof(TRI))))
+    goto memerr;
+
+  for(i=0; i< T_FLAGS; i++)
+   if(!(SM_NTH_FLAGS(sm,i)=(int4 *)realloc((char *)SM_NTH_FLAGS(sm,i),fbytes)))
+	goto memerr;
+
+  SM_MAX_TRIS(sm) = max_tris;
+  return(max_tris);
+
+memerr:
+	error(SYSTEM, "out of memory in smRealloc_mesh()");
+}
+
+/* Allocate and initialize a new mesh with max_verts and max_tris */
+int
+smAlloc_mesh(sm,max_verts,max_tris)
+SM *sm;
 int max_verts,max_tris;
 {
-    int i,nbytes,vbytes,fbytes;
+    int fbytes,i;
 
-    vbytes = max_verts*sizeof(VERT);
-    fbytes = T_TOTAL_FLAG_BYTES(max_tris);
-    nbytes = vbytes + max_tris*sizeof(TRI) +T_FLAGS*fbytes + 8;
-    for(i = 1024; nbytes > i; i <<= 1)
-		;
-    /* check if casting works correctly */
-    max_tris = (i-vbytes-8)/(sizeof(TRI) + T_FLAG_BYTES);
-    fbytes = T_TOTAL_FLAG_BYTES(max_tris);
-    
-    SM_BASE(sm)=(char *)malloc(vbytes+max_tris*sizeof(TRI)+T_FLAGS*fbytes);
+    fbytes = FLAG_BYTES(max_tris);
 
-    if (SM_BASE(sm) == NULL)
-       return(0);
+    if(!(SM_TRIS(sm) = (TRI *)realloc(NULL,max_tris*sizeof(TRI))))
+      goto memerr;
 
-    SM_TRIS(sm) = (TRI *)SM_BASE(sm);
-    SM_VERTS(sm) = (VERT *)(SM_TRIS(sm) + max_tris);
+    if(!(SM_VERTS(sm) = (VERT *)realloc(NULL,max_verts*sizeof(VERT))))
+      goto memerr;
 
-    SM_NTH_FLAGS(sm,0) = (int4 *)(SM_VERTS(sm) + max_verts);
-    for(i=1; i < T_FLAGS; i++)
-       SM_NTH_FLAGS(sm,i) = (int4 *)(SM_NTH_FLAGS(sm,i-1)+fbytes/sizeof(int4));
-    
+    for(i=0; i< T_FLAGS; i++)
+      if(!(SM_NTH_FLAGS(sm,i)=(int4 *)realloc(NULL,fbytes)))
+	goto memerr;
+
     SM_MAX_VERTS(sm) = max_verts;
     SM_MAX_TRIS(sm) = max_tris;
 
-    smClear_mesh(sm);
+    realloc_cnt = max_verts >> 1;
 
+    smInit_mesh(sm);
+
     return(max_tris);
+memerr:
+	error(SYSTEM, "out of memory in smAlloc_mesh()");
 }
 
 
-
 int
-smAlloc_locator(sm)
+smAlloc_tri(sm)
 SM *sm;
 {
-  STREE  *st;
-  
-  st = SM_LOCATOR(sm);
- 
-  st = stAlloc(st);
- 
-  if(st)
-    return(TRUE);
-  else
-    return(FALSE);
+  int id;
+
+  /* First check if there are any tris on the free list */
+  /* Otherwise, have we reached the end of the list yet?*/
+  if(SM_NUM_TRI(sm) < SM_MAX_TRIS(sm))
+    return(SM_NUM_TRI(sm)++);
+
+  if((id = SM_FREE_TRIS(sm)) != -1)
+  {
+    SM_FREE_TRIS(sm) =  T_NEXT_FREE(SM_NTH_TRI(sm,id));
+    return(id);
+  }
+
+  /*Else: realloc */
+  smRealloc_mesh(sm);
+  return(SM_NUM_TRI(sm)++);
 }
 
+smFree_mesh(sm)
+SM *sm;
+{
+  int i;
+
+  if(SM_TRIS(sm))
+    free(SM_TRIS(sm));
+  if(SM_VERTS(sm))
+    free(SM_VERTS(sm));
+  for(i=0; i< T_FLAGS; i++)
+    if(SM_NTH_FLAGS(sm,i))
+      free(SM_NTH_FLAGS(sm,i));
+}
+
+  
 /* Initialize/clear global smL sample list for at least n samples */
 smAlloc(max_samples)
    register int max_samples;
@@ -154,56 +201,53 @@ smAlloc(max_samples)
   unsigned nbytes;
   register unsigned i;
   int total_points;
-  int max_tris;
+  int max_tris,n;
 
+  n = max_samples;
   /* If this is the first call, allocate sample,vertex and triangle lists */
   if(!smMesh)
   {
     if(!(smMesh = (SM *)malloc(sizeof(SM))))
-       error(SYSTEM,"smAlloc():Unable to allocate memory");
+       error(SYSTEM,"smAlloc():Unable to allocate memory\n");
     bzero(smMesh,sizeof(SM));
   }
   else
   {   /* If existing structure: first deallocate */
-    if(SM_BASE(smMesh))
-      free(SM_BASE(smMesh));
-    if(SM_SAMP_BASE(smMesh))
-       free(SM_SAMP_BASE(smMesh));
-  }
+    smFree_mesh(smMesh);
+    smFree_samples(smMesh);
+    smFree_locator(smMesh);
+ }
 
   /* First allocate at least n samples + extra points:at least enough
    necessary to form the BASE MESH- Default = 4;
   */
-  max_samples = smAlloc_samples(smMesh, max_samples,SM_EXTRA_POINTS);
+  SM_SAMP(smMesh) = sAlloc(&n,SM_EXTRA_POINTS);
 
-  total_points = max_samples + SM_EXTRA_POINTS;
-  max_tris = total_points*2;
+  total_points = n + SM_EXTRA_POINTS;
 
+  max_tris = total_points*4;
   /* Now allocate space for mesh vertices and triangles */
-  max_tris = smAlloc_tris(smMesh, total_points, max_tris);
+  max_tris = smAlloc_mesh(smMesh, total_points, max_tris);
 
   /* Initialize the structure for point,triangle location.
    */
   smAlloc_locator(smMesh);
-
 }
 
 
 
-smInit_mesh(sm,vp)
+smInit_sm(sm,vp)
 SM *sm;
 FVECT vp;
 {
 
-  /* NOTE: Should be elsewhere?*/
   smDist_sum = 0;
   smNew_tri_cnt = 0;
 
-  VCOPY(SM_VIEW_CENTER(smMesh),vp);
-  smClear_locator(sm);
-  smInit_locator(sm,vp,0);
-  smClear_aux_samples(sm);
-  smClear_mesh(sm);  
+  VCOPY(SM_VIEW_CENTER(sm),vp);
+  smInit_locator(sm,vp);
+  smInit_samples(sm);
+  smInit_mesh(sm);  
   smCreate_base_mesh(sm,SM_DEFAULT);
 }
 
@@ -224,6 +268,7 @@ smInit(n)
 {
   int max_vertices;
 
+
   /* If n <=0, Just clear the existing structures */
   if(n <= 0)
   {
@@ -237,7 +282,8 @@ smInit(n)
   max_vertices = n + SM_EXTRA_POINTS;
 
   /* If the current mesh contains enough room, clear and return */
-  if(smMesh && max_vertices <= SM_MAX_VERTS(smMesh))
+  if(smMesh && (max_vertices <= SM_MAX_VERTS(smMesh)) && SM_MAX_SAMP(smMesh) <=
+     n && SM_MAX_POINTS(smMesh) <= max_vertices)
   {
     smClear(smMesh);
     return(SM_MAX_SAMP(smMesh));
@@ -251,15 +297,14 @@ smInit(n)
 }
 
 
-int
-smLocator_apply_func(sm,v0,v1,v2,func,arg)
-SM *sm;
-FVECT v0,v1,v2;
-int (*func)();
-int *arg;
+smLocator_apply_func(sm,v0,v1,v2,edge_func,tri_func,argptr)
+   SM *sm;
+   FVECT v0,v1,v2;
+   int (*edge_func)();
+   int (*tri_func)();
+   int *argptr;
 {
   STREE *st;
-  int found;
   FVECT p0,p1,p2;
 
   st = SM_LOCATOR(sm);
@@ -268,201 +313,233 @@ int *arg;
   VSUB(p1,v1,SM_VIEW_CENTER(sm));
   VSUB(p2,v2,SM_VIEW_CENTER(sm));
 
-  found = stApply_to_tri_cells(st,p0,p1,p2,func,arg);
+  stApply_to_tri(st,p0,p1,p2,edge_func,tri_func,argptr);
 
-  return(found);
 }
 
+QUADTREE 
+delete_tri_set(qt,optr,del_set)
+QUADTREE qt;
+OBJECT *optr,*del_set;
+{
 
-int
-add_tri_expand(qtptr,q0,q1,q2,t0,t1,t2,n,arg,t_id)
+  int set_size,i,t_id;
+  OBJECT *rptr,r_set[QT_MAXSET+1];
+  OBJECT *tptr,t_set[QT_MAXSET+1],*sptr;
+
+  /* First need to check if set size larger than QT_MAXSET: if so
+     need to allocate larger array
+     */
+  if((set_size = MAX(QT_SET_CNT(optr),QT_SET_CNT(del_set))) >QT_MAXSET)
+    rptr = (OBJECT *)malloc((set_size+1)*sizeof(OBJECT));
+  else
+    rptr = r_set;
+  if(!rptr)
+    goto memerr;
+  setintersect(rptr,del_set,optr);
+	  
+  if(QT_SET_CNT(rptr) > 0)
+  {
+    /* First need to check if set size larger than QT_MAXSET: if so
+       need to allocate larger array
+       */
+    sptr = QT_SET_PTR(rptr);
+    for(i = QT_SET_CNT(rptr); i > 0; i--)
+      {
+	t_id = QT_SET_NEXT_ELEM(sptr);
+	qt = qtdelelem(qt,t_id);
+      }
+  }
+  /* If we allocated memory: free it */
+  if(rptr != r_set)
+    free(rptr);
+
+  return(qt);
+memerr:
+    error(SYSTEM,"delete_tri_set():Unable to allocate memory");
+}
+
+QUADTREE
+expand_node(qt,q0,q1,q2,optr,n)
+QUADTREE qt;
+FVECT q0,q1,q2;
+OBJECT *optr;
+int n;
+{
+  OBJECT *tptr,t_set[QT_MAXSET+1];
+  int i,t_id,found;
+  TRI *t;
+  FVECT v0,v1,v2;
+
+  if(QT_SET_CNT(optr) > QT_MAXSET)
+    tptr = (OBJECT *)malloc((QT_SET_CNT(optr)+1)*sizeof(OBJECT));
+  else
+    tptr = t_set;
+  if(!tptr)
+    goto memerr;
+
+  qtgetset(tptr,qt);
+  /* If set size exceeds threshold: subdivide cell and reinsert tris*/
+  qtfreeleaf(qt);
+  qtSubdivide(qt);
+
+  for(optr = QT_SET_PTR(tptr),i=QT_SET_CNT(tptr); i > 0; i--)
+  {
+    t_id = QT_SET_NEXT_ELEM(optr);
+    t = SM_NTH_TRI(smMesh,t_id);
+    if(!T_IS_VALID(t))
+      continue;
+    VSUB(v0,SM_T_NTH_WV(smMesh,t,0),SM_VIEW_CENTER(smMesh));
+    VSUB(v1,SM_T_NTH_WV(smMesh,t,1),SM_VIEW_CENTER(smMesh));
+    VSUB(v2,SM_T_NTH_WV(smMesh,t,2),SM_VIEW_CENTER(smMesh));
+    qt = qtAdd_tri(qt,q0,q1,q2,v0,v1,v2,t_id,n);
+  }
+  /* If we allocated memory: free it */
+  if( tptr != t_set)
+    free(tptr);
+
+  return(qt);
+memerr:
+    error(SYSTEM,"expand_node():Unable to allocate memory");
+}
+
+add_tri_expand(qtptr,f,argptr,q0,q1,q2,t0,t1,t2,n)
 QUADTREE *qtptr;
+int *f;
+ADD_ARGS *argptr;
 FVECT q0,q1,q2;
 FVECT t0,t1,t2;
 int n;
-int *arg;
-int t_id;
 {
-    OBJECT tset[QT_MAXSET+1],*optr;	
-    int i,id,found;
-    FVECT v0,v1,v2;
-    TRI *tri;
-#ifdef DEBUG_TEST_DRIVER
-    Pick_tri = t_id;
-    Picking = TRUE;
-#endif    
-    
-    if(QT_IS_EMPTY(*qtptr))
-    {
-      *qtptr = qtaddelem(*qtptr,t_id);
-      return(TRUE);
-    }
-    
+  int t_id;
+  OBJECT *optr,*del_set;
+
+  t_id = argptr->t_id;
+
+  if(QT_IS_EMPTY(*qtptr))
+  {
+    *qtptr = qtaddelem(*qtptr,t_id);
+    return;
+  }
+  if(!QT_LEAF_IS_FLAG(*qtptr))
+  {
     optr = qtqueryset(*qtptr);
-    if(!inset(optr,t_id))
-    {
-      if(QT_SET_CNT(optr) < QT_MAXSET)
-	*qtptr = qtaddelem(*qtptr,t_id);
-      else
-	{
-#ifdef DEBUG
-	  eputs("add_tri_expand():no room in set\n");
-#endif
-	  return(FALSE);
-	}
-    }
-    optr = qtqueryset(*qtptr);
-    if(QT_SET_CNT(optr) >= QT_SET_THRESHOLD)
-      if (n < QT_MAX_LEVELS)
-      {
-	qtgetset(tset,*qtptr);
-	/* If set size exceeds threshold: subdivide cell and reinsert tris*/
-	qtfreeleaf(*qtptr);
-	qtSubdivide(qtptr);
 
-	for(optr = QT_SET_PTR(tset),i=QT_SET_CNT(tset); i > 0; i--)
-	{
-	  id = QT_SET_NEXT_ELEM(optr);
-	  tri = SM_NTH_TRI(smMesh,id);
-	  VSUB(v0,SM_T_NTH_WV(smMesh,tri,0),SM_VIEW_CENTER(smMesh));
-	  VSUB(v1,SM_T_NTH_WV(smMesh,tri,1),SM_VIEW_CENTER(smMesh));
-	  VSUB(v2,SM_T_NTH_WV(smMesh,tri,2),SM_VIEW_CENTER(smMesh));
-	  found = qtAdd_tri(qtptr,q0,q1,q2,v0,v1,v2,id,n);
-#ifdef DEBUG
-	  if(!found)
-	    eputs("add_tri_expand():Reinsert\n");
-#endif
-	}
-	return(QT_MODIFIED);
-      }
-      else
-	if(QT_SET_CNT(optr) < QT_MAXSET)
-	{
-#ifdef DEBUG_TEST_DRIVER	      
-	  eputs("add_tri_expand():too many levels:can't expand\n");
-#endif
-	  return(TRUE);
-	}
-	else
-	  {
-#ifdef DEBUG	      
-	    eputs("add_tri_expand():too many tris inset:can't add\n");
-#endif
-	    return(FALSE);
-	  }
+    if(del_set=argptr->del_set)
+      *qtptr = delete_tri_set(*qtptr,optr,del_set);
+    *qtptr = qtaddelem(*qtptr,t_id);
+  }
+  if (n >= QT_MAX_LEVELS)
+    return;
+  optr = qtqueryset(*qtptr);
+  if(QT_SET_CNT(optr) < QT_SET_THRESHOLD)
+    return;
+  *qtptr = expand_node(*qtptr,q0,q1,q2,optr,n);    
 }
 
 
-int
-add_tri(qtptr,fptr,t_id)
+
+add_tri(qtptr,fptr,argptr)
    QUADTREE *qtptr;
    int *fptr;
-   int t_id;
+   ADD_ARGS *argptr;
 {
 
-  OBJECT *optr;
+  OBJECT *optr,*del_set;
+  int t_id;
 
-#ifdef DEBUG_TEST_DRIVER
-    Pick_tri = t_id;
-    Picking = TRUE;
-#endif    
-    if(QT_IS_EMPTY(*qtptr))
+  t_id = argptr->t_id;
+
+
+  if(QT_IS_EMPTY(*qtptr))
+  {
+    *qtptr = qtaddelem(*qtptr,t_id);
+    if(!QT_FLAG_FILL_TRI(*fptr))
+      (*fptr)++;
+    return;
+  }
+  if(QT_LEAF_IS_FLAG(*qtptr))
+    return;
+  
+  optr = qtqueryset(*qtptr);
+  
+  if(del_set = argptr->del_set)
+    *qtptr = delete_tri_set(*qtptr,optr,del_set);
+
+  if(!QT_IS_EMPTY(*qtptr))
     {
-	*qtptr = qtaddelem(*qtptr,t_id);
-	if(!QT_FLAG_FILL_TRI(*fptr))
-	  (*fptr)++;
+      optr = qtqueryset(*qtptr);
+      if(QT_SET_CNT(optr) >= QT_SET_THRESHOLD)
+	(*fptr) |= QT_EXPAND;
     }
-    else
-     {
-       optr = qtqueryset(*qtptr);
-       if(!inset(optr,t_id))
-       {
-	 if(QT_SET_CNT(optr) < QT_MAXSET)
-	 {
-	   if(QT_SET_CNT(optr) >= QT_SET_THRESHOLD)
-	     (*fptr) |= QT_EXPAND;
-	   if(!QT_FLAG_FILL_TRI(*fptr))
-	     (*fptr)++;
-	   *qtptr = qtaddelem(*qtptr,t_id);
-	 }
-	 else
-	   {
-#ifdef DEBUG_TESTDRIVER	      
-	     eputs("add_tri():exceeded set size\n");
-#endif
-	     return(FALSE);
-	   }
-       }
-     }
-    return(TRUE);
+  if(!QT_FLAG_FILL_TRI(*fptr))
+    (*fptr)++;
+  *qtptr = qtaddelem(*qtptr,t_id);
+  
 }
 
 
-int
-stInsert_tri(st,t_id,t0,t1,t2)
-   STREE *st;
-   int t_id;
-   FVECT t0,t1,t2;
-{
-    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;
-    VSUB(dir,t1,t0);
-    stTrace_edge(st,t0,dir,1.0,add_tri,&f,t_id);
-    VSUB(dir,t2,t1);
-    stTrace_edge(st,t1,dir,1.0,add_tri,&f,t_id);
-    VSUB(dir,t0,t2);
-    stTrace_edge(st,t2,dir,1.0,add_tri,&f,t_id);
-    /* Now visit interior */
-    if(QT_FLAG_FILL_TRI(f) || QT_FLAG_UPDATE(f))
-       stVisit_tri_interior(st,t0,t1,t2,add_tri_expand,&f,t_id);
-}
-
-smLocator_add_tri(sm,t_id,v0_id,v1_id,v2_id)
+smLocator_add_tri(sm,t_id,v0_id,v1_id,v2_id,del_set)
 SM *sm;
 int t_id;
-int v0_id,v1_id,v2_id;  
+int v0_id,v1_id,v2_id;
+OBJECT *del_set;
 {
   STREE *st;
   FVECT v0,v1,v2;
-  
+  ADD_ARGS args;
+
   st = SM_LOCATOR(sm);
 
+#ifdef DEBUG
+  if((v0_id == INVALID) || (v1_id == INVALID) || (v2_id == INVALID))
+    error(CONSISTENCY,"Invalid ids 1\n");
+#endif
+
   VSUB(v0,SM_NTH_WV(sm,v0_id),SM_VIEW_CENTER(sm));
   VSUB(v1,SM_NTH_WV(sm,v1_id),SM_VIEW_CENTER(sm));
   VSUB(v2,SM_NTH_WV(sm,v2_id),SM_VIEW_CENTER(sm));
 
-  stUpdate_tri(st,t_id,v0,v1,v2,add_tri,add_tri_expand);
+  qtClearAllFlags();
+  args.t_id = t_id;
+  args.del_set = del_set;
 
+  stApply_to_tri(st,v0,v1,v2,add_tri,add_tri_expand,&args);
+
 }
 
 /* Add a triangle to the base array with vertices v1-v2-v3 */
 int
-smAdd_tri(sm, v0_id,v1_id,v2_id,tptr)
+smAdd_tri(sm, v0_id,v1_id,v2_id)
 SM *sm;
 int v0_id,v1_id,v2_id;
-TRI **tptr;
 {
     int t_id;
     TRI *t;
+#ifdef DEBUG
+    if(v0_id==v1_id || v0_id==v2_id || v1_id==v2_id)
+    {
+	eputs("smAdd_tri: invalid vertex ids\n");
+	return(INVALID);
+	}
+#endif    
+    t_id = smAlloc_tri(sm);
 
-
-    if(SM_TRI_CNT(sm)+1 > SM_MAX_TRIS(sm))
-      error(SYSTEM,"smAdd_tri():Too many triangles");
-
-    /* Get the id for the next available triangle */
-    SM_FREE_TRI_ID(sm,t_id);
     if(t_id == -1)
       return(t_id);
 
     t = SM_NTH_TRI(sm,t_id);
+
     T_CLEAR_NBRS(t);
+    /* set the triangle vertex ids */
+    T_NTH_V(t,0) = v0_id;
+    T_NTH_V(t,1) = v1_id;
+    T_NTH_V(t,2) = v2_id;
 
+    SM_NTH_VERT(sm,v0_id) = t_id;
+    SM_NTH_VERT(sm,v1_id) = t_id;
+    SM_NTH_VERT(sm,v2_id) = t_id;
+
     if(SM_BASE_ID(sm,v0_id) || SM_BASE_ID(sm,v1_id) || SM_BASE_ID(sm,v2_id))
     {
       smClear_tri_flags(sm,t_id);
@@ -470,155 +547,64 @@ TRI **tptr;
     }
     else
     {
-      SM_CLEAR_NTH_T_BASE(sm,t_id);
+      SM_CLR_NTH_T_BASE(sm,t_id);
       SM_SET_NTH_T_ACTIVE(sm,t_id);
-      SM_SET_NTH_T_LRU(sm,t_id);
       SM_SET_NTH_T_NEW(sm,t_id);
-      SM_NUM_TRIS(sm)++;
+      S_SET_FLAG(T_NTH_V(t,0));
+      S_SET_FLAG(T_NTH_V(t,1));
+      S_SET_FLAG(T_NTH_V(t,2));
+      SM_SAMPLE_TRIS(sm)++;
       smNew_tri_cnt++;
     }
-    /* set the triangle vertex ids */
-    T_NTH_V(t,0) = v0_id;
-    T_NTH_V(t,1) = v1_id;
-    T_NTH_V(t,2) = v2_id;
 
-    SM_NTH_VERT(sm,v0_id) = t_id;
-    SM_NTH_VERT(sm,v1_id) = t_id;
-    SM_NTH_VERT(sm,v2_id) = t_id;
-
-    if(t)
-       *tptr = t;
     /* return initialized triangle */
     return(t_id);
 }
 
-int
-smClosest_vertex_in_tri(sm,v0_id,v1_id,v2_id,p,eps)
-SM *sm;
-int v0_id,v1_id,v2_id;
-FVECT p;
-double eps;
-{
-    FVECT v;
-    double d,d0,d1,d2;
-    int closest = -1;
 
-    if(v0_id != -1)
-    {
-      smDir(sm,v,v0_id);
-      d0 = DIST(p,v);
-      if(eps < 0 || d0 < eps)
-      {
-	closest = v0_id;
-	d = d0;
-      }
-    }
-    if(v1_id != -1)
-    {
-      smDir(sm,v,v1_id);
-      d1 = DIST(p,v);
-      if(closest== -1)
-      {
-	  if(eps < 0 || d1 < eps)
-	   {
-	       closest = v1_id;
-	       d = d1;
-	   }
-      }
-      else
-	if(d1 < d0)
-        {
-	  if((eps < 0) ||  d1 < eps)
-	  {
-	    closest = v1_id;
-	    d = d1;
-	  }
-	}
-    }
-    if(v2_id != -1)
-    {
-      smDir(sm,v,v2_id);
-      d2 = DIST(p,v);
-      if((eps < 0) ||  d2 < eps)
-	 if(closest == -1 ||(d2 < d))
-	     return(v2_id);
-    }
-    return(closest);
-}
-
-
-int
-smClosest_vertex_in_w_tri(sm,v0_id,v1_id,v2_id,p)
-SM *sm;
-int v0_id,v1_id,v2_id;
-FVECT p;
-{
-    FVECT v;
-    double d,d0,d1,d2;
-    int closest;
-
-    VCOPY(v,SM_NTH_WV(sm,v0_id));
-    d = d0 = DIST(p,v);
-    closest = v0_id;
-    
-    VCOPY(v,SM_NTH_WV(sm,v1_id));
-    d1 = DIST(p,v);
-    if(d1 < d0)
-    {
-      closest = v1_id;
-      d = d1;
-    }
-    VCOPY(v,SM_NTH_WV(sm,v2_id));
-    d2 = DIST(p,v);
-    if(d2 < d)
-      return(v2_id);
-    else
-      return(closest);
-}
-
 void
-smTris_swap_edge(sm,t_id,t1_id,e,e1,tn_id,tn1_id,add,del)
+smTris_swap_edge(sm,t_id,t1_id,e,e1,tn_id,tn1_id,add_ptr,delptr)
    SM *sm;
    int t_id,t1_id;
    int e,e1;
    int *tn_id,*tn1_id;
-   LIST **add,**del;
+   LIST **add_ptr;
+   QUADTREE *delptr;
 
 {
-    TRI *t,*t1;
-    TRI *ta,*tb;
     int verts[3],enext,eprev,e1next,e1prev;
-    TRI *n;
+    TRI *n,*ta,*tb,*t,*t1;
     FVECT p1,p2,p3;
     int ta_id,tb_id;
-    /* swap diagonal (e relative to t, and e1 relative to t1)
-      defined by quadrilateral
-      formed by t,t1- swap for the opposite diagonal
+    /* form new diagonal (e relative to t, and e1 relative to t1)
+      defined by quadrilateral formed by t,t1- swap for the opposite diagonal
    */
-    t = SM_NTH_TRI(sm,t_id);
-    t1 = SM_NTH_TRI(sm,t1_id);
     enext = (e+1)%3;
     eprev = (e+2)%3;
     e1next = (e1+1)%3;
     e1prev = (e1+2)%3;
-    verts[e] = T_NTH_V(t,e);
-    verts[enext] = T_NTH_V(t1,e1prev);
-    verts[eprev] = T_NTH_V(t,eprev);
-    ta_id = smAdd_tri(sm,verts[0],verts[1],verts[2],&ta);
-    *add = push_data(*add,ta_id);
-    verts[e1] = T_NTH_V(t1,e1);
-    verts[e1next] = T_NTH_V(t,eprev);
-    verts[e1prev] = T_NTH_V(t1,e1prev);
-    tb_id = smAdd_tri(sm,verts[0],verts[1],verts[2],&tb);
-    *add = push_data(*add,tb_id);
+    verts[e] = T_NTH_V(SM_NTH_TRI(sm,t_id),e);
+    verts[enext] = T_NTH_V(SM_NTH_TRI(sm,t_id),enext);
+    verts[eprev] = T_NTH_V(SM_NTH_TRI(sm,t1_id),e1);
+    ta_id = smAdd_tri(sm,verts[0],verts[1],verts[2]);
+    *add_ptr = push_data(*add_ptr,ta_id);
+    verts[e1] = T_NTH_V(SM_NTH_TRI(sm,t1_id),e1);
+    verts[e1next] = T_NTH_V(SM_NTH_TRI(sm,t1_id),e1next);
+    verts[e1prev] = T_NTH_V(SM_NTH_TRI(sm,t_id),e);
+    tb_id = smAdd_tri(sm,verts[0],verts[1],verts[2]);
+    *add_ptr = push_data(*add_ptr,tb_id);
 
+    ta = SM_NTH_TRI(sm,ta_id);
+    tb = SM_NTH_TRI(sm,tb_id);
+    t = SM_NTH_TRI(sm,t_id);
+    t1 = SM_NTH_TRI(sm,t1_id);
     /* set the neighbors */
     T_NTH_NBR(ta,e) = T_NTH_NBR(t1,e1next);
     T_NTH_NBR(tb,e1) = T_NTH_NBR(t,enext);
-    T_NTH_NBR(ta,enext) = tb_id;
-    T_NTH_NBR(tb,e1next) = ta_id;
-    T_NTH_NBR(ta,eprev) = T_NTH_NBR(t,eprev);
-    T_NTH_NBR(tb,e1prev) = T_NTH_NBR(t1,e1prev);    
+    T_NTH_NBR(ta,enext)= tb_id;
+    T_NTH_NBR(tb,e1next)= ta_id;
+    T_NTH_NBR(ta,eprev)=T_NTH_NBR(t,eprev);
+    T_NTH_NBR(tb,e1prev)=T_NTH_NBR(t1,e1prev);    
 
     /* Reset neighbor pointers of original neighbors */
     n = SM_NTH_TRI(sm,T_NTH_NBR(t,enext));
@@ -632,87 +618,93 @@ smTris_swap_edge(sm,t_id,t1_id,e,e1,tn_id,tn1_id,add,d
     T_NTH_NBR(n,T_NTH_NBR_PTR(t1_id,n)) = tb_id;
 
     /* Delete two parent triangles */
+    if(remove_from_list(t_id,add_ptr))
+       smDelete_tri(sm,t_id); 
+    else
+      *delptr = qtaddelem(*delptr,t_id);
 
-    *del = push_data(*del,t_id);
-    T_VALID_FLAG(t) = -1;
-    *del = push_data(*del,t1_id);
-    T_VALID_FLAG(t1) = -1;
+    if(remove_from_list(t1_id,add_ptr))
+       smDelete_tri(sm,t1_id); 
+    else
+      *delptr = qtaddelem(*delptr,t1_id);
+
     *tn_id = ta_id;
     *tn1_id = tb_id;
 }
 
-smUpdate_locator(sm,add_list,del_list)
+smUpdate_locator(sm,add_list,del_set)
 SM *sm;
-LIST *add_list,*del_list;
+LIST *add_list;
+OBJECT *del_set;
 {
-  int t_id;
+  int t_id,i;
   TRI *t;
+  OBJECT *optr;
+  
   while(add_list)
   {
     t_id = pop_list(&add_list);
     t = SM_NTH_TRI(sm,t_id);
-    if(!T_IS_VALID(t))
-    {
-      T_VALID_FLAG(t) = 1;
-      continue;
-    }
-    smLocator_add_tri(sm,t_id,T_NTH_V(t,0),T_NTH_V(t,1),T_NTH_V(t,2));
+    smLocator_add_tri(sm,t_id,T_NTH_V(t,0),T_NTH_V(t,1),T_NTH_V(t,2),del_set);
   }
-  
-  while(del_list)
+
+  optr = QT_SET_PTR(del_set);
+  for(i = QT_SET_CNT(del_set); i > 0; i--)
   {
-    t_id = pop_list(&del_list);
-    t = SM_NTH_TRI(sm,t_id);
-    if(!T_IS_VALID(t))
-    { 
-      smLocator_remove_tri(sm,t_id);
-    }
-    smDelete_tri(sm,t_id); 
+      t_id = QT_SET_NEXT_ELEM(optr);
+#if 0
+      t = SM_NTH_TRI(sm,t_id);
+      smLocator_remove_tri(sm,t_id,T_NTH_V(t,0),T_NTH_V(t,1),T_NTH_V(t,2));
+#endif
+      smDelete_tri(sm,t_id); 
   }
 }
 /* MUST add check for constrained edges */
 int
-smFix_tris(sm,id,tlist,add_list,del_list)
+smFix_tris(sm,id,tlist,add_list,delptr)
 SM *sm;
 int id;
-LIST *tlist;
-LIST *add_list,*del_list;
+LIST *tlist,*add_list;
+QUADTREE *delptr;
 {
     TRI *t,*t_opp;
-    FVECT p,p1,p2,p3;
+    FVECT p,p0,p1,p2;
     int e,e1,swapped = 0;
     int t_id,t_opp_id;
 
-
     VSUB(p,SM_NTH_WV(sm,id),SM_VIEW_CENTER(sm));
     while(tlist)
     {
 	t_id = pop_list(&tlist);
+#ifdef DEBUG
+	if(t_id==INVALID || t_id > smMesh->num_tri)
+	  error(CONSISTENCY,"Invalid tri id smFix_tris()\n");
+#endif
         t = SM_NTH_TRI(sm,t_id);
-        e = (T_WHICH_V(t,id)+1)%3;
+        e = T_WHICH_V(t,id);
         t_opp_id = T_NTH_NBR(t,e);
-        t_opp = SM_NTH_TRI(sm,t_opp_id);
-	/*
-	VSUB(p1,SM_T_NTH_WV(sm,t_opp,0),SM_VIEW_CENTER(sm));
-	VSUB(p2,SM_T_NTH_WV(sm,t_opp,1),SM_VIEW_CENTER(sm));
-	VSUB(p3,SM_T_NTH_WV(sm,t_opp,2),SM_VIEW_CENTER(sm));
-	*/
-	smDir(sm,p1,T_NTH_V(t_opp,0));
-	smDir(sm,p2,T_NTH_V(t_opp,1));
-	smDir(sm,p3,T_NTH_V(t_opp,2));
-	if(point_in_cone(p,p1,p2,p3))
+#ifdef DEBUG
+	if(t_opp_id==INVALID || t_opp_id > smMesh->num_tri)
+	  error(CONSISTENCY,"Invalid tri id smFix_tris()\n");
+#endif
+	t_opp = SM_NTH_TRI(sm,t_opp_id);
+
+	smDir_in_cone(sm,p0,T_NTH_V(t_opp,0));
+	smDir_in_cone(sm,p1,T_NTH_V(t_opp,1));
+	smDir_in_cone(sm,p2,T_NTH_V(t_opp,2));
+	if(point_in_cone(p,p0,p1,p2))
 	{
 	    swapped = 1;
 	    e1 = T_NTH_NBR_PTR(t_id,t_opp);
 	    /* check list for t_opp and Remove if there */
 	    remove_from_list(t_opp_id,&tlist);
 	    smTris_swap_edge(sm,t_id,t_opp_id,e,e1,&t_id,&t_opp_id,
-			     &add_list,&del_list);
+			     &add_list,delptr);
 	    tlist = push_data(tlist,t_id);
 	    tlist = push_data(tlist,t_opp_id);
 	}
     }
-    smUpdate_locator(sm,add_list,del_list);
+    smUpdate_locator(sm,add_list,qtqueryset(*delptr));
     return(swapped);
 }
 
@@ -726,34 +718,14 @@ TRI *t;
 int id;
 {
   int t_id;
-  int tri;
+  int nbr_id;
 
   /* Want the edge for which "id" is the destination */
-  t_id = (T_WHICH_V(t,id)+ 2)% 3;
-  tri = T_NTH_NBR(t,t_id);
-  return(tri);
+  t_id = (T_WHICH_V(t,id)+ 1)% 3;
+  nbr_id = T_NTH_NBR(t,t_id);
+  return(nbr_id);
 }
 
-void
-smReplace_point(sm,tri,id,nid)
-SM *sm;
-TRI *tri;
-int id,nid;
-{
-  TRI *t;
-  int t_id;
-  
-  T_NTH_V(tri,T_WHICH_V(tri,id)) = nid;
-
-  t_id = smTri_next_ccw_nbr(sm,tri,nid);
-  while((t = SM_NTH_TRI(sm,t_id)) != tri)
-  {
-      T_NTH_V(t,T_WHICH_V(t,id)) = nid;
-      t_id = smTri_next_ccw_nbr(sm,t,nid);
-  }
-} 
-
-
 smClear_tri_flags(sm,id)
 SM *sm;
 int id;
@@ -761,243 +733,431 @@ int id;
   int i;
 
   for(i=0; i < T_FLAGS; i++)
-    SM_CLEAR_NTH_T_FLAG(sm,id,i);
+    SM_CLR_NTH_T_FLAG(sm,id,i);
 
 }
 
 /* Locate the point-id in the point location structure: */
 int
-smReplace_vertex(sm,c,dir,p,tri_id,snew_id,type,which)
+smInsert_samp(sm,s_id,tri_id)
    SM *sm;
-   COLR c;
-   FVECT dir,p;
-   int tri_id,snew_id;
-   int type,which;
-{
-    int n_id,s_id;
-    TRI *tri;
-
-    tri = SM_NTH_TRI(sm,tri_id);
-    /* Get the sample that corresponds to the "which" vertex of "tri" */
-    /* NEED: have re-init that sets clock flag */
-    /* If this is a base-sample: create a new sample and replace
-       all references to the base sample with references to the
-       new sample
-       */
-    s_id = T_NTH_V(tri,which);
-    if(SM_BASE_ID(sm,s_id))
-     {
-	 if(snew_id != -1)
-	   n_id = smAdd_sample_point(sm,c,dir,p);
-	 else
-	   n_id = snew_id;
-	 smReplace_point(sm,tri,s_id,n_id); 
-	 s_id = n_id;
-     }
-    else /* If the sample exists, reset the values */
-       /* NOTE: This test was based on the SPHERICAL coordinates
-	       of the point: If we are doing a multiple-sample-per
-	       SPHERICAL pixel: we will want to test for equality-
-	       and do other processing: for now: SINGLE SAMPLE PER
-	       PIXEL
-	       */
-      /* NOTE: snew_id needs to be marked as invalid?*/
-      if(snew_id == -1)
-	smInit_sample(sm,s_id,c,dir,p);
-    else
-      smReset_sample(sm,s_id,snew_id);
-    return(s_id);
-}
-
-
-/* Locate the point-id in the point location structure: */
-int
-smInsert_point_in_tri(sm,c,dir,p,s_id,tri_id)
-   SM *sm;
-   COLR c;
-   FVECT dir,p;
    int s_id,tri_id;
 {
-    TRI *tri,*t0,*t1,*t2,*nbr;
-    int v0_id,v1_id,v2_id,n_id;
-    int t0_id,t1_id,t2_id;
-    LIST *tlist,*add_list,*del_list;
+    int v0_id,v1_id,v2_id;
+    int t0_id,t1_id,t2_id,replaced;
+    LIST *tlist,*add_list;
+    OBJECT del_set[2];
+    QUADTREE delnode;
     FVECT npt;
+    TRI *tri,*nbr;
 
-    add_list = del_list = NULL;
-    if(s_id == SM_INVALID)
-       s_id = smAdd_sample_point(sm,c,dir,p);
-    
-    tri = SM_NTH_TRI(sm,tri_id);
-    v0_id = T_NTH_V(tri,0);
-    v1_id = T_NTH_V(tri,1);
-    v2_id = T_NTH_V(tri,2);
+    add_list = NULL;
 
-    n_id = -1;
-    if(SM_BASE_ID(sm,v0_id)||SM_BASE_ID(sm,v1_id)||SM_BASE_ID(sm,v2_id))
-    {
-      smDir(sm,npt,s_id);
-	    /* Change to an add and delete */
-      t0_id = (SM_BASE_ID(sm,v0_id))?v0_id:-1;
-      t1_id = (SM_BASE_ID(sm,v1_id))?v1_id:-1;
-      t2_id = (SM_BASE_ID(sm,v2_id))?v2_id:-1;	
-      n_id = smClosest_vertex_in_tri(sm,t0_id,t1_id,t2_id,npt,P_REPLACE_EPS);
-    }
-    t0_id = smAdd_tri(sm,s_id,v0_id,v1_id,&t0);
+    v0_id = T_NTH_V(SM_NTH_TRI(sm,tri_id),0);
+    v1_id = T_NTH_V(SM_NTH_TRI(sm,tri_id),1);
+    v2_id = T_NTH_V(SM_NTH_TRI(sm,tri_id),2);
+
+    t0_id = smAdd_tri(sm,s_id,v0_id,v1_id);
     /* Add triangle to the locator */
     
     add_list = push_data(add_list,t0_id);
 
-    t1_id = smAdd_tri(sm,s_id,v1_id,v2_id,&t1);	
+    t1_id = smAdd_tri(sm,s_id,v1_id,v2_id);
     add_list = push_data(add_list,t1_id);
 
-    t2_id = smAdd_tri(sm,s_id,v2_id,v0_id,&t2);
+    t2_id = smAdd_tri(sm,s_id,v2_id,v0_id);
     add_list = push_data(add_list,t2_id);
 
+    /* CAUTION: tri must be assigned after Add_tri: pointers may change*/
+    tri = SM_NTH_TRI(sm,tri_id);
+
     /* Set the neighbor pointers for the new tris */
-    T_NTH_NBR(t0,0) = t2_id;
-    T_NTH_NBR(t0,1) = T_NTH_NBR(tri,0);
-    T_NTH_NBR(t0,2) = t1_id;
-    T_NTH_NBR(t1,0) = t0_id;
-    T_NTH_NBR(t1,1) = T_NTH_NBR(tri,1);
-    T_NTH_NBR(t1,2) = t2_id;
-    T_NTH_NBR(t2,0) = t1_id;
-    T_NTH_NBR(t2,1) = T_NTH_NBR(tri,2);
-    T_NTH_NBR(t2,2) = t0_id;
+    T_NTH_NBR(SM_NTH_TRI(sm,t0_id),0) = T_NTH_NBR(tri,2);
+    T_NTH_NBR(SM_NTH_TRI(sm,t0_id),1) = t1_id;
+    T_NTH_NBR(SM_NTH_TRI(sm,t0_id),2) = t2_id;
+    T_NTH_NBR(SM_NTH_TRI(sm,t1_id),0) = T_NTH_NBR(tri,0);
+    T_NTH_NBR(SM_NTH_TRI(sm,t1_id),1) = t2_id;
+    T_NTH_NBR(SM_NTH_TRI(sm,t1_id),2) = t0_id;
+    T_NTH_NBR(SM_NTH_TRI(sm,t2_id),0) = T_NTH_NBR(tri,1);
+    T_NTH_NBR(SM_NTH_TRI(sm,t2_id),1) = t0_id;
+    T_NTH_NBR(SM_NTH_TRI(sm,t2_id),2) = t1_id;
 
     /* Reset the neigbor pointers for the neighbors of the original */
     nbr = SM_NTH_TRI(sm,T_NTH_NBR(tri,0));
-    T_NTH_NBR(nbr,T_NTH_NBR_PTR(tri_id,nbr)) = t0_id;
-    nbr = SM_NTH_TRI(sm,T_NTH_NBR(tri,1));
     T_NTH_NBR(nbr,T_NTH_NBR_PTR(tri_id,nbr)) = t1_id;
-    nbr = SM_NTH_TRI(sm,T_NTH_NBR(tri,2));
+    nbr = SM_NTH_TRI(sm,T_NTH_NBR(tri,1));
     T_NTH_NBR(nbr,T_NTH_NBR_PTR(tri_id,nbr)) = t2_id;
+    nbr = SM_NTH_TRI(sm,T_NTH_NBR(tri,2));
+    T_NTH_NBR(nbr,T_NTH_NBR_PTR(tri_id,nbr)) = t0_id;
 	
-    del_list = push_data(del_list,tri_id);
-    T_VALID_FLAG(tri) = -1;
+    del_set[0] = 1; del_set[1] = tri_id;
+    delnode = qtnewleaf(del_set);
 
     /* Fix up the new triangles*/
     tlist = push_data(NULL,t0_id);
     tlist = push_data(tlist,t1_id);
     tlist = push_data(tlist,t2_id);
 
-    smFix_tris(sm,s_id,tlist,add_list,del_list);
+    smFix_tris(sm,s_id,tlist,add_list,&delnode);
 
-    if(n_id != -1)
-       smDelete_point(sm,n_id);
+    qtfreeleaf(delnode);
 
-    return(s_id);
+    return(TRUE);
 }
     
 
 int
-smPointLocate(sm,pt,norm)
+smTri_in_set(sm,p,optr)
 SM *sm;
-FVECT pt;
-int norm;
+FVECT p;
+OBJECT *optr;
 {
-  STREE *st;
-  int tri;
-  FVECT npt;
+  int i,t_id;
+  FVECT n,v0,v1,v2;
+  TRI *t;
   
-  st = SM_LOCATOR(sm);
-  if(norm)
+  for (i = QT_SET_CNT(optr),optr = QT_SET_PTR(optr);i > 0; i--)
   {
-      VSUB(npt,pt,SM_VIEW_CENTER(sm));
-      tri = stPoint_locate(st,npt);
+    /* Find the first triangle that pt falls */
+    t_id = QT_SET_NEXT_ELEM(optr);
+    t = SM_NTH_TRI(sm,t_id);
+    if(!T_IS_VALID(t))
+      continue;
+    VSUB(v0,SM_T_NTH_WV(sm,t,0),SM_VIEW_CENTER(sm));
+    VSUB(v1,SM_T_NTH_WV(sm,t,1),SM_VIEW_CENTER(sm));
+    VSUB(v2,SM_T_NTH_WV(sm,t,2),SM_VIEW_CENTER(sm));
+
+    if(EQUAL_VEC3(v0,p) || EQUAL_VEC3(v1,p) || EQUAL_VEC3(v2,p))
+       return(t_id);
+    
+    VCROSS(n,v0,v1);
+    if(DOT(n,p) >0.0)
+      continue;
+    VCROSS(n,v1,v2);
+    if(DOT(n,p) > 0.0)
+      continue;
+
+    VCROSS(n,v2,v0);
+    if(DOT(n,p) > 0.0)
+      continue;
+
+    return(t_id);
   }
-  else
-     tri = stPoint_locate(st,pt);
-  return(tri);
+  return(INVALID);
 }
 
-QUADTREE
-smPointLocateCell(sm,pt,norm,v0,v1,v2)
+int
+smPointLocateTri(sm,p)
 SM *sm;
-FVECT pt;
-int norm;
-FVECT v0,v1,v2;
+FVECT p;
 {
-  STREE *st;
-  QUADTREE *qtptr;
-  FVECT npt;
+  QUADTREE qt,*optr;
+  FVECT tpt;
 
-  st = SM_LOCATOR(sm);
-  if(norm)
-  {
-      VSUB(npt,pt,SM_VIEW_CENTER(sm));
-  
-      qtptr = stPoint_locate_cell(st,npt,v0,v1,v2);
-  }
-  else
-     qtptr = stPoint_locate_cell(st,pt,v0,v1,v2);
-
-  if(qtptr)
-    return(*qtptr);
-  else
-    return(EMPTY);
+  VSUB(tpt,p,SM_VIEW_CENTER(sm));
+  qt = smPointLocateCell(sm,tpt);
+  if(QT_IS_EMPTY(qt))
+    return(INVALID);  
+ 
+  optr = qtqueryset(qt);
+  return(smTri_in_set(sm,tpt,optr));
 }
 
+
+/*
+   Determine whether this sample should:
+   a) be added to the mesh by subdividing the triangle
+   b) ignored
+   c) replace values of an existing mesh vertex
+
+   In case a, the routine will return TRUE, for b,c will return FALSE
+   In case a, also determine if this sample should cause the deletion of
+   an existing vertex. If so *rptr will contain the id of the sample to delete
+   after the new sample has been added.
+
+   ASSUMPTION: this will not be called with a new sample that is
+               a BASE point.
+
+   The following tests are performed (in order) to determine the fate
+   of the sample:
+
+   1) If the world space point of the sample coincides with one of the
+      triangle vertex samples: The values of the triangle sample are
+      replaced with the new values and FALSE is returned
+   2) If the new sample is close in ws, and close in the spherical projection
+      to one of the triangle vertex samples:
+         pick the point with dir closest to that of the canonical view
+	 If it is the new point: mark existing point for deletion,and return
+	 TRUE,else return FALSE
+   3) If the spherical projection of new is < REPLACE_EPS from a base point:
+      add new and mark the base for deletion: return TRUE
+   4) If the addition of the new sample point would introduce a "puncture"
+      or cause new triangles with large depth differences:return FALSE
+     This attempts to throw out points that should be occluded   
+*/
 int
-smAdd_sample_to_mesh(sm,c,dir,pt,s_id)
+smTest_sample(sm,tri_id,c,dir,p,o_id,rptr)
    SM *sm;
+   int tri_id;
    COLR c;
-   FVECT dir,pt;
-   int s_id;
+   FVECT dir,p;
+   int o_id;
+   int *rptr;
 {
-    int t_id;
-    double d;
-    FVECT p;
-    
-    /* If new, foreground pt */
-    if(pt)
+    TRI *tri;
+    double d,d2,dnear,dspt,d01,d12,d20,s0,s1,s2,ds,dv;
+    int vid[3],i,nearid,norm,dcnt,bcnt;
+    FVECT diff[3],spt,npt;
+    int tonemap;
+
+    *rptr = INVALID;
+    bcnt = dcnt = 0;
+    tri = SM_NTH_TRI(sm,tri_id);
+    vid[0] = T_NTH_V(tri,0);
+    vid[1] = T_NTH_V(tri,1);
+    vid[2] = T_NTH_V(tri,2);
+
+    /* TEST 1: Test if the new point has the same world space point as
+       one of the existing triangle vertices
+     */
+    for(i=0; i<3; i++)
     {
-	/* NOTE: This should be elsewhere! */
-	d = DIST(pt,SM_VIEW_CENTER(smMesh));
-	smDist_sum += 1.0/d;
-	/************************************/
-	t_id = smPointLocate(smMesh,pt,TRUE); 
-	if(t_id >= 0)
-	   s_id = smInsert_point_in_tri(smMesh,c,dir,pt,s_id,t_id);
-#ifdef DEBUG
-	   else
-	     {
-	       c[0] = 255;c[1]=0;c[2]=0;
-	       s_id = smAdd_sample_point(sm,c,dir,pt);	       
-	       eputs("smAdd_sample_to_mesh(): not found fg\n");
-	     }
-#endif
+	if(SM_BASE_ID(sm,vid[i]))
+	{
+	  bcnt++;
+	   continue;
+	}
+	if(SM_DIR_ID(sm,vid[i]))
+	   dcnt++;
+	VSUB(diff[i],SM_NTH_WV(sm,vid[i]),p);
+	/* If same world point: replace */
+	if(FZERO_VEC3(diff[i]))
+	{
+	  tonemap = (SM_TONE_MAP(sm) > vid[i]);
+	  sInit_samp(SM_SAMP(sm),vid[i],c,dir,p,o_id,tonemap);
+	  return(FALSE);
+	}
     }
-    else if(s_id != -1)
+    if(!dir)
+      return(TRUE);
+    /* TEST 2: If the new sample is close in ws, and close in the spherical
+       projection to one of the triangle vertex samples
+    */
+    norm = FALSE;
+    if(bcnt + dcnt != 3)
     {
-	VCOPY(p,SM_NTH_WV(sm,s_id));
-	if(SM_NTH_W_DIR(sm,s_id) != -1)
+      VSUB(spt,p,SM_VIEW_CENTER(sm));
+      ds = DOT(spt,spt);
+      dnear = FHUGE;
+      for(i=0; i<3; i++)
 	{
-	    /* NOTE: This should be elsewhere! */
-	    d = DIST(p,SM_VIEW_CENTER(smMesh));
-	    smDist_sum += 1.0/d;
-	    /************************************/
+	  if(SM_BASE_ID(sm,vid[i]) || SM_DIR_ID(sm,vid[i]))
+	    continue;
+	  d = DOT(diff[i],diff[i])/ds;
+	  if(d < dnear)
+	    {
+	      dnear = d;
+	      nearid=vid[i];
+	    }
 	}
-	t_id = smPointLocate(smMesh,p,TRUE); 
-	if(t_id != -1)
-	   s_id = smInsert_point_in_tri(smMesh,c,dir,p,s_id,t_id);
+
+      if(dnear <=  smMinSampDiff*smMinSampDiff)
+	{
+	  /* Pick the point with dir closest to that of the canonical view
+	     if it is the new sample: mark existing point for deletion
+	     */
+	  normalize(spt);
+	  norm = TRUE;
+	  VSUB(npt,SM_NTH_WV(sm,nearid),SM_VIEW_CENTER(sm));
+	  normalize(npt);
+	  d = fdir2diff(SM_NTH_W_DIR(sm,nearid), npt);
+	  d2 = 2. - 2.*DOT(dir,spt);
+	  /* The existing sample is a better sample:punt */
+	  if(d2 > d)
+	    return(FALSE);
+	  else
+	    {
+		/* The new sample is better: mark the existing one
+		   for deletion after the new one is added*/
+	      *rptr = nearid;
+	      return(TRUE);
+	    }
+	}
+    }
+  /* TEST 3: If the spherical projection of new is < S_REPLACE_EPS
+     from a base point
+  */
+    if(bcnt)
+    {   
+	dnear = FHUGE;
+	if(bcnt + dcnt ==3)
+	  VSUB(spt,p,SM_VIEW_CENTER(sm));
+	if(!norm)
+	  normalize(spt);
+
+	for(i=0; i<3; i++)
+	{
+	    if(!SM_BASE_ID(sm,vid[i]))
+	       continue;
+	    VSUB(npt,SM_NTH_WV(sm,vid[i]),SM_VIEW_CENTER(sm));
+	    d = DIST_SQ(npt,spt);
+	    if(d < S_REPLACE_EPS && d < dnear)
+	       {
+		   dnear = d;
+		   nearid = vid[i];
+	       }
+	}
+	if(dnear != FHUGE)
+	{
+	    /* add new and mark the base for deletion */
+	    *rptr = nearid;
+	    return(TRUE);
+	}
+    }
+	
+  /* TEST 4:
+     If the addition of the new sample point would introduce a "puncture"
+     or cause new triangles with large depth differences:dont add:    
+     */
+    if(bcnt || dcnt)
+       return(TRUE);
+    /* If the new point is in front of the existing points- add */
+    dv = DIST_SQ(SM_NTH_WV(sm,vid[0]),SM_VIEW_CENTER(sm));
+    if(ds < dv)
+      return(TRUE);
+
+    d01 = DIST_SQ(SM_NTH_WV(sm,vid[1]),SM_NTH_WV(sm,vid[0]));
+    s0 = DOT(diff[0],diff[0]);
+    if(s0 < S_REPLACE_SCALE*d01)
+       return(TRUE);
+    d12 = DIST_SQ(SM_NTH_WV(sm,vid[2]),SM_NTH_WV(sm,vid[1]));
+    if(s0 < S_REPLACE_SCALE*d12)
+       return(TRUE);    
+    d20 = DIST_SQ(SM_NTH_WV(sm,vid[0]),SM_NTH_WV(sm,vid[2]));
+    if(s0 < S_REPLACE_SCALE*d20)
+       return(TRUE);    
+    d = MIN3(d01,d12,d20);
+    s1 = DOT(diff[1],diff[1]);
+    if(s1 < S_REPLACE_SCALE*d)
+       return(TRUE);
+    s2 = DOT(diff[2],diff[2]);
+    if(s2 < S_REPLACE_SCALE*d)
+       return(TRUE);    
+
+  /* TEST 5:
+     Check to see if triangle is relatively large and should therefore
+     be subdivided anyway.
+     */
+    dv *= DIST_SQ(SM_NTH_WV(sm,vid[1]),SM_VIEW_CENTER(sm));
+    dv *= DIST_SQ(SM_NTH_WV(sm,vid[2]),SM_VIEW_CENTER(sm));
+    if (d01*d12*d20/dv > S_REPLACE_TRI)
+	return(TRUE);
+
+    return(FALSE);
+}
+
+
+int
+smAlloc_samp(sm,c,dir,pt,o_id)
+SM *sm;
+COLR c;
+FVECT dir,pt;
+int o_id;
+{
+  int s_id,replaced,cnt;
+  SAMP *s;
+  FVECT p;
+
+  s = SM_SAMP(sm);
+  s_id = sAlloc_samp(s,&replaced);
+
+  cnt=0;
+  while(replaced)
+  {
+    if(smRemoveVertex(sm,s_id))
+      break;
+    s_id = sAlloc_samp(s,&replaced);
+    cnt++;
+    if(cnt > S_MAX_SAMP(s))
+      error(CONSISTENCY,"smAlloc_samp():unable to find free samp\n");
+  }
+  
+  /* If sample is being added in the middle of the sample array: tone 
+     map individually
+   */
+  /* Initialize sample */
+  sInit_samp(s,s_id,c,dir,pt,o_id,(SM_TONE_MAP(sm)>s_id));	  
+  
+  return(s_id);
+}
+
+int
+smAdd_samp(sm,c,dir,p,o_id)
+   SM *sm;
+   COLR c;
+   FVECT dir,p;
+   int o_id;
+{
+  int t_id,s_id,r_id;
+  double d;
+  FVECT wpt;
+
+  r_id = INVALID;
+  /* If sample is a world space point */
+  if(p)
+  {
+    t_id = smPointLocateTri(sm,p);
+    if(t_id == INVALID)
+      {
 #ifdef DEBUG
-	   else
-	      eputs("smAdd_sample_to_mesh():not found reinsert\n");
+	eputs("smAddSamp(): unable to locate tri containing sample \n");
 #endif
+	return(INVALID);
+      }
+      /* if not a base id, Test to see if this sample should be added */
+    if(!SM_BASE_ID(sm,o_id))
+    {
+      if(!smTest_sample(sm,t_id,c,dir,p,o_id,&r_id))
+	return(INVALID);
+      /* Allocate space for a sample and initialize */
+      s_id = smAlloc_samp(smMesh,c,dir,p,o_id);
     }
-    /* Is a BG(sky point) */
     else
-       {
-	   t_id = smPointLocate(smMesh,dir,FALSE); 
-	   if(t_id != -1)
-	      s_id = smInsert_point_in_tri(smMesh,c,dir,NULL,s_id,t_id);
-
+      s_id = o_id;
+  }
+    /* If sample is a direction vector */
+    else
+    {
+      VADD(wpt,dir,SM_VIEW_CENTER(smMesh));
+      t_id = smPointLocateTri(sm,wpt);
+      if(t_id == INVALID)
+	{
 #ifdef DEBUG
-	      else
-		 eputs("smAdd_sample_to_mesh(): not found bg\n");
+	  eputs("smAddSamp(): unable to locate tri containing sample \n");
 #endif
-       }
+	  return(INVALID);
+	}
+      /* Test to see if this sample should be added */
+      if(!smTest_sample(sm,t_id,c,NULL,wpt,o_id,&r_id))
+	return(INVALID);
+      /* Allocate space for a sample and initialize */
+      s_id = smAlloc_samp(smMesh,c,NULL,wpt,o_id);
+    }
+  if(!SM_BASE_ID(sm,s_id) && !SM_DIR_ID(sm,s_id))
+    {
+      /* If not a base or sky point, add distance from the 
+	 viewcenter to average*/
+      d = DIST(SM_NTH_WV(sm,s_id),SM_VIEW_CENTER(smMesh));
+      smDist_sum += 1.0/d;
+    }
+    smInsert_samp(smMesh,s_id,t_id);
+
+    /* If new sample replaces existing one- remove that vertex now */
+    if(r_id != INVALID)
+    {
+      smRemoveVertex(sm,r_id);
+      sDelete_samp(SM_SAMP(sm),r_id);
+    }
     return(s_id);
 }
 
@@ -1017,72 +1177,32 @@ smNewSamp(c,dir,p)
 COLR c;
 FVECT dir;
 FVECT p;
-
 {
     int s_id;
-    int debug=0;
-    
+
     /* First check if this the first sample: if so initialize mesh */
     if(SM_NUM_SAMP(smMesh) == 0)
-      smInit_mesh(smMesh,odev.v.vp);
-    if(!debug)
-       s_id = smAdd_sample_to_mesh(smMesh,c,dir,p,-1);
+    {
+      smInit_sm(smMesh,odev.v.vp);
+      sClear_all_flags(SM_SAMP(smMesh));
+    }
+    /* Add the sample to the mesh */
+    s_id = smAdd_samp(smMesh,c,dir,p,INVALID);
+
     return(s_id);
     
  }    
-/*
- * int
- * smFindsamp(orig, dir): intersect ray with 3D rep. and find closest sample
- * FVECT	orig, dir;
- *
- * Find the closest sample to the given ray.  Return -1 on failure.
- */
-
-/*
- * smClean()		: display has been wiped clean
- *
- * Called after display has been effectively cleared, meaning that all
- * geometry must be resent down the pipeline in the next call to smUpdate().
- */
-
-
-/*
- * smUpdate(vp, qua)	: update OpenGL output geometry for view vp
- * VIEW	*vp;		: desired view
- * int	qua;		: quality level (percentage on linear time scale)
- *
- * Draw new geometric representation using OpenGL calls.  Assume that the
- * view has already been set up and the correct frame buffer has been
- * selected for drawing.  The quality level is on a linear scale, where 100%
- * is full (final) quality.  It is not necessary to redraw geometry that has
- * been output since the last call to smClean().
- */
-
-
 int
-smClear_vert(sm,id)
-SM *sm;
-int id;
-{
-    if(SM_INVALID_POINT_ID(sm,id))
-       return(FALSE);
-    
-    SM_NTH_VERT(sm,id) = SM_INVALID;
-
-    return(TRUE);
-}
-
-int
-smAdd_base_vertex(sm,v,d)
+smAdd_base_vertex(sm,v)
    SM *sm;
-   FVECT v,d;
+   FVECT v;
 {
   int id;
 
   /* First add coordinate to the sample array */
-  id = smAdd_aux_point(sm,v,d);
-  if(id == -1)
-    return(SM_INVALID);
+  id = sAdd_base_point(SM_SAMP(sm),v);
+  if(id == INVALID)
+    return(INVALID);
   /* Initialize triangle pointer to -1 */
   smClear_vert(sm,id);
   return(id);
@@ -1100,47 +1220,52 @@ smCreate_base_mesh(sm,type)
 SM *sm;
 int type;
 {
-  int i,id,tri_id,nbr_id;
+  int i,s_id,tri_id,nbr_id;
   int p[4],ids[4];
   int v0_id,v1_id,v2_id;
-  TRI *tris[4];
-  FVECT d,pt,cntr;
+  FVECT d,pt,cntr,v0,v1,v2;
   
   /* First insert the base vertices into the sample point array */
 
   for(i=0; i < 4; i++)
   {
-    VCOPY(cntr,stDefault_base[i]);
+    VCOPY(cntr,smDefault_base[i]);
     cntr[0] += .01;
     cntr[1] += .02;
     cntr[2] += .03;
     VADD(cntr,cntr,SM_VIEW_CENTER(sm));
-    /* WONT use dir */
-    point_on_sphere(d,cntr,SM_VIEW_CENTER(sm));
-    id = smAdd_base_vertex(sm,cntr,d);
-    /* test to make sure vertex allocated */
-    if(id != -1)
-      p[i] = id;
-    else
-      return(0);
+    p[i]  = smAdd_base_vertex(sm,cntr);
   }
   /* Create the base triangles */
   for(i=0;i < 4; i++)
   {
-    v0_id = p[stTri_verts[i][0]];
-    v1_id = p[stTri_verts[i][1]];
-    v2_id = p[stTri_verts[i][2]];
-    if((ids[i] = smAdd_tri(sm, v0_id,v1_id,v2_id,&(tris[i])))== -1)
-     return(0);
-    smLocator_add_tri(sm,ids[i],v0_id,v1_id,v2_id);
+    v0_id = p[smTri_verts[i][0]];
+    v1_id = p[smTri_verts[i][1]];
+    v2_id = p[smTri_verts[i][2]];
+    ids[i] = smAdd_tri(sm, v0_id,v1_id,v2_id);
+    smLocator_add_tri(sm,ids[i],v0_id,v1_id,v2_id,NULL);
   }
+  
   /* Set neighbors */
 
   for(tri_id=0;tri_id < 4; tri_id++)
-     for(nbr_id=0; nbr_id < 3; nbr_id++)
-	T_NTH_NBR(tris[tri_id],nbr_id) = smBase_nbrs[tri_id][nbr_id];
+   for(nbr_id=0; nbr_id < 3; nbr_id++)
+    T_NTH_NBR(SM_NTH_TRI(sm,ids[tri_id]),nbr_id) = smBase_nbrs[tri_id][nbr_id];
 
-  return(1);
+  /* Now add the centroids of the faces */
+  for(tri_id=0;tri_id < 4; tri_id++)
+  {
+      VCOPY(v0,SM_T_NTH_WV(sm,SM_NTH_TRI(sm,ids[tri_id]),0));
+      VCOPY(v1,SM_T_NTH_WV(sm,SM_NTH_TRI(sm,ids[tri_id]),1));
+      VCOPY(v2,SM_T_NTH_WV(sm,SM_NTH_TRI(sm,ids[tri_id]),2));
+      tri_centroid(v0,v1,v2,cntr);
+      VSUB(cntr,cntr,SM_VIEW_CENTER(sm));
+      normalize(cntr);
+      VADD(cntr,cntr,SM_VIEW_CENTER(sm));
+      s_id = smAdd_base_vertex(sm,cntr);
+      smAdd_samp(sm,NULL,NULL,cntr,s_id);
+  }
+ return(1);
 
 }
 
@@ -1152,11 +1277,13 @@ smNext_tri_flag_set(sm,i,which,b)
      int b;
 {
 
-  for(; i < SM_TRI_CNT(sm);i++)
+  for(; i < SM_NUM_TRI(sm);i++)
   {
 
-      if(!SM_IS_NTH_T_FLAG(sm,i,which))
+    if(!T_IS_VALID(SM_NTH_TRI(sm,i)))
 	 continue;
+    if(!SM_IS_NTH_T_FLAG(sm,i,which))
+	continue;
     if(!b)
       break;
     if((b==1) && !SM_BG_TRI(sm,i))
@@ -1175,7 +1302,7 @@ smNext_valid_tri(sm,i)
      int i;
 {
 
-  while( i < SM_TRI_CNT(sm) && !T_IS_VALID(SM_NTH_TRI(sm,i)))
+  while( i < SM_NUM_TRI(sm) && !T_IS_VALID(SM_NTH_TRI(sm,i)))
      i++;
      
   return(i);
@@ -1183,72 +1310,75 @@ smNext_valid_tri(sm,i)
 
 
 
-qtTri_from_id(t_id,v0,v1,v2,n0,n1,n2,v0_idp,v1_idp,v2_idp) 
+qtTri_from_id(t_id,v0,v1,v2)
 int t_id;
 FVECT v0,v1,v2;
-FVECT n0,n1,n2;
-int *v0_idp,*v1_idp,*v2_idp;
 {
   TRI *t;
-  int v0_id,v1_id,v2_id;
   
   t = SM_NTH_TRI(smMesh,t_id);
-  v0_id = T_NTH_V(t,0);
-  v1_id = T_NTH_V(t,1);
-  v2_id = T_NTH_V(t,2);
-
-  if(v0)
-  {
-      VCOPY(v0,SM_NTH_WV(smMesh,v0_id));
-      VCOPY(v1,SM_NTH_WV(smMesh,v1_id));
-      VCOPY(v2,SM_NTH_WV(smMesh,v2_id));
-  }
-  if(n0)
-  {
-      smDir(smMesh,n0,v0_id);
-      smDir(smMesh,n1,v1_id);
-      smDir(smMesh,n2,v2_id);
-
-  }
-  if(v0_idp)
-     {
-	 *v0_idp = v0_id;
-	 *v1_idp = v1_id;
-	 *v2_idp = v2_id;
-     }
+  if(!T_IS_VALID(t))
+    return(0);
+  VSUB(v0,SM_T_NTH_WV(smMesh,t,0),SM_VIEW_CENTER(smMesh));
+  VSUB(v1,SM_T_NTH_WV(smMesh,t,1),SM_VIEW_CENTER(smMesh));
+  VSUB(v2,SM_T_NTH_WV(smMesh,t,2),SM_VIEW_CENTER(smMesh));
+  return(1);
 }
 
 
-/*
- * int
- * smFindSamp(FVECT orig, FVECT dir) 
- *
- * Find the closest sample to the given ray.  Returns sample id, -1 on failure.
- * "dir" is assumed to be normalized
- */
-
-  
-
-smRebuild_mesh(sm,vp)
+smRebuild_mesh(sm,v)
    SM *sm;
-   FVECT vp;
+   VIEW *v;
 {
-    int i;
-    FVECT dir;
-    COLR c;
-    FVECT p,ov;
+    int i,j,cnt;
+    FVECT p,ov,dir;
+    double d;
 
+#ifdef DEBUG
+    eputs("smRebuild_mesh(): rebuilding....");
+#endif
     /* Clear the mesh- and rebuild using the current sample array */
+    /* Remember the current number of samples */
+    cnt = SM_NUM_SAMP(sm);
+    /* Calculate the difference between the current and new viewpoint*/
+    /* Will need to subtract this off of sky points */
+    VCOPY(ov,SM_VIEW_CENTER(sm));
+    /* Initialize the mesh to 0 samples and the base triangles */
 
-    VSUB(ov,vp,SM_VIEW_CENTER(sm));
-    smInit_mesh(sm,vp);
-    
-    SM_FOR_ALL_SAMPLES(sm,i)
+    /* Go through all samples and add in if in the new view frustum, and
+       the dir is <= 30 degrees from new view
+     */
+    smInit_sm(sm,v->vp);
+    for(i=0; i < cnt; i++)
     {
-	if(SM_NTH_W_DIR(sm,i)==-1)
-	   VADD(SM_NTH_WV(sm,i),SM_NTH_WV(sm,i),ov);	    
-	smAdd_sample_to_mesh(sm,NULL,NULL,NULL,i);	
+      /* First check if sample visible(conservative approx: if one of tris 
+	 attached to sample is in frustum)	 */
+      if(!S_IS_FLAG(i))
+	continue;
+
+      /* Next test if direction > 30 from current direction */
+	if(SM_NTH_W_DIR(sm,i)!=-1)
+	{
+	    VSUB(p,SM_NTH_WV(sm,i),v->vp);
+	    normalize(p);
+	    decodedir(dir,SM_NTH_W_DIR(sm,i));
+	    d = 2. - 2.*DOT(dir,p);
+	    if (d > MAXDIFF2)
+	      continue;
+	    VCOPY(p,SM_NTH_WV(sm,i));
+	    smAdd_samp(sm,NULL,dir,p,i);
+	}
+	else
+	{
+	  VSUB(dir,SM_NTH_WV(sm,i),ov);
+	  smAdd_samp(sm,NULL,dir,NULL,i);
+	}
+
     }
+   smNew_tri_cnt = SM_SAMPLE_TRIS(sm);
+#ifdef DEBUG
+    eputs("smRebuild_mesh():done\n");
+#endif
 }
 
 int
@@ -1266,8 +1396,11 @@ intersect_tri_set(t_set,orig,dir,pt)
     for(i = QT_SET_CNT(t_set); i > 0; i--)
     {
 	t_id = QT_SET_NEXT_ELEM(optr);
-
+	if(SM_IS_NTH_T_BASE(smMesh,t_id))
+	   continue;
 	t = SM_NTH_TRI(smMesh,t_id);
+	if(!T_IS_VALID(t))
+	  continue;
 	pid0 = T_NTH_V(t,0);
 	pid1 = T_NTH_V(t,1);
 	pid2 = T_NTH_V(t,2);
@@ -1291,36 +1424,67 @@ intersect_tri_set(t_set,orig,dir,pt)
     return(-1);
 }
 
-int
-ray_trace_check_set(qtptr,orig,dir,tptr,os)
+/* OS is constrained to be <= QT_MAXCSET : if the set exceeds this, the
+ results of check_set are conservative
+*/
+
+ray_trace_check_set(qtptr,fptr,argptr)
    QUADTREE *qtptr;
-   FVECT orig,dir;
-   int *tptr;
-   OBJECT *os;
+   int *fptr;
+   RT_ARGS *argptr;
 {
-    OBJECT tset[QT_MAXSET+1];	
+    OBJECT tset[QT_MAXSET+1],*tptr;	
     double dt,t;
     int found;
-    FVECT o;
     
- 
+    if(QT_LEAF_IS_FLAG(*qtptr))
+      {
+	QT_FLAG_SET_DONE(*fptr);
+#if DEBUG
+	eputs("ray_trace_check_set():Already visited this node:aborting\n");
+#endif
+	return;
+      }
     if(!QT_IS_EMPTY(*qtptr))
-     {
-	 VADD(o,orig,SM_VIEW_CENTER(smMesh));
-	 qtgetset(tset,*qtptr);
+      {
+	tptr = qtqueryset(*qtptr);
+	if(QT_SET_CNT(tptr) > QT_MAXSET)
+	  tptr = (OBJECT *)malloc((QT_SET_CNT(tptr)+1)*sizeof(OBJECT));
+	else
+	  tptr = tset;
+	if(!tptr)
+	  goto memerr;
+    
+	qtgetset(tptr,*qtptr);
 	 /* Check triangles in set against those seen so far(os):only
 	    check new triangles for intersection (t_set') 
 	    */
-	 check_set(tset,os);
-	 if((found = intersect_tri_set(tset,o,dir,NULL))!= -1)
-         {
-	     *tptr = found;
-	     return(QT_DONE);
-	 }
+       check_set_large(tptr,argptr->os);
+       if((found = intersect_tri_set(tptr,argptr->orig,argptr->dir,NULL))!= -1)
+       {
+	     argptr->t_id = found;
+	     if(tptr != tset)
+	       free(tptr);
+	     QT_FLAG_SET_DONE(*fptr);
+	     return;
        }
-    return(FALSE);
+       if(tptr != tset)
+	 free(tptr);
+     }
+    return;
+memerr:
+    error(SYSTEM,"ray_trace_check_set():Unable to allocate memory");
 }
 
+
+/*
+ * int
+ * smFindSamp(FVECT orig, FVECT dir) 
+ *
+ * Find the closest sample to the given ray.  Returns sample id, -1 on failure.
+ * "dir" is assumed to be normalized
+ */
+
 int
 smFindSamp(orig,dir)
 FVECT orig,dir;
@@ -1349,14 +1513,24 @@ FVECT orig,dir;
   /* orig will be updated-so preserve original value */
   if(!smMesh)
      return;
+#ifdef TEST_DRIVER
+  Picking= TRUE;
+#endif  
   point_on_sphere(b,orig,SM_VIEW_CENTER(smMesh));
   d = -DOT(b,dir);
   if(EQUAL_VEC3(orig,SM_VIEW_CENTER(smMesh)) || EQUAL(fabs(d),1.0))
   {
-      qt = smPointLocateCell(smMesh,dir,FALSE,NULL,NULL,NULL);
+      qt = smPointLocateCell(smMesh,dir);
       /* Test triangles in the set for intersection with Ray:returns
 	 first found
       */
+#ifdef DEBUG
+      if(QT_IS_EMPTY(qt))
+      {
+	eputs("smFindSamp(): point not found");
+	return;
+      }
+#endif
       ts = qtqueryset(qt);
       s_id = intersect_tri_set(ts,orig,dir,p);
 #ifdef DEBUG_TEST_DRIVER
@@ -1365,16 +1539,22 @@ FVECT orig,dir;
   }
   else
   {
-    OBJECT t_set[QT_MAXCSET+1];
+    OBJECT t_set[QT_MAXCSET + 1];
+    RT_ARGS rt;
+
     /* Test each of the root triangles against point id */
     QT_CLEAR_SET(t_set);
     VSUB(o,orig,SM_VIEW_CENTER(smMesh));
     ST_CLEAR_FLAGS(SM_LOCATOR(smMesh));
-    s_id=stTrace_ray(SM_LOCATOR(smMesh),o,dir,ray_trace_check_set,&s_id,t_set);
+    rt.t_id = -1;
+    rt.os = t_set;
+    VCOPY(rt.orig,orig);
+    VCOPY(rt.dir,dir);
+    stTrace_ray(SM_LOCATOR(smMesh),o,dir,ray_trace_check_set,&rt);
+    s_id = rt.t_id;
  }    
   return(s_id);
 }
-