Commit 69e7a2da authored by Isaak Lim's avatar Isaak Lim
Browse files

fixed redefinition of distPointTriangleSquaredStable

parent efe49f19
......@@ -460,10 +460,10 @@ distPointTriangleSquared( const Vec& _p,
template <class Vec>
typename Vec::value_type
distPointTriangleSquared( const Vec& _p,
const Vec& _v0,
const Vec& _v1,
const Vec& _v2,
Vec& _nearestPoint )
const Vec& _v0,
const Vec& _v1,
const Vec& _v2,
Vec& _nearestPoint )
{
return distPointTriangleSquared(_p, _v0, _v1, _v2, _nearestPoint, false);
}
......@@ -487,279 +487,6 @@ distPointTriangleSquaredStable( 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();
}
//-----------------------------------------------------------------------------
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)
......
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