|
@@ -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;
|