Commit 489bbfe8 authored by Pierre Kraemer's avatar Pierre Kraemer

update squaredDistancePoint2Face and add closestPointInTriangle

parent 099d7609
...@@ -57,6 +57,19 @@ typename PFP::REAL squaredDistancePoint2FacePlane(typename PFP::MAP& map, Face f ...@@ -57,6 +57,19 @@ typename PFP::REAL squaredDistancePoint2FacePlane(typename PFP::MAP& map, Face f
template <typename PFP> template <typename PFP>
typename PFP::REAL squaredDistancePoint2Face(typename PFP::MAP& map, Face f, const VertexAttribute<typename PFP::VEC3, typename PFP::MAP>& position, const typename PFP::VEC3& P) ; typename PFP::REAL squaredDistancePoint2Face(typename PFP::MAP& map, Face f, const VertexAttribute<typename PFP::VEC3, typename PFP::MAP>& position, const typename PFP::VEC3& P) ;
/**
* compute the barycentric coordinates of the point in the triangle f that is closest to point P
* @param map the map
* @param f a triangle face
* @param position the vertex attribute storing positions
* @param P the point
* @param u barycentric coordinate 1 of closest point
* @param v barycentric coordinate 2 of closest point
* @param w barycentric coordinate 3 of closest point
*/
template <typename PFP>
void closestPointInTriangle(typename PFP::MAP& map, Face f, const VertexAttribute<typename PFP::VEC3, typename PFP::MAP>& position, const typename PFP::VEC3& P, double& u, double& v, double& w) ;
/** /**
* compute squared distance from point to an edge * compute squared distance from point to an edge
* @param map the map * @param map the map
......
...@@ -71,6 +71,15 @@ typename PFP::REAL squaredDistancePoint2Face(typename PFP::MAP& map, Face f, con ...@@ -71,6 +71,15 @@ typename PFP::REAL squaredDistancePoint2Face(typename PFP::MAP& map, Face f, con
return dist2; return dist2;
} }
template <typename PFP>
void closestPointInTriangle(typename PFP::MAP& map, Face f, const VertexAttribute<typename PFP::VEC3, typename PFP::MAP>& position, const typename PFP::VEC3& P, double& u, double& v, double& w)
{
Dart d = map.phi1(f.dart);
Dart e = map.phi1(d);
Geom::closestPointInTriangle(P, position[f.dart], position[d], position[e], u, v, w);
}
template <typename PFP> template <typename PFP>
typename PFP::REAL squaredDistancePoint2Edge(typename PFP::MAP& map, Edge e, const VertexAttribute<typename PFP::VEC3, typename PFP::MAP>& position, const typename PFP::VEC3& P) typename PFP::REAL squaredDistancePoint2Edge(typename PFP::MAP& map, Edge e, const VertexAttribute<typename PFP::VEC3, typename PFP::MAP>& position, const typename PFP::VEC3& P)
{ {
......
...@@ -56,7 +56,7 @@ template <typename VEC3> ...@@ -56,7 +56,7 @@ template <typename VEC3>
typename VEC3::DATA_TYPE distancePoint2TrianglePlane(const VEC3& P, const VEC3& A, const VEC3& B, const VEC3& C) ; typename VEC3::DATA_TYPE distancePoint2TrianglePlane(const VEC3& P, const VEC3& A, const VEC3& B, const VEC3& C) ;
/** /**
* compute squared distance from point to triangle * compute squared distance from point P to triangle ABC
* @param P the point * @param P the point
* @param A triangle point 1 * @param A triangle point 1
* @param B triangle point 2 * @param B triangle point 2
...@@ -66,6 +66,19 @@ typename VEC3::DATA_TYPE distancePoint2TrianglePlane(const VEC3& P, const VEC3& ...@@ -66,6 +66,19 @@ typename VEC3::DATA_TYPE distancePoint2TrianglePlane(const VEC3& P, const VEC3&
template <typename VEC3> template <typename VEC3>
typename VEC3::DATA_TYPE squaredDistancePoint2Triangle(const VEC3& P, const VEC3& A, const VEC3& B, const VEC3& C) ; typename VEC3::DATA_TYPE squaredDistancePoint2Triangle(const VEC3& P, const VEC3& A, const VEC3& B, const VEC3& C) ;
/**
* compute the barycentric coordinates of the point in the triangle ABC that is closest to point P
* @param P the point
* @param A triangle point 1
* @param B triangle point 2
* @param C triangle point 3
* @param u barycentric coordinate 1 of closest point
* @param v barycentric coordinate 2 of closest point
* @param w barycentric coordinate 3 of closest point
*/
template <typename VEC3>
void closestPointInTriangle(const VEC3& P, const VEC3& A, const VEC3& B, const VEC3& C, double& u, double& v, double& w) ;
/** /**
* compute squared distance from point to line * compute squared distance from point to line
* @param P the point * @param P the point
......
...@@ -46,127 +46,260 @@ inline typename VEC3::DATA_TYPE distancePoint2TrianglePlane(const VEC3& P, const ...@@ -46,127 +46,260 @@ inline typename VEC3::DATA_TYPE distancePoint2TrianglePlane(const VEC3& P, const
return plane.distance(P) ; return plane.distance(P) ;
} }
// implemented following :
// Distance Between Point and Triangle in 3D
// http://www.geometrictools.com/Documentation/DistancePoint3Triangle3.pdf
template <typename VEC3> template <typename VEC3>
typename VEC3::DATA_TYPE squaredDistancePoint2Triangle(const VEC3& P, const VEC3& A, const VEC3& B, const VEC3& C) typename VEC3::DATA_TYPE squaredDistancePoint2Triangle(const VEC3& P, const VEC3& A, const VEC3& B, const VEC3& C)
{ {
VEC3 vPA = A - P ; VEC3 D = A - P ;
VEC3 vAB = B - A ; VEC3 E0 = B - A ;
VEC3 vAC = C - A ; VEC3 E1 = C - A ;
double fA00 = vAB.norm2() ;
double fA01 = vAB * vAC ; double a = E0.norm2() ;
double fA11 = vAC.norm2() ; double b = E0 * E1 ;
double fB0 = vPA * vAB ; double c = E1.norm2() ;
double fB1 = vPA * vAC ; double d = E0 * D ;
double fC = vPA.norm2() ; double e = E1 * D ;
double fDet = fabs(fA00*fA11-fA01*fA01); double f = D.norm2() ;
double fS = fA01*fB1-fA11*fB0;
double fT = fA01*fB0-fA00*fB1; double det = fabs(a*c - b*b);
double fSqrDistance; double s = b*e - c*d;
double t = b*d - a*e;
if (fS + fT <= fDet) double sqrDistance;
if (s + t <= det)
{ {
if (fS < 0.0f) if (s < 0.0f)
{ {
if (fT < 0.0f) // region 4 if (t < 0.0f) // region 4
{ {
if (fB0 < 0.0f) if (d < 0.0)
{ {
fT = 0.0f; t = 0.0;
if (-fB0 >= fA00) { fS = 1.0f; fSqrDistance = fA00+(2.0f)*fB0+fC; } if (-d >= a) { s = 1.0; sqrDistance = a + 2.0*d + f; }
else { fS = -fB0/fA00; fSqrDistance = fB0*fS+fC; } else { s = -d/a; sqrDistance = d*s + f; }
} }
else else
{ {
fS = 0.0f; s = 0.0;
if (fB1 >= 0.0f) { fT = 0.0f; fSqrDistance = fC; } if (e >= 0.0) { t = 0.0; sqrDistance = f; }
else if (-fB1 >= fA11) { fT = 1.0f; fSqrDistance = fA11+(2.0f)*fB1+fC; } else if (-e >= c) { t = 1.0; sqrDistance = c + 2.0*e + f; }
else { fT = -fB1/fA11; fSqrDistance = fB1*fT+fC; } else { t = -e/c; sqrDistance = e*t + f; }
} }
} }
else // region 3 else // region 3
{ {
fS = 0.0f; s = 0.0;
if (fB1 >= 0.0f) { fT = 0.0f; fSqrDistance = fC; } if (e >= 0.0) { t = 0.0; sqrDistance = f; }
else if (-fB1 >= fA11) { fT = 1.0f; fSqrDistance = fA11+(2.0f)*fB1+fC; } else if (-e >= c) { t = 1.0; sqrDistance = c + 2.0*e + f; }
else { fT = -fB1/fA11; fSqrDistance = fB1*fT+fC; } else { t = -e/c; sqrDistance = e*t + f; }
} }
} }
else if (fT < 0.0f) // region 5 else if (t < 0.0) // region 5
{ {
fT = 0.0f; t = 0.0;
if (fB0 >= 0.0f) { fS = 0.0f; fSqrDistance = fC; } if (d >= 0.0) { s = 0.0; sqrDistance = f; }
else if (-fB0 >= fA00) { fS = 1.0f; fSqrDistance = fA00+(2.0f)*fB0+fC; } else if (-d >= a) { s = 1.0; sqrDistance = a + 2.0*d + f; }
else { fS = -fB0/fA00; fSqrDistance = fB0*fS+fC; } else { s = -d/a; sqrDistance = d*s + f; }
} }
else // region 0 else // region 0
{ {
// minimum at interior point // minimum at interior point
double fInvDet = (1.0f)/fDet; double invDet = 1.0 / det;
fS *= fInvDet; s *= invDet;
fT *= fInvDet; t *= invDet;
fSqrDistance = fS*(fA00*fS+fA01*fT+(2.0f)*fB0) + fT*(fA01*fS+fA11*fT+(2.0f)*fB1)+fC; sqrDistance = s * (a*s + b*t + 2.0*d) + t * (b*s + c*t + 2.0*e) + f;
} }
} }
else else
{ {
double fTmp0, fTmp1, fNumer, fDenom; double tmp0, tmp1, numer, denom;
if (fS < 0.0f) // region 2 if (s < 0.0f) // region 2
{ {
fTmp0 = fA01 + fB0; tmp0 = b + d;
fTmp1 = fA11 + fB1; tmp1 = c + e;
if (fTmp1 > fTmp0) if (tmp1 > tmp0)
{ {
fNumer = fTmp1 - fTmp0; numer = tmp1 - tmp0;
fDenom = fA00-2.0f*fA01+fA11; denom = a - 2.0*b + c;
if (fNumer >= fDenom) { fS = 1.0f; fT = 0.0f; fSqrDistance = fA00+(2.0f)*fB0+fC; } if (numer >= denom) { s = 1.0; t = 0.0; sqrDistance = a + 2.0*d + f; }
else { fS = fNumer/fDenom; fT = 1.0f - fS; fSqrDistance = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+(2.0f)*fB1)+fC; } else { s = numer/denom; t = 1.0 - s; sqrDistance = s * (a*s + b*t + 2.0*d) + t * (b*s + c*t + 2.0*e) + f; }
} }
else else
{ {
fS = 0.0f; s = 0.0;
if (fTmp1 <= 0.0f) { fT = 1.0f; fSqrDistance = fA11+(2.0f)*fB1+fC; } if (tmp1 <= 0.0) { t = 1.0; sqrDistance = c + 2.0*e + f; }
else if (fB1 >= 0.0f) { fT = 0.0f; fSqrDistance = fC; } else if (e >= 0.0) { t = 0.0; sqrDistance = f; }
else { fT = -fB1/fA11; fSqrDistance = fB1*fT+fC; } else { t = -e/c; sqrDistance = e*t + f; }
} }
} }
else if (fT < 0.0f) // region 6 else if (t < 0.0f) // region 6
{ {
fTmp0 = fA01 + fB1; tmp0 = b + e;
fTmp1 = fA00 + fB0; tmp1 = a + d;
if (fTmp1 > fTmp0) if (tmp1 > tmp0)
{ {
fNumer = fTmp1 - fTmp0; numer = tmp1 - tmp0;
fDenom = fA00-(2.0f)*fA01+fA11; denom = a - 2.0*b + c;
if (fNumer >= fDenom) { fT = 1.0f; fS = 0.0f; fSqrDistance = fA11+(2.0f)*fB1+fC; } if (numer >= denom) { t = 1.0; s = 0.0; sqrDistance = c + 2.0*e + f; }
else { fT = fNumer/fDenom; fS = 1.0f - fT; fSqrDistance = fS*(fA00*fS+fA01*fT+(2.0f)*fB0) + fT*(fA01*fS+fA11*fT+(2.0f)*fB1)+fC; } else { t = numer/denom; s = 1.0 - t; sqrDistance = s * (a*s + b*t + 2.0*d) + t * (b*s + c*t + 2.0*e) + f; }
} }
else else
{ {
fT = 0.0f; t = 0.0;
if (fTmp1 <= 0.0f) { fS = 1.0f; fSqrDistance = fA00+(2.0f)*fB0+fC; } if (tmp1 <= 0.0) { s = 1.0; sqrDistance = a + 2.0*d + f; }
else if (fB0 >= 0.0f) { fS = 0.0f; fSqrDistance = fC; } else if (d >= 0.0) { s = 0.0; sqrDistance = f; }
else { fS = -fB0/fA00; fSqrDistance = fB0*fS+fC; } else { s = -d/a; sqrDistance = d*s + f; }
} }
} }
else // region 1 else // region 1
{ {
fNumer = fA11 + fB1 - fA01 - fB0; numer = c + e - b - d;
if (fNumer <= 0.0f) { fS = 0.0f; fT = 1.0f; fSqrDistance = fA11+(2.0f)*fB1+fC; } if (numer <= 0.0) { s = 0.0; t = 1.0; sqrDistance = c + 2.0*e + f; }
else else
{ {
fDenom = fA00-2.0f*fA01+fA11; denom = a - 2.0*b + c;
if (fNumer >= fDenom) { fS = 1.0f; fT = 0.0f; fSqrDistance = fA00+(2.0f)*fB0+fC; } if (numer >= denom) { s = 1.0; t = 0.0; sqrDistance = a + 2.0*d + f; }
else { fS = fNumer/fDenom; fT = 1.0f - fS; fSqrDistance = fS*(fA00*fS+fA01*fT+(2.0f)*fB0) + fT*(fA01*fS+fA11*fT+(2.0f)*fB1)+fC; } else { s = numer/denom; t = 1.0 - s; sqrDistance = s * (a*s + b*t + 2.0*d) + t * (b*s + c*t + 2.0*e) + f; }
} }
} }
} }
// sqrDistance = s * (a*s + b*t + 2.0*d) + t * (b*s + c*t + 2.0*e) + f;
// account for numerical round-off error // account for numerical round-off error
if (fSqrDistance < 0.0f) if (sqrDistance < 0.0)
fSqrDistance = 0.0f; sqrDistance = 0.0;
return sqrDistance;
}
template <typename VEC3>
void closestPointInTriangle(const VEC3& P, const VEC3& A, const VEC3& B, const VEC3& C, double& u, double& v, double& w)
{
VEC3 D = A - P ;
VEC3 E0 = B - A ;
VEC3 E1 = C - A ;
double a = E0.norm2() ;
double b = E0 * E1 ;
double c = E1.norm2() ;
double d = E0 * D ;
double e = E1 * D ;
double f = D.norm2() ;
double det = fabs(a*c - b*b);
double s = b*e - c*d;
double t = b*d - a*e;
if (s + t <= det)
{
if (s < 0.0f)
{
if (t < 0.0f) // region 4
{
if (d < 0.0)
{
t = 0.0;
if (-d >= a) { s = 1.0; }
else { s = -d/a; }
}
else
{
s = 0.0;
if (e >= 0.0) { t = 0.0; }
else if (-e >= c) { t = 1.0; }
else { t = -e/c; }
}
}
else // region 3
{
s = 0.0;
if (e >= 0.0) { t = 0.0; }
else if (-e >= c) { t = 1.0; }
else { t = -e/c; }
}
}
else if (t < 0.0) // region 5
{
t = 0.0;
if (d >= 0.0) { s = 0.0; }
else if (-d >= a) { s = 1.0; }
else { s = -d/a; }
}
else // region 0
{
// minimum at interior point
double invDet = 1.0 / det;
s *= invDet;
t *= invDet;
}
}
else
{
double tmp0, tmp1, numer, denom;
if (s < 0.0f) // region 2
{
tmp0 = b + d;
tmp1 = c + e;
if (tmp1 > tmp0)
{
numer = tmp1 - tmp0;
denom = a - 2.0*b + c;
if (numer >= denom) { s = 1.0; t = 0.0; }
else { s = numer/denom; t = 1.0 - s; }
}
else
{
s = 0.0;
if (tmp1 <= 0.0) { t = 1.0; }
else if (e >= 0.0) { t = 0.0; }
else { t = -e/c; }
}
}
else if (t < 0.0f) // region 6
{
tmp0 = b + e;
tmp1 = a + d;
if (tmp1 > tmp0)
{
numer = tmp1 - tmp0;
denom = a - 2.0*b + c;
if (numer >= denom) { t = 1.0; s = 0.0; }
else { t = numer/denom; s = 1.0 - t; }
}
else
{
t = 0.0;
if (tmp1 <= 0.0) { s = 1.0; }
else if (d >= 0.0) { s = 0.0; }
else { s = -d/a; }
}
}
else // region 1
{
numer = c + e - b - d;
if (numer <= 0.0) { s = 0.0; t = 1.0; }
else
{
denom = a - 2.0*b + c;
if (numer >= denom) { s = 1.0; t = 0.0; }
else { s = numer/denom; t = 1.0 - s; }
}
}
}
// u = s;
// v = t;
// w = 1.0 - s - t;
return fSqrDistance; u = 1.0 - s - t;
v = s;
w = t;
} }
template <typename VEC3> template <typename VEC3>
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment