Math: move closest_point in from AC_Avoid

libraries/AP_Math/vector2.h

@@ -176,18 +176,43 @@ struct Vector2
         return perpendicular2;
     }
 
-    // thanks to grumdrig (http://stackoverflow.com/questions/849211/shortest-distance-between-a-point-and-a-line-segment)
+    /*
+     * Returns the point closest to p on the line segment (v,w).
+     *
+     * Comments and implementation taken from
+     * http://stackoverflow.com/questions/849211/shortest-distance-between-a-point-and-a-line-segment
+     */
+    static Vector2<T> closest_point(const Vector2<T> &p, const Vector2<T> &v, const Vector2<T> &w)
+    {
+        // length squared of line segment
+        const float l2 = (v - w).length_squared();
+        if (l2 < FLT_EPSILON) {
+            // v == w case
+            return v;
+        }
+        // Consider the line extending the segment, parameterized as v + t (w - v).
+        // We find projection of point p onto the line.
+        // It falls where t = [(p-v) . (w-v)] / |w-v|^2
+        // We clamp t from [0,1] to handle points outside the segment vw.
+        const float t = ((p - v) * (w - v)) / l2;
+        if (t <= 0) {
+            return v;
+        } else if (t >= 1) {
+            return w;
+        } else {
+            return v + (w - v)*t;
+        }
+    }
+
     // w defines a line segment from the origin
     // p is a point
     // returns the closest distance between the radial and the point
     static float closest_distance_between_radial_and_point(const Vector2<T> &w,
                                                            const Vector2<T> &p)
     {
-        const float len = w.length_squared();
+        const Vector2<T> closest = closest_point(p, Vector2<T>(0,0), w);
-        if (len < FLT_EPSILON) return p.length();
+        const Vector2<T> delta = closest - p;
-        const float t = fmax(0, fmin(1, (p*w) / len));
+        return delta.length();
-        const Vector2<T> projection = w * t;
-        return (p-projection).length();
     }
 
 };