From 8d9badbc1d01498d5d50d96ee74e24e35dc323f9 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Tue, 19 Aug 2014 11:55:17 +0200
Subject: [PATCH] Replace TriangleDistance::triDistance by sqrTriDistance.
sqrt should be applied after method call.
---
include/fcl/intersect.h | 111 ++++++++++++++------
include/fcl/traversal/traversal_node_bvhs.h | 8 +-
src/intersect.cpp | 58 +++++-----
src/traversal/traversal_node_bvhs.cpp | 18 ++--
4 files changed, 121 insertions(+), 74 deletions(-)
diff --git a/include/fcl/intersect.h b/include/fcl/intersect.h
index ac5153f8..1149d42f 100644
--- a/include/fcl/intersect.h
+++ b/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);
};
diff --git a/include/fcl/traversal/traversal_node_bvhs.h b/include/fcl/traversal/traversal_node_bvhs.h
index 7ef99756..dd54045e 100644
--- a/include/fcl/traversal/traversal_node_bvhs.h
+++ b/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)
{
diff --git a/src/intersect.cpp b/src/intersect.cpp
index 66bd4a8f..6a08069b 100644
--- a/src/intersect.cpp
+++ b/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);
}
diff --git a/src/traversal/traversal_node_bvhs.cpp b/src/traversal/traversal_node_bvhs.cpp
index 72cfc5e8..4e44ced4 100644
--- a/src/traversal/traversal_node_bvhs.cpp
+++ b/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)
{
--
GitLab