### 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* 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!