src/hd/sm_geom.c

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

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

/*
 *  sm_geom.c
 */

#include "standard.h"
#include "sm_geom.h"

tri_centroid(v0,v1,v2,c)
FVECT v0,v1,v2,c;
{
/* Average three triangle vertices to give centroid: return in c */
  c[0] = (v0[0] + v1[0] + v2[0])/3.0;
  c[1] = (v0[1] + v1[1] + v2[1])/3.0;
  c[2] = (v0[2] + v1[2] + v2[2])/3.0;
}


int
vec3_equal(v1,v2)
FVECT v1,v2;
{
   return(EQUAL(v1[0],v2[0]) && EQUAL(v1[1],v2[1])&& EQUAL(v1[2],v2[2]));
}
#if 0
extern FVECT Norm[500];
extern int Ncnt;
#endif

int
convex_angle(v0,v1,v2)
FVECT v0,v1,v2;
{
    double dp,dp1;
    int test,test1;
    FVECT nv0,nv1,nv2;
    FVECT cp01,cp12,cp;

    /* test sign of (v0Xv1)X(v1Xv2). v1 */
    /* un-Simplified: */ 
    VCOPY(nv0,v0);
    normalize(nv0);
    VCOPY(nv1,v1);
    normalize(nv1);
    VCOPY(nv2,v2);
    normalize(nv2);

    VCROSS(cp01,nv0,nv1);
    VCROSS(cp12,nv1,nv2);
    VCROSS(cp,cp01,cp12);
    normalize(cp);
    dp1 = DOT(cp,nv1);
    if(dp1 <= 1e-8 || dp1 >= (1-1e-8)) /* Test if on other side,or colinear*/
      test1 = FALSE;
    else
      test1 = TRUE;

    dp =   nv0[2]*nv1[0]*nv2[1] -  nv0[2]*nv1[1]*nv2[0] - nv0[0]*nv1[2]*nv2[1] 
         + nv0[0]*nv1[1]*nv2[2] +  nv0[1]*nv1[2]*nv2[0] - nv0[1]*nv1[0]*nv2[2];
    
    if(dp <= 1e-8 || dp1 >= (1-1e-8))
      test = FALSE;
    else
      test = TRUE;

    if(test != test1)
      fprintf(stderr,"test %f simplified  %f\n",dp1,dp);
    return(test1);
}

/* calculates the normal of a face contour using Newell's formula. e

               a =  SUMi (yi - yi+1)(zi + zi+1);
               b =  SUMi (zi - zi+1)(xi + xi+1)
               c =  SUMi (xi - xi+1)(yi + yi+1)
*/
double
tri_normal(v0,v1,v2,n,norm)
FVECT v0,v1,v2,n;
int norm;
{
  double mag;

  n[0] = (v0[2] + v1[2]) * (v0[1] - v1[1]) + 
         (v1[2] + v2[2]) * (v1[1] - v2[1])  +
         (v2[2] + v0[2]) * (v2[1] - v0[1]);
  
  n[1] = (v0[2] - v1[2]) * (v0[0] + v1[0]) +
           (v1[2] - v2[2]) * (v1[0] + v2[0]) +
            (v2[2] - v0[2]) * (v2[0] + v0[0]);
  
  n[2] = (v0[1] + v1[1]) * (v0[0] - v1[0]) +
         (v1[1] + v2[1]) * (v1[0] - v2[0]) +
         (v2[1] + v0[1]) * (v2[0] - v0[0]);

  if(!norm)
     return(0);
  
  mag = normalize(n);

  return(mag);
}


tri_plane_equation(v0,v1,v2,peqptr,norm)
   FVECT v0,v1,v2; 
   FPEQ *peqptr;
   int norm;
{
    tri_normal(v0,v1,v2,FP_N(*peqptr),norm);

    FP_D(*peqptr) = -(DOT(FP_N(*peqptr),v0));
}


/* returns TRUE if ray from origin in direction v intersects plane defined
  * by normal plane_n, and plane_d. If plane is not parallel- returns
  * intersection point if r != NULL. If tptr!= NULL returns value of
  * t, if parallel, returns t=FHUGE
  */
int
intersect_vector_plane(v,peq,tptr,r)
   FVECT v;
   FPEQ peq;
   double *tptr;
   FVECT r;
{
  double t,d;
  int hit;
    /*
      Plane is Ax + By + Cz +D = 0:
      plane[0] = A,plane[1] = B,plane[2] = C,plane[3] = D
    */

    /* line is  l = p1 + (p2-p1)t, p1=origin */

    /* Solve for t: */
  d = -(DOT(FP_N(peq),v));
  if(ZERO(d))
  {
      t = FHUGE;
      hit = 0;
  }
  else
  {
      t =  FP_D(peq)/d;
      if(t < 0 )
         hit = 0;
      else
         hit = 1;
      if(r)
         {
             r[0] = v[0]*t;
             r[1] = v[1]*t;
             r[2] = v[2]*t;
         }
  }
    if(tptr)
       *tptr = t;
  return(hit);
}

int
intersect_ray_plane(orig,dir,peq,pd,r)
   FVECT orig,dir;
   FPEQ peq;
   double *pd;
   FVECT r;
{
  double t,d;
  int hit;
    /*
      Plane is Ax + By + Cz +D = 0:
      plane[0] = A,plane[1] = B,plane[2] = C,plane[3] = D
    */
     /*  A(orig[0] + dxt) + B(orig[1] + dyt) + C(orig[2] + dzt) + pd = 0
         t = -(DOT(plane_n,orig)+ plane_d)/(DOT(plane_n,d))
       line is  l = p1 + (p2-p1)t 
     */
    /* Solve for t: */
  d = DOT(FP_N(peq),dir);
  if(ZERO(d))
     return(0);
  t =  -(DOT(FP_N(peq),orig) + FP_D(peq))/d; 

  if(t < 0)
       hit = 0;
    else
       hit = 1;

  if(r)
     VSUM(r,orig,dir,t);

    if(pd)
       *pd = t;
  return(hit);
}


int
intersect_ray_oplane(orig,dir,n,pd,r)
   FVECT orig,dir;
   FVECT n;
   double *pd;
   FVECT r;
{
  double t,d;
  int hit;
    /*
      Plane is Ax + By + Cz +D = 0:
      plane[0] = A,plane[1] = B,plane[2] = C,plane[3] = D
    */
     /*  A(orig[0] + dxt) + B(orig[1] + dyt) + C(orig[2] + dzt) + pd = 0
         t = -(DOT(plane_n,orig)+ plane_d)/(DOT(plane_n,d))
       line is  l = p1 + (p2-p1)t 
     */
    /* Solve for t: */
    d= DOT(n,dir);
    if(ZERO(d))
       return(0);
    t =  -(DOT(n,orig))/d;
    if(t < 0)
       hit = 0;
    else
       hit = 1;

  if(r)
     VSUM(r,orig,dir,t);

    if(pd)
       *pd = t;
  return(hit);
}

/* Assumption: know crosses plane:dont need to check for 'on' case */
intersect_edge_coord_plane(v0,v1,w,r)
FVECT v0,v1;
int w;
FVECT r;
{
  FVECT dv;
  int wnext;
  double t;

  VSUB(dv,v1,v0);
  t = -v0[w]/dv[w];
  r[w] = 0.0;
  wnext = (w+1)%3;
  r[wnext] = v0[wnext] + dv[wnext]*t;
  wnext = (w+2)%3;
  r[wnext] = v0[wnext] + dv[wnext]*t;
}

int
intersect_edge_plane(e0,e1,peq,pd,r)
   FVECT e0,e1;
   FPEQ peq;
   double *pd;
   FVECT r;
{
  double t;
  int hit;
  FVECT d;
  /*
      Plane is Ax + By + Cz +D = 0:
      plane[0] = A,plane[1] = B,plane[2] = C,plane[3] = D
    */
     /*  A(orig[0] + dxt) + B(orig[1] + dyt) + C(orig[2] + dzt) + pd = 0
         t = -(DOT(plane_n,orig)+ plane_d)/(DOT(plane_n,d))
       line is  l = p1 + (p2-p1)t 
     */
    /* Solve for t: */
  VSUB(d,e1,e0);
  t =  -(DOT(FP_N(peq),e0) + FP_D(peq))/(DOT(FP_N(peq),d));
    if(t < 0)
       hit = 0;
    else
       hit = 1;

  VSUM(r,e0,d,t);

    if(pd)
       *pd = t;
  return(hit);
}

int
point_set_in_stri(v0,v1,v2,p,n,nset,sides)
FVECT v0,v1,v2,p;
FVECT n[3];
int *nset;
int sides[3];

{
    double d;
    /* Find the normal to the triangle ORIGIN:v0:v1 */
    if(!NTH_BIT(*nset,0))
    {
        VCROSS(n[0],v0,v1);
        SET_NTH_BIT(*nset,0);
    }
    /* Test the point for sidedness */
    d  = DOT(n[0],p);

    if(d > 0.0)
     {
       sides[0] =  GT_OUT;
       sides[1] = sides[2] = GT_INVALID;
       return(FALSE);
      }
    else
       sides[0] = GT_INTERIOR;
       
    /* Test next edge */
    if(!NTH_BIT(*nset,1))
    {
        VCROSS(n[1],v1,v2);
        SET_NTH_BIT(*nset,1);
    }
    /* Test the point for sidedness */
    d  = DOT(n[1],p);
    if(d > 0.0)
    {
        sides[1] = GT_OUT;
        sides[2] = GT_INVALID;
        return(FALSE);
    }
    else 
       sides[1] = GT_INTERIOR;
    /* Test next edge */
    if(!NTH_BIT(*nset,2))
    {
        VCROSS(n[2],v2,v0);
        SET_NTH_BIT(*nset,2);
    }
    /* Test the point for sidedness */
    d  = DOT(n[2],p);
    if(d > 0.0)
    {
      sides[2] = GT_OUT;
      return(FALSE);
    }
    else 
       sides[2] = GT_INTERIOR;
    /* Must be interior to the pyramid */
    return(GT_INTERIOR);
}


set_sidedness_tests(t0,t1,t2,p0,p1,p2,test,sides,nset,n)
   FVECT t0,t1,t2,p0,p1,p2;
   int test[3];
   int sides[3][3];
   int nset;
   FVECT n[3];
{
    int t;
    double d;

    
    /* p=0 */
    test[0] = 0;
    if(sides[0][0] == GT_INVALID)
    {
      if(!NTH_BIT(nset,0))
        VCROSS(n[0],t0,t1);
      /* Test the point for sidedness */
      d  = DOT(n[0],p0);
      if(d >= 0.0)
        SET_NTH_BIT(test[0],0);
    }
    else
      if(sides[0][0] != GT_INTERIOR)
        SET_NTH_BIT(test[0],0);

    if(sides[0][1] == GT_INVALID)
    {
      if(!NTH_BIT(nset,1))
        VCROSS(n[1],t1,t2);
        /* Test the point for sidedness */
        d  = DOT(n[1],p0);
        if(d >= 0.0)
          SET_NTH_BIT(test[0],1);
    }
    else
      if(sides[0][1] != GT_INTERIOR)
        SET_NTH_BIT(test[0],1);

    if(sides[0][2] == GT_INVALID)
    {
      if(!NTH_BIT(nset,2))
        VCROSS(n[2],t2,t0);
      /* Test the point for sidedness */
      d  = DOT(n[2],p0);
      if(d >= 0.0)
        SET_NTH_BIT(test[0],2);
    }
    else
      if(sides[0][2] != GT_INTERIOR)
        SET_NTH_BIT(test[0],2);

    /* p=1 */
    test[1] = 0;
    /* t=0*/
    if(sides[1][0] == GT_INVALID)
    {
      if(!NTH_BIT(nset,0))
        VCROSS(n[0],t0,t1);
      /* Test the point for sidedness */
      d  = DOT(n[0],p1);
      if(d >= 0.0)
        SET_NTH_BIT(test[1],0);
    }
    else
      if(sides[1][0] != GT_INTERIOR)
        SET_NTH_BIT(test[1],0);
    
    /* t=1 */
    if(sides[1][1] == GT_INVALID)
    {
      if(!NTH_BIT(nset,1))
        VCROSS(n[1],t1,t2);
      /* Test the point for sidedness */
      d  = DOT(n[1],p1);
      if(d >= 0.0)
        SET_NTH_BIT(test[1],1);
    }
    else
      if(sides[1][1] != GT_INTERIOR)
        SET_NTH_BIT(test[1],1);
       
    /* t=2 */
    if(sides[1][2] == GT_INVALID)
    {
      if(!NTH_BIT(nset,2))
        VCROSS(n[2],t2,t0);
      /* Test the point for sidedness */
      d  = DOT(n[2],p1);
      if(d >= 0.0)
        SET_NTH_BIT(test[1],2);
    }
    else
      if(sides[1][2] != GT_INTERIOR)
        SET_NTH_BIT(test[1],2);

    /* p=2 */
    test[2] = 0;
    /* t = 0 */
    if(sides[2][0] == GT_INVALID)
    {
      if(!NTH_BIT(nset,0))
        VCROSS(n[0],t0,t1);
      /* Test the point for sidedness */
      d  = DOT(n[0],p2);
      if(d >= 0.0)
        SET_NTH_BIT(test[2],0);
    }
    else
      if(sides[2][0] != GT_INTERIOR)
        SET_NTH_BIT(test[2],0);
    /* t=1 */
    if(sides[2][1] == GT_INVALID)
    {
      if(!NTH_BIT(nset,1))
        VCROSS(n[1],t1,t2);
      /* Test the point for sidedness */
      d  = DOT(n[1],p2);
      if(d >= 0.0)
        SET_NTH_BIT(test[2],1);
    }
    else
      if(sides[2][1] != GT_INTERIOR)
        SET_NTH_BIT(test[2],1);
    /* t=2 */
    if(sides[2][2] == GT_INVALID)
    {
      if(!NTH_BIT(nset,2))
        VCROSS(n[2],t2,t0);
      /* Test the point for sidedness */
      d  = DOT(n[2],p2);
      if(d >= 0.0)
        SET_NTH_BIT(test[2],2);
    }
    else
      if(sides[2][2] != GT_INTERIOR)
        SET_NTH_BIT(test[2],2);
}

double
point_on_sphere(ps,p,c)
FVECT ps,p,c;
{
  double d;
    VSUB(ps,p,c);    
    d= normalize(ps);
    return(d);
}

int
point_in_stri(v0,v1,v2,p)
FVECT v0,v1,v2,p;
{
    double d;
    FVECT n;  

    VCROSS(n,v0,v1);
    /* Test the point for sidedness */
    d  = DOT(n,p);
    if(d > 0.0)
      return(FALSE);

    /* Test next edge */
    VCROSS(n,v1,v2);
    /* Test the point for sidedness */
    d  = DOT(n,p);
    if(d > 0.0)
       return(FALSE);

    /* Test next edge */
    VCROSS(n,v2,v0);
    /* Test the point for sidedness */
    d  = DOT(n,p);
    if(d > 0.0)
       return(FALSE);
    /* Must be interior to the pyramid */
    return(GT_INTERIOR);
}


int
ray_intersect_tri(orig,dir,v0,v1,v2,pt)
FVECT orig,dir;
FVECT v0,v1,v2;
FVECT pt;
{
  FVECT p0,p1,p2,p;
  FPEQ peq;
  int type;

  VSUB(p0,v0,orig);
  VSUB(p1,v1,orig);
  VSUB(p2,v2,orig);

  if(point_in_stri(p0,p1,p2,dir))
  {
      /* Intersect the ray with the triangle plane */
      tri_plane_equation(v0,v1,v2,&peq,FALSE);
      return(intersect_ray_plane(orig,dir,peq,NULL,pt));
  }
  return(FALSE);
}


calculate_view_frustum(vp,hv,vv,horiz,vert,near,far,fnear,ffar)
FVECT vp,hv,vv;
double horiz,vert,near,far;
FVECT fnear[4],ffar[4];
{
    double height,width;
    FVECT t,nhv,nvv,ndv;
    double w2,h2;
    /* Calculate the x and y dimensions of the near face */
    /* hv and vv are the horizontal and vertical vectors in the
       view frame-the magnitude is the dimension of the front frustum
       face at z =1
    */
    VCOPY(nhv,hv);
    VCOPY(nvv,vv);
    w2 = normalize(nhv);
    h2 = normalize(nvv);
    /* Use similar triangles to calculate the dimensions at z=near */
    width  = near*0.5*w2;
    height = near*0.5*h2;

    VCROSS(ndv,nvv,nhv);
    /* Calculate the world space points corresponding to the 4 corners
       of the front face of the view frustum
     */
    fnear[0][0] =  width*nhv[0] + height*nvv[0] + near*ndv[0] + vp[0] ;
    fnear[0][1] =  width*nhv[1] + height*nvv[1] + near*ndv[1] + vp[1];
    fnear[0][2] =  width*nhv[2] + height*nvv[2] + near*ndv[2] + vp[2];
    fnear[1][0] = -width*nhv[0] + height*nvv[0] + near*ndv[0] + vp[0];
    fnear[1][1] = -width*nhv[1] + height*nvv[1] + near*ndv[1] + vp[1];
    fnear[1][2] = -width*nhv[2] + height*nvv[2] + near*ndv[2] + vp[2];
    fnear[2][0] = -width*nhv[0] - height*nvv[0] + near*ndv[0] + vp[0];
    fnear[2][1] = -width*nhv[1] - height*nvv[1] + near*ndv[1] + vp[1];
    fnear[2][2] = -width*nhv[2] - height*nvv[2] + near*ndv[2] + vp[2];
    fnear[3][0] =  width*nhv[0] - height*nvv[0] + near*ndv[0] + vp[0];
    fnear[3][1] =  width*nhv[1] - height*nvv[1] + near*ndv[1] + vp[1];
    fnear[3][2] =  width*nhv[2] - height*nvv[2] + near*ndv[2] + vp[2];

    /* Now do the far face */
    width  = far*0.5*w2;
    height = far*0.5*h2;
    ffar[0][0] =  width*nhv[0] + height*nvv[0] + far*ndv[0] + vp[0] ;
    ffar[0][1] =  width*nhv[1] + height*nvv[1] + far*ndv[1] + vp[1] ;
    ffar[0][2] =  width*nhv[2] + height*nvv[2] + far*ndv[2] + vp[2] ;
    ffar[1][0] = -width*nhv[0] + height*nvv[0] + far*ndv[0] + vp[0] ;
    ffar[1][1] = -width*nhv[1] + height*nvv[1] + far*ndv[1] + vp[1] ;
    ffar[1][2] = -width*nhv[2] + height*nvv[2] + far*ndv[2] + vp[2] ;
    ffar[2][0] = -width*nhv[0] - height*nvv[0] + far*ndv[0] + vp[0] ;
    ffar[2][1] = -width*nhv[1] - height*nvv[1] + far*ndv[1] + vp[1] ;
    ffar[2][2] = -width*nhv[2] - height*nvv[2] + far*ndv[2] + vp[2] ;
    ffar[3][0] =  width*nhv[0] - height*nvv[0] + far*ndv[0] + vp[0] ;
    ffar[3][1] =  width*nhv[1] - height*nvv[1] + far*ndv[1] + vp[1] ;
    ffar[3][2] =  width*nhv[2] - height*nvv[2] + far*ndv[2] + vp[2] ;
}

int
max_index(v,r)
FVECT v;
double *r;
{
  double p[3];
  int i;

  p[0] = fabs(v[0]);
  p[1] = fabs(v[1]);
  p[2] = fabs(v[2]);
  i = (p[0]>=p[1])?((p[0]>=p[2])?0:2):((p[1]>=p[2])?1:2);  
  if(r)
    *r = p[i];
  return(i);
}

int
closest_point_in_tri(p0,p1,p2,p,p0id,p1id,p2id)
FVECT p0,p1,p2,p;
int p0id,p1id,p2id;
{
    double d,d1;
    int i;
    
    d =  DIST_SQ(p,p0);
    d1 = DIST_SQ(p,p1);
    if(d < d1)
    {
      d1 = DIST_SQ(p,p2);
      i = (d1 < d)?p2id:p0id;
    }
    else
    {
      d = DIST_SQ(p,p2);
      i = (d < d1)? p2id:p1id;
    }
    return(i);
}

/* Find the normalized barycentric coordinates of p relative to
 * triangle v0,v1,v2. Return result in coord
 */
bary2d(x1,y1,x2,y2,x3,y3,px,py,coord)
double x1,y1,x2,y2,x3,y3;
double px,py;
double coord[3];
{
  double a;

  a =  (x2 - x1) * (y3 - y1) - (x3 - x1) * (y2 - y1);
  coord[0] = ((x2 - px) * (y3 - py) - (x3 - px) * (y2 - py)) / a; 
  coord[1] = ((x3 - px) * (y1 - py) - (x1 - px) * (y3 - py)) / a;
  coord[2] = ((x1 - px) * (y2 - py) - (x2 - px) * (y1 - py)) / a;
 
}


bary_parent(coord,i)
BCOORD coord[3];
int i;
{
  switch(i) {
  case 0:
    /* update bary for child */
    coord[0] = (coord[0] >> 1) + MAXBCOORD4;
    coord[1] >>= 1;
    coord[2] >>= 1;
    break;
  case 1:
    coord[0] >>= 1;
    coord[1]  = (coord[1] >> 1) + MAXBCOORD4;
    coord[2] >>= 1;
    break;
    
  case 2:
    coord[0] >>= 1;
    coord[1] >>= 1;
    coord[2] = (coord[2] >> 1) + MAXBCOORD4;
    break;
    
  case 3:
    coord[0] = MAXBCOORD4 - (coord[0] >> 1);
    coord[1] = MAXBCOORD4 - (coord[1] >> 1);
    coord[2] = MAXBCOORD4 - (coord[2] >> 1);
    break;
#ifdef DEBUG
  default:
    eputs("bary_parent():Invalid child\n");
    break;
#endif
  }
}

bary_from_child(coord,child,next)
BCOORD coord[3];
int child,next;
{
#ifdef DEBUG
  if(child <0 || child > 3)
  {
    eputs("bary_from_child():Invalid child\n");
    return;
  }
  if(next <0 || next > 3)
  {
    eputs("bary_from_child():Invalid next\n");
    return;
  }
#endif
  if(next == child)
    return;

  switch(child){
  case 0:
      coord[0] = 0;
      coord[1] = MAXBCOORD2 - coord[1];
      coord[2] = MAXBCOORD2 - coord[2];
      break;
  case 1:
      coord[0] = MAXBCOORD2 - coord[0];
      coord[1] = 0;
      coord[2] = MAXBCOORD2 - coord[2];
      break;
  case 2:
      coord[0] = MAXBCOORD2 - coord[0];
      coord[1] = MAXBCOORD2 - coord[1];
      coord[2] = 0;
    break;
  case 3:
    switch(next){
    case 0:
      coord[0] = 0;
      coord[1] = MAXBCOORD2 - coord[1];
      coord[2] = MAXBCOORD2 - coord[2];
      break;
    case 1:
      coord[0] = MAXBCOORD2 - coord[0];
      coord[1] = 0;
      coord[2] = MAXBCOORD2 - coord[2];
      break;
    case 2:
      coord[0] = MAXBCOORD2 - coord[0];
      coord[1] = MAXBCOORD2 - coord[1];
      coord[2] = 0;
      break;
    }
    break;
  }
}

int
bary_child(coord)
BCOORD coord[3];
{
  int i;

  if(coord[0] > MAXBCOORD4)
  { 
      /* update bary for child */
      coord[0] = (coord[0]<< 1) - MAXBCOORD2;
      coord[1] <<= 1;
      coord[2] <<= 1;
      return(0);
  }
  else
    if(coord[1] > MAXBCOORD4)
    {
      coord[0] <<= 1;
      coord[1] = (coord[1] << 1) - MAXBCOORD2;
      coord[2] <<= 1;
      return(1);
    }
    else
      if(coord[2] > MAXBCOORD4)
      {
        coord[0] <<= 1;
        coord[1] <<= 1;
        coord[2] = (coord[2] << 1) - MAXBCOORD2;
        return(2);
      }
      else
         {
           coord[0] = MAXBCOORD2 - (coord[0] << 1);
           coord[1] = MAXBCOORD2 - (coord[1] << 1);
           coord[2] = MAXBCOORD2 - (coord[2] << 1);
           return(3);
         }
}

int
bary_nth_child(coord,i)
BCOORD coord[3];
int i;
{

  switch(i){
  case 0:
    /* update bary for child */
    coord[0] = (coord[0]<< 1) - MAXBCOORD2;
    coord[1] <<= 1;
    coord[2] <<= 1;
    break;
  case 1:
    coord[0] <<= 1;
    coord[1] = (coord[1] << 1) - MAXBCOORD2;
    coord[2] <<= 1;
    break;
  case 2:
    coord[0] <<= 1;
    coord[1] <<= 1;
    coord[2] = (coord[2] << 1) - MAXBCOORD2;
    break;
  case 3:
    coord[0] = MAXBCOORD2 - (coord[0] << 1);
    coord[1] = MAXBCOORD2 - (coord[1] << 1);
    coord[2] = MAXBCOORD2 - (coord[2] << 1);
    break;
  }
}


Revision:	3.12
Committed:	Thu Jun 10 15:22:22 1999 UTC (24 years, 10 months ago) by gwlarson
Content type:	text/plain
Branch:	MAIN
Changes since 3.11:	+66 -321 lines
Log Message:	Implemented sample quadtree in place of triangle quadtree Made geometric predicates more robust Added #define LORES which utilizes a single precision floating point sample array, the default is a double sample array Added topology DEBUG commands (for DEBUG > 1) Made code optimizations
#	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_geom.c
9			*/
10
11			#include "standard.h"
12			#include "sm_geom.h"
13
14			tri_centroid(v0,v1,v2,c)
15			FVECT v0,v1,v2,c;
16			{
17			/* Average three triangle vertices to give centroid: return in c */
18			c[0] = (v0[0] + v1[0] + v2[0])/3.0;
19			c[1] = (v0[1] + v1[1] + v2[1])/3.0;
20			c[2] = (v0[2] + v1[2] + v2[2])/3.0;
21			}
22
23
24			int
25			vec3_equal(v1,v2)
26			FVECT v1,v2;
27			{
28			return(EQUAL(v1[0],v2[0]) && EQUAL(v1[1],v2[1])&& EQUAL(v1[2],v2[2]));
29			}
30	gwlarson	3.8	#if 0
31			extern FVECT Norm[500];
32			extern int Ncnt;
33			#endif
34	gwlarson	3.1
35			int
36			convex_angle(v0,v1,v2)
37			FVECT v0,v1,v2;
38			{
39	gwlarson	3.12	double dp,dp1;
40			int test,test1;
41			FVECT nv0,nv1,nv2;
42			FVECT cp01,cp12,cp;
43	gwlarson	3.8
44			/* test sign of (v0Xv1)X(v1Xv2). v1 */
45	gwlarson	3.12	/* un-Simplified: */
46			VCOPY(nv0,v0);
47			normalize(nv0);
48			VCOPY(nv1,v1);
49			normalize(nv1);
50			VCOPY(nv2,v2);
51			normalize(nv2);
52
53			VCROSS(cp01,nv0,nv1);
54			VCROSS(cp12,nv1,nv2);
55	gwlarson	3.1	VCROSS(cp,cp01,cp12);
56	gwlarson	3.12	normalize(cp);
57			dp1 = DOT(cp,nv1);
58			if(dp1 <= 1e-8 \|\| dp1 >= (1-1e-8)) /* Test if on other side,or colinear*/
59			test1 = FALSE;
60			else
61			test1 = TRUE;
62
63			dp = nv0[2]nv1[0]nv2[1] - nv0[2]nv1[1]nv2[0] - nv0[0]nv1[2]nv2[1]
64			+ nv0[0]nv1[1]nv2[2] + nv0[1]nv1[2]nv2[0] - nv0[1]nv1[0]nv2[2];
65
66			if(dp <= 1e-8 \|\| dp1 >= (1-1e-8))
67			test = FALSE;
68			else
69			test = TRUE;
70
71			if(test != test1)
72			fprintf(stderr,"test %f simplified %f\n",dp1,dp);
73			return(test1);
74	gwlarson	3.1	}
75
76			/* calculates the normal of a face contour using Newell's formula. e
77
78	gwlarson	3.9	a = SUMi (yi - yi+1)(zi + zi+1);
79	gwlarson	3.1	b = SUMi (zi - zi+1)(xi + xi+1)
80			c = SUMi (xi - xi+1)(yi + yi+1)
81			*/
82			double
83			tri_normal(v0,v1,v2,n,norm)
84			FVECT v0,v1,v2,n;
85	gwlarson	3.4	int norm;
86	gwlarson	3.1	{
87			double mag;
88
89			n[0] = (v0[2] + v1[2]) * (v0[1] - v1[1]) +
90			(v1[2] + v2[2]) * (v1[1] - v2[1]) +
91			(v2[2] + v0[2]) * (v2[1] - v0[1]);
92
93			n[1] = (v0[2] - v1[2]) * (v0[0] + v1[0]) +
94			(v1[2] - v2[2]) * (v1[0] + v2[0]) +
95	gwlarson	3.9	(v2[2] - v0[2]) * (v2[0] + v0[0]);
96	gwlarson	3.1
97			n[2] = (v0[1] + v1[1]) * (v0[0] - v1[0]) +
98			(v1[1] + v2[1]) * (v1[0] - v2[0]) +
99			(v2[1] + v0[1]) * (v2[0] - v0[0]);
100
101			if(!norm)
102			return(0);
103
104			mag = normalize(n);
105
106			return(mag);
107			}
108
109
110	gwlarson	3.7	tri_plane_equation(v0,v1,v2,peqptr,norm)
111	gwlarson	3.12	FVECT v0,v1,v2;
112	gwlarson	3.7	FPEQ *peqptr;
113	gwlarson	3.4	int norm;
114	gwlarson	3.1	{
115	gwlarson	3.7	tri_normal(v0,v1,v2,FP_N(*peqptr),norm);
116	gwlarson	3.1
117	gwlarson	3.7	FP_D(peqptr) = -(DOT(FP_N(peqptr),v0));
118	gwlarson	3.1	}
119
120
121	gwlarson	3.4	/* returns TRUE if ray from origin in direction v intersects plane defined
122			* by normal plane_n, and plane_d. If plane is not parallel- returns
123			* intersection point if r != NULL. If tptr!= NULL returns value of
124			* t, if parallel, returns t=FHUGE
125			*/
126	gwlarson	3.1	int
127	gwlarson	3.7	intersect_vector_plane(v,peq,tptr,r)
128			FVECT v;
129			FPEQ peq;
130	gwlarson	3.2	double *tptr;
131	gwlarson	3.1	FVECT r;
132			{
133	gwlarson	3.4	double t,d;
134	gwlarson	3.1	int hit;
135			/*
136			Plane is Ax + By + Cz +D = 0:
137			plane[0] = A,plane[1] = B,plane[2] = C,plane[3] = D
138			*/
139
140			/* line is l = p1 + (p2-p1)t, p1=origin */
141
142			/* Solve for t: */
143	gwlarson	3.7	d = -(DOT(FP_N(peq),v));
144	gwlarson	3.4	if(ZERO(d))
145			{
146			t = FHUGE;
147			hit = 0;
148			}
149			else
150			{
151	gwlarson	3.7	t = FP_D(peq)/d;
152	gwlarson	3.4	if(t < 0 )
153			hit = 0;
154			else
155			hit = 1;
156			if(r)
157			{
158			r[0] = v[0]*t;
159			r[1] = v[1]*t;
160			r[2] = v[2]*t;
161			}
162			}
163	gwlarson	3.2	if(tptr)
164			*tptr = t;
165	gwlarson	3.1	return(hit);
166			}
167
168			int
169	gwlarson	3.7	intersect_ray_plane(orig,dir,peq,pd,r)
170	gwlarson	3.1	FVECT orig,dir;
171	gwlarson	3.7	FPEQ peq;
172	gwlarson	3.1	double *pd;
173			FVECT r;
174			{
175	gwlarson	3.8	double t,d;
176	gwlarson	3.1	int hit;
177			/*
178			Plane is Ax + By + Cz +D = 0:
179			plane[0] = A,plane[1] = B,plane[2] = C,plane[3] = D
180			*/
181	gwlarson	3.2	/* A(orig[0] + dxt) + B(orig[1] + dyt) + C(orig[2] + dzt) + pd = 0
182			t = -(DOT(plane_n,orig)+ plane_d)/(DOT(plane_n,d))
183			line is l = p1 + (p2-p1)t
184			*/
185	gwlarson	3.1	/* Solve for t: */
186	gwlarson	3.8	d = DOT(FP_N(peq),dir);
187			if(ZERO(d))
188			return(0);
189			t = -(DOT(FP_N(peq),orig) + FP_D(peq))/d;
190
191			if(t < 0)
192	gwlarson	3.4	hit = 0;
193			else
194	gwlarson	3.1	hit = 1;
195	gwlarson	3.4
196			if(r)
197			VSUM(r,orig,dir,t);
198
199			if(pd)
200			*pd = t;
201			return(hit);
202			}
203
204
205			int
206	gwlarson	3.7	intersect_ray_oplane(orig,dir,n,pd,r)
207			FVECT orig,dir;
208			FVECT n;
209			double *pd;
210			FVECT r;
211			{
212	gwlarson	3.8	double t,d;
213	gwlarson	3.7	int hit;
214			/*
215			Plane is Ax + By + Cz +D = 0:
216			plane[0] = A,plane[1] = B,plane[2] = C,plane[3] = D
217			*/
218			/* A(orig[0] + dxt) + B(orig[1] + dyt) + C(orig[2] + dzt) + pd = 0
219			t = -(DOT(plane_n,orig)+ plane_d)/(DOT(plane_n,d))
220			line is l = p1 + (p2-p1)t
221			*/
222			/* Solve for t: */
223	gwlarson	3.8	d= DOT(n,dir);
224			if(ZERO(d))
225			return(0);
226			t = -(DOT(n,orig))/d;
227	gwlarson	3.7	if(t < 0)
228			hit = 0;
229			else
230			hit = 1;
231
232			if(r)
233			VSUM(r,orig,dir,t);
234
235			if(pd)
236			*pd = t;
237			return(hit);
238			}
239
240	gwlarson	3.9	/* Assumption: know crosses plane:dont need to check for 'on' case */
241			intersect_edge_coord_plane(v0,v1,w,r)
242			FVECT v0,v1;
243			int w;
244			FVECT r;
245			{
246			FVECT dv;
247			int wnext;
248			double t;
249	gwlarson	3.7
250	gwlarson	3.9	VSUB(dv,v1,v0);
251			t = -v0[w]/dv[w];
252			r[w] = 0.0;
253			wnext = (w+1)%3;
254			r[wnext] = v0[wnext] + dv[wnext]*t;
255			wnext = (w+2)%3;
256			r[wnext] = v0[wnext] + dv[wnext]*t;
257			}
258
259	gwlarson	3.7	int
260			intersect_edge_plane(e0,e1,peq,pd,r)
261	gwlarson	3.4	FVECT e0,e1;
262	gwlarson	3.7	FPEQ peq;
263	gwlarson	3.4	double *pd;
264			FVECT r;
265			{
266			double t;
267			int hit;
268			FVECT d;
269			/*
270			Plane is Ax + By + Cz +D = 0:
271			plane[0] = A,plane[1] = B,plane[2] = C,plane[3] = D
272			*/
273			/* A(orig[0] + dxt) + B(orig[1] + dyt) + C(orig[2] + dzt) + pd = 0
274			t = -(DOT(plane_n,orig)+ plane_d)/(DOT(plane_n,d))
275			line is l = p1 + (p2-p1)t
276			*/
277			/* Solve for t: */
278			VSUB(d,e1,e0);
279	gwlarson	3.7	t = -(DOT(FP_N(peq),e0) + FP_D(peq))/(DOT(FP_N(peq),d));
280	gwlarson	3.4	if(t < 0)
281			hit = 0;
282	gwlarson	3.1	else
283	gwlarson	3.4	hit = 1;
284	gwlarson	3.1
285	gwlarson	3.4	VSUM(r,e0,d,t);
286	gwlarson	3.1
287			if(pd)
288			*pd = t;
289			return(hit);
290			}
291
292			int
293	gwlarson	3.4	point_set_in_stri(v0,v1,v2,p,n,nset,sides)
294	gwlarson	3.1	FVECT v0,v1,v2,p;
295			FVECT n[3];
296	gwlarson	3.4	int *nset;
297			int sides[3];
298	gwlarson	3.1
299			{
300	gwlarson	3.4	double d;
301	gwlarson	3.1	/* Find the normal to the triangle ORIGIN:v0:v1 */
302			if(!NTH_BIT(*nset,0))
303			{
304	gwlarson	3.8	VCROSS(n[0],v0,v1);
305	gwlarson	3.1	SET_NTH_BIT(*nset,0);
306			}
307			/* Test the point for sidedness */
308			d = DOT(n[0],p);
309
310	gwlarson	3.4	if(d > 0.0)
311			{
312			sides[0] = GT_OUT;
313			sides[1] = sides[2] = GT_INVALID;
314			return(FALSE);
315	gwlarson	3.1	}
316			else
317			sides[0] = GT_INTERIOR;
318
319			/* Test next edge */
320			if(!NTH_BIT(*nset,1))
321			{
322	gwlarson	3.8	VCROSS(n[1],v1,v2);
323	gwlarson	3.1	SET_NTH_BIT(*nset,1);
324			}
325			/* Test the point for sidedness */
326			d = DOT(n[1],p);
327	gwlarson	3.4	if(d > 0.0)
328	gwlarson	3.1	{
329			sides[1] = GT_OUT;
330			sides[2] = GT_INVALID;
331			return(FALSE);
332			}
333			else
334			sides[1] = GT_INTERIOR;
335			/* Test next edge */
336			if(!NTH_BIT(*nset,2))
337			{
338	gwlarson	3.8	VCROSS(n[2],v2,v0);
339	gwlarson	3.1	SET_NTH_BIT(*nset,2);
340			}
341			/* Test the point for sidedness */
342			d = DOT(n[2],p);
343	gwlarson	3.4	if(d > 0.0)
344	gwlarson	3.1	{
345	gwlarson	3.4	sides[2] = GT_OUT;
346			return(FALSE);
347	gwlarson	3.1	}
348			else
349			sides[2] = GT_INTERIOR;
350			/* Must be interior to the pyramid */
351			return(GT_INTERIOR);
352			}
353
354
355
356			set_sidedness_tests(t0,t1,t2,p0,p1,p2,test,sides,nset,n)
357			FVECT t0,t1,t2,p0,p1,p2;
358	gwlarson	3.4	int test[3];
359			int sides[3][3];
360			int nset;
361	gwlarson	3.1	FVECT n[3];
362			{
363	gwlarson	3.4	int t;
364	gwlarson	3.1	double d;
365
366
367			/* p=0 */
368			test[0] = 0;
369			if(sides[0][0] == GT_INVALID)
370			{
371			if(!NTH_BIT(nset,0))
372	gwlarson	3.8	VCROSS(n[0],t0,t1);
373	gwlarson	3.1	/* Test the point for sidedness */
374			d = DOT(n[0],p0);
375	gwlarson	3.4	if(d >= 0.0)
376	gwlarson	3.1	SET_NTH_BIT(test[0],0);
377			}
378			else
379			if(sides[0][0] != GT_INTERIOR)
380			SET_NTH_BIT(test[0],0);
381
382			if(sides[0][1] == GT_INVALID)
383			{
384			if(!NTH_BIT(nset,1))
385	gwlarson	3.8	VCROSS(n[1],t1,t2);
386	gwlarson	3.1	/* Test the point for sidedness */
387			d = DOT(n[1],p0);
388	gwlarson	3.4	if(d >= 0.0)
389	gwlarson	3.1	SET_NTH_BIT(test[0],1);
390			}
391			else
392			if(sides[0][1] != GT_INTERIOR)
393			SET_NTH_BIT(test[0],1);
394
395			if(sides[0][2] == GT_INVALID)
396			{
397			if(!NTH_BIT(nset,2))
398	gwlarson	3.8	VCROSS(n[2],t2,t0);
399	gwlarson	3.1	/* Test the point for sidedness */
400			d = DOT(n[2],p0);
401	gwlarson	3.4	if(d >= 0.0)
402	gwlarson	3.1	SET_NTH_BIT(test[0],2);
403			}
404			else
405			if(sides[0][2] != GT_INTERIOR)
406			SET_NTH_BIT(test[0],2);
407
408			/* p=1 */
409			test[1] = 0;
410			/* t=0*/
411			if(sides[1][0] == GT_INVALID)
412			{
413			if(!NTH_BIT(nset,0))
414	gwlarson	3.8	VCROSS(n[0],t0,t1);
415	gwlarson	3.1	/* Test the point for sidedness */
416			d = DOT(n[0],p1);
417	gwlarson	3.4	if(d >= 0.0)
418	gwlarson	3.1	SET_NTH_BIT(test[1],0);
419			}
420			else
421			if(sides[1][0] != GT_INTERIOR)
422			SET_NTH_BIT(test[1],0);
423
424			/* t=1 */
425			if(sides[1][1] == GT_INVALID)
426			{
427			if(!NTH_BIT(nset,1))
428	gwlarson	3.8	VCROSS(n[1],t1,t2);
429	gwlarson	3.1	/* Test the point for sidedness */
430			d = DOT(n[1],p1);
431	gwlarson	3.4	if(d >= 0.0)
432	gwlarson	3.1	SET_NTH_BIT(test[1],1);
433			}
434			else
435			if(sides[1][1] != GT_INTERIOR)
436			SET_NTH_BIT(test[1],1);
437
438			/* t=2 */
439			if(sides[1][2] == GT_INVALID)
440			{
441			if(!NTH_BIT(nset,2))
442	gwlarson	3.8	VCROSS(n[2],t2,t0);
443	gwlarson	3.1	/* Test the point for sidedness */
444			d = DOT(n[2],p1);
445	gwlarson	3.4	if(d >= 0.0)
446	gwlarson	3.1	SET_NTH_BIT(test[1],2);
447			}
448			else
449			if(sides[1][2] != GT_INTERIOR)
450			SET_NTH_BIT(test[1],2);
451
452			/* p=2 */
453			test[2] = 0;
454			/* t = 0 */
455			if(sides[2][0] == GT_INVALID)
456			{
457			if(!NTH_BIT(nset,0))
458	gwlarson	3.8	VCROSS(n[0],t0,t1);
459	gwlarson	3.1	/* Test the point for sidedness */
460			d = DOT(n[0],p2);
461	gwlarson	3.4	if(d >= 0.0)
462	gwlarson	3.1	SET_NTH_BIT(test[2],0);
463			}
464			else
465			if(sides[2][0] != GT_INTERIOR)
466			SET_NTH_BIT(test[2],0);
467			/* t=1 */
468			if(sides[2][1] == GT_INVALID)
469			{
470			if(!NTH_BIT(nset,1))
471	gwlarson	3.8	VCROSS(n[1],t1,t2);
472	gwlarson	3.1	/* Test the point for sidedness */
473			d = DOT(n[1],p2);
474	gwlarson	3.4	if(d >= 0.0)
475	gwlarson	3.1	SET_NTH_BIT(test[2],1);
476			}
477			else
478			if(sides[2][1] != GT_INTERIOR)
479			SET_NTH_BIT(test[2],1);
480			/* t=2 */
481			if(sides[2][2] == GT_INVALID)
482			{
483			if(!NTH_BIT(nset,2))
484	gwlarson	3.8	VCROSS(n[2],t2,t0);
485	gwlarson	3.1	/* Test the point for sidedness */
486			d = DOT(n[2],p2);
487	gwlarson	3.4	if(d >= 0.0)
488	gwlarson	3.1	SET_NTH_BIT(test[2],2);
489			}
490			else
491			if(sides[2][2] != GT_INTERIOR)
492			SET_NTH_BIT(test[2],2);
493			}
494
495	gwlarson	3.12	double
496			point_on_sphere(ps,p,c)
497			FVECT ps,p,c;
498			{
499			double d;
500			VSUB(ps,p,c);
501			d= normalize(ps);
502			return(d);
503			}
504	gwlarson	3.1
505			int
506	gwlarson	3.12	point_in_stri(v0,v1,v2,p)
507			FVECT v0,v1,v2,p;
508	gwlarson	3.1	{
509	gwlarson	3.12	double d;
510			FVECT n;
511	gwlarson	3.9
512	gwlarson	3.12	VCROSS(n,v0,v1);
513			/* Test the point for sidedness */
514			d = DOT(n,p);
515			if(d > 0.0)
516			return(FALSE);
517	gwlarson	3.1
518	gwlarson	3.12	/* Test next edge */
519			VCROSS(n,v1,v2);
520			/* Test the point for sidedness */
521			d = DOT(n,p);
522			if(d > 0.0)
523			return(FALSE);
524	gwlarson	3.1
525	gwlarson	3.12	/* Test next edge */
526			VCROSS(n,v2,v0);
527			/* Test the point for sidedness */
528			d = DOT(n,p);
529			if(d > 0.0)
530			return(FALSE);
531			/* Must be interior to the pyramid */
532			return(GT_INTERIOR);
533			}
534	gwlarson	3.1
535
536			int
537	gwlarson	3.4	ray_intersect_tri(orig,dir,v0,v1,v2,pt)
538	gwlarson	3.1	FVECT orig,dir;
539			FVECT v0,v1,v2;
540			FVECT pt;
541			{
542	gwlarson	3.7	FVECT p0,p1,p2,p;
543			FPEQ peq;
544	gwlarson	3.4	int type;
545
546	gwlarson	3.5	VSUB(p0,v0,orig);
547			VSUB(p1,v1,orig);
548			VSUB(p2,v2,orig);
549
550	gwlarson	3.4	if(point_in_stri(p0,p1,p2,dir))
551	gwlarson	3.1	{
552			/* Intersect the ray with the triangle plane */
553	gwlarson	3.7	tri_plane_equation(v0,v1,v2,&peq,FALSE);
554			return(intersect_ray_plane(orig,dir,peq,NULL,pt));
555	gwlarson	3.1	}
556	gwlarson	3.4	return(FALSE);
557	gwlarson	3.1	}
558
559
560			calculate_view_frustum(vp,hv,vv,horiz,vert,near,far,fnear,ffar)
561			FVECT vp,hv,vv;
562			double horiz,vert,near,far;
563			FVECT fnear[4],ffar[4];
564			{
565			double height,width;
566			FVECT t,nhv,nvv,ndv;
567			double w2,h2;
568			/* Calculate the x and y dimensions of the near face */
569			/* hv and vv are the horizontal and vertical vectors in the
570			view frame-the magnitude is the dimension of the front frustum
571			face at z =1
572			*/
573			VCOPY(nhv,hv);
574			VCOPY(nvv,vv);
575			w2 = normalize(nhv);
576			h2 = normalize(nvv);
577			/* Use similar triangles to calculate the dimensions at z=near */
578			width = near0.5w2;
579			height = near0.5h2;
580
581			VCROSS(ndv,nvv,nhv);
582			/* Calculate the world space points corresponding to the 4 corners
583			of the front face of the view frustum
584			*/
585			fnear[0][0] = widthnhv[0] + heightnvv[0] + near*ndv[0] + vp[0] ;
586			fnear[0][1] = widthnhv[1] + heightnvv[1] + near*ndv[1] + vp[1];
587			fnear[0][2] = widthnhv[2] + heightnvv[2] + near*ndv[2] + vp[2];
588			fnear[1][0] = -widthnhv[0] + heightnvv[0] + near*ndv[0] + vp[0];
589			fnear[1][1] = -widthnhv[1] + heightnvv[1] + near*ndv[1] + vp[1];
590			fnear[1][2] = -widthnhv[2] + heightnvv[2] + near*ndv[2] + vp[2];
591			fnear[2][0] = -widthnhv[0] - heightnvv[0] + near*ndv[0] + vp[0];
592			fnear[2][1] = -widthnhv[1] - heightnvv[1] + near*ndv[1] + vp[1];
593			fnear[2][2] = -widthnhv[2] - heightnvv[2] + near*ndv[2] + vp[2];
594			fnear[3][0] = widthnhv[0] - heightnvv[0] + near*ndv[0] + vp[0];
595			fnear[3][1] = widthnhv[1] - heightnvv[1] + near*ndv[1] + vp[1];
596			fnear[3][2] = widthnhv[2] - heightnvv[2] + near*ndv[2] + vp[2];
597
598			/* Now do the far face */
599			width = far0.5w2;
600			height = far0.5h2;
601			ffar[0][0] = widthnhv[0] + heightnvv[0] + far*ndv[0] + vp[0] ;
602			ffar[0][1] = widthnhv[1] + heightnvv[1] + far*ndv[1] + vp[1] ;
603			ffar[0][2] = widthnhv[2] + heightnvv[2] + far*ndv[2] + vp[2] ;
604			ffar[1][0] = -widthnhv[0] + heightnvv[0] + far*ndv[0] + vp[0] ;
605			ffar[1][1] = -widthnhv[1] + heightnvv[1] + far*ndv[1] + vp[1] ;
606			ffar[1][2] = -widthnhv[2] + heightnvv[2] + far*ndv[2] + vp[2] ;
607			ffar[2][0] = -widthnhv[0] - heightnvv[0] + far*ndv[0] + vp[0] ;
608			ffar[2][1] = -widthnhv[1] - heightnvv[1] + far*ndv[1] + vp[1] ;
609			ffar[2][2] = -widthnhv[2] - heightnvv[2] + far*ndv[2] + vp[2] ;
610			ffar[3][0] = widthnhv[0] - heightnvv[0] + far*ndv[0] + vp[0] ;
611			ffar[3][1] = widthnhv[1] - heightnvv[1] + far*ndv[1] + vp[1] ;
612			ffar[3][2] = widthnhv[2] - heightnvv[2] + far*ndv[2] + vp[2] ;
613			}
614
615	gwlarson	3.6	int
616			max_index(v,r)
617			FVECT v;
618			double *r;
619			{
620			double p[3];
621			int i;
622	gwlarson	3.1
623	gwlarson	3.6	p[0] = fabs(v[0]);
624			p[1] = fabs(v[1]);
625			p[2] = fabs(v[2]);
626			i = (p[0]>=p[1])?((p[0]>=p[2])?0:2):((p[1]>=p[2])?1:2);
627			if(r)
628			*r = p[i];
629			return(i);
630			}
631	gwlarson	3.1
632	gwlarson	3.6	int
633			closest_point_in_tri(p0,p1,p2,p,p0id,p1id,p2id)
634			FVECT p0,p1,p2,p;
635			int p0id,p1id,p2id;
636			{
637			double d,d1;
638			int i;
639
640			d = DIST_SQ(p,p0);
641			d1 = DIST_SQ(p,p1);
642			if(d < d1)
643			{
644			d1 = DIST_SQ(p,p2);
645			i = (d1 < d)?p2id:p0id;
646			}
647			else
648			{
649			d = DIST_SQ(p,p2);
650			i = (d < d1)? p2id:p1id;
651			}
652			return(i);
653			}
654	gwlarson	3.2
655			/* Find the normalized barycentric coordinates of p relative to
656			* triangle v0,v1,v2. Return result in coord
657			*/
658			bary2d(x1,y1,x2,y2,x3,y3,px,py,coord)
659			double x1,y1,x2,y2,x3,y3;
660			double px,py;
661			double coord[3];
662			{
663			double a;
664
665			a = (x2 - x1) * (y3 - y1) - (x3 - x1) * (y2 - y1);
666			coord[0] = ((x2 - px) * (y3 - py) - (x3 - px) * (y2 - py)) / a;
667			coord[1] = ((x3 - px) * (y1 - py) - (x1 - px) * (y3 - py)) / a;
668	gwlarson	3.6	coord[2] = ((x1 - px) * (y2 - py) - (x2 - px) * (y1 - py)) / a;
669	gwlarson	3.2
670			}
671
672	gwlarson	3.4
673
674	gwlarson	3.2
675	gwlarson	3.4	bary_parent(coord,i)
676	gwlarson	3.6	BCOORD coord[3];
677			int i;
678	gwlarson	3.2	{
679	gwlarson	3.6	switch(i) {
680			case 0:
681			/* update bary for child */
682	gwlarson	3.9	coord[0] = (coord[0] >> 1) + MAXBCOORD4;
683	gwlarson	3.6	coord[1] >>= 1;
684			coord[2] >>= 1;
685			break;
686			case 1:
687			coord[0] >>= 1;
688	gwlarson	3.9	coord[1] = (coord[1] >> 1) + MAXBCOORD4;
689	gwlarson	3.6	coord[2] >>= 1;
690			break;
691
692			case 2:
693			coord[0] >>= 1;
694			coord[1] >>= 1;
695	gwlarson	3.9	coord[2] = (coord[2] >> 1) + MAXBCOORD4;
696	gwlarson	3.6	break;
697
698			case 3:
699	gwlarson	3.9	coord[0] = MAXBCOORD4 - (coord[0] >> 1);
700			coord[1] = MAXBCOORD4 - (coord[1] >> 1);
701			coord[2] = MAXBCOORD4 - (coord[2] >> 1);
702	gwlarson	3.6	break;
703			#ifdef DEBUG
704			default:
705	gwlarson	3.9	eputs("bary_parent():Invalid child\n");
706	gwlarson	3.6	break;
707			#endif
708			}
709	gwlarson	3.2	}
710
711	gwlarson	3.9	bary_from_child(coord,child,next)
712	gwlarson	3.6	BCOORD coord[3];
713			int child,next;
714			{
715			#ifdef DEBUG
716			if(child <0 \|\| child > 3)
717			{
718	gwlarson	3.9	eputs("bary_from_child():Invalid child\n");
719	gwlarson	3.6	return;
720			}
721			if(next <0 \|\| next > 3)
722			{
723	gwlarson	3.9	eputs("bary_from_child():Invalid next\n");
724	gwlarson	3.6	return;
725			}
726			#endif
727			if(next == child)
728			return;
729
730			switch(child){
731			case 0:
732			coord[0] = 0;
733	gwlarson	3.9	coord[1] = MAXBCOORD2 - coord[1];
734			coord[2] = MAXBCOORD2 - coord[2];
735	gwlarson	3.6	break;
736			case 1:
737	gwlarson	3.9	coord[0] = MAXBCOORD2 - coord[0];
738	gwlarson	3.6	coord[1] = 0;
739	gwlarson	3.9	coord[2] = MAXBCOORD2 - coord[2];
740	gwlarson	3.6	break;
741			case 2:
742	gwlarson	3.9	coord[0] = MAXBCOORD2 - coord[0];
743			coord[1] = MAXBCOORD2 - coord[1];
744	gwlarson	3.6	coord[2] = 0;
745			break;
746			case 3:
747			switch(next){
748			case 0:
749			coord[0] = 0;
750	gwlarson	3.9	coord[1] = MAXBCOORD2 - coord[1];
751			coord[2] = MAXBCOORD2 - coord[2];
752	gwlarson	3.6	break;
753			case 1:
754	gwlarson	3.9	coord[0] = MAXBCOORD2 - coord[0];
755	gwlarson	3.6	coord[1] = 0;
756	gwlarson	3.9	coord[2] = MAXBCOORD2 - coord[2];
757	gwlarson	3.6	break;
758			case 2:
759	gwlarson	3.9	coord[0] = MAXBCOORD2 - coord[0];
760			coord[1] = MAXBCOORD2 - coord[1];
761	gwlarson	3.6	coord[2] = 0;
762			break;
763			}
764			break;
765			}
766			}
767
768	gwlarson	3.4	int
769	gwlarson	3.9	bary_child(coord)
770	gwlarson	3.6	BCOORD coord[3];
771	gwlarson	3.4	{
772	gwlarson	3.6	int i;
773
774	gwlarson	3.9	if(coord[0] > MAXBCOORD4)
775	gwlarson	3.6	{
776			/* update bary for child */
777	gwlarson	3.9	coord[0] = (coord[0]<< 1) - MAXBCOORD2;
778	gwlarson	3.6	coord[1] <<= 1;
779			coord[2] <<= 1;
780			return(0);
781			}
782			else
783	gwlarson	3.9	if(coord[1] > MAXBCOORD4)
784	gwlarson	3.4	{
785	gwlarson	3.6	coord[0] <<= 1;
786	gwlarson	3.9	coord[1] = (coord[1] << 1) - MAXBCOORD2;
787	gwlarson	3.6	coord[2] <<= 1;
788			return(1);
789	gwlarson	3.4	}
790			else
791	gwlarson	3.9	if(coord[2] > MAXBCOORD4)
792	gwlarson	3.6	{
793			coord[0] <<= 1;
794			coord[1] <<= 1;
795	gwlarson	3.9	coord[2] = (coord[2] << 1) - MAXBCOORD2;
796	gwlarson	3.6	return(2);
797			}
798			else
799			{
800	gwlarson	3.9	coord[0] = MAXBCOORD2 - (coord[0] << 1);
801			coord[1] = MAXBCOORD2 - (coord[1] << 1);
802			coord[2] = MAXBCOORD2 - (coord[2] << 1);
803	gwlarson	3.6	return(3);
804			}
805			}
806
807	gwlarson	3.7	int
808	gwlarson	3.9	bary_nth_child(coord,i)
809	gwlarson	3.7	BCOORD coord[3];
810			int i;
811			{
812
813			switch(i){
814			case 0:
815			/* update bary for child */
816	gwlarson	3.9	coord[0] = (coord[0]<< 1) - MAXBCOORD2;
817	gwlarson	3.7	coord[1] <<= 1;
818			coord[2] <<= 1;
819			break;
820			case 1:
821			coord[0] <<= 1;
822	gwlarson	3.9	coord[1] = (coord[1] << 1) - MAXBCOORD2;
823	gwlarson	3.7	coord[2] <<= 1;
824			break;
825			case 2:
826			coord[0] <<= 1;
827			coord[1] <<= 1;
828	gwlarson	3.9	coord[2] = (coord[2] << 1) - MAXBCOORD2;
829	gwlarson	3.7	break;
830			case 3:
831	gwlarson	3.9	coord[0] = MAXBCOORD2 - (coord[0] << 1);
832			coord[1] = MAXBCOORD2 - (coord[1] << 1);
833			coord[2] = MAXBCOORD2 - (coord[2] << 1);
834	gwlarson	3.7	break;
835			}
836			}
837	gwlarson	3.2
838
839	gwlarson	3.1