From dbf337e1b3e63cac7b023016e1dbe22f5815bd1e Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 29 May 2019 20:46:09 +0900
Subject: [PATCH] AP_Math: move closest_point to cpp

---
 libraries/AP_Math/vector2.cpp | 30 ++++++++++++++++++++++++++++++
 libraries/AP_Math/vector2.h   | 22 +---------------------
 2 files changed, 31 insertions(+), 21 deletions(-)

diff --git a/libraries/AP_Math/vector2.cpp b/libraries/AP_Math/vector2.cpp
index 093e230a95..2518d678c2 100644
--- a/libraries/AP_Math/vector2.cpp
+++ b/libraries/AP_Math/vector2.cpp
@@ -300,6 +300,35 @@ Vector2<T> Vector2<T>::perpendicular(const Vector2<T> &pos_delta, const Vector2<
     return perpendicular2;
 }
 
+/*
+ * 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
+ */
+template <typename T>
+Vector2<T> 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;
+    }
+}
+
 // only define for float
 template float Vector2<float>::length_squared(void) const;
 template float Vector2<float>::length(void) const;
@@ -324,6 +353,7 @@ template float Vector2<float>::angle(const Vector2<float> &v) const;
 template float Vector2<float>::angle(void) const;
 template bool Vector2<float>::segment_intersection(const Vector2<float>& seg1_start, const Vector2<float>& seg1_end, const Vector2<float>& seg2_start, const Vector2<float>& seg2_end, Vector2<float>& intersection);
 template bool Vector2<float>::circle_segment_intersection(const Vector2<float>& seg_start, const Vector2<float>& seg_end, const Vector2<float>& circle_center, float radius, Vector2<float>& intersection);
+template Vector2<float> Vector2<float>::closest_point(const Vector2<float> &p, const Vector2<float> &v, const Vector2<float> &w);
 
 template bool Vector2<long>::operator ==(const Vector2<long> &v) const;
 
diff --git a/libraries/AP_Math/vector2.h b/libraries/AP_Math/vector2.h
index d07986160f..7b9906cac7 100644
--- a/libraries/AP_Math/vector2.h
+++ b/libraries/AP_Math/vector2.h
@@ -161,27 +161,7 @@ struct Vector2
      * 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;
-        }
-    }
+    static Vector2<T> closest_point(const Vector2<T> &p, const Vector2<T> &v, const Vector2<T> &w);
 
     // w defines a line segment from the origin
     // p is a point