1 files changed, 105 insertions, 0 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;