Method distance_to_segment

libraries/AP_Math/vector3.cpp:468–490

// distance from the tip of this vector to a line segment specified by two vectors
template <typename T>
T Vector3<T>::distance_to_segment(const Vector3<T> &seg_start, const Vector3<T> &seg_end) const
{
    // triangle side lengths
    const T a = (*this-seg_start).length();
    const T b = (seg_start-seg_end).length();
    const T c = (seg_end-*this).length();

    // protect against divide by zero later
    if (::is_zero(b)) {
        return 0.0f;
    }

    // semiperimeter of triangle
    const T s = (a+b+c) * 0.5f;

    T 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;
    }
    const T area = safe_sqrt(area_squared);
    return 2.0f*area/b;
}

// Shortest distance between point(p) to a point contained in the line segment defined by w1,w2
template <typename T>