summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--core/math/geometry_3d.cpp105
-rw-r--r--core/math/geometry_3d.h92
-rw-r--r--tests/core/math/test_geometry_3d.h2
3 files changed, 108 insertions, 91 deletions
diff --git a/core/math/geometry_3d.cpp b/core/math/geometry_3d.cpp
index ec96753c79..9238293b48 100644
--- a/core/math/geometry_3d.cpp
+++ b/core/math/geometry_3d.cpp
@@ -35,6 +35,111 @@
#include "thirdparty/misc/clipper.hpp"
#include "thirdparty/misc/polypartition.h"
+void Geometry3D::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.
+
+ 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;
+ }
+ }
+
+ r_ps = (1 - s) * p_p0 + s * p_p1;
+ r_qt = (1 - t) * p_q0 + t * p_q1;
+}
+
+real_t Geometry3D::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 Geometry3D::MeshData::optimize_vertices() {
HashMap<int, int> vtx_remap;
diff --git a/core/math/geometry_3d.h b/core/math/geometry_3d.h
index 59c56906f4..e5ace9db72 100644
--- a/core/math/geometry_3d.h
+++ b/core/math/geometry_3d.h
@@ -37,96 +37,8 @@
class Geometry3D {
public:
- 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 it's a 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.lerp(p2, mua);
- c2 = q1.lerp(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.0f; // Force using point P0 on segment S1
- sD = 1.0f; // 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.0f) { // sc < 0 => the s=0 edge is visible.
- sN = 0.0f;
- 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.0f) { // tc < 0 => the t=0 edge is visible.
- tN = 0.0f;
- // Recompute sc for this edge.
- if (-d < 0.0f) {
- sN = 0.0f;
- } 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.0f) {
- 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.0f : sN / sD);
- tc = (Math::is_zero_approx(tN) ? 0.0f : 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;
diff --git a/tests/core/math/test_geometry_3d.h b/tests/core/math/test_geometry_3d.h
index 99a4ef2d46..23bbf1e183 100644
--- a/tests/core/math/test_geometry_3d.h
+++ b/tests/core/math/test_geometry_3d.h
@@ -63,7 +63,7 @@ TEST_CASE("[Geometry3D] Closest Distance Between Segments") {
p_1(p_p_1), p_2(p_p_2), p_3(p_p_3), p_4(p_p_4), want(p_want){};
};
Vector<Case> tt;
- tt.push_back(Case(Vector3(1, -2, 0), Vector3(1, 2, 0), Vector3(-1, 2, 0), Vector3(-1, -2, 0), 0.0f));
+ tt.push_back(Case(Vector3(1, -2, 0), Vector3(1, 2, 0), Vector3(-1, 2, 0), Vector3(-1, -2, 0), 2.0f));
for (int i = 0; i < tt.size(); ++i) {
Case current_case = tt[i];
float out = Geometry3D::get_closest_distance_between_segments(current_case.p_1, current_case.p_2, current_case.p_3, current_case.p_4);
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-22 01:19:35 +0000