From 226e29cfcaf1fd83608345e1c2ca3b0f7a170487 Mon Sep 17 00:00:00 2001 From: Marcel Admiraal Date: Fri, 3 Dec 2021 18:43:49 +0000 Subject: [PATCH] Fix Geometry::get_closest_points_between_segments() returns NaN Also fix Geometry::get_closest_distance_between_segments() returning incorrect values. --- core/math/geometry.cpp | 111 +++++++++++++++++++++++++++++++++++++---- core/math/geometry.h | 92 +--------------------------------- 2 files changed, 103 insertions(+), 100 deletions(-) diff --git a/core/math/geometry.cpp b/core/math/geometry.cpp index 88c3f1587..4a92c6637 100644 --- a/core/math/geometry.cpp +++ b/core/math/geometry.cpp @@ -40,19 +40,110 @@ #define SCALE_FACTOR 100000.0 // Based on CMP_EPSILON. -// This implementation is very inefficient, commenting unless bugs happen. See the other one. -/* -bool Geometry::is_point_in_polygon(const Vector2 &p_point, const Vector &p_polygon) { +void Geometry::get_closest_points_between_segments(const Vector3 &p_p0, const Vector3 &p_p1, const Vector3 &p_q0, const Vector3 &p_q1, Vector3 &r_ps, Vector3 &r_qt) { + // Based on David Eberly's Computation of Distance Between Line Segments algorithm. - Vector indices = Geometry::triangulate_polygon(p_polygon); - for (int j = 0; j + 3 <= indices.size(); j += 3) { - int i1 = indices[j], i2 = indices[j + 1], i3 = indices[j + 2]; - if (Geometry::is_point_in_triangle(p_point, p_polygon[i1], p_polygon[i2], p_polygon[i3])) - return true; + Vector3 p = p_p1 - p_p0; + Vector3 q = p_q1 - p_q0; + Vector3 r = p_p0 - p_q0; + + real_t a = p.dot(p); + real_t b = p.dot(q); + real_t c = q.dot(q); + real_t d = p.dot(r); + real_t e = q.dot(r); + + real_t s = 0.0f; + real_t t = 0.0f; + + real_t det = a * c - b * b; + if (det > CMP_EPSILON) { + // Non-parallel segments + real_t bte = b * e; + real_t ctd = c * d; + + if (bte <= ctd) { + // s <= 0.0f + if (e <= 0.0f) { + // t <= 0.0f + s = (-d >= a ? 1 : (-d > 0.0f ? -d / a : 0.0f)); + t = 0.0f; + } else if (e < c) { + // 0.0f < t < 1 + s = 0.0f; + t = e / c; + } else { + // t >= 1 + s = (b - d >= a ? 1 : (b - d > 0.0f ? (b - d) / a : 0.0f)); + t = 1; + } + } else { + // s > 0.0f + s = bte - ctd; + if (s >= det) { + // s >= 1 + if (b + e <= 0.0f) { + // t <= 0.0f + s = (-d <= 0.0f ? 0.0f : (-d < a ? -d / a : 1)); + t = 0.0f; + } else if (b + e < c) { + // 0.0f < t < 1 + s = 1; + t = (b + e) / c; + } else { + // t >= 1 + s = (b - d <= 0.0f ? 0.0f : (b - d < a ? (b - d) / a : 1)); + t = 1; + } + } else { + // 0.0f < s < 1 + real_t ate = a * e; + real_t btd = b * d; + + if (ate <= btd) { + // t <= 0.0f + s = (-d <= 0.0f ? 0.0f : (-d >= a ? 1 : -d / a)); + t = 0.0f; + } else { + // t > 0.0f + t = ate - btd; + if (t >= det) { + // t >= 1 + s = (b - d <= 0.0f ? 0.0f : (b - d >= a ? 1 : (b - d) / a)); + t = 1; + } else { + // 0.0f < t < 1 + s /= det; + t /= det; + } + } + } + } + } else { + // Parallel segments + if (e <= 0.0f) { + s = (-d <= 0.0f ? 0.0f : (-d >= a ? 1 : -d / a)); + t = 0.0f; + } else if (e >= c) { + s = (b - d <= 0.0f ? 0.0f : (b - d >= a ? 1 : (b - d) / a)); + t = 1; + } else { + s = 0.0f; + t = e / c; + } } - return false; + + r_ps = (1 - s) * p_p0 + s * p_p1; + r_qt = (1 - t) * p_q0 + t * p_q1; +} + +real_t Geometry::get_closest_distance_between_segments(const Vector3 &p_p0, const Vector3 &p_p1, const Vector3 &p_q0, const Vector3 &p_q1) { + Vector3 ps; + Vector3 qt; + get_closest_points_between_segments(p_p0, p_p1, p_q0, p_q1, ps, qt); + Vector3 st = qt - ps; + return st.length(); } -*/ void Geometry::OccluderMeshData::clear() { faces.clear(); diff --git a/core/math/geometry.h b/core/math/geometry.h index c22b11825..78c923cc3 100644 --- a/core/math/geometry.h +++ b/core/math/geometry.h @@ -101,96 +101,8 @@ public: return Math::sqrt((c1 - c2).dot(c1 - c2)); } - static void get_closest_points_between_segments(const Vector3 &p1, const Vector3 &p2, const Vector3 &q1, const Vector3 &q2, Vector3 &c1, Vector3 &c2) { -// Do the function 'd' as defined by pb. I think is is dot product of some sort. -#define d_of(m, n, o, p) ((m.x - n.x) * (o.x - p.x) + (m.y - n.y) * (o.y - p.y) + (m.z - n.z) * (o.z - p.z)) - - // Calculate the parametric position on the 2 curves, mua and mub. - real_t mua = (d_of(p1, q1, q2, q1) * d_of(q2, q1, p2, p1) - d_of(p1, q1, p2, p1) * d_of(q2, q1, q2, q1)) / (d_of(p2, p1, p2, p1) * d_of(q2, q1, q2, q1) - d_of(q2, q1, p2, p1) * d_of(q2, q1, p2, p1)); - real_t mub = (d_of(p1, q1, q2, q1) + mua * d_of(q2, q1, p2, p1)) / d_of(q2, q1, q2, q1); - - // Clip the value between [0..1] constraining the solution to lie on the original curves. - if (mua < 0) { - mua = 0; - } - if (mub < 0) { - mub = 0; - } - if (mua > 1) { - mua = 1; - } - if (mub > 1) { - mub = 1; - } - c1 = p1.linear_interpolate(p2, mua); - c2 = q1.linear_interpolate(q2, mub); - } - - static real_t get_closest_distance_between_segments(const Vector3 &p_from_a, const Vector3 &p_to_a, const Vector3 &p_from_b, const Vector3 &p_to_b) { - Vector3 u = p_to_a - p_from_a; - Vector3 v = p_to_b - p_from_b; - Vector3 w = p_from_a - p_to_a; - real_t a = u.dot(u); // Always >= 0 - real_t b = u.dot(v); - real_t c = v.dot(v); // Always >= 0 - real_t d = u.dot(w); - real_t e = v.dot(w); - real_t D = a * c - b * b; // Always >= 0 - real_t sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0 - real_t tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0 - - // Compute the line parameters of the two closest points. - if (D < (real_t)CMP_EPSILON) { // The lines are almost parallel. - sN = 0; // Force using point P0 on segment S1 - sD = 1; // to prevent possible division by 0.0 later. - tN = e; - tD = c; - } else { // Get the closest points on the infinite lines - sN = (b * e - c * d); - tN = (a * e - b * d); - if (sN < 0) { // sc < 0 => the s=0 edge is visible. - sN = 0; - tN = e; - tD = c; - } else if (sN > sD) { // sc > 1 => the s=1 edge is visible. - sN = sD; - tN = e + b; - tD = c; - } - } - - if (tN < 0) { // tc < 0 => the t=0 edge is visible. - tN = 0; - // Recompute sc for this edge. - if (-d < 0) { - sN = 0; - } else if (-d > a) { - sN = sD; - } else { - sN = -d; - sD = a; - } - } else if (tN > tD) { // tc > 1 => the t=1 edge is visible. - tN = tD; - // Recompute sc for this edge. - if ((-d + b) < 0) { - sN = 0; - } else if ((-d + b) > a) { - sN = sD; - } else { - sN = (-d + b); - sD = a; - } - } - // Finally do the division to get sc and tc. - sc = (Math::is_zero_approx(sN) ? 0 : sN / sD); - tc = (Math::is_zero_approx(tN) ? 0 : tN / tD); - - // Get the difference of the two closest points. - Vector3 dP = w + (sc * u) - (tc * v); // = S1(sc) - S2(tc) - - return dP.length(); // Return the closest distance. - } + static void get_closest_points_between_segments(const Vector3 &p_p0, const Vector3 &p_p1, const Vector3 &p_q0, const Vector3 &p_q1, Vector3 &r_ps, Vector3 &r_qt); + static real_t get_closest_distance_between_segments(const Vector3 &p_p0, const Vector3 &p_p1, const Vector3 &p_q0, const Vector3 &p_q1); static inline bool ray_intersects_triangle(const Vector3 &p_from, const Vector3 &p_dir, const Vector3 &p_v0, const Vector3 &p_v1, const Vector3 &p_v2, Vector3 *r_res = nullptr) { Vector3 e1 = p_v1 - p_v0;