From 547f0efd5752a8867e9cb0429c23e4b62899da7b Mon Sep 17 00:00:00 2001 From: Rishabh Date: Thu, 27 May 2021 00:33:22 +0530 Subject: [PATCH] AP_Math: Add segment to plane intersection function --- libraries/AP_Math/vector3.cpp | 57 +++++++++++++++++++++++++---------- libraries/AP_Math/vector3.h | 7 +++-- 2 files changed, 46 insertions(+), 18 deletions(-) diff --git a/libraries/AP_Math/vector3.cpp b/libraries/AP_Math/vector3.cpp index 9ad63f94a1..71ffc08944 100644 --- a/libraries/AP_Math/vector3.cpp +++ b/libraries/AP_Math/vector3.cpp @@ -512,12 +512,12 @@ Vector3 Vector3::point_on_line_closest_to_other_point(const Vector3 &w1 return (closest_point + w1); } -// Shortest distance between two line segments +// Closest point between two line segments // This implementation is borrowed from: http://geomalgorithms.com/a07-_distance.html // INPUT: 4 points corresponding to start and end of two line segments -// OUTPUT: shortest distance, and closest point on segment 2, from segment 1, gets passed on reference as "intersection" +// OUTPUT: closest point on segment 2, from segment 1, gets passed on reference as "closest_point" template -float Vector3::segment_to_segment_dist(const Vector3& seg1_start, const Vector3& seg1_end, const Vector3& seg2_start, const Vector3& seg2_end, Vector3& intersection) +void Vector3::segment_to_segment_closest_point(const Vector3& seg1_start, const Vector3& seg1_end, const Vector3& seg2_start, const Vector3& seg2_end, Vector3& closest_point) { // direction vectors const Vector3 line1 = seg1_end - seg1_start; @@ -532,24 +532,24 @@ float Vector3::segment_to_segment_dist(const Vector3& seg1_start, const Ve const float e = line2*diff; const float discriminant = (a*c) - (b*b); - float sc, sN, sD = discriminant; // sc = sN / sD, default sD = D >= 0 + float sN, sD = discriminant; // default sD = D >= 0 float tc, tN, tD = discriminant; // tc = tN / tD, default tD = D >= 0 - + if (discriminant < FLT_EPSILON) { sN = 0.0; // force using point seg1_start on line 1 sD = 1.0; // to prevent possible division by 0.0 later tN = e; tD = c; - } else { + } else { // get the closest points on the infinite lines sN = (b*e - c*d); tN = (a*e - b*d); - if (sN < 0.0) { + if (sN < 0.0) { // sc < 0 => the s=0 edge is visible sN = 0.0; tN = e; tD = c; - } else if (sN > sD) { + } else if (sN > sD) { // sc > 1 => the s=1 edge is visible sN = sD; tN = e + b; @@ -557,7 +557,7 @@ float Vector3::segment_to_segment_dist(const Vector3& seg1_start, const Ve } } - if (tN < 0.0) { + if (tN < 0.0) { // tc < 0 => the t=0 edge is visible tN = 0.0; // recompute sc for this edge @@ -569,7 +569,7 @@ float Vector3::segment_to_segment_dist(const Vector3& seg1_start, const Ve sN = -d; sD = a; } - } else if (tN > tD) { + } else if (tN > tD) { // tc > 1 => the t=1 edge is visible tN = tD; // recompute sc for this edge @@ -582,14 +582,39 @@ float Vector3::segment_to_segment_dist(const Vector3& seg1_start, const Ve sD = a; } } - // finally do the division to get sc and tc - sc = (fabsf(sN) < FLT_EPSILON ? 0.0 : sN / sD); + // finally do the division to get tc tc = (fabsf(tN) < FLT_EPSILON ? 0.0 : tN / tD); - const Vector3 closest_line_segment = diff + (line1*sc) - (line2*tc); - const float len = closest_line_segment.length(); - intersection = seg2_start + line2*tc; - return len; + // closest point on seg2 + closest_point = seg2_start + line2*tc; +} + +// Returns true if the passed 3D segment passes through a plane defined by plane normal, and a point on the plane +template +bool Vector3::segment_plane_intersect(const Vector3& seg_start, const Vector3& seg_end, const Vector3& plane_normal, const Vector3& plane_point) +{ + Vector3 u = seg_end - seg_start; + Vector3 w = seg_start - plane_point; + + float D = plane_normal * u; + float N = -(plane_normal * w); + + if (fabsf(D) < FLT_EPSILON) { + if (::is_zero(N)) { + // segment lies in this plane + return true; + } else { + // does not intersect + return false; + } + } + const float sI = N / D; + if (sI < 0 || sI > 1) { + // does not intersect + return false; + } + // intersects at unique point + return true; } // define for float and double diff --git a/libraries/AP_Math/vector3.h b/libraries/AP_Math/vector3.h index f6d05ae2ab..751303dadd 100644 --- a/libraries/AP_Math/vector3.h +++ b/libraries/AP_Math/vector3.h @@ -277,8 +277,11 @@ public: // This implementation is borrowed from: http://geomalgorithms.com/a07-_distance.html // INPUT: 4 points corresponding to start and end of two line segments - // OUTPUT: shortest distance between segments, and closest point on segment 2, from segment 1, gets passed on reference as "intersection" - static float segment_to_segment_dist(const Vector3& seg1_start, const Vector3& seg1_end, const Vector3& seg2_start, const Vector3& seg2_end, Vector3& intersection) WARN_IF_UNUSED; + // OUTPUT: closest point on segment 2, from segment 1, gets passed on reference as "closest_point" + static void segment_to_segment_closest_point(const Vector3& seg1_start, const Vector3& seg1_end, const Vector3& seg2_start, const Vector3& seg2_end, Vector3& closest_point); + + // Returns true if the passed 3D segment passes through a plane defined by plane normal, and a point on the plane + static bool segment_plane_intersect(const Vector3& seg_start, const Vector3& seg_end, const Vector3& plane_normal, const Vector3& plane_point); }; typedef Vector3 Vector3i;