From 3315ec5acc901d2564b81cc4c0f98be69ed56dc2 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 18 Jun 2021 11:17:50 +1000
Subject: [PATCH] AP_Math: added tofloat() and todouble() methods to Vector2
 and Vector3

---
 libraries/AP_Math/vector2.cpp | 19 ++++++++++++++++---
 libraries/AP_Math/vector2.h   |  7 +++++++
 libraries/AP_Math/vector3.h   |  8 +++++++-
 3 files changed, 30 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_Math/vector2.cpp b/libraries/AP_Math/vector2.cpp
index 8397d5336a..26f468b264 100644
--- a/libraries/AP_Math/vector2.cpp
+++ b/libraries/AP_Math/vector2.cpp
@@ -204,10 +204,10 @@ template <typename T>
 bool Vector2<T>::circle_segment_intersection(const Vector2<T>& seg_start, const Vector2<T>& seg_end, const Vector2<T>& circle_center, float radius, Vector2<T>& intersection)
 {
     // calculate segment start and end as offsets from circle's center
-    const Vector2f seg_start_local = seg_start - circle_center;
+    const Vector2<T> seg_start_local = seg_start - circle_center;
 
     // calculate vector from start to end
-    const Vector2f seg_end_minus_start = seg_end - seg_start;
+    const Vector2<T> seg_end_minus_start = seg_end - seg_start;
 
     const float a = sq(seg_end_minus_start.x) + sq(seg_end_minus_start.y);
     const float b = 2 * ((seg_end_minus_start.x * seg_start_local.x) + (seg_end_minus_start.y * seg_start_local.y));
@@ -448,8 +448,21 @@ void Vector2<T>::rotate(float angle_rad)
     y = ry;
 }
 
-// only define for float
+template <typename T>
+Vector2<double> Vector2<T>::todouble(void) const
+{
+    return Vector2d{x,y};
+}
+
+template <typename T>
+Vector2<float> Vector2<T>::tofloat(void) const
+{
+    return Vector2f{float(x),float(y)};
+}
+
+// define for float and double
 template class Vector2<float>;
+template class Vector2<double>;
 
 // define some ops for int and long
 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 34c0a52473..903ded4e52 100644
--- a/libraries/AP_Math/vector2.h
+++ b/libraries/AP_Math/vector2.h
@@ -162,6 +162,12 @@ struct Vector2
     // rotate vector by angle in radians
     void rotate(float angle_rad);
 
+    /*
+      conversion to/from double
+     */
+    Vector2<float> tofloat() const;
+    Vector2<double> todouble() const;
+    
     // given a position p1 and a velocity v1 produce a vector
     // perpendicular to v1 maximising distance from p1
     static Vector2<T> perpendicular(const Vector2<T> &pos_delta, const Vector2<T> &v1);
@@ -269,3 +275,4 @@ typedef Vector2<uint16_t>       Vector2ui;
 typedef Vector2<int32_t>        Vector2l;
 typedef Vector2<uint32_t>       Vector2ul;
 typedef Vector2<float>          Vector2f;
+typedef Vector2<double>          Vector2d;
diff --git a/libraries/AP_Math/vector3.h b/libraries/AP_Math/vector3.h
index d3ac153eab..1479914b6d 100644
--- a/libraries/AP_Math/vector3.h
+++ b/libraries/AP_Math/vector3.h
@@ -264,7 +264,13 @@ public:
 
     // extrapolate position given bearing and pitch (in degrees) and distance
     void offset_bearing(float bearing, float pitch, float distance);
-    
+
+    /*
+      conversion to/from double
+     */
+    Vector3<float> tofloat() const;
+    Vector3<double> todouble() const;
+
     // given a position p1 and a velocity v1 produce a vector
     // perpendicular to v1 maximising distance from p1.  If p1 is the
     // zero vector the return from the function will always be the