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,24 +532,24 @@ 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) {
         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<T>::segment_to_segment_dist(const Vector3<T>& 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<T>::segment_to_segment_dist(const Vector3<T>& 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<T>::segment_to_segment_dist(const Vector3<T>& 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<T> 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 <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" 
-    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;
+    // 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<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;