pointClosedTriangle

 

struct VECTOR

{

 float x, y, z;

};

 

 

 

//

// set value of VECTOR

void setVector(VECTOR & v, const float & x, const float & y,const float & z)

{

 v.x = x;

 v.y = y;

 v.z = z;

}

 

//

// v1-v2

VECTOR sub( const VECTOR & v1, const VECTOR &  v2 )

{

 VECTOR v;

 v.x = v1.x-v2.x;

 v.y = v1.y-v2.y;

 v.z = v1.z-v2.z;

 

 return v;

}

 

//

// v1.v2

float DotProduct(const VECTOR & v1, const VECTOR & v2)

{

 return v1.x*v2.x+v1.y*v2.y+v1.z*v2.z;;

}

 

//

// point to triangle

// from <<Geometric tools for computer graphics>>

// return  distance

float Point2Triangle(const VECTOR & point, const VECTOR & v0, const VECTOR & v1, const VECTOR & v2)

{

 return sqrt( Point2Triangle_LenSqr( point, v0,v1,v2 ) );

}

 

 

//

// point to triangle

// from <<Geometric tools for computer graphics>>

// return Square distance

float Point2Triangle_LenSqr(const VECTOR & point, const VECTOR & v0, const VECTOR & v1, const VECTOR & v2)

{

 // basic variable

 VECTOR dd = sub(v0, point);

 VECTOR e0 = sub(v1, v0);

 VECTOR e1 = sub(v2, v0);

 

 float a,b,c,d,e,f;

 a = DotProduct(e0,e0);

 b = DotProduct(e0,e1);

 c = DotProduct(e1,e1);

 d = DotProduct(e0,dd);

 e = DotProduct(e1,dd);

 f = DotProduct(dd,dd);

 

 // region

 float det = abs(a*c-b*b);

 float s = b*e-c*d;

 float t = b*d-a*e;

 

 // region 0

 if ( s>=0 && s<=det && t >=0 && t<=det  && s+t<=det+1e-6 )

 {

float invDet = 1/det;

s *= invDet;

t *= invDet;

float a*s*s+2*b*s*t+c*t*t+2*d*s+2*e*t+f;

 }

 

 // determine the region

 int region;

 if (s+t <= det)

 {

  if (s<0)

  {

   if (t<0)

   {

region = 4;

   }

   else

   {

region = 3;

   }

  }

  else

  {

region = 5;

  }

 }

 else

 {

  if (s<0)

  {

   region = 2;

  }

  else if (t<0)

  {

   region = 6;

  }

  else

  {

   region = 1;

  }

 }

 

 // region 1

 if (region == 1)

 {

  float numer = c+e-b-d;

  if (numer <=0)

  {

   s = 0;

  }

  else

  {

   float denom = a-2*b+c;

   s = (numer >= denom ? 1:numer/denom);

  }

  t = 1-s;

 }

 

 // region 3

 if (region == 3)

 {

  s = 0;

  t = (e>=0 ? 0:(-e>c ? 1:-e/c));

 }

 

 // region 5

 if (region == 5)

 {

  t = 0;

  s = (d>=0 ? 0:(-d>a ? 1:-d/a));

 }

 

 // region 2

 if (region == 2)

 {

  float tmp0 = b+d;

  float tmp1 = c+e;

  if (tmp1 > tmp0)

  {

   float numer = tmp1-tmp0;

   float denom = a-2*b+c;

   s = (numer >= denom ? 1:numer/denom);

   t = 1-s;

  }

  else

  {

   s = 0;

   t = (e>=0 ? 0:(-e>c ? 1:-e/c)); 

  }

 }

 // region 4

 if (region == 4)

 {

  if (e<0)

  {

   s = 0;

   t = t = (e>=0 ? 0:(-e>c ? 1:-e/c)); 

  }

  else

  {

   t = 0;

   s = (d>=0 ? 0:(-d>a ? 1:-d/a));

  }

 }

 // region 6

 if (region == 6)

 {

  if (a+d>0)

  {

   t = 0;

   s = (d>=0 ? 0:(-d>a ? 1:-d/a));

  }

  else

  {

   float numer = c+e-b-d;

   float denom = a-2*b+c;

   s = (numer >= denom ? 1:numer/denom);

   t = 1-s;

  }

 }

 

 return a*s*s+2*b*s*t+c*t*t+2*d*s+2*e*t+f;

}