Commit 8d9badbc authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Replace TriangleDistance::triDistance by sqrTriDistance.

  sqrt should be applied after method call.
parent d9275791
......@@ -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);
};
......
......@@ -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)
{
......
......@@ -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);
}
......
......@@ -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)
{
......
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