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]));
}


int
convex_angle(v0,v1,v2)
FVECT v0,v1,v2;
{
    FVECT cp01,cp12,cp;
    
    /* test sign of (v0Xv1)X(v1Xv2). v1 */
    VCROSS(cp01,v0,v1);
    VCROSS(cp12,v1,v2);
    VCROSS(cp,cp01,cp12);
    if(DOT(cp,v1) < 0)
       return(FALSE);
    return(TRUE);
}

/* 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;
char 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,n,nd,norm)
   FVECT v0,v1,v2,n;
   double *nd;
   char norm;
{
    tri_normal(v0,v1,v2,n,norm);

    *nd = -(DOT(n,v0));
}

int
point_relative_to_plane(p,n,nd)
   FVECT p,n;
   double nd;
{
    double d;
    
    d = p[0]*n[0] + p[1]*n[1] + p[2]*n[2] + nd;
    if(d < 0)
       return(-1);
    if(ZERO(d))
       return(0);
    else
       return(1);
}

/* From quad_edge-code */
int
point_in_circle_thru_origin(p,p0,p1)
FVECT p;
FVECT p0,p1;
{

    double dp0,dp1;
    double dp,det;
    
    dp0 = DOT_VEC2(p0,p0);
    dp1 = DOT_VEC2(p1,p1);
    dp  = DOT_VEC2(p,p);    

    det = -dp0*CROSS_VEC2(p1,p) + dp1*CROSS_VEC2(p0,p) - dp*CROSS_VEC2(p0,p1);
    
    return (det > 0);
}


point_on_sphere(ps,p,c)
FVECT ps,p,c;
{
    VSUB(ps,p,c);    
    normalize(ps);
}


int
intersect_vector_plane(v,plane_n,plane_d,pd,r)
   FVECT v,plane_n;
   double plane_d;
   double *pd;
   FVECT r;
{
  double t;
  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: */
    t =  plane_d/-(DOT(plane_n,v));
    if(t >0 || ZERO(t))
       hit = 1;
    else
       hit = 0;
    r[0] = v[0]*t;
    r[1] = v[1]*t;
    r[2] = v[2]*t;
    if(pd)
       *pd = t;
  return(hit);
}

int
intersect_ray_plane(orig,dir,plane_n,plane_d,pd,r)
   FVECT orig,dir;
   FVECT plane_n;
   double plane_d;
   double *pd;
   FVECT r;
{
  double t;
  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: */
    t =  -(DOT(plane_n,orig) + plane_d)/(DOT(plane_n,dir));
    if(ZERO(t) || t >0)
       hit = 1;
    else
       hit = 0;

  VSUM(r,orig,dir,t);

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


int
point_in_cone(p,p0,p1,p2)
FVECT p;
FVECT p0,p1,p2;
{
    FVECT n;
    FVECT np,x_axis,y_axis;
    double d1,d2,d;
    
    /* Find the equation of the circle defined by the intersection
       of the cone with the plane defined by p1,p2,p3- project p into
       that plane and do an in-circle test in the plane
     */
    
    /* find the equation of the plane defined by p1-p3 */
    tri_plane_equation(p0,p1,p2,n,&d,FALSE);

    /* define a coordinate system on the plane: the x axis is in
       the direction of np2-np1, and the y axis is calculated from
       n cross x-axis
     */
    /* Project p onto the plane */
    if(!intersect_vector_plane(p,n,d,NULL,np))
        return(FALSE);

    /* create coordinate system on  plane: p2-p1 defines the x_axis*/
    VSUB(x_axis,p1,p0);
    normalize(x_axis);
    /* The y axis is  */
    VCROSS(y_axis,n,x_axis);
    normalize(y_axis);

    VSUB(p1,p1,p0);
    VSUB(p2,p2,p0);
    VSUB(np,np,p0);
    
    p1[0] = VLEN(p1);
    p1[1] = 0;

    d1 = DOT(p2,x_axis);
    d2 = DOT(p2,y_axis);
    p2[0] = d1;
    p2[1] = d2;
    
    d1 = DOT(np,x_axis);
    d2 = DOT(np,y_axis);
    np[0] = d1;
    np[1] = d2;

    /* perform the in-circle test in the new coordinate system */
    return(point_in_circle_thru_origin(np,p1,p2));
}

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

{
    float d;

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

    if(ZERO(d))
       sides[0] = GT_EDGE;
    else
       if(d > 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],v2,v1);
        SET_NTH_BIT(*nset,1);
    }
    /* Test the point for sidedness */
    d  = DOT(n[1],p);
    if(ZERO(d))
    {
        sides[1] = GT_EDGE;
        /* If on plane 0-and on plane 1: lies on edge */
        if(sides[0] == GT_EDGE)
        {
            *which = 1;
            sides[2] = GT_INVALID;
            return(GT_EDGE);
        }
    }
    else if(d > 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],v0,v2);
        SET_NTH_BIT(*nset,2);
    }
    /* Test the point for sidedness */
    d  = DOT(n[2],p);
    if(ZERO(d))
    {
        sides[2] = GT_EDGE;

        /* If on plane 0 and 2: lies on edge 0*/
        if(sides[0] == GT_EDGE)
           {
               *which = 0;
               return(GT_EDGE);
           }
        /* If on plane 1 and 2: lies on edge  2*/
        if(sides[1] == GT_EDGE)
           {
               *which = 2;
               return(GT_EDGE);
           }
        /* otherwise: on face 2 */
        else
           {
               *which = 2;
               return(GT_FACE);
           }
    }
    else if(d > 0)
      {
        sides[2] = GT_OUT;
        return(FALSE);
      }
    /* If on edge */
    else 
       sides[2] = GT_INTERIOR;
    
    /* If on plane 0 only: on face 0 */
    if(sides[0] == GT_EDGE)
    {
        *which = 0;
        return(GT_FACE);
    }
    /* If on plane 1 only: on face 1 */
    if(sides[1] == GT_EDGE)
    {
        *which = 1;
        return(GT_FACE);
    }
    /* Must be interior to the pyramid */
    return(GT_INTERIOR);
}


int
test_single_point_against_spherical_tri(v0,v1,v2,p,which)
FVECT v0,v1,v2,p;
char *which;
{
    float d;
    FVECT n;  
    char sides[3];

    /* First test if point coincides with any of the vertices */
    if(EQUAL_VEC3(p,v0))
    {
        *which = 0;
        return(GT_VERTEX);
    }
    if(EQUAL_VEC3(p,v1))
    {
        *which = 1;
        return(GT_VERTEX);
    }
    if(EQUAL_VEC3(p,v2))
    {
        *which = 2;
        return(GT_VERTEX);
    }
    VCROSS(n,v1,v0);
    /* Test the point for sidedness */
    d  = DOT(n,p);
    if(ZERO(d))
       sides[0] = GT_EDGE;
    else
       if(d > 0)
          return(FALSE);
       else 
          sides[0] = GT_INTERIOR;
    /* Test next edge */
    VCROSS(n,v2,v1);
    /* Test the point for sidedness */
    d  = DOT(n,p);
    if(ZERO(d))
    {
        sides[1] = GT_EDGE;
        /* If on plane 0-and on plane 1: lies on edge */
        if(sides[0] == GT_EDGE)
        {
            *which = 1;
            return(GT_VERTEX);
        }
    }
    else if(d > 0)
       return(FALSE);
    else 
       sides[1] = GT_INTERIOR;

    /* Test next edge */
    VCROSS(n,v0,v2);
    /* Test the point for sidedness */
    d  = DOT(n,p);
    if(ZERO(d))
    {
        sides[2] = GT_EDGE;
        
        /* If on plane 0 and 2: lies on edge 0*/
        if(sides[0] == GT_EDGE)
        {
            *which = 0;
            return(GT_VERTEX);
        }
        /* If on plane 1 and 2: lies on edge  2*/
        if(sides[1] == GT_EDGE)
        {
            *which = 2;
            return(GT_VERTEX);
        }
        /* otherwise: on face 2 */
        else
       {
           return(GT_FACE);
       }
    }
    else if(d > 0)
       return(FALSE);
    /* Must be interior to the pyramid */
    return(GT_FACE);
}

int
test_vertices_for_tri_inclusion(t0,t1,t2,p0,p1,p2,nset,n,avg,pt_sides)
FVECT t0,t1,t2,p0,p1,p2;
char *nset;
FVECT n[3];
FVECT avg;
char pt_sides[3][3];

{
    char below_plane[3],on_edge,test;
    char which;

    SUM_3VEC3(avg,t0,t1,t2); 
    on_edge = 0;
    *nset = 0;
    /* Test vertex v[i] against triangle j*/
    /* Check if v[i] lies below plane defined by avg of 3 vectors
       defining triangle
       */

    /* test point 0 */
    if(DOT(avg,p0) < 0)
      below_plane[0] = 1;
    else
      below_plane[0]=0;
    /* Test if b[i] lies in or on triangle a */
    test = test_point_against_spherical_tri(t0,t1,t2,p0,
                                                 n,nset,&which,pt_sides[0]);
    /* If pts[i] is interior: done */
    if(!below_plane[0])
      {
        if(test == GT_INTERIOR) 
          return(TRUE);
        /* Remember if b[i] fell on one of the 3 defining planes */
        if(test)
          on_edge++;
      }
    /* Now test point 1*/

    if(DOT(avg,p1) < 0)
      below_plane[1] = 1;
    else
      below_plane[1]=0;
    /* Test if b[i] lies in or on triangle a */
    test = test_point_against_spherical_tri(t0,t1,t2,p1,
                                                 n,nset,&which,pt_sides[1]);
    /* If pts[i] is interior: done */
    if(!below_plane[1])
    {
      if(test == GT_INTERIOR) 
        return(TRUE);
      /* Remember if b[i] fell on one of the 3 defining planes */
      if(test)
        on_edge++;
    }
    
    /* Now test point 2 */
    if(DOT(avg,p2) < 0)
      below_plane[2] = 1;
    else
      below_plane[2]=0;
        /* Test if b[i] lies in or on triangle a */
    test = test_point_against_spherical_tri(t0,t1,t2,p2,
                                                 n,nset,&which,pt_sides[2]);

    /* If pts[i] is interior: done */
    if(!below_plane[2])
      {
        if(test == GT_INTERIOR) 
          return(TRUE);
        /* Remember if b[i] fell on one of the 3 defining planes */
        if(test)
          on_edge++;
      }

    /* If all three points below separating plane: trivial reject */
    if(below_plane[0] && below_plane[1] && below_plane[2])
       return(FALSE);
    /* Accept if all points lie on a triangle vertex/edge edge- accept*/
    if(on_edge == 3)
       return(TRUE);
    /* Now check vertices in a against triangle b */
    return(FALSE);
}


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

    
    /* p=0 */
    test[0] = 0;
    if(sides[0][0] == GT_INVALID)
    {
      if(!NTH_BIT(nset,0))
        VCROSS(n[0],t1,t0);
      /* Test the point for sidedness */
      d  = DOT(n[0],p0);
      if(d >= 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],t2,t1);
        /* Test the point for sidedness */
        d  = DOT(n[1],p0);
        if(d >= 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],t0,t2);
      /* Test the point for sidedness */
      d  = DOT(n[2],p0);
      if(d >= 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],t1,t0);
      /* Test the point for sidedness */
      d  = DOT(n[0],p1);
      if(d >= 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],t2,t1);
      /* Test the point for sidedness */
      d  = DOT(n[1],p1);
      if(d >= 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],t0,t2);
      /* Test the point for sidedness */
      d  = DOT(n[2],p1);
      if(d >= 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],t1,t0);
      /* Test the point for sidedness */
      d  = DOT(n[0],p2);
      if(d >= 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],t2,t1);
      /* Test the point for sidedness */
      d  = DOT(n[1],p2);
      if(d >= 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],t0,t2);
      /* Test the point for sidedness */
      d  = DOT(n[2],p2);
      if(d >= 0)
        SET_NTH_BIT(test[2],2);
    }
    else
      if(sides[2][2] != GT_INTERIOR)
        SET_NTH_BIT(test[2],2);
}


int
spherical_tri_intersect(a1,a2,a3,b1,b2,b3)
FVECT a1,a2,a3,b1,b2,b3;
{
  char which,test,n_set[2];
  char sides[2][3][3],i,j,inext,jnext;
  char tests[2][3];
  FVECT n[2][3],p,avg[2];
 
  /* Test the vertices of triangle a against the pyramid formed by triangle
     b and the origin. If any vertex of a is interior to triangle b, or
     if all 3 vertices of a are ON the edges of b,return TRUE. Remember
     the results of the edge normal and sidedness tests for later.
   */
 if(test_vertices_for_tri_inclusion(a1,a2,a3,b1,b2,b3,
                                    &(n_set[0]),n[0],avg[0],sides[1]))
     return(TRUE);
  
 if(test_vertices_for_tri_inclusion(b1,b2,b3,a1,a2,a3,
                                    &(n_set[1]),n[1],avg[1],sides[0]))
     return(TRUE);


  set_sidedness_tests(b1,b2,b3,a1,a2,a3,tests[0],sides[0],n_set[1],n[1]);
  if(tests[0][0]&tests[0][1]&tests[0][2])
    return(FALSE);

  set_sidedness_tests(a1,a2,a3,b1,b2,b3,tests[1],sides[1],n_set[0],n[0]);
  if(tests[1][0]&tests[1][1]&tests[1][2])
    return(FALSE);

  for(j=0; j < 3;j++)
  {
    jnext = (j+1)%3;
    /* IF edge b doesnt cross any great circles of a, punt */
    if(tests[1][j] & tests[1][jnext])
      continue;
    for(i=0;i<3;i++)
    {
      inext = (i+1)%3;
      /* IF edge a doesnt cross any great circles of b, punt */
      if(tests[0][i] & tests[0][inext])
        continue;
      /* Now find the great circles that cross and test */
      if((NTH_BIT(tests[0][i],j)^(NTH_BIT(tests[0][inext],j))) 
          && (NTH_BIT(tests[1][j],i)^NTH_BIT(tests[1][jnext],i)))
      {
        VCROSS(p,n[0][i],n[1][j]);
                     
        /* If zero cp= done */
        if(ZERO_VEC3(p))
          continue;
        /* check above both planes */
        if(DOT(avg[0],p) < 0 || DOT(avg[1],p) < 0)
          {
            NEGATE_VEC3(p);
            if(DOT(avg[0],p) < 0 || DOT(avg[1],p) < 0)
              continue;
          }
        return(TRUE);
      }
    }
  } 
  return(FALSE);
}

int
ray_intersect_tri(orig,dir,v0,v1,v2,pt,wptr)
FVECT orig,dir;
FVECT v0,v1,v2;
FVECT pt;
char *wptr;
{
  FVECT p0,p1,p2,p,n;
  char type,which;
  double pd;
  
  point_on_sphere(p0,v0,orig);
  point_on_sphere(p1,v1,orig);
  point_on_sphere(p2,v2,orig);
  type = test_single_point_against_spherical_tri(p0,p1,p2,dir,&which);

  if(type)
  {
      /* Intersect the ray with the triangle plane */
      tri_plane_equation(v0,v1,v2,n,&pd,FALSE);
      intersect_ray_plane(orig,dir,n,pd,NULL,pt);        
  }
  if(wptr)
    *wptr = which;

  return(type);
}


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
spherical_polygon_edge_intersect(a0,a1,b0,b1)
FVECT a0,a1,b0,b1;
{
    FVECT na,nb,avga,avgb,p;
    double d;
    int sb0,sb1,sa0,sa1;

    /* First test if edge b straddles great circle of a */
    VCROSS(na,a0,a1);
    d = DOT(na,b0);
    sb0 = ZERO(d)?0:(d<0)? -1: 1;
    d = DOT(na,b1);
    sb1 = ZERO(d)?0:(d<0)? -1: 1;
    /* edge b entirely on one side of great circle a: edges cannot intersect*/
    if(sb0*sb1 > 0)
       return(FALSE);
    /* test if edge a straddles great circle of b */
    VCROSS(nb,b0,b1);
    d = DOT(nb,a0);
    sa0 = ZERO(d)?0:(d<0)? -1: 1;
    d = DOT(nb,a1);
    sa1 = ZERO(d)?0:(d<0)? -1: 1;
    /* edge a entirely on one side of great circle b: edges cannot intersect*/
    if(sa0*sa1 > 0)
       return(FALSE);

    /* Find one of intersection points of the great circles */
    VCROSS(p,na,nb);
    /* If they lie on same great circle: call an intersection */
    if(ZERO_VEC3(p))
       return(TRUE);

    VADD(avga,a0,a1);
    VADD(avgb,b0,b1);
    if(DOT(avga,p) < 0 || DOT(avgb,p) < 0)
    {
      NEGATE_VEC3(p);
      if(DOT(avga,p) < 0 || DOT(avgb,p) < 0)
        return(FALSE);
    }
    if((!sb0 || !sb1) && (!sa0 || !sa1))
      return(FALSE);
    return(TRUE);
}

Revision:	3.1
Committed:	Wed Aug 19 17:45:24 1998 UTC (25 years, 8 months ago) by gwlarson
Content type:	text/plain
Branch:	MAIN
Log Message:	Initial revision
#	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
31
32			int
33			convex_angle(v0,v1,v2)
34			FVECT v0,v1,v2;
35			{
36			FVECT cp01,cp12,cp;
37
38			/* test sign of (v0Xv1)X(v1Xv2). v1 */
39			VCROSS(cp01,v0,v1);
40			VCROSS(cp12,v1,v2);
41			VCROSS(cp,cp01,cp12);
42			if(DOT(cp,v1) < 0)
43			return(FALSE);
44			return(TRUE);
45			}
46
47			/* calculates the normal of a face contour using Newell's formula. e
48
49			a = SUMi (yi - yi+1)(zi + zi+1)
50			b = SUMi (zi - zi+1)(xi + xi+1)
51			c = SUMi (xi - xi+1)(yi + yi+1)
52			*/
53			double
54			tri_normal(v0,v1,v2,n,norm)
55			FVECT v0,v1,v2,n;
56			char norm;
57			{
58			double mag;
59
60			n[0] = (v0[2] + v1[2]) * (v0[1] - v1[1]) +
61			(v1[2] + v2[2]) * (v1[1] - v2[1]) +
62			(v2[2] + v0[2]) * (v2[1] - v0[1]);
63
64			n[1] = (v0[2] - v1[2]) * (v0[0] + v1[0]) +
65			(v1[2] - v2[2]) * (v1[0] + v2[0]) +
66			(v2[2] - v0[2]) * (v2[0] + v0[0]);
67
68
69			n[2] = (v0[1] + v1[1]) * (v0[0] - v1[0]) +
70			(v1[1] + v2[1]) * (v1[0] - v2[0]) +
71			(v2[1] + v0[1]) * (v2[0] - v0[0]);
72
73			if(!norm)
74			return(0);
75
76
77			mag = normalize(n);
78
79			return(mag);
80			}
81
82
83			tri_plane_equation(v0,v1,v2,n,nd,norm)
84			FVECT v0,v1,v2,n;
85			double *nd;
86			char norm;
87			{
88			tri_normal(v0,v1,v2,n,norm);
89
90			*nd = -(DOT(n,v0));
91			}
92
93			int
94			point_relative_to_plane(p,n,nd)
95			FVECT p,n;
96			double nd;
97			{
98			double d;
99
100			d = p[0]n[0] + p[1]n[1] + p[2]*n[2] + nd;
101			if(d < 0)
102			return(-1);
103			if(ZERO(d))
104			return(0);
105			else
106			return(1);
107			}
108
109			/* From quad_edge-code */
110			int
111			point_in_circle_thru_origin(p,p0,p1)
112			FVECT p;
113			FVECT p0,p1;
114			{
115
116			double dp0,dp1;
117			double dp,det;
118
119			dp0 = DOT_VEC2(p0,p0);
120			dp1 = DOT_VEC2(p1,p1);
121			dp = DOT_VEC2(p,p);
122
123			det = -dp0CROSS_VEC2(p1,p) + dp1CROSS_VEC2(p0,p) - dp*CROSS_VEC2(p0,p1);
124
125			return (det > 0);
126			}
127
128
129
130			point_on_sphere(ps,p,c)
131			FVECT ps,p,c;
132			{
133			VSUB(ps,p,c);
134			normalize(ps);
135			}
136
137
138			int
139			intersect_vector_plane(v,plane_n,plane_d,pd,r)
140			FVECT v,plane_n;
141			double plane_d;
142			double *pd;
143			FVECT r;
144			{
145			double t;
146			int hit;
147			/*
148			Plane is Ax + By + Cz +D = 0:
149			plane[0] = A,plane[1] = B,plane[2] = C,plane[3] = D
150			*/
151
152			/* line is l = p1 + (p2-p1)t, p1=origin */
153
154			/* Solve for t: */
155			t = plane_d/-(DOT(plane_n,v));
156			if(t >0 \|\| ZERO(t))
157			hit = 1;
158			else
159			hit = 0;
160			r[0] = v[0]*t;
161			r[1] = v[1]*t;
162			r[2] = v[2]*t;
163			if(pd)
164			*pd = t;
165			return(hit);
166			}
167
168			int
169			intersect_ray_plane(orig,dir,plane_n,plane_d,pd,r)
170			FVECT orig,dir;
171			FVECT plane_n;
172			double plane_d;
173			double *pd;
174			FVECT r;
175			{
176			double t;
177			int hit;
178			/*
179			Plane is Ax + By + Cz +D = 0:
180			plane[0] = A,plane[1] = B,plane[2] = C,plane[3] = D
181			*/
182			/* A(orig[0] + dxt) + B(orig[1] + dyt) + C(orig[2] + dzt) + pd = 0 */
183			/* t = -(DOT(plane_n,orig)+ plane_d)/(DOT(plane_n,d))
184			/* line is l = p1 + (p2-p1)t */
185			/* Solve for t: */
186			t = -(DOT(plane_n,orig) + plane_d)/(DOT(plane_n,dir));
187			if(ZERO(t) \|\| t >0)
188			hit = 1;
189			else
190			hit = 0;
191
192			VSUM(r,orig,dir,t);
193
194			if(pd)
195			*pd = t;
196			return(hit);
197			}
198
199
200			int
201			point_in_cone(p,p0,p1,p2)
202			FVECT p;
203			FVECT p0,p1,p2;
204			{
205			FVECT n;
206			FVECT np,x_axis,y_axis;
207			double d1,d2,d;
208
209			/* Find the equation of the circle defined by the intersection
210			of the cone with the plane defined by p1,p2,p3- project p into
211			that plane and do an in-circle test in the plane
212			*/
213
214			/* find the equation of the plane defined by p1-p3 */
215			tri_plane_equation(p0,p1,p2,n,&d,FALSE);
216
217			/* define a coordinate system on the plane: the x axis is in
218			the direction of np2-np1, and the y axis is calculated from
219			n cross x-axis
220			*/
221			/* Project p onto the plane */
222			if(!intersect_vector_plane(p,n,d,NULL,np))
223			return(FALSE);
224
225			/* create coordinate system on plane: p2-p1 defines the x_axis*/
226			VSUB(x_axis,p1,p0);
227			normalize(x_axis);
228			/* The y axis is */
229			VCROSS(y_axis,n,x_axis);
230			normalize(y_axis);
231
232			VSUB(p1,p1,p0);
233			VSUB(p2,p2,p0);
234			VSUB(np,np,p0);
235
236			p1[0] = VLEN(p1);
237			p1[1] = 0;
238
239			d1 = DOT(p2,x_axis);
240			d2 = DOT(p2,y_axis);
241			p2[0] = d1;
242			p2[1] = d2;
243
244			d1 = DOT(np,x_axis);
245			d2 = DOT(np,y_axis);
246			np[0] = d1;
247			np[1] = d2;
248
249			/* perform the in-circle test in the new coordinate system */
250			return(point_in_circle_thru_origin(np,p1,p2));
251			}
252
253			int
254			test_point_against_spherical_tri(v0,v1,v2,p,n,nset,which,sides)
255			FVECT v0,v1,v2,p;
256			FVECT n[3];
257			char *nset;
258			char *which;
259			char sides[3];
260
261			{
262			float d;
263
264			/* Find the normal to the triangle ORIGIN:v0:v1 */
265			if(!NTH_BIT(*nset,0))
266			{
267			VCROSS(n[0],v1,v0);
268			SET_NTH_BIT(*nset,0);
269			}
270			/* Test the point for sidedness */
271			d = DOT(n[0],p);
272
273			if(ZERO(d))
274			sides[0] = GT_EDGE;
275			else
276			if(d > 0)
277			{
278			sides[0] = GT_OUT;
279			sides[1] = sides[2] = GT_INVALID;
280			return(FALSE);
281			}
282			else
283			sides[0] = GT_INTERIOR;
284
285			/* Test next edge */
286			if(!NTH_BIT(*nset,1))
287			{
288			VCROSS(n[1],v2,v1);
289			SET_NTH_BIT(*nset,1);
290			}
291			/* Test the point for sidedness */
292			d = DOT(n[1],p);
293			if(ZERO(d))
294			{
295			sides[1] = GT_EDGE;
296			/* If on plane 0-and on plane 1: lies on edge */
297			if(sides[0] == GT_EDGE)
298			{
299			*which = 1;
300			sides[2] = GT_INVALID;
301			return(GT_EDGE);
302			}
303			}
304			else if(d > 0)
305			{
306			sides[1] = GT_OUT;
307			sides[2] = GT_INVALID;
308			return(FALSE);
309			}
310			else
311			sides[1] = GT_INTERIOR;
312			/* Test next edge */
313			if(!NTH_BIT(*nset,2))
314			{
315
316			VCROSS(n[2],v0,v2);
317			SET_NTH_BIT(*nset,2);
318			}
319			/* Test the point for sidedness */
320			d = DOT(n[2],p);
321			if(ZERO(d))
322			{
323			sides[2] = GT_EDGE;
324
325			/* If on plane 0 and 2: lies on edge 0*/
326			if(sides[0] == GT_EDGE)
327			{
328			*which = 0;
329			return(GT_EDGE);
330			}
331			/* If on plane 1 and 2: lies on edge 2*/
332			if(sides[1] == GT_EDGE)
333			{
334			*which = 2;
335			return(GT_EDGE);
336			}
337			/* otherwise: on face 2 */
338			else
339			{
340			*which = 2;
341			return(GT_FACE);
342			}
343			}
344			else if(d > 0)
345			{
346			sides[2] = GT_OUT;
347			return(FALSE);
348			}
349			/* If on edge */
350			else
351			sides[2] = GT_INTERIOR;
352
353			/* If on plane 0 only: on face 0 */
354			if(sides[0] == GT_EDGE)
355			{
356			*which = 0;
357			return(GT_FACE);
358			}
359			/* If on plane 1 only: on face 1 */
360			if(sides[1] == GT_EDGE)
361			{
362			*which = 1;
363			return(GT_FACE);
364			}
365			/* Must be interior to the pyramid */
366			return(GT_INTERIOR);
367			}
368
369
370
371
372			int
373			test_single_point_against_spherical_tri(v0,v1,v2,p,which)
374			FVECT v0,v1,v2,p;
375			char *which;
376			{
377			float d;
378			FVECT n;
379			char sides[3];
380
381			/* First test if point coincides with any of the vertices */
382			if(EQUAL_VEC3(p,v0))
383			{
384			*which = 0;
385			return(GT_VERTEX);
386			}
387			if(EQUAL_VEC3(p,v1))
388			{
389			*which = 1;
390			return(GT_VERTEX);
391			}
392			if(EQUAL_VEC3(p,v2))
393			{
394			*which = 2;
395			return(GT_VERTEX);
396			}
397			VCROSS(n,v1,v0);
398			/* Test the point for sidedness */
399			d = DOT(n,p);
400			if(ZERO(d))
401			sides[0] = GT_EDGE;
402			else
403			if(d > 0)
404			return(FALSE);
405			else
406			sides[0] = GT_INTERIOR;
407			/* Test next edge */
408			VCROSS(n,v2,v1);
409			/* Test the point for sidedness */
410			d = DOT(n,p);
411			if(ZERO(d))
412			{
413			sides[1] = GT_EDGE;
414			/* If on plane 0-and on plane 1: lies on edge */
415			if(sides[0] == GT_EDGE)
416			{
417			*which = 1;
418			return(GT_VERTEX);
419			}
420			}
421			else if(d > 0)
422			return(FALSE);
423			else
424			sides[1] = GT_INTERIOR;
425
426			/* Test next edge */
427			VCROSS(n,v0,v2);
428			/* Test the point for sidedness */
429			d = DOT(n,p);
430			if(ZERO(d))
431			{
432			sides[2] = GT_EDGE;
433
434			/* If on plane 0 and 2: lies on edge 0*/
435			if(sides[0] == GT_EDGE)
436			{
437			*which = 0;
438			return(GT_VERTEX);
439			}
440			/* If on plane 1 and 2: lies on edge 2*/
441			if(sides[1] == GT_EDGE)
442			{
443			*which = 2;
444			return(GT_VERTEX);
445			}
446			/* otherwise: on face 2 */
447			else
448			{
449			return(GT_FACE);
450			}
451			}
452			else if(d > 0)
453			return(FALSE);
454			/* Must be interior to the pyramid */
455			return(GT_FACE);
456			}
457
458			int
459			test_vertices_for_tri_inclusion(t0,t1,t2,p0,p1,p2,nset,n,avg,pt_sides)
460			FVECT t0,t1,t2,p0,p1,p2;
461			char *nset;
462			FVECT n[3];
463			FVECT avg;
464			char pt_sides[3][3];
465
466			{
467			char below_plane[3],on_edge,test;
468			char which;
469
470			SUM_3VEC3(avg,t0,t1,t2);
471			on_edge = 0;
472			*nset = 0;
473			/* Test vertex v[i] against triangle j*/
474			/* Check if v[i] lies below plane defined by avg of 3 vectors
475			defining triangle
476			*/
477
478			/* test point 0 */
479			if(DOT(avg,p0) < 0)
480			below_plane[0] = 1;
481			else
482			below_plane[0]=0;
483			/* Test if b[i] lies in or on triangle a */
484			test = test_point_against_spherical_tri(t0,t1,t2,p0,
485			n,nset,&which,pt_sides[0]);
486			/* If pts[i] is interior: done */
487			if(!below_plane[0])
488			{
489			if(test == GT_INTERIOR)
490			return(TRUE);
491			/* Remember if b[i] fell on one of the 3 defining planes */
492			if(test)
493			on_edge++;
494			}
495			/* Now test point 1*/
496
497			if(DOT(avg,p1) < 0)
498			below_plane[1] = 1;
499			else
500			below_plane[1]=0;
501			/* Test if b[i] lies in or on triangle a */
502			test = test_point_against_spherical_tri(t0,t1,t2,p1,
503			n,nset,&which,pt_sides[1]);
504			/* If pts[i] is interior: done */
505			if(!below_plane[1])
506			{
507			if(test == GT_INTERIOR)
508			return(TRUE);
509			/* Remember if b[i] fell on one of the 3 defining planes */
510			if(test)
511			on_edge++;
512			}
513
514			/* Now test point 2 */
515			if(DOT(avg,p2) < 0)
516			below_plane[2] = 1;
517			else
518			below_plane[2]=0;
519			/* Test if b[i] lies in or on triangle a */
520			test = test_point_against_spherical_tri(t0,t1,t2,p2,
521			n,nset,&which,pt_sides[2]);
522
523			/* If pts[i] is interior: done */
524			if(!below_plane[2])
525			{
526			if(test == GT_INTERIOR)
527			return(TRUE);
528			/* Remember if b[i] fell on one of the 3 defining planes */
529			if(test)
530			on_edge++;
531			}
532
533			/* If all three points below separating plane: trivial reject */
534			if(below_plane[0] && below_plane[1] && below_plane[2])
535			return(FALSE);
536			/* Accept if all points lie on a triangle vertex/edge edge- accept*/
537			if(on_edge == 3)
538			return(TRUE);
539			/* Now check vertices in a against triangle b */
540			return(FALSE);
541			}
542
543
544			set_sidedness_tests(t0,t1,t2,p0,p1,p2,test,sides,nset,n)
545			FVECT t0,t1,t2,p0,p1,p2;
546			char test[3];
547			char sides[3][3];
548			char nset;
549			FVECT n[3];
550			{
551			char t;
552			double d;
553
554
555			/* p=0 */
556			test[0] = 0;
557			if(sides[0][0] == GT_INVALID)
558			{
559			if(!NTH_BIT(nset,0))
560			VCROSS(n[0],t1,t0);
561			/* Test the point for sidedness */
562			d = DOT(n[0],p0);
563			if(d >= 0)
564			SET_NTH_BIT(test[0],0);
565			}
566			else
567			if(sides[0][0] != GT_INTERIOR)
568			SET_NTH_BIT(test[0],0);
569
570			if(sides[0][1] == GT_INVALID)
571			{
572			if(!NTH_BIT(nset,1))
573			VCROSS(n[1],t2,t1);
574			/* Test the point for sidedness */
575			d = DOT(n[1],p0);
576			if(d >= 0)
577			SET_NTH_BIT(test[0],1);
578			}
579			else
580			if(sides[0][1] != GT_INTERIOR)
581			SET_NTH_BIT(test[0],1);
582
583			if(sides[0][2] == GT_INVALID)
584			{
585			if(!NTH_BIT(nset,2))
586			VCROSS(n[2],t0,t2);
587			/* Test the point for sidedness */
588			d = DOT(n[2],p0);
589			if(d >= 0)
590			SET_NTH_BIT(test[0],2);
591			}
592			else
593			if(sides[0][2] != GT_INTERIOR)
594			SET_NTH_BIT(test[0],2);
595
596			/* p=1 */
597			test[1] = 0;
598			/* t=0*/
599			if(sides[1][0] == GT_INVALID)
600			{
601			if(!NTH_BIT(nset,0))
602			VCROSS(n[0],t1,t0);
603			/* Test the point for sidedness */
604			d = DOT(n[0],p1);
605			if(d >= 0)
606			SET_NTH_BIT(test[1],0);
607			}
608			else
609			if(sides[1][0] != GT_INTERIOR)
610			SET_NTH_BIT(test[1],0);
611
612			/* t=1 */
613			if(sides[1][1] == GT_INVALID)
614			{
615			if(!NTH_BIT(nset,1))
616			VCROSS(n[1],t2,t1);
617			/* Test the point for sidedness */
618			d = DOT(n[1],p1);
619			if(d >= 0)
620			SET_NTH_BIT(test[1],1);
621			}
622			else
623			if(sides[1][1] != GT_INTERIOR)
624			SET_NTH_BIT(test[1],1);
625
626			/* t=2 */
627			if(sides[1][2] == GT_INVALID)
628			{
629			if(!NTH_BIT(nset,2))
630			VCROSS(n[2],t0,t2);
631			/* Test the point for sidedness */
632			d = DOT(n[2],p1);
633			if(d >= 0)
634			SET_NTH_BIT(test[1],2);
635			}
636			else
637			if(sides[1][2] != GT_INTERIOR)
638			SET_NTH_BIT(test[1],2);
639
640			/* p=2 */
641			test[2] = 0;
642			/* t = 0 */
643			if(sides[2][0] == GT_INVALID)
644			{
645			if(!NTH_BIT(nset,0))
646			VCROSS(n[0],t1,t0);
647			/* Test the point for sidedness */
648			d = DOT(n[0],p2);
649			if(d >= 0)
650			SET_NTH_BIT(test[2],0);
651			}
652			else
653			if(sides[2][0] != GT_INTERIOR)
654			SET_NTH_BIT(test[2],0);
655			/* t=1 */
656			if(sides[2][1] == GT_INVALID)
657			{
658			if(!NTH_BIT(nset,1))
659			VCROSS(n[1],t2,t1);
660			/* Test the point for sidedness */
661			d = DOT(n[1],p2);
662			if(d >= 0)
663			SET_NTH_BIT(test[2],1);
664			}
665			else
666			if(sides[2][1] != GT_INTERIOR)
667			SET_NTH_BIT(test[2],1);
668			/* t=2 */
669			if(sides[2][2] == GT_INVALID)
670			{
671			if(!NTH_BIT(nset,2))
672			VCROSS(n[2],t0,t2);
673			/* Test the point for sidedness */
674			d = DOT(n[2],p2);
675			if(d >= 0)
676			SET_NTH_BIT(test[2],2);
677			}
678			else
679			if(sides[2][2] != GT_INTERIOR)
680			SET_NTH_BIT(test[2],2);
681			}
682
683
684			int
685			spherical_tri_intersect(a1,a2,a3,b1,b2,b3)
686			FVECT a1,a2,a3,b1,b2,b3;
687			{
688			char which,test,n_set[2];
689			char sides[2][3][3],i,j,inext,jnext;
690			char tests[2][3];
691			FVECT n[2][3],p,avg[2];
692
693			/* Test the vertices of triangle a against the pyramid formed by triangle
694			b and the origin. If any vertex of a is interior to triangle b, or
695			if all 3 vertices of a are ON the edges of b,return TRUE. Remember
696			the results of the edge normal and sidedness tests for later.
697			*/
698			if(test_vertices_for_tri_inclusion(a1,a2,a3,b1,b2,b3,
699			&(n_set[0]),n[0],avg[0],sides[1]))
700			return(TRUE);
701
702			if(test_vertices_for_tri_inclusion(b1,b2,b3,a1,a2,a3,
703			&(n_set[1]),n[1],avg[1],sides[0]))
704			return(TRUE);
705
706
707			set_sidedness_tests(b1,b2,b3,a1,a2,a3,tests[0],sides[0],n_set[1],n[1]);
708			if(tests[0][0]&tests[0][1]&tests[0][2])
709			return(FALSE);
710
711			set_sidedness_tests(a1,a2,a3,b1,b2,b3,tests[1],sides[1],n_set[0],n[0]);
712			if(tests[1][0]&tests[1][1]&tests[1][2])
713			return(FALSE);
714
715			for(j=0; j < 3;j++)
716			{
717			jnext = (j+1)%3;
718			/* IF edge b doesnt cross any great circles of a, punt */
719			if(tests[1][j] & tests[1][jnext])
720			continue;
721			for(i=0;i<3;i++)
722			{
723			inext = (i+1)%3;
724			/* IF edge a doesnt cross any great circles of b, punt */
725			if(tests[0][i] & tests[0][inext])
726			continue;
727			/* Now find the great circles that cross and test */
728			if((NTH_BIT(tests[0][i],j)^(NTH_BIT(tests[0][inext],j)))
729			&& (NTH_BIT(tests[1][j],i)^NTH_BIT(tests[1][jnext],i)))
730			{
731			VCROSS(p,n[0][i],n[1][j]);
732
733			/* If zero cp= done */
734			if(ZERO_VEC3(p))
735			continue;
736			/* check above both planes */
737			if(DOT(avg[0],p) < 0 \|\| DOT(avg[1],p) < 0)
738			{
739			NEGATE_VEC3(p);
740			if(DOT(avg[0],p) < 0 \|\| DOT(avg[1],p) < 0)
741			continue;
742			}
743			return(TRUE);
744			}
745			}
746			}
747			return(FALSE);
748			}
749
750			int
751			ray_intersect_tri(orig,dir,v0,v1,v2,pt,wptr)
752			FVECT orig,dir;
753			FVECT v0,v1,v2;
754			FVECT pt;
755			char *wptr;
756			{
757			FVECT p0,p1,p2,p,n;
758			char type,which;
759			double pd;
760
761			point_on_sphere(p0,v0,orig);
762			point_on_sphere(p1,v1,orig);
763			point_on_sphere(p2,v2,orig);
764			type = test_single_point_against_spherical_tri(p0,p1,p2,dir,&which);
765
766			if(type)
767			{
768			/* Intersect the ray with the triangle plane */
769			tri_plane_equation(v0,v1,v2,n,&pd,FALSE);
770			intersect_ray_plane(orig,dir,n,pd,NULL,pt);
771			}
772			if(wptr)
773			*wptr = which;
774
775			return(type);
776			}
777
778
779			calculate_view_frustum(vp,hv,vv,horiz,vert,near,far,fnear,ffar)
780			FVECT vp,hv,vv;
781			double horiz,vert,near,far;
782			FVECT fnear[4],ffar[4];
783			{
784			double height,width;
785			FVECT t,nhv,nvv,ndv;
786			double w2,h2;
787			/* Calculate the x and y dimensions of the near face */
788			/* hv and vv are the horizontal and vertical vectors in the
789			view frame-the magnitude is the dimension of the front frustum
790			face at z =1
791			*/
792			VCOPY(nhv,hv);
793			VCOPY(nvv,vv);
794			w2 = normalize(nhv);
795			h2 = normalize(nvv);
796			/* Use similar triangles to calculate the dimensions at z=near */
797			width = near0.5w2;
798			height = near0.5h2;
799
800			VCROSS(ndv,nvv,nhv);
801			/* Calculate the world space points corresponding to the 4 corners
802			of the front face of the view frustum
803			*/
804			fnear[0][0] = widthnhv[0] + heightnvv[0] + near*ndv[0] + vp[0] ;
805			fnear[0][1] = widthnhv[1] + heightnvv[1] + near*ndv[1] + vp[1];
806			fnear[0][2] = widthnhv[2] + heightnvv[2] + near*ndv[2] + vp[2];
807			fnear[1][0] = -widthnhv[0] + heightnvv[0] + near*ndv[0] + vp[0];
808			fnear[1][1] = -widthnhv[1] + heightnvv[1] + near*ndv[1] + vp[1];
809			fnear[1][2] = -widthnhv[2] + heightnvv[2] + near*ndv[2] + vp[2];
810			fnear[2][0] = -widthnhv[0] - heightnvv[0] + near*ndv[0] + vp[0];
811			fnear[2][1] = -widthnhv[1] - heightnvv[1] + near*ndv[1] + vp[1];
812			fnear[2][2] = -widthnhv[2] - heightnvv[2] + near*ndv[2] + vp[2];
813			fnear[3][0] = widthnhv[0] - heightnvv[0] + near*ndv[0] + vp[0];
814			fnear[3][1] = widthnhv[1] - heightnvv[1] + near*ndv[1] + vp[1];
815			fnear[3][2] = widthnhv[2] - heightnvv[2] + near*ndv[2] + vp[2];
816
817			/* Now do the far face */
818			width = far0.5w2;
819			height = far0.5h2;
820			ffar[0][0] = widthnhv[0] + heightnvv[0] + far*ndv[0] + vp[0] ;
821			ffar[0][1] = widthnhv[1] + heightnvv[1] + far*ndv[1] + vp[1] ;
822			ffar[0][2] = widthnhv[2] + heightnvv[2] + far*ndv[2] + vp[2] ;
823			ffar[1][0] = -widthnhv[0] + heightnvv[0] + far*ndv[0] + vp[0] ;
824			ffar[1][1] = -widthnhv[1] + heightnvv[1] + far*ndv[1] + vp[1] ;
825			ffar[1][2] = -widthnhv[2] + heightnvv[2] + far*ndv[2] + vp[2] ;
826			ffar[2][0] = -widthnhv[0] - heightnvv[0] + far*ndv[0] + vp[0] ;
827			ffar[2][1] = -widthnhv[1] - heightnvv[1] + far*ndv[1] + vp[1] ;
828			ffar[2][2] = -widthnhv[2] - heightnvv[2] + far*ndv[2] + vp[2] ;
829			ffar[3][0] = widthnhv[0] - heightnvv[0] + far*ndv[0] + vp[0] ;
830			ffar[3][1] = widthnhv[1] - heightnvv[1] + far*ndv[1] + vp[1] ;
831			ffar[3][2] = widthnhv[2] - heightnvv[2] + far*ndv[2] + vp[2] ;
832			}
833
834
835
836
837			int
838			spherical_polygon_edge_intersect(a0,a1,b0,b1)
839			FVECT a0,a1,b0,b1;
840			{
841			FVECT na,nb,avga,avgb,p;
842			double d;
843			int sb0,sb1,sa0,sa1;
844
845			/* First test if edge b straddles great circle of a */
846			VCROSS(na,a0,a1);
847			d = DOT(na,b0);
848			sb0 = ZERO(d)?0:(d<0)? -1: 1;
849			d = DOT(na,b1);
850			sb1 = ZERO(d)?0:(d<0)? -1: 1;
851			/* edge b entirely on one side of great circle a: edges cannot intersect*/
852			if(sb0*sb1 > 0)
853			return(FALSE);
854			/* test if edge a straddles great circle of b */
855			VCROSS(nb,b0,b1);
856			d = DOT(nb,a0);
857			sa0 = ZERO(d)?0:(d<0)? -1: 1;
858			d = DOT(nb,a1);
859			sa1 = ZERO(d)?0:(d<0)? -1: 1;
860			/* edge a entirely on one side of great circle b: edges cannot intersect*/
861			if(sa0*sa1 > 0)
862			return(FALSE);
863
864			/* Find one of intersection points of the great circles */
865			VCROSS(p,na,nb);
866			/* If they lie on same great circle: call an intersection */
867			if(ZERO_VEC3(p))
868			return(TRUE);
869
870			VADD(avga,a0,a1);
871			VADD(avgb,b0,b1);
872			if(DOT(avga,p) < 0 \|\| DOT(avgb,p) < 0)
873			{
874			NEGATE_VEC3(p);
875			if(DOT(avga,p) < 0 \|\| DOT(avgb,p) < 0)
876			return(FALSE);
877			}
878			if((!sb0 \|\| !sb1) && (!sa0 \|\| !sa1))
879			return(FALSE);
880			return(TRUE);
881			}
882