Replace TriangleDistance::triDistance by sqrTriDistance.

  sqrt should be applied after method call.

include/fcl/intersect.h

@@ -299,40 +299,83 @@ public:
   static void segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, const Vec3f& B,
                         Vec3f& VEC, Vec3f& X, Vec3f& Y);
 
-  /// @brief Compute the closest points on two triangles given their absolute coordinate, and returns the distance between them
-  /// S and T are two triangles
-  /// If the triangles are disjoint, P and Q give the closet points of S and T respectively. However,
-  /// if the triangles overlap, P and Q are basically a random pair of points from the triangles, not
-  /// coincident points on the intersection of the triangles, as might be expected.
-  static FCL_REAL triDistance(const Vec3f S[3], const Vec3f T[3], Vec3f& P, Vec3f& Q);
-
-  static FCL_REAL triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
-                              const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
-                              Vec3f& P, Vec3f& Q);
-
-  /// @brief Compute the closest points on two triangles given the relative transform between them, and returns the distance between them
-  /// S and T are two triangles
-  /// If the triangles are disjoint, P and Q give the closet points of S and T respectively. However,
-  /// if the triangles overlap, P and Q are basically a random pair of points from the triangles, not
-  /// coincident points on the intersection of the triangles, as might be expected.
-  /// The returned P and Q are both in the coordinate of the first triangle's coordinate
-  static FCL_REAL triDistance(const Vec3f S[3], const Vec3f T[3],
-                              const Matrix3f& R, const Vec3f& Tl,
-                              Vec3f& P, Vec3f& Q);
-
-  static FCL_REAL triDistance(const Vec3f S[3], const Vec3f T[3],
-                              const Transform3f& tf,
-                              Vec3f& P, Vec3f& Q);
-
-  static FCL_REAL triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
-                              const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
-                              const Matrix3f& R, const Vec3f& Tl,
-                              Vec3f& P, Vec3f& Q);
-
-  static FCL_REAL triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
-                              const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
-                              const Transform3f& tf,
-                              Vec3f& P, Vec3f& Q);
+  /// Compute squared distance between triangles
+  /// \param S and T are two triangles
+  /// \retval P, Q closest points if triangles do not intersect.
+  /// \return squared distance if triangles do not intersect, 0 otherwise.
+  /// If the triangles are disjoint, P and Q give the closet points of
+  /// S and T respectively. However,
+  /// if the triangles overlap, P and Q are basically a random pair of points
+  /// from the triangles, not coincident points on the intersection of the
+  /// triangles, as might be expected.
+  static FCL_REAL sqrTriDistance (const Vec3f S[3], const Vec3f T[3],
+				  Vec3f& P, Vec3f& Q);
+
+  static FCL_REAL sqrTriDistance (const Vec3f& S1, const Vec3f& S2,
+				  const Vec3f& S3, const Vec3f& T1,
+				  const Vec3f& T2, const Vec3f& T3,
+				  Vec3f& P, Vec3f& Q);
+
+  /// Compute squared distance between triangles
+  /// \param S and T are two triangles
+  /// \param R, Tl, rotation and translation applied to T,
+  /// \retval P, Q closest points if triangles do not intersect.
+  /// \return squared distance if triangles do not intersect, 0 otherwise.
+  /// If the triangles are disjoint, P and Q give the closet points of
+  /// S and T respectively. However,
+  /// if the triangles overlap, P and Q are basically a random pair of points
+  /// from the triangles, not coincident points on the intersection of the
+  /// triangles, as might be expected.
+  static FCL_REAL sqrTriDistance (const Vec3f S[3], const Vec3f T[3],
+				  const Matrix3f& R, const Vec3f& Tl,
+				  Vec3f& P, Vec3f& Q);
+
+  /// Compute squared distance between triangles
+  /// \param S and T are two triangles
+  /// \param tf, rotation and translation applied to T,
+  /// \retval P, Q closest points if triangles do not intersect.
+  /// \return squared distance if triangles do not intersect, 0 otherwise.
+  /// If the triangles are disjoint, P and Q give the closet points of
+  /// S and T respectively. However,
+  /// if the triangles overlap, P and Q are basically a random pair of points
+  /// from the triangles, not coincident points on the intersection of the
+  /// triangles, as might be expected.
+  static FCL_REAL sqrTriDistance (const Vec3f S[3], const Vec3f T[3],
+				  const Transform3f& tf,
+				  Vec3f& P, Vec3f& Q);
+
+
+  /// Compute squared distance between triangles
+  /// \param S1, S2, S3 and T1, T2, T3 are triangle vertices
+  /// \param R, Tl, rotation and translation applied to T1, T2, T3,
+  /// \retval P, Q closest points if triangles do not intersect.
+  /// \return squared distance if triangles do not intersect, 0 otherwise.
+  /// If the triangles are disjoint, P and Q give the closet points of
+  /// S and T respectively. However,
+  /// if the triangles overlap, P and Q are basically a random pair of points
+  /// from the triangles, not coincident points on the intersection of the
+  /// triangles, as might be expected.
+  static FCL_REAL sqrTriDistance (const Vec3f& S1, const Vec3f& S2,
+				  const Vec3f& S3, const Vec3f& T1,
+				  const Vec3f& T2, const Vec3f& T3,
+				  const Matrix3f& R, const Vec3f& Tl,
+				  Vec3f& P, Vec3f& Q);
+
+  /// Compute squared distance between triangles
+  /// \param S1, S2, S3 and T1, T2, T3 are triangle vertices
+  /// \param tf, rotation and translation applied to T1, T2, T3,
+  /// \retval P, Q closest points if triangles do not intersect.
+  /// \return squared distance if triangles do not intersect, 0 otherwise.
+  /// If the triangles are disjoint, P and Q give the closet points of
+  /// S and T respectively. However,
+  /// if the triangles overlap, P and Q are basically a random pair of points
+  /// from the triangles, not coincident points on the intersection of the
+  /// triangles, as might be expected.
+  static FCL_REAL sqrTriDistance (const Vec3f& S1, const Vec3f& S2,
+				  const Vec3f& S3, const Vec3f& T1,
+				  const Vec3f& T2, const Vec3f& T3,
+				  const Transform3f& tf,
+				  Vec3f& P, Vec3f& Q);
 
 };
 

include/fcl/traversal/traversal_node_bvhs.h

@@ -594,8 +594,8 @@ public:
     // nearest point pair
     Vec3f P1, P2;
 
-    FCL_REAL d = TriangleDistance::triDistance(t11, t12, t13, t21, t22, t23,
-                                               P1, P2);
+    FCL_REAL d = sqrt (TriangleDistance::sqrTriDistance
+		       (t11, t12, t13, t21, t22, t23, P1, P2));
 
     if(this->request.enable_nearest_points)
     {
@@ -737,8 +737,8 @@ public:
     // nearest point pair
     Vec3f P1, P2;
 
-    FCL_REAL d = TriangleDistance::triDistance(p1, p2, p3, q1, q2, q3,
-                                               P1, P2);
+    FCL_REAL d = sqrt (TriangleDistance::sqrTriDistance
+		       (p1, p2, p3, q1, q2, q3, P1, P2));
 
     if(d < this->min_distance)
     {

src/intersect.cpp

@@ -1287,7 +1287,8 @@ void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q,
 }
 
 
-FCL_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3], Vec3f& P, Vec3f& Q)
+FCL_REAL TriangleDistance::sqrTriDistance
+(const Vec3f S[3], const Vec3f T[3], Vec3f& P, Vec3f& Q)
 {
   // Compute vectors along the 6 sides
 
@@ -1342,7 +1343,7 @@ FCL_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3], Vec3f
         Z = T[(j+2)%3] - Q;
         FCL_REAL b = Z.dot(VEC);
 
-        if((a <= 0) && (b >= 0)) return sqrt(dd);
+        if((a <= 0) && (b >= 0)) return dd;
 
         FCL_REAL p = V.dot(VEC);
 
@@ -1432,7 +1433,7 @@ FCL_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3], Vec3f
             // the T triangle; the other point is on the face of S
             P = T[point] + Sn * (Tp[point] / Snl);
             Q = T[point];
-            return (P - Q).length();
+            return (P - Q).sqrLength();
           }
         }
       }
@@ -1488,7 +1489,7 @@ FCL_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3], Vec3f
           {
             P = S[point];
             Q = S[point] + Tn * (Sp[point] / Tnl);
-            return (P - Q).length();
+            return (P - Q).sqrLength();
           }
         }
       }
@@ -1504,70 +1505,73 @@ FCL_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3], Vec3f
   {
     P = minP;
     Q = minQ;
-    return sqrt(mindd);
+    return mindd;
   }
   else return 0;
 }
 
 
-FCL_REAL TriangleDistance::triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
-                                       const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
-                                       Vec3f& P, Vec3f& Q)
+FCL_REAL TriangleDistance::sqrTriDistance
+(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
+ const Vec3f& T1, const Vec3f& T2, const Vec3f& T3, Vec3f& P, Vec3f& Q)
 {
   Vec3f S[3];
   Vec3f T[3];
   S[0] = S1; S[1] = S2; S[2] = S3;
   T[0] = T1; T[1] = T2; T[2] = T3;
 
-  return triDistance(S, T, P, Q);
+  return sqrTriDistance (S, T, P, Q);
 }
 
-FCL_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3],
-                                       const Matrix3f& R, const Vec3f& Tl,
-                                       Vec3f& P, Vec3f& Q)
+FCL_REAL TriangleDistance::sqrTriDistance (const Vec3f S[3], const Vec3f T[3],
+					   const Matrix3f& R, const Vec3f& Tl,
+					   Vec3f& P, Vec3f& Q)
 {
   Vec3f T_transformed[3];
   T_transformed[0] = R * T[0] + Tl;
   T_transformed[1] = R * T[1] + Tl;
   T_transformed[2] = R * T[2] + Tl;
 
-  return triDistance(S, T_transformed, P, Q);
+  return sqrTriDistance (S, T_transformed, P, Q);
 }
 
 
-FCL_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3],
-                                       const Transform3f& tf,
-                                       Vec3f& P, Vec3f& Q)
+FCL_REAL TriangleDistance::sqrTriDistance (const Vec3f S[3], const Vec3f T[3],
+					   const Transform3f& tf,
+					   Vec3f& P, Vec3f& Q)
 {
   Vec3f T_transformed[3];
   T_transformed[0] = tf.transform(T[0]);
   T_transformed[1] = tf.transform(T[1]);
   T_transformed[2] = tf.transform(T[2]);
 
-  return triDistance(S, T_transformed, P, Q);
+  return sqrTriDistance (S, T_transformed, P, Q);
 }
 
 
-FCL_REAL TriangleDistance::triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
-                                       const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
-                                       const Matrix3f& R, const Vec3f& Tl,
-                                       Vec3f& P, Vec3f& Q)
+FCL_REAL TriangleDistance::sqrTriDistance
+(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
+ const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
+ const Matrix3f& R, const Vec3f& Tl,
+ Vec3f& P, Vec3f& Q)
 {
   Vec3f T1_transformed = R * T1 + Tl;
   Vec3f T2_transformed = R * T2 + Tl;
   Vec3f T3_transformed = R * T3 + Tl;
-  return triDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q);
+  return sqrTriDistance (S1, S2, S3, T1_transformed, T2_transformed,
+			 T3_transformed, P, Q);
 }
 
-FCL_REAL TriangleDistance::triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
-                                       const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
-                                       const Transform3f& tf,
-                                       Vec3f& P, Vec3f& Q)
+FCL_REAL TriangleDistance::sqrTriDistance
+(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
+ const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
+ const Transform3f& tf,
+ Vec3f& P, Vec3f& Q)
 {
   Vec3f T1_transformed = tf.transform(T1);
   Vec3f T2_transformed = tf.transform(T2);
   Vec3f T3_transformed = tf.transform(T3);
-  return triDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q);
+  return sqrTriDistance (S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q);
 }
 
 

src/traversal/traversal_node_bvhs.cpp

@@ -167,9 +167,8 @@ static inline void meshDistanceOrientedNodeLeafTesting(int b1, int b2,
   // nearest point pair
   Vec3f P1, P2;
 
-  FCL_REAL d = TriangleDistance::triDistance(t11, t12, t13, t21, t22, t23,
-                                             R, T,
-                                             P1, P2);
+  FCL_REAL d = sqrt (TriangleDistance::sqrTriDistance
+		     (t11, t12, t13, t21, t22, t23, R, T, P1, P2));
 
   if(request.enable_nearest_points)
     result.update(d, model1, model2, primitive_id1, primitive_id2, P1, P2);
@@ -336,9 +335,11 @@ static inline void distancePreprocessOrientedNode(const BVHModel<BV>* model1, co
   init_tri2_points[2] = vertices2[init_tri2[2]];
 
   Vec3f p1, p2;
-  FCL_REAL distance = TriangleDistance::triDistance(init_tri1_points[0], init_tri1_points[1], init_tri1_points[2],
-                                                    init_tri2_points[0], init_tri2_points[1], init_tri2_points[2],
-                                                    R, T, p1, p2);
+  FCL_REAL distance = sqrt (TriangleDistance::sqrTriDistance
+			    (init_tri1_points[0], init_tri1_points[1],
+			     init_tri1_points[2], init_tri2_points[0],
+			     init_tri2_points[1], init_tri2_points[2],
+			     R, T, p1, p2));
 
   if(request.enable_nearest_points)
     result.update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2);
@@ -563,9 +564,8 @@ void meshConservativeAdvancementOrientedNodeLeafTesting(int b1, int b2,
   // nearest point pair
   Vec3f P1, P2;
 
-  FCL_REAL d = TriangleDistance::triDistance(t11, t12, t13, t21, t22, t23,
-                                             R, T,
-                                             P1, P2);
+  FCL_REAL d = sqrt (TriangleDistance::sqrTriDistance
+		     (t11, t12, t13, t21, t22, t23, R, T, P1, P2));
 
   if(d < min_distance)
   {