AP_Math: Added function to calculate shortest distance betwwen point and line segment in 3D

libraries/AP_Math/vector3.cpp

@@ -444,6 +444,32 @@ float Vector3<T>::distance_to_segment(const Vector3<T> &seg_start, const Vector3
     return 2.0f*area/b;
 }
 
+// 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> line_vec = w2-w1;
+    const Vector3<T> p_vec = p - w1;
+    
+    const float line_vec_len = line_vec.length();
+    // protection against divide by zero
+    if(::is_zero(line_vec_len)) {
+        return 0.0f;
+    }
+
+    const float scale = 1/line_vec_len;
+    const Vector3<T> unit_vec = line_vec * scale;
+    const Vector3<T> scaled_p_vec = p_vec * scale;
+
+    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;
+}
+
 // define for float
 template void Vector3<float>::rotate(enum Rotation);
 template void Vector3<float>::rotate_inverse(enum Rotation);
@@ -467,7 +493,7 @@ template bool Vector3<float>::is_nan(void) const;
 template bool Vector3<float>::is_inf(void) const;
 template float Vector3<float>::angle(const Vector3<float> &v) const;
 template float Vector3<float>::distance_to_segment(const Vector3<float> &seg_start, const Vector3<float> &seg_end) const;
-
+template float Vector3<float>::closest_distance_between_line_and_point(const Vector3<float> &w1, const Vector3<float> &w2, const Vector3<float> &p);
 // define needed ops for Vector3l
 template Vector3<int32_t> &Vector3<int32_t>::operator +=(const Vector3<int32_t> &v);
 

libraries/AP_Math/vector3.h

@@ -256,6 +256,8 @@ public:
         return perpendicular;
     }
 
+    // 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);
 };
 
 typedef Vector3<int16_t>                Vector3i;