From 2457bf71d45aef448fcd596a10e8134d8476434b Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 29 May 2019 21:02:32 +0900
Subject: [PATCH] AP_Math: add simplified vector2f::closest_point

this simpler version assumes the line segment originates at the origin
---
 libraries/AP_Math/vector2.cpp | 24 ++++++++++++++++++++++++
 libraries/AP_Math/vector2.h   |  7 +++++++
 2 files changed, 31 insertions(+)

diff --git a/libraries/AP_Math/vector2.cpp b/libraries/AP_Math/vector2.cpp
index b4cd504ff5..1fba830272 100644
--- a/libraries/AP_Math/vector2.cpp
+++ b/libraries/AP_Math/vector2.cpp
@@ -329,6 +329,30 @@ Vector2<T> Vector2<T>::closest_point(const Vector2<T> &p, const Vector2<T> &v, c
     }
 }
 
+/*
+ * Returns the point closest to p on the line segment (0,w).
+ *
+ * this is a simplification of closest point with a general segment, with v=(0,0)
+ */
+template <typename T>
+Vector2<T> Vector2<T>::closest_point(const Vector2<T> &p, const Vector2<T> &w)
+{
+    // length squared of line segment
+    const float l2 = w.length_squared();
+    if (l2 < FLT_EPSILON) {
+        // v == w case
+        return w;
+    }
+    const float t = (p * w) / l2;
+    if (t <= 0) {
+        return Vector2<T>(0,0);
+    } else if (t >= 1) {
+        return w;
+    } else {
+        return w*t;
+    }
+}
+
 // w defines a line segment from the origin
 // p is a point
 // returns the square of the closest distance between the radial and the point
diff --git a/libraries/AP_Math/vector2.h b/libraries/AP_Math/vector2.h
index 8e4fcf7b3a..4f1fd08016 100644
--- a/libraries/AP_Math/vector2.h
+++ b/libraries/AP_Math/vector2.h
@@ -163,6 +163,13 @@ struct Vector2
      */
     static Vector2<T> closest_point(const Vector2<T> &p, const Vector2<T> &v, const Vector2<T> &w);
 
+    /*
+     * Returns the point closest to p on the line segment (0,w).
+     *
+     * this is a simplification of closest point with a general segment, with v=(0,0)
+     */
+    static Vector2<T> closest_point(const Vector2<T> &p, const Vector2<T> &w);
+
     // w defines a line segment from the origin
     // p is a point
     // returns the square of the closest distance between the radial and the point