AP_Math: Add segment to plane intersection function

libraries/AP_Math/vector3.cpp

@@ -512,12 +512,12 @@ Vector3<T> Vector3<T>::point_on_line_closest_to_other_point(const Vector3<T> &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 <typename T>
-float Vector3<T>::segment_to_segment_dist(const Vector3<T>& seg1_start, const Vector3<T>& seg1_end, const Vector3<T>& seg2_start, const Vector3<T>& seg2_end, Vector3<T>& intersection)
+void Vector3<T>::segment_to_segment_closest_point(const Vector3<T>& seg1_start, const Vector3<T>& seg1_end, const Vector3<T>& seg2_start, const Vector3<T>& seg2_end, Vector3<T>& closest_point)
 {
     // direction vectors
     const Vector3<T> line1 = seg1_end - seg1_start;
@@ -532,7 +532,7 @@ float Vector3<T>::segment_to_segment_dist(const Vector3<T>& 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) {
@@ -582,14 +582,39 @@ float Vector3<T>::segment_to_segment_dist(const Vector3<T>& seg1_start, const Ve
             sD = a;
         }
     }
-    // finally do the division to get sc and tc
+    // finally do the division to get tc
-    sc = (fabsf(sN) < FLT_EPSILON ? 0.0 : sN / sD);
     tc = (fabsf(tN) < FLT_EPSILON ? 0.0 : tN / tD);
 
-    const Vector3<T> closest_line_segment = diff + (line1*sc) - (line2*tc);
+    // closest point on seg2
-    const float len = closest_line_segment.length();
+    closest_point = seg2_start + line2*tc;
-    intersection = seg2_start + line2*tc;
+}
-    return len;
+
+// Returns true if the passed 3D segment passes through a plane defined by plane normal, and a point on the plane
+template <typename T>
+bool Vector3<T>::segment_plane_intersect(const Vector3<T>& seg_start, const Vector3<T>& seg_end, const Vector3<T>& plane_normal, const Vector3<T>& plane_point)
+{
+    Vector3<T> u = seg_end - seg_start;
+    Vector3<T> 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

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" 
+    // OUTPUT: closest point on segment 2, from segment 1, gets passed on reference as "closest_point"
-    static float segment_to_segment_dist(const Vector3<T>& seg1_start, const Vector3<T>& seg1_end, const Vector3<T>& seg2_start, const Vector3<T>& seg2_end, Vector3<T>& intersection) WARN_IF_UNUSED;
+    static void segment_to_segment_closest_point(const Vector3<T>& seg1_start, const Vector3<T>& seg1_end, const Vector3<T>& seg2_start, const Vector3<T>& seg2_end, Vector3<T>& 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<T>& seg_start, const Vector3<T>& seg_end, const Vector3<T>& plane_normal, const Vector3<T>& plane_point);
 };
 
 typedef Vector3<int16_t>                Vector3i;