PointTriangle.h

Go to the documentation of this file.

/** @file PointTriangle.h
 */
#define vecdot(a,b) ((a).x*(b).x + (a).y*(b).y + (a).z*(b).z)
#define vecdotproduct(a,b) ((coord){(a).y*(b).z - (a).z*(b).y,  \
                                    (a).z*(b).x - (a).x*(b).z,  \
                              (a).x*(b).y - (a).y*(b).x})
#define vecdiff(a,b)  ((coord){(a).x - (b).x, (a).y - (b).y, (a).z - (b).z})
#define vecdist2(a,b) (sq((a).x - (b).x) + sq((a).y - (b).y) + sq((a).z - (b).z))
 
/**
 * Returns the squared distance between point P and triangle P0, P1,
 * P2. Squared distance is returned in d2.  s and t returns the
 * closest point in parametric form in terms of edges P0P1 and P0P2.
 */
double PointTriangleDistance (const coord * P,
            const coord * P0,
            const coord * P1,
            const coord * P2,
            double * s, double * t)
{
  // From http://www.geometrictools.com/Documentation/DistancePoint3Triangle3.pdf
  double d2;
  coord diff = vecdiff (*P0, *P);
  coord edge0 = vecdiff(*P1, *P0);
  coord edge1 = vecdiff(*P2, *P0);
  double a00 = vecdot(edge0, edge0);
  double a01 = vecdot(edge0, edge1);
  double a11 = vecdot(edge1, edge1);
  double b0 = vecdot(diff, edge0);
  double b1 = vecdot(diff, edge1);
  double c = vecdot(diff, diff);
  double det = fabs(a00*a11 - a01*a01);
  *s = a01*b1 - a11*b0;
  *t = a01*b0 - a00*b1;
  
  if (*s + *t <= det)
  {
    if (*s < 0.0)
    {
      if (*t < 0.0)  // region 4
      {
        if (b0 < 0.0)
        {
          *t = 0.0;
          if (-b0 >= a00)
          {
            *s = 1.0;
            d2 = a00 + 2.0*b0 + c;
          }
          else
          {
            *s = -b0/a00;
            d2 = b0**s + c;
          }
        }
        else
        {
          *s = 0.0;
          if (b1 >= 0.0)
          {
            *t = 0.0;
            d2 = c;
          }
          else if (-b1 >= a11)
          {
            *t = 1.0;
            d2 = a11 + 2.0*b1 + c;
          }
          else
          {
            *t = -b1/a11;
            d2 = b1**t + c;
          }
        }
      }
      else  // region 3
      {
        *s = 0.0;
        if (b1 >= 0.0)
        {
          *t = 0.0;
          d2 = c;
        }
        else if (-b1 >= a11)
        {
          *t = 1.0;
          d2 = a11 + 2.0*b1 + c;
        }
        else
        {
          *t = -b1/a11;
          d2 = b1**t + c;
        }
      }
    }
    else if (*t < 0.0)  // region 5
    {
      *t = 0.0;
      if (b0 >= 0.0)
      {
        *s = 0.0;
        d2 = c;
      }
      else if (-b0 >= a00)
      {
        *s = 1.0;
        d2 = a00 + 2.0*b0 + c;
      }
      else
      {
        *s = -b0/a00;
        d2 = b0**s + c;
      }
    }
    else  // region 0
    {
      if (det == 0.) // degenerate triangle
  d2 = HUGE;
      else {
  double invDet = 1.0/det;
  *s *= invDet;
  *t *= invDet;
  d2 = *s*(a00**s + a01**t + 2.0*b0) + *t*(a01**s + a11**t + 2.0*b1) + c;
      }
    }
  }
  else
  {
    if (*s < 0.0)  // region 2
    {
      double tmp0 = a01 + b0;
      double tmp1 = a11 + b1;
      if (tmp1 > tmp0)
      {
        double numer = tmp1 - tmp0;
        double denom = a00 - 2.0*a01 + a11;
        if (numer >= denom)
        {
          *s = 1.0;
          *t = 0.0;
          d2 = a00 + 2.0*b0 + c;
        }
        else
        {
          *s = numer/denom;
          *t = 1.0 - *s;
          d2 = *s*(a00**s + a01**t + 2.0*b0) + *t*(a01**s + a11**t + 2.0*b1) + c;
        }
      }
      else
      {
        *s = 0.0;
        if (tmp1 <= 0.0)
        {
          *t = 1.0;
          d2 = a11 + 2.0*b1 + c;
        }
        else if (b1 >= 0.0)
        {
          *t = 0.0;
          d2 = c;
        }
        else
        {
          *t = -b1/a11;
          d2 = b1**t + c;
        }
      }
    }
    else if (*t < 0.0)  // region 6
    {
      double tmp0 = a01 + b1;
      double tmp1 = a00 + b0;
      if (tmp1 > tmp0)
      {
        double numer = tmp1 - tmp0;
        double denom = a00 - 2.0*a01 + a11;
        if (numer >= denom)
        {
          *t = 1.0;
          *s = 0.0;
          d2 = a11 + 2.0*b1 + c;
        }
        else
        {
          *t = numer/denom;
          *s = 1.0 - *t;
          d2 = *s*(a00**s + a01**t + 2.0*b0) + *t*(a01**s + a11**t + 2.0*b1) + c;
        }
      }
      else
      {
        *t = 0.0;
        if (tmp1 <= 0.0)
        {
          *s = 1.0;
          d2 = a00 + 2.0*b0 + c;
        }
        else if (b0 >= 0.0)
        {
          *s = 0.0;
          d2 = c;
        }
        else
        {
          *s = -b0/a00;
          d2 = b0**s + c;
        }
      }
    }
    else  // region 1
    {
      double numer = a11 + b1 - a01 - b0;
      if (numer <= 0.0)
      {
        *s = 0.0;
        *t = 1.0;
        d2 = a11 + 2.0*b1 + c;
      }
      else
      {
        double denom = a00 - 2.0*a01 + a11;
        if (numer >= denom)
        {
          *s = 1.0;
          *t = 0.0;
          d2 = a00 + 2.0*b0 + c;
        }
        else
        {
          *s = numer/denom;
          *t = 1.0 - *s;
          d2 = *s*(a00**s + a01**t + 2.0*b0) + *t*(a01**s + a11**t + 2.0*b1) + c;
        }
      }
    }
  }
  
  // Account for numerical round-off error
  if (d2 < 0.0)
  {
    d2 = 0.0;
  }
 
  return d2;
}
 
int PointTriangleOrientation (const coord * P,
            const coord * P0,
            const coord * P1,
            const coord * P2)
{
  coord diff = vecdiff (*P0, *P);
  coord edge0 = vecdiff (*P1, *P0);
  coord edge1 = vecdiff (*P2, *P0);
  coord n = vecdotproduct (edge0, edge1);
  return sign (vecdot (diff, n));
}
 
/**
   Returns the squared distance between p and [p0:p1].
*/
double PointSegmentDistance (const coord * p, const coord * p0, const coord * p1,
           coord * segmentClosest, double * segmentParameter)
{
  // The direction vector is not unit length.  The normalization is deferred
  // until it is needed.
  coord direction = vecdiff (*p1, *p0);
  coord diff = vecdiff (*p, *p1);
  double t = vecdot(direction, diff);
  if (t >= (double)0)
    {
      *segmentParameter = (double)1;
      *segmentClosest = *p1;
    }
  else
    {
      diff = vecdiff (*p, *p0);
      t = vecdot(direction, diff);
      if (t <= (double)0)
        {
    *segmentParameter = (double)0;
    *segmentClosest = *p0;
        }
      else
        {
    double sqrLength = vecdot(direction, direction);
    if (sqrLength > (double)0)
            {
        t /= sqrLength;
        *segmentParameter = t;
        (*segmentClosest).z = 0.;
        for (int _d = 0; _d < dimension; _d++)
    (*segmentClosest).x = (*p0).x + t * direction.x;
            }
    else
            {
        *segmentParameter = (double)0;
        *segmentClosest = *p0;
            }
        }
    }
 
  diff = vecdiff (*p, *segmentClosest);
  return vecdot(diff, diff);
}
 
int PointSegmentOrientation (const coord * P,
           const coord * P0,
           const coord * P1)
{
  coord diff = vecdiff (*P0, *P);
  coord edge0 = vecdiff (*P0, *P1);
  coord n = vecdotproduct (diff, edge0);
  return sign(n.z);
}