From 854eacab92d25b20c97b184af6b5c8eccbd0f69f Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Wed, 18 Dec 2024 18:26:26 +0000
Subject: [PATCH] AP_Math: add template versions of
 Polygon_closest_distance_point()

---
 libraries/AP_Math/polygon.cpp | 14 ++++++++------
 libraries/AP_Math/polygon.h   |  3 ++-
 libraries/AP_Math/vector2.cpp |  7 +++++++
 3 files changed, 17 insertions(+), 7 deletions(-)

diff --git a/libraries/AP_Math/polygon.cpp b/libraries/AP_Math/polygon.cpp
index fa5f348e9f..a53a82c253 100644
--- a/libraries/AP_Math/polygon.cpp
+++ b/libraries/AP_Math/polygon.cpp
@@ -123,7 +123,8 @@ template bool Polygon_outside<int32_t>(const Vector2l &P, const Vector2l *V, uns
 template bool Polygon_complete<int32_t>(const Vector2l *V, unsigned n);
 template bool Polygon_outside<float>(const Vector2f &P, const Vector2f *V, unsigned n);
 template bool Polygon_complete<float>(const Vector2f *V, unsigned n);
-
+template float Polygon_closest_distance_point<float>(const Vector2f *V, unsigned N, const Vector2f &p);
+template float Polygon_closest_distance_point<int32_t>(const Vector2l *V, unsigned N, const Vector2l &p);
 
 /*
   determine if the polygon of N verticies defined by points V is
@@ -200,14 +201,15 @@ float Polygon_closest_distance_line(const Vector2f *V, unsigned N, const Vector2
   return the closest distance that point p comes to an edge of closed
   polygon V, defined by N points
  */
-float Polygon_closest_distance_point(const Vector2f *V, unsigned N, const Vector2f &p)
+template <typename T>
+float Polygon_closest_distance_point(const Vector2<T> *V, unsigned N, const Vector2<T> &p)
 {
-    float closest_sq = FLT_MAX;
+    T closest_sq = std::numeric_limits<T>::max();
     for (uint8_t i=0; i<N-1; i++) {
-        const Vector2f &v1 = V[i];
-        const Vector2f &v2 = V[i+1];
+        const Vector2<T> &v1 = V[i];
+        const Vector2<T> &v2 = V[i+1];
 
-        float dist_sq = Vector2f::closest_distance_between_line_and_point_squared(v1, v2, p);
+        T dist_sq = Vector2<T>::closest_distance_between_line_and_point_squared(v1, v2, p);
         if (dist_sq < closest_sq) {
             closest_sq = dist_sq;
         }
diff --git a/libraries/AP_Math/polygon.h b/libraries/AP_Math/polygon.h
index e85071d357..7211eba132 100644
--- a/libraries/AP_Math/polygon.h
+++ b/libraries/AP_Math/polygon.h
@@ -43,4 +43,5 @@ float Polygon_closest_distance_line(const Vector2f *V, unsigned N, const Vector2
   return the closest distance that a point p comes to an edge of
   closed polygon V, defined by N points
  */
-float Polygon_closest_distance_point(const Vector2f *V, unsigned N, const Vector2f &p);
+template <typename T>
+float Polygon_closest_distance_point(const Vector2<T> *V, unsigned N, const Vector2<T> &p);
diff --git a/libraries/AP_Math/vector2.cpp b/libraries/AP_Math/vector2.cpp
index 149bcd08e0..a7e5943a56 100644
--- a/libraries/AP_Math/vector2.cpp
+++ b/libraries/AP_Math/vector2.cpp
@@ -20,6 +20,13 @@
 
 #include "AP_Math.h"
 
+template float Vector2f::closest_distance_between_line_and_point_squared(const Vector2f &w1,
+                                                              const Vector2f &w2,
+                                                              const Vector2f &p);
+template int32_t Vector2l::closest_distance_between_line_and_point_squared(const Vector2l &w1,
+                                                              const Vector2l &w2,
+                                                              const Vector2l &p);
+
 template <typename T>
 T Vector2<T>::length_squared() const
 {