AP_Math: Add new methods to support 3D avoidance

libraries/AP_Math/vector3.cpp

@@ -454,9 +454,18 @@ float Vector3<T>::distance_to_segment(const Vector3<T> &seg_start, const Vector3
 }
 
 // Shortest distance between point(p) to a point contained in the line segment defined by w1,w2
-// this is based on the explanation given here: www.fundza.com/vectors/point2line/index.html
 template <typename T>
 float Vector3<T>::closest_distance_between_line_and_point(const Vector3<T> &w1, const Vector3<T> &w2, const Vector3<T> &p)
+{    
+    const Vector3<T> nearest = point_on_line_closest_to_other_point(w1, w2, p);
+    const float dist = (nearest - p).length();
+    return dist;
+}
+
+// Point in the line segment defined by w1,w2 which is closest to point(p)
+// this is based on the explanation given here: www.fundza.com/vectors/point2line/index.html
+template <typename T>
+Vector3<T> Vector3<T>::point_on_line_closest_to_other_point(const Vector3<T> &w1, const Vector3<T> &w2, const Vector3<T> &p)
 {   
     const Vector3<T> line_vec = w2-w1;
     const Vector3<T> p_vec = p - w1;
@@ -464,7 +473,7 @@ float Vector3<T>::closest_distance_between_line_and_point(const Vector3<T> &w1,
     const float line_vec_len = line_vec.length();
     // protection against divide by zero
     if(::is_zero(line_vec_len)) {
-        return 0.0f;
+        return {0.0f, 0.0f, 0.0f};
     }
 
     const float scale = 1/line_vec_len;
@@ -474,9 +483,88 @@ float Vector3<T>::closest_distance_between_line_and_point(const Vector3<T> &w1,
     float dot_product = unit_vec * scaled_p_vec;
     dot_product = constrain_float(dot_product,0.0f,1.0f); 
  
-    const Vector3<T> nearest = line_vec * dot_product;
-    const float dist = (nearest - p_vec).length();
-    return dist;
+    const Vector3<T> closest_point = line_vec * dot_product;
+    return (closest_point + w1);
+}
+
+// Shortest distance 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" 
+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)
+{
+    // direction vectors
+    const Vector3<T> line1 = seg1_end - seg1_start;
+    const Vector3<T> line2 = seg2_end - seg2_start;
+
+    const Vector3<T> diff = seg1_start - seg2_start;
+
+    const float a = line1*line1;
+    const float b = line1*line2;
+    const float c = line2*line2;
+    const float d = line1*diff;
+    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 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 {                 
+        // get the closest points on the infinite lines
+        sN = (b*e - c*d);
+        tN = (a*e - b*d);
+        if (sN < 0.0) {        
+            // sc < 0 => the s=0 edge is visible
+            sN = 0.0;
+            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.0) {            
+        // tc < 0 => the t=0 edge is visible
+        tN = 0.0;
+        // recompute sc for this edge
+        if (-d < 0.0) {
+            sN = 0.0;
+        } 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.0) {
+            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 = (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);
+    const float len = closest_line_segment.length();
+    intersection = seg2_start + line2*tc;
+    return len;
 }
 
 // define for float and double

libraries/AP_Math/vector3.h

@@ -265,6 +265,14 @@ public:
 
     // Shortest distance between point(p) to a point contained in the line segment defined by w1,w2
     static float closest_distance_between_line_and_point(const Vector3<T> &w1, const Vector3<T> &w2, const Vector3<T> &p);
+
+    // Point in the line segment defined by w1,w2 which is closest to point(p)
+    static Vector3<T> point_on_line_closest_to_other_point(const Vector3<T> &w1, const Vector3<T> &w2, const Vector3<T> &p);
+
+    // 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;
 };
 
 typedef Vector3<int16_t>                Vector3i;