basic/geometry/geo_rangesensor/raytrace.h

Go to the documentation of this file.

#if !defined(raytrace_h__INCLUDED_)
#define raytrace_h__INCLUDED_

#include <math.h>

// Code has been placed in this .h file because it seems to be
// necesscary in order to make the functions inline.

//since we're using floats, we may need a larger factor than 0.000001
#define EPSILON 0.000001
#define CROSS(dest,v1,v2) \
  dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \
  dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \
dest[2]=v1[0]*v2[1]-v1[1]*v2[0];
#define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])
#define SUB(dest,v1,v2) \
  dest[0]=v1[0]-v2[0]; \
  dest[1]=v1[1]-v2[1]; \
dest[2]=v1[2]-v2[2]; 

/* code rewritten to do tests on the sign of the determinant */
/* the division is at the end in the code                    */
inline int intersect_triangle(double orig[3], double dir[3],
//                      Vector4 vert0, Vector4 vert1, Vector4  vert2,
                        double vert0[3], double vert1[3], double vert2[3],
                        double edge1[3], double edge2[3],
                        double *t, double *u, double *v)
{
   double tvec[3], pvec[3], qvec[3];
   double det; //,inv_det;

   /* begin calculating determinant - also used to calculate U parameter */
   CROSS(pvec, dir, edge2);

   /* if determinant is near zero, ray lies in plane of triangle */
   det = DOT(edge1, pvec);

   if (det > EPSILON)
   {
      /* calculate distance from vert0 to ray origin */
      SUB(tvec, orig, vert0);
      
      /* calculate U parameter and test bounds */
      *u = DOT(tvec, pvec);
      if (*u < 0.0 || *u > det)
         return 0;
      
      /* prepare to test V parameter */
      CROSS(qvec, tvec, edge1);
      
      /* calculate V parameter and test bounds */
      *v = DOT(dir, qvec);
      if (*v < 0.0 || *u + *v > det)
         return 0;
      
   }
   else if(det < -EPSILON)
   {
      /* calculate distance from vert0 to ray origin */
      SUB(tvec, orig, vert0);
      
      /* calculate U parameter and test bounds */
      *u = DOT(tvec, pvec);
      if (*u > 0.0 || *u < det)
         return 0;
      
      /* prepare to test V parameter */
      CROSS(qvec, tvec, edge1);
      
      /* calculate V parameter and test bounds */
      *v = DOT(dir, qvec) ;
      if (*v > 0.0 || *u + *v < det)
         return 0;
   }
   else return 0;  /* ray is parallell to the plane of the triangle */

   /* calculate t, ray intersects triangle */
   *t = DOT(edge2, qvec) / det;

   return 1;
}

/* code rewritten to do tests on the sign of the determinant */
/* the division is at the end in the code                    */
inline int intersect_triangle_backup(double orig[3], double dir[3],
//                      Vector4 vert0, Vector4 vert1, Vector4  vert2,
                        double vert0[3], double vert1[3], double vert2[3],
                        double *t, double *u, double *v)
{
   double edge1[3], edge2[3], tvec[3], pvec[3], qvec[3];
   double det; //,inv_det;

   /* find vectors for two edges sharing vert0 */
   SUB(edge1, vert1, vert0);
   SUB(edge2, vert2, vert0);

   /* begin calculating determinant - also used to calculate U parameter */
   CROSS(pvec, dir, edge2);

   /* if determinant is near zero, ray lies in plane of triangle */
   det = DOT(edge1, pvec);

   if (det > EPSILON)
   {
      /* calculate distance from vert0 to ray origin */
      SUB(tvec, orig, vert0);
      
      /* calculate U parameter and test bounds */
      *u = DOT(tvec, pvec);
      if (*u < 0.0 || *u > det)
         return 0;
      
      /* prepare to test V parameter */
      CROSS(qvec, tvec, edge1);
      
      /* calculate V parameter and test bounds */
      *v = DOT(dir, qvec);
      if (*v < 0.0 || *u + *v > det)
         return 0;
      
   }
   else if(det < -EPSILON)
   {
      /* calculate distance from vert0 to ray origin */
      SUB(tvec, orig, vert0);
      
      /* calculate U parameter and test bounds */
      *u = DOT(tvec, pvec);
      if (*u > 0.0 || *u < det)
         return 0;
      
      /* prepare to test V parameter */
      CROSS(qvec, tvec, edge1);
      
      /* calculate V parameter and test bounds */
      *v = DOT(dir, qvec) ;
      if (*v > 0.0 || *u + *v < det)
         return 0;
   }
   else return 0;  /* ray is parallell to the plane of the triangle */


   //inv_det = 1.0 / det;

   /* calculate t, ray intersects triangle */
   *t = DOT(edge2, qvec) / det; // * inv_det;
   //(*u) *= inv_det;
   //(*v) *= inv_det;

   return 1;
}


// two triangles sharing the 4 ccw points vert0, vert1, vert2, vert3.
// where:
//   vert0, vert2 are the "wing" points,
//   vert1, vert3 form the axis connecting the wings
inline int
intersect_4pt(double orig[3], double dir[3],
                   double vert0[3], double vert1[3], double vert2[3], double vert3[3],
                   double *t, double *u, double *v)
{
  double edge1[3], edge2[3], edge3[3], tvec[3], pvec[3], qvec[3];
  double det,inv_det,utemp;
  
  /* find vectors for two edges sharing vert0 */
  SUB(edge1, vert1, vert0);
  SUB(edge2, vert2, vert0);
  
  /* calculate distance from vert0 to ray origin */
  SUB(tvec, orig, vert0);
  
  /* begin calculating determinant - also used to calculate U parameter */
  CROSS(pvec, dir, edge2);
  
  /* if determinant is near zero, ray lies in plane of triangle */
  det = DOT(edge1, pvec);
  
  /* the non-culling branch */
  if (det > -EPSILON && det < EPSILON)
    /*in case triangles 1 and 2 aren't on same plane, but still share edge2*/
    goto trysecond;
  inv_det = 1 / det;
  
  /* calculate U parameter and test bounds */
  utemp = DOT(tvec, pvec);
  *u = utemp * inv_det;
  if (*u < 0.0 || *u > 1.0)
    goto trysecond;
  
  /* prepare to test V parameter */
  CROSS(qvec, tvec, edge1);
  
  /* calculate V parameter and test bounds */
  *v = DOT(dir, qvec) * inv_det;
  if (*v < 0.0 || *u + *v > 1.0)
    goto trysecond;
  
  /* calculate t, ray intersects triangle */
  *t = DOT(edge2, qvec) * inv_det;

  return 1;

  trysecond:

  /* begin testing for 4th point (vert3)*/
  SUB(edge3, vert3, vert0);
  det = DOT (edge3,pvec);

  /* the non-culling branch */
  /*in case triangles 1 and 2 aren't on same plane, but still share edge2*/
  if (det > -EPSILON && det < EPSILON) 
    return 0;
  inv_det = 1 / det;

  *u = utemp * inv_det;
  if (*u < 0.0 || *u > 1.0)
    return 0;

  /* prepare to test V parameter */
  CROSS(qvec, tvec, edge3);
  
  /* calculate V parameter and test bounds */
  *v = DOT(dir, qvec) * inv_det;
  if (*v < 0.0 || *u + *v > 1.0)
    return 0;
  
  /* calculate t, ray intersects 4pt object */
  *t = DOT(edge2, qvec) * inv_det;
  return 1;
  
}


// two triangles on the same plane sharing the 4 ccw points vert0, vert1, vert2, vert3.
inline int
intersect_quad(double orig[3], double dir[3],
                   double vert0[3], double vert1[3], double vert2[3], double vert3[3],
                   double *t, double *u, double *v)
{
  double edge1[3], edge2[3], edge3[3], tvec[3], pvec[3], qvec[3];
  double det,inv_det,utemp;
  
  /* find vectors for two edges sharing vert0 */
  SUB(edge1, vert1, vert0);
  SUB(edge2, vert2, vert0);
  
  /* begin calculating determinant - also used to calculate U parameter */
  CROSS(pvec, dir, edge2);
  
  /* if determinant is near zero, ray lies in plane of triangle */
  det = DOT(edge1, pvec);
  
  /* the non-culling branch */
  if (det > -EPSILON && det < EPSILON)
    /*we know the other triangle on same plane is also parallel to the eye ray*/
    return 0;
  inv_det = 1 / det;
  
  /* calculate distance from vert0 to ray origin */
  SUB(tvec, orig, vert0);
  
  /* calculate U parameter and test bounds */
  utemp = DOT(tvec, pvec);
  *u = utemp * inv_det;
  if (*u < 0.0 || *u > 1.0)
    goto trysecond;
  
  /* prepare to test V parameter */
  CROSS(qvec, tvec, edge1);
  
  /* calculate V parameter and test bounds */
  *v = DOT(dir, qvec) * inv_det;
  if (*v < 0.0 || *u + *v > 1.0)
    goto trysecond;
  
  /* calculate t, ray intersects triangle */
  *t = DOT(edge2, qvec) * inv_det;

  return 1;

  trysecond:

  /* begin testing for 4th point (vert3)*/
  SUB(edge3, vert3, vert0);
  det = DOT (edge3,pvec);

  /* the non-culling branch */
  /*in case triangles 1 and 2 aren't on same plane, but still share edge2*/
  if (det > -EPSILON && det < EPSILON) 
    return 0;
  inv_det = 1 / det;

  *u = utemp * inv_det;
  if (*u < 0.0 || *u > 1.0)
    return 0;

  /* prepare to test V parameter */
  CROSS(qvec, tvec, edge3);
  
  /* calculate V parameter and test bounds */
  *v = DOT(dir, qvec) * inv_det;
  if (*v < 0.0 || *u + *v > 1.0)
    return 0;
  
  /* calculate t, ray intersects 4pt object */
  *t = DOT(edge2, qvec) * inv_det;
  return 1;
  
}


inline int
intersect_sphere(double loc[3], double dir[3], double center[3], double radius, 
                                                                 double& t, double maxt)
{
        double b, c, d;
        double t1, t2;
        double diff[3];
        
        diff[0] = center[0] - loc[0];
        diff[1] = center[1] - loc[1];
        diff[2] = center[2] - loc[2];
        
        c = DOT(diff, diff) - radius*radius;
        b = -2 * DOT(diff, dir);
        d = b*b - 4*c;
        
        if (d <= 0)
                return 0;

        d = (double) sqrt(d);
        t1 = (-b+d)/2;
        t2 = (-b-d)/2;
        
        if (t1 < EPSILON || t1 >= maxt)
                t1 = -1;
        if (t2 < EPSILON || t2 >= maxt)
                t2 = -1;
        if (t1 < 0) {
                if (t2 < 0)
      return 0;
                t = t2;
        }
        else {
                if (t2 < 0)
      t = t1;
                else
      t = (t1 < t2) ? t1 : t2;
        }
        return (t > EPSILON);
}

/*inline void normalize (double & a, double & b, double & c)
{
  double x;
  x = (double)sqrt(1/( (a)*(a)+(b)*(b)+(c)*(c) ));
  a = a * x;
  b = b * x;
  c = c * x;
}*/

inline void normalize (double & a, double & b, double & c)
{
  double x;
  x = (double)sqrt(1/( (a)*(a)+(b)*(b)+(c)*(c) ));
  a = a * x;
  b = b * x;
  c = c * x;
}

#endif

---