From 0779cf436ec6267670ae59946c5276b1bfb966cf Mon Sep 17 00:00:00 2001
From: Rishabh <f20171602@hyderabad.bits-pilani.ac.in>
Date: Mon, 8 Jun 2020 12:30:27 +0530
Subject: [PATCH] AP_Math: Added function to calculate shortest distance
 betwwen point and line segment in 3D

---
 libraries/AP_Math/vector3.cpp | 28 +++++++++++++++++++++++++++-
 libraries/AP_Math/vector3.h   |  2 ++
 2 files changed, 29 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Math/vector3.cpp b/libraries/AP_Math/vector3.cpp
index a16deabd78..d7b651ff9c 100644
--- a/libraries/AP_Math/vector3.cpp
+++ b/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);
 
diff --git a/libraries/AP_Math/vector3.h b/libraries/AP_Math/vector3.h
index 0cd80d4e4a..0760c9fa15 100644
--- a/libraries/AP_Math/vector3.h
+++ b/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;