From 489bbfe88bc2c12132d2ad40d4f4c462cb57c35c Mon Sep 17 00:00:00 2001 From: Pierre Kraemer Date: Tue, 21 Jul 2015 17:36:31 +0200 Subject: [PATCH] update squaredDistancePoint2Face and add closestPointInTriangle --- CGoGN/include/Algo/Geometry/distances.h | 13 + CGoGN/include/Algo/Geometry/distances.hpp | 9 + CGoGN/include/Geometry/distances.h | 15 +- CGoGN/include/Geometry/distances.hpp | 369 +++++++++++++++------- 4 files changed, 287 insertions(+), 119 deletions(-) diff --git a/CGoGN/include/Algo/Geometry/distances.h b/CGoGN/include/Algo/Geometry/distances.h index 8c950bdbd..41bc79ae6 100644 --- a/CGoGN/include/Algo/Geometry/distances.h +++ b/CGoGN/include/Algo/Geometry/distances.h @@ -57,6 +57,19 @@ typename PFP::REAL squaredDistancePoint2FacePlane(typename PFP::MAP& map, Face f template typename PFP::REAL squaredDistancePoint2Face(typename PFP::MAP& map, Face f, const VertexAttribute& 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 +void closestPointInTriangle(typename PFP::MAP& map, Face f, const VertexAttribute& position, const typename PFP::VEC3& P, double& u, double& v, double& w) ; + /** * compute squared distance from point to an edge * @param map the map diff --git a/CGoGN/include/Algo/Geometry/distances.hpp b/CGoGN/include/Algo/Geometry/distances.hpp index 08f9d20f9..7720cd1fb 100644 --- a/CGoGN/include/Algo/Geometry/distances.hpp +++ b/CGoGN/include/Algo/Geometry/distances.hpp @@ -71,6 +71,15 @@ typename PFP::REAL squaredDistancePoint2Face(typename PFP::MAP& map, Face f, con return dist2; } +template +void closestPointInTriangle(typename PFP::MAP& map, Face f, const VertexAttribute& 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::REAL squaredDistancePoint2Edge(typename PFP::MAP& map, Edge e, const VertexAttribute& position, const typename PFP::VEC3& P) { diff --git a/CGoGN/include/Geometry/distances.h b/CGoGN/include/Geometry/distances.h index a0ed31058..aae717ac0 100644 --- a/CGoGN/include/Geometry/distances.h +++ b/CGoGN/include/Geometry/distances.h @@ -56,7 +56,7 @@ template 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 A triangle point 1 * @param B triangle point 2 @@ -66,6 +66,19 @@ typename VEC3::DATA_TYPE distancePoint2TrianglePlane(const VEC3& P, const VEC3& template 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 +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 * @param P the point diff --git a/CGoGN/include/Geometry/distances.hpp b/CGoGN/include/Geometry/distances.hpp index 2cf2293f8..6d0d86e4e 100644 --- a/CGoGN/include/Geometry/distances.hpp +++ b/CGoGN/include/Geometry/distances.hpp @@ -46,127 +46,260 @@ inline typename VEC3::DATA_TYPE distancePoint2TrianglePlane(const VEC3& P, const return plane.distance(P) ; } +// implemented following : +// Distance Between Point and Triangle in 3D +// http://www.geometrictools.com/Documentation/DistancePoint3Triangle3.pdf template typename VEC3::DATA_TYPE squaredDistancePoint2Triangle(const VEC3& P, const VEC3& A, const VEC3& B, const VEC3& C) { - VEC3 vPA = A - P ; - VEC3 vAB = B - A ; - VEC3 vAC = C - A ; - double fA00 = vAB.norm2() ; - double fA01 = vAB * vAC ; - double fA11 = vAC.norm2() ; - double fB0 = vPA * vAB ; - double fB1 = vPA * vAC ; - double fC = vPA.norm2() ; - double fDet = fabs(fA00*fA11-fA01*fA01); - double fS = fA01*fB1-fA11*fB0; - double fT = fA01*fB0-fA00*fB1; - double fSqrDistance; - - if (fS + fT <= fDet) - { - if (fS < 0.0f) - { - if (fT < 0.0f) // region 4 - { - if (fB0 < 0.0f) - { - fT = 0.0f; - if (-fB0 >= fA00) { fS = 1.0f; fSqrDistance = fA00+(2.0f)*fB0+fC; } - else { fS = -fB0/fA00; fSqrDistance = fB0*fS+fC; } - } - else - { - fS = 0.0f; - if (fB1 >= 0.0f) { fT = 0.0f; fSqrDistance = fC; } - else if (-fB1 >= fA11) { fT = 1.0f; fSqrDistance = fA11+(2.0f)*fB1+fC; } - else { fT = -fB1/fA11; fSqrDistance = fB1*fT+fC; } - } - } - else // region 3 - { - fS = 0.0f; - if (fB1 >= 0.0f) { fT = 0.0f; fSqrDistance = fC; } - else if (-fB1 >= fA11) { fT = 1.0f; fSqrDistance = fA11+(2.0f)*fB1+fC; } - else { fT = -fB1/fA11; fSqrDistance = fB1*fT+fC; } - } - } - else if (fT < 0.0f) // region 5 - { - fT = 0.0f; - if (fB0 >= 0.0f) { fS = 0.0f; fSqrDistance = fC; } - else if (-fB0 >= fA00) { fS = 1.0f; fSqrDistance = fA00+(2.0f)*fB0+fC; } - else { fS = -fB0/fA00; fSqrDistance = fB0*fS+fC; } - } - else // region 0 - { - // minimum at interior point - double fInvDet = (1.0f)/fDet; - fS *= fInvDet; - fT *= fInvDet; - fSqrDistance = fS*(fA00*fS+fA01*fT+(2.0f)*fB0) + fT*(fA01*fS+fA11*fT+(2.0f)*fB1)+fC; - } - } - else - { - double fTmp0, fTmp1, fNumer, fDenom; - - if (fS < 0.0f) // region 2 - { - fTmp0 = fA01 + fB0; - fTmp1 = fA11 + fB1; - if (fTmp1 > fTmp0) - { - fNumer = fTmp1 - fTmp0; - fDenom = fA00-2.0f*fA01+fA11; - if (fNumer >= fDenom) { fS = 1.0f; fT = 0.0f; fSqrDistance = fA00+(2.0f)*fB0+fC; } - 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 - { - fS = 0.0f; - if (fTmp1 <= 0.0f) { fT = 1.0f; fSqrDistance = fA11+(2.0f)*fB1+fC; } - else if (fB1 >= 0.0f) { fT = 0.0f; fSqrDistance = fC; } - else { fT = -fB1/fA11; fSqrDistance = fB1*fT+fC; } - } - } - else if (fT < 0.0f) // region 6 - { - fTmp0 = fA01 + fB1; - fTmp1 = fA00 + fB0; - if (fTmp1 > fTmp0) - { - fNumer = fTmp1 - fTmp0; - fDenom = fA00-(2.0f)*fA01+fA11; - if (fNumer >= fDenom) { fT = 1.0f; fS = 0.0f; fSqrDistance = fA11+(2.0f)*fB1+fC; } - 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 - { - fT = 0.0f; - if (fTmp1 <= 0.0f) { fS = 1.0f; fSqrDistance = fA00+(2.0f)*fB0+fC; } - else if (fB0 >= 0.0f) { fS = 0.0f; fSqrDistance = fC; } - else { fS = -fB0/fA00; fSqrDistance = fB0*fS+fC; } - } - } - else // region 1 - { - fNumer = fA11 + fB1 - fA01 - fB0; - if (fNumer <= 0.0f) { fS = 0.0f; fT = 1.0f; fSqrDistance = fA11+(2.0f)*fB1+fC; } - else - { - fDenom = fA00-2.0f*fA01+fA11; - if (fNumer >= fDenom) { fS = 1.0f; fT = 0.0f; fSqrDistance = fA00+(2.0f)*fB0+fC; } - 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; } - } - } - } - - // account for numerical round-off error - if (fSqrDistance < 0.0f) - fSqrDistance = 0.0f; - - return fSqrDistance; + 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; + + double sqrDistance; + + 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; sqrDistance = a + 2.0*d + f; } + else { s = -d/a; sqrDistance = d*s + f; } + } + else + { + s = 0.0; + if (e >= 0.0) { t = 0.0; sqrDistance = f; } + else if (-e >= c) { t = 1.0; sqrDistance = c + 2.0*e + f; } + else { t = -e/c; sqrDistance = e*t + f; } + } + } + else // region 3 + { + s = 0.0; + if (e >= 0.0) { t = 0.0; sqrDistance = f; } + else if (-e >= c) { t = 1.0; sqrDistance = c + 2.0*e + f; } + else { t = -e/c; sqrDistance = e*t + f; } + } + } + else if (t < 0.0) // region 5 + { + t = 0.0; + if (d >= 0.0) { s = 0.0; sqrDistance = f; } + else if (-d >= a) { s = 1.0; sqrDistance = a + 2.0*d + f; } + else { s = -d/a; sqrDistance = d*s + f; } + } + else // region 0 + { + // minimum at interior point + double invDet = 1.0 / det; + s *= invDet; + t *= invDet; + sqrDistance = s * (a*s + b*t + 2.0*d) + t * (b*s + c*t + 2.0*e) + f; + } + } + 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; sqrDistance = a + 2.0*d + f; } + 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 + { + s = 0.0; + if (tmp1 <= 0.0) { t = 1.0; sqrDistance = c + 2.0*e + f; } + else if (e >= 0.0) { t = 0.0; sqrDistance = f; } + else { t = -e/c; sqrDistance = e*t + f; } + } + } + 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; sqrDistance = c + 2.0*e + f; } + 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 + { + t = 0.0; + if (tmp1 <= 0.0) { s = 1.0; sqrDistance = a + 2.0*d + f; } + else if (d >= 0.0) { s = 0.0; sqrDistance = f; } + else { s = -d/a; sqrDistance = d*s + f; } + } + } + else // region 1 + { + numer = c + e - b - d; + if (numer <= 0.0) { s = 0.0; t = 1.0; sqrDistance = c + 2.0*e + f; } + else + { + denom = a - 2.0*b + c; + if (numer >= denom) { s = 1.0; t = 0.0; sqrDistance = a + 2.0*d + f; } + 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 + if (sqrDistance < 0.0) + sqrDistance = 0.0; + + return sqrDistance; +} + +template +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; + + u = 1.0 - s - t; + v = s; + w = t; } template -- GitLab