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

ACG/Geometry/Algorithms.cc

@@ -313,6 +313,147 @@ distPointLineSquared( const Vec& _p,
   return d1.sqrnorm();
 }    
 
+  //-----------------------------------------------------------------------------
+
+template <class Vec>
+typename Vec::value_type
+distPointTriangleSquared( const Vec& _p,
+                          const Vec& _v0,
+                          const Vec& _v1,
+                          const Vec& _v2,
+                          Vec& _nearestPoint,
+                          bool _stable)
+{
+  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) {
+    if (_stable) {
+      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);
+      }
+    } else {
+      std::cerr << "distPointTriangleSquared: Degenerated triangle !\n";
+      return -1.0;
+    }
+  }
+  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();
+}
+
 //-----------------------------------------------------------------------------
 
 
@@ -323,6 +464,173 @@ distPointTriangleSquared( const Vec& _p,
 			  const Vec& _v1,
 			  const Vec& _v2,
 			  Vec& _nearestPoint )
+{
+  return distPointTriangleSquared(_p, _v0, _v1, _v2, _nearestPoint, false);
+}
+
+
+//-----------------------------------------------------------------------------
+
+
+template <class Vec>
+typename Vec::value_type
+distPointTriangleSquaredStable( const Vec& _p,
+                                const Vec& _v0,
+                                const Vec& _v1,
+                                const Vec& _v2,
+                                Vec& _nearestPoint )
+{
+  return distPointTriangleSquared(_p, _v0, _v1, _v2, _nearestPoint, true);
+}
+
+
+//-----------------------------------------------------------------------------
+
+
+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;
@@ -332,8 +640,18 @@ distPointTriangleSquared( const Vec& _p,
   
   // Check if the triangle is degenerated
   if (d < FLT_MIN && d > -FLT_MIN) {
-    std::cerr << "distPointTriangleSquared: Degenerated triangle !\n";
-    return -1.0;
+    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;
 

ACG/Geometry/Algorithms.hh

@@ -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
 *
 *