AP_Math: add Vector3f::distance_to_segment

libraries/AP_Math/vector3.cpp

@@ -390,6 +390,32 @@ Matrix3<T> Vector3<T>::mul_rowcol(const Vector3<T> &v2) const
                       v1.z * v2.x, v1.z * v2.y, v1.z * v2.z);
 }
 
+// distance from the tip of this vector to a line segment specified by two vectors
+template <typename T>
+float Vector3<T>::distance_to_segment(const Vector3<T> &seg_start, const Vector3<T> &seg_end) const
+{
+    // triangle side lengths
+    float a = (*this-seg_start).length();
+    float b = (seg_start-seg_end).length();
+    float c = (seg_end-*this).length();
+
+    // protect against divide by zero later
+    if (fabsf(b) < FLT_EPSILON) {
+        return 0.0f;
+    }
+
+    // semiperimeter of triangle
+    float s = (a+b+c)/2.0f;
+
+    float area_squared = s*(s-a)*(s-b)*(s-c);
+    // area must be constrained above 0 because a triangle could have 3 points could be on a line and float rounding could push this under 0
+    if (area_squared < 0.0f) {
+        area_squared = 0.0f;
+    }
+    float area = safe_sqrt(area_squared);
+    return 2.0f*area/b;
+}
+
 // define for float
 template void Vector3<float>::rotate(enum Rotation);
 template void Vector3<float>::rotate_inverse(enum Rotation);
@@ -412,6 +438,7 @@ template bool Vector3<float>::operator !=(const Vector3<float> &v) const;
 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;
 
 // define needed ops for Vector3l
 template Vector3<int32_t> &Vector3<int32_t>::operator +=(const Vector3<int32_t> &v);

libraries/AP_Math/vector3.h

@@ -218,6 +218,9 @@ public:
         return (dist_x*dist_x + dist_y*dist_y + dist_z*dist_z);
     }
 
+    // distance from the tip of this vector to a line segment specified by two vectors
+    float distance_to_segment(const Vector3<T> &seg_start, const Vector3<T> &seg_end) const;
+
     // given a position p1 and a velocity v1 produce a vector
     // perpendicular to v1 maximising distance from p1.  If p1 is the
     // zero vector the return from the function will always be the