Skip to content
GitLab
Commit 489bbfe8 authored by Pierre Kraemer's avatar Pierre Kraemer
Browse files

update squaredDistancePoint2Face and add closestPointInTriangle

parent 099d7609
Hide whitespace changes
Inline Side-by-side
CGoGN/include/Algo/Geometry/distances.h
View file @ 489bbfe8
......@@ -57,6 +57,19 @@ typename PFP::REAL squaredDistancePoint2FacePlane(typename PFP::MAP& map, Face f
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) ;
/**
* 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
* @param map the map
......
CGoGN/include/Algo/Geometry/distances.hpp
View file @ 489bbfe8
......@@ -71,6 +71,15 @@ typename PFP::REAL squaredDistancePoint2Face(typename PFP::MAP& map, Face f, con
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>
typename PFP::REAL squaredDistancePoint2Edge(typename PFP::MAP& map, Edge e, const VertexAttribute<typename PFP::VEC3, typename PFP::MAP>& position, const typename PFP::VEC3& P)
{
......
CGoGN/include/Geometry/distances.h
View file @ 489bbfe8
......@@ -56,7 +56,7 @@ template <typename VEC3>
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>
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
* @param P the point
......
CGoGN/include/Geometry/distances.hpp
View file @ 489bbfe8
......@@ -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>
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 <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;
u = 1.0 - s - t;
v = s;
w = t;
}
template <typename VEC3>
......
Supports Markdown
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