Commit e8c832dd authored by Isaak Lim's avatar Isaak Lim
Browse files

added stable versions (for degenerate triangles return the distance to the...

added stable versions (for degenerate triangles return the distance to the longest edge) for distPointTriangleSquared and distPointTriangle
parent 4f9506f6
......@@ -443,6 +443,143 @@ distPointTriangleSquared( const Vec& _p,
//-----------------------------------------------------------------------------
template <class Vec>
typename Vec::value_type
distPointTriangleSquaredStable( const Vec& _p,
const Vec& _v0,
const Vec& _v1,
const Vec& _v2,
Vec& _nearestPoint )
{
Vec v0v1 = _v1 - _v0;
Vec v0v2 = _v2 - _v0;
Vec n = v0v1 % v0v2; // not normalized !
typename Vec::value_type d = n.sqrnorm();
// Check if the triangle is degenerated
if (d < FLT_MIN && d > -FLT_MIN) {
const double l0 = v0v1.sqrnorm();
const double l1 = v0v2.sqrnorm();
const double l2 = (_v2 - _v1).sqrnorm();
if (l0 > l1 && l0 > l2) {
return distPointLineSquared(_p, _v0, _v1, &_nearestPoint);
}
else if (l1 > l0 && l1 > l2) {
return distPointLineSquared(_p, _v0, _v2, &_nearestPoint);
}
else {
return distPointLineSquared(_p, _v1, _v2, &_nearestPoint);
}
}
typename Vec::value_type invD = typename Vec::value_type(1.0) / d;
// these are not needed for every point, should still perform
// better with many points against one triangle
Vec v1v2 = _v2 - _v1;
typename Vec::value_type inv_v0v2_2 = typename Vec::value_type(1.0) / v0v2.sqrnorm();
typename Vec::value_type inv_v0v1_2 = typename Vec::value_type(1.0) / v0v1.sqrnorm();
typename Vec::value_type inv_v1v2_2 = typename Vec::value_type(1.0) / v1v2.sqrnorm();
Vec v0p = _p - _v0;
Vec t = v0p % n;
typename Vec::value_type s01, s02, s12;
typename Vec::value_type a = (t | v0v2) * -invD;
typename Vec::value_type b = (t | v0v1) * invD;
if (a < 0)
{
// Calculate the distance to an edge or a corner vertex
s02 = ( v0v2 | v0p ) * inv_v0v2_2;
if (s02 < 0.0)
{
s01 = ( v0v1 | v0p ) * inv_v0v1_2;
if (s01 <= 0.0) {
v0p = _v0;
} else if (s01 >= 1.0) {
v0p = _v1;
} else {
v0p = _v0 + v0v1 * s01;
}
} else if (s02 > 1.0) {
s12 = ( v1v2 | ( _p - _v1 )) * inv_v1v2_2;
if (s12 >= 1.0) {
v0p = _v2;
} else if (s12 <= 0.0) {
v0p = _v1;
} else {
v0p = _v1 + v1v2 * s12;
}
} else {
v0p = _v0 + v0v2 * s02;
}
} else if (b < 0.0) {
// Calculate the distance to an edge or a corner vertex
s01 = ( v0v1 | v0p ) * inv_v0v1_2;
if (s01 < 0.0)
{
s02 = ( v0v2 | v0p ) * inv_v0v2_2;
if (s02 <= 0.0) {
v0p = _v0;
} else if (s02 >= 1.0) {
v0p = _v2;
} else {
v0p = _v0 + v0v2 * s02;
}
} else if (s01 > 1.0) {
s12 = ( v1v2 | ( _p - _v1 )) * inv_v1v2_2;
if (s12 >= 1.0) {
v0p = _v2;
} else if (s12 <= 0.0) {
v0p = _v1;
} else {
v0p = _v1 + v1v2 * s12;
}
} else {
v0p = _v0 + v0v1 * s01;
}
} else if (a+b > 1.0) {
// Calculate the distance to an edge or a corner vertex
s12 = ( v1v2 | ( _p - _v1 )) * inv_v1v2_2;
if (s12 >= 1.0) {
s02 = ( v0v2 | v0p ) * inv_v0v2_2;
if (s02 <= 0.0) {
v0p = _v0;
} else if (s02 >= 1.0) {
v0p = _v2;
} else {
v0p = _v0 + v0v2*s02;
}
} else if (s12 <= 0.0) {
s01 = ( v0v1 | v0p ) * inv_v0v1_2;
if (s01 <= 0.0) {
v0p = _v0;
} else if (s01 >= 1.0) {
v0p = _v1;
} else {
v0p = _v0 + v0v1 * s01;
}
} else {
v0p = _v1 + v1v2 * s12;
}
} else {
// Calculate the distance to an interior point of the triangle
_nearestPoint = _p - n*((n|v0p) * invD);
return (_nearestPoint - _p).sqrnorm();
}
_nearestPoint = v0p;
return (_nearestPoint - _p).sqrnorm();
}
//-----------------------------------------------------------------------------
//
// Modified code of Dave Eberly (www.magic-software.com)
//
......
......@@ -308,6 +308,17 @@ distPointTriangleSquared( const Vec& _p,
const Vec& _v2,
Vec& _nearestPoint );
/** \brief squared distance from point _p to triangle (_v0, _v1, _v2)
* In the stable version the distance to the longest edge is returned if the triangle is degenerate.
*/
template <class Vec>
typename Vec::value_type
distPointTriangleSquaredStable( const Vec& _p,
const Vec& _v0,
const Vec& _v1,
const Vec& _v2,
Vec& _nearestPoint );
/// distance from point _p to triangle (_v0, _v1, _v2)
template <class Vec>
typename Vec::value_type
......@@ -318,6 +329,18 @@ distPointTriangle( const Vec& _p,
Vec& _nearestPoint )
{ return sqrt(distPointTriangleSquared(_p, _v0, _v1, _v2, _nearestPoint)); }
/** \brief distance from point _p to triangle (_v0, _v1, _v2)
* In the stable version the distance to the longest edge is returned if the triangle is degenerate.
*/
template <class Vec>
typename Vec::value_type
distPointTriangleStable( const Vec& _p,
const Vec& _v0,
const Vec& _v1,
const Vec& _v2,
Vec& _nearestPoint )
{ return sqrt(distPointTriangleSquaredStable(_p, _v0, _v1, _v2, _nearestPoint)); }
/** \brief Checks the distance from a point to a plane
*
*
......
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