AP_Math: allow for double EKF build

libraries/AP_Math/AP_Math.cpp

@@ -297,6 +297,7 @@ T constrain_value_line(const T amt, const T low, const T high, uint32_t line)
 }
 
 template float constrain_value_line<float>(const float amt, const float low, const float high, uint32_t line);
+template double constrain_value_line<double>(const double amt, const double low, const double high, uint32_t line);
 
 template <typename T>
 T constrain_value(const T amt, const T low, const T high)
@@ -386,15 +387,15 @@ bool rotation_equal(enum Rotation r1, enum Rotation r2)
  * rot_ef_to_bf is a rotation matrix to rotate from earth-frame (NED) to body frame
  * angular_rate is rad/sec
  */
-Vector3f get_vel_correction_for_sensor_offset(const Vector3f &sensor_offset_bf, const Matrix3f &rot_ef_to_bf, const Vector3f &angular_rate)
+Vector3F get_vel_correction_for_sensor_offset(const Vector3F &sensor_offset_bf, const Matrix3F &rot_ef_to_bf, const Vector3F &angular_rate)
 {
     if (sensor_offset_bf.is_zero()) {
-        return Vector3f();
+        return Vector3F();
     }
 
     // correct velocity
-    const Vector3f vel_offset_body = angular_rate % sensor_offset_bf;
-    return rot_ef_to_bf.mul_transpose(vel_offset_body) * -1.0f;
+    const Vector3F vel_offset_body = angular_rate % sensor_offset_bf;
+    return rot_ef_to_bf.mul_transpose(vel_offset_body) * -1.0;
 }
 
 /*
@@ -418,6 +419,13 @@ void fill_nanf(float *f, uint16_t count)
         *f++ = n;
     }
 }
+
+void fill_nanf(double *f, uint16_t count)
+{
+    while (count--) {
+        *f++ = 0;
+    }
+}
 #endif
 
 // Convert 16-bit fixed-point to float

libraries/AP_Math/AP_Math.h

@@ -20,6 +20,18 @@
 #include "location.h"
 #include "control.h"
 
+#if HAL_WITH_EKF_DOUBLE
+typedef Vector2<double> Vector2F;
+typedef Vector3<double> Vector3F;
+typedef Matrix3<double> Matrix3F;
+typedef QuaternionD QuaternionF;
+#else
+typedef Vector2<float> Vector2F;
+typedef Vector3<float> Vector3F;
+typedef Matrix3<float> Matrix3F;
+typedef Quaternion QuaternionF;
+#endif
+
 // define AP_Param types AP_Vector3f and Ap_Matrix3f
 AP_PARAMDEFV(Vector3f, Vector3f, AP_PARAM_VECTOR3F);
 
@@ -149,6 +161,7 @@ template <typename T>
 T constrain_value_line(const T amt, const T low, const T high, uint32_t line);
 
 #define constrain_float(amt, low, high) constrain_value_line(float(amt), float(low), float(high), uint32_t(__LINE__))
+#define constrain_ftype(amt, low, high) constrain_value_line(ftype(amt), ftype(low), ftype(high), uint32_t(__LINE__))
 
 inline int16_t constrain_int16(const int16_t amt, const int16_t low, const int16_t high)
 {
@@ -178,9 +191,9 @@ static inline constexpr float degrees(float rad)
 }
 
 template<typename T>
-float sq(const T val)
+ftype sq(const T val)
 {
-    float v = static_cast<float>(val);
+    ftype v = static_cast<ftype>(val);
     return v*v;
 }
 
@@ -189,7 +202,7 @@ float sq(const T val)
  * dimension.
  */
 template<typename T, typename... Params>
-float sq(const T first, const Params... parameters)
+ftype sq(const T first, const Params... parameters)
 {
     return sq(first) + sq(parameters...);
 }
@@ -199,9 +212,9 @@ float sq(const T first, const Params... parameters)
  * dimension.
  */
 template<typename T, typename U, typename... Params>
-float norm(const T first, const U second, const Params... parameters)
+ftype norm(const T first, const U second, const Params... parameters)
 {
-    return sqrtf(sq(first, second, parameters...));
+    return sqrtF(sq(first, second, parameters...));
 }
 
 template<typename A, typename B>
@@ -288,7 +301,7 @@ bool rotation_equal(enum Rotation r1, enum Rotation r2) WARN_IF_UNUSED;
  * rot_ef_to_bf is a rotation matrix to rotate from earth-frame (NED) to body frame
  * angular_rate is rad/sec
  */
-Vector3f get_vel_correction_for_sensor_offset(const Vector3f &sensor_offset_bf, const Matrix3f &rot_ef_to_bf, const Vector3f &angular_rate);
+Vector3F get_vel_correction_for_sensor_offset(const Vector3F &sensor_offset_bf, const Matrix3F &rot_ef_to_bf, const Vector3F &angular_rate);
 
 /*
   calculate a low pass filter alpha value
@@ -298,6 +311,7 @@ float calc_lowpass_alpha_dt(float dt, float cutoff_freq);
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 // fill an array of float with NaN, used to invalidate memory in SITL
 void fill_nanf(float *f, uint16_t count);
+void fill_nanf(double *f, uint16_t count);
 #endif
 
 // from https://embeddedartistry.com/blog/2018/07/12/simple-fixed-point-conversion-in-c/

libraries/AP_Math/examples/matrix_alg/matrix_alg.cpp

@@ -13,7 +13,7 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL();
 
 #define MAT_ALG_ACCURACY    1e-4f
 
-typedef float ftype;
+typedef float Ftype;
 
 static uint16_t get_random(void)
 {
@@ -25,7 +25,7 @@ static uint16_t get_random(void)
 }
 
 
-static void show_matrix(ftype *A, int n) {
+static void show_matrix(Ftype *A, int n) {
     for (int i = 0; i < n; i++) {
         for (int j = 0; j < n; j++)
             printf("%.10f  ", A[i * n + j]);
@@ -33,7 +33,7 @@ static void show_matrix(ftype *A, int n) {
     }
 }
 
-static bool compare_mat(const ftype *A, const ftype *B, const uint8_t n)
+static bool compare_mat(const Ftype *A, const Ftype *B, const uint8_t n)
 {
     for(uint8_t i = 0; i < n; i++) {
         for(uint8_t j = 0; j < n; j++) {
@@ -48,8 +48,8 @@ static bool compare_mat(const ftype *A, const ftype *B, const uint8_t n)
 static void test_matrix_inverse(void)
 {
     //fast inverses
-    ftype test_mat[25],ident_mat[25];
-    ftype out_mat[25], out_mat2[25], mat[25];
+    Ftype test_mat[25],ident_mat[25];
+    Ftype out_mat[25], out_mat2[25], mat[25];
     for(uint8_t i = 0;i<25;i++) {
         test_mat[i] = powf(-1,i)*get_random()/0.7f;
     }

libraries/AP_Math/ftype.h

@@ -0,0 +1,53 @@
+#pragma once
+
+/*
+  allow for builds with either single or double precision EKF
+ */
+
+#include <AP_HAL/AP_HAL.h>
+
+/*
+  capital F is used to denote the chosen float type (float or double)
+ */
+
+#if HAL_WITH_EKF_DOUBLE
+typedef double ftype;
+#define sinF(x) sin(x)
+#define acosF(x) acos(x)
+#define asinF(x) asin(x)
+#define cosF(x) cos(x)
+#define tanF(x) tan(x)
+#define atanF(x) atan(x)
+#define atan2F(x,y) atan2(x,y)
+#define sqrtF(x) sqrt(x)
+#define fmaxF(x,y) fmax(x,y)
+#define powF(x,y) pow(x,y)
+#define logF(x) log(x)
+#define fabsF(x) fabs(x)
+#define ceilF(x) ceil(x)
+#define fminF(x,y) fmin(x,y)
+#define toftype todouble
+#else
+typedef float ftype;
+#define acosF(x) acosf(x)
+#define asinF(x) asinf(x)
+#define sinF(x) sinf(x)
+#define cosF(x) cosf(x)
+#define tanF(x) tanf(x)
+#define atanF(x) atanf(x)
+#define atan2F(x,y) atan2f(x,y)
+#define sqrtF(x) sqrtf(x)
+#define fmaxF(x,y) fmaxf(x,y)
+#define powF(x,y) powf(x,y)
+#define logF(x) logf(x)
+#define fabsF(x) fabsf(x)
+#define ceilF(x) ceilf(x)
+#define fminF(x,y) fminf(x,y)
+#define toftype tofloat
+#endif
+
+#if MATH_CHECK_INDEXES
+#define ZERO_FARRAY(a) a.zero()
+#else
+#define ZERO_FARRAY(a) memset(a, 0, sizeof(a))
+#endif

libraries/AP_Math/matrix3.cpp

@@ -23,14 +23,14 @@
 // create a rotation matrix given some euler angles
 // this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
 template <typename T>
-void Matrix3<T>::from_euler(float roll, float pitch, float yaw)
+void Matrix3<T>::from_euler(T roll, T pitch, T yaw)
 {
-    const float cp = cosf(pitch);
-    const float sp = sinf(pitch);
-    const float sr = sinf(roll);
-    const float cr = cosf(roll);
-    const float sy = sinf(yaw);
-    const float cy = cosf(yaw);
+    const T cp = cosF(pitch);
+    const T sp = sinF(pitch);
+    const T sr = sinF(roll);
+    const T cr = cosF(roll);
+    const T sy = sinF(yaw);
+    const T cy = cosF(yaw);
 
     a.x = cp * cy;
     a.y = (sr * sp * cy) - (cr * sy);
@@ -46,7 +46,7 @@ void Matrix3<T>::from_euler(float roll, float pitch, float yaw)
 // calculate euler angles from a rotation matrix
 // this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
 template <typename T>
-void Matrix3<T>::to_euler(float *roll, float *pitch, float *yaw) const
+void Matrix3<T>::to_euler(T *roll, T *pitch, T *yaw) const
 {
     if (pitch != nullptr) {
         *pitch = -safe_asin(c.x);
@@ -80,7 +80,7 @@ void Matrix3<T>::from_rotation(enum Rotation rotation)
 template <typename T>
 Vector3<T> Matrix3<T>::to_euler312() const
 {
-    return Vector3<T>(asinf(c.y),
+    return Vector3<T>(asinF(c.y),
                       atan2f(-c.x, c.z),
                       atan2f(-a.y, b.y));
 }
@@ -89,14 +89,14 @@ Vector3<T> Matrix3<T>::to_euler312() const
   fill the matrix from Euler angles in radians in 312 convention
 */
 template <typename T>
-void Matrix3<T>::from_euler312(float roll, float pitch, float yaw)
+void Matrix3<T>::from_euler312(T roll, T pitch, T yaw)
 {
-    const float c3 = cosf(pitch);
-    const float s3 = sinf(pitch);
-    const float s2 = sinf(roll);
-    const float c2 = cosf(roll);
-    const float s1 = sinf(yaw);
-    const float c1 = cosf(yaw);
+    const T c3 = cosF(pitch);
+    const T s3 = sinF(pitch);
+    const T s2 = sinF(roll);
+    const T c2 = cosF(roll);
+    const T s1 = sinF(yaw);
+    const T c1 = cosF(yaw);
 
     a.x = c1 * c3 - s1 * s2 * s3;
     b.y = c1 * c2;
@@ -127,7 +127,7 @@ void Matrix3<T>::rotate(const Vector3<T> &g)
 template <typename T>
 void Matrix3<T>::normalize(void)
 {
-    const float error = a * b;
+    const T error = a * b;
     const Vector3<T> t0 = a - (b * (0.5f * error));
     const Vector3<T> t1 = b - (a * (0.5f * error));
     const Vector3<T> t2 = t0 % t1;
@@ -238,15 +238,15 @@ void Matrix3<T>::zero(void)
 // create rotation matrix for rotation about the vector v by angle theta
 // See: http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToMatrix/
 template <typename T>
-void Matrix3<T>::from_axis_angle(const Vector3<T> &v, float theta)
+void Matrix3<T>::from_axis_angle(const Vector3<T> &v, T theta)
 {
-    const float C = cosf(theta);
-    const float S = sinf(theta);
-    const float t = 1.0f - C;
+    const T C = cosF(theta);
+    const T S = sinF(theta);
+    const T t = 1.0f - C;
     const Vector3<T> normv = v.normalized();
-    const float x = normv.x;
-    const float y = normv.y;
-    const float z = normv.z;
+    const T x = normv.x;
+    const T y = normv.y;
+    const T z = normv.z;
 
     a.x = t*x*x + C;
     a.y = t*x*y - z*S;

libraries/AP_Math/matrix3.h

@@ -37,9 +37,14 @@
 //
 #pragma once
 
+#include "ftype.h"
+
 #include "vector3.h"
 #include "vector2.h"
 
+template <typename T>
+class Vector3;
+
 // 3x3 matrix with elements of type T
 template <typename T>
 class Matrix3 {
@@ -231,13 +236,13 @@ public:
     }
 
     // create a rotation matrix from Euler angles
-    void        from_euler(float roll, float pitch, float yaw);
+    void        from_euler(T roll, T pitch, T yaw);
 
     // create eulers from a rotation matrix.
     // roll is from -Pi to Pi
     // pitch is from -Pi/2 to Pi/2
     // yaw is from -Pi to Pi
-    void        to_euler(float *roll, float *pitch, float *yaw) const;
+    void        to_euler(T *roll, T *pitch, T *yaw) const;
 
     // create matrix from rotation enum
     void from_rotation(enum Rotation rotation);
@@ -252,7 +257,7 @@ public:
     /*
       fill the matrix from Euler angles in radians in 312 convention
     */
-    void from_euler312(float roll, float pitch, float yaw);
+    void from_euler312(T roll, T pitch, T yaw);
 
     // apply an additional rotation from a body frame gyro vector
     // to a rotation matrix.
@@ -261,11 +266,12 @@ public:
     // create rotation matrix for rotation about the vector v by angle theta
     // See: https://en.wikipedia.org/wiki/Rotation_matrix#General_rotations
     // "Rotation matrix from axis and angle"
-    void        from_axis_angle(const Vector3<T> &v, float theta);
+    void        from_axis_angle(const Vector3<T> &v, T theta);
     
     // normalize a rotation matrix
     void        normalize(void);
 
+    // double/float conversion
     Matrix3<double> todouble(void) const {
         return Matrix3<double>(a.todouble(), b.todouble(), c.todouble());
     }

libraries/AP_Math/quaternion.cpp

@@ -22,17 +22,43 @@
 #include <AP_InternalError/AP_InternalError.h>
 
 // return the rotation matrix equivalent for this quaternion
-void Quaternion::rotation_matrix(Matrix3f &m) const
-{
-    const float q3q3 = q3 * q3;
-    const float q3q4 = q3 * q4;
-    const float q2q2 = q2 * q2;
-    const float q2q3 = q2 * q3;
-    const float q2q4 = q2 * q4;
-    const float q1q2 = q1 * q2;
-    const float q1q3 = q1 * q3;
-    const float q1q4 = q1 * q4;
-    const float q4q4 = q4 * q4;
+template <typename T>
+void QuaternionT<T>::rotation_matrix(Matrix3d &m) const
+{
+    const T q3q3 = q3 * q3;
+    const T q3q4 = q3 * q4;
+    const T q2q2 = q2 * q2;
+    const T q2q3 = q2 * q3;
+    const T q2q4 = q2 * q4;
+    const T q1q2 = q1 * q2;
+    const T q1q3 = q1 * q3;
+    const T q1q4 = q1 * q4;
+    const T q4q4 = q4 * q4;
+
+    m.a.x = 1.0f-2.0f*(q3q3 + q4q4);
+    m.a.y = 2.0f*(q2q3 - q1q4);
+    m.a.z = 2.0f*(q2q4 + q1q3);
+    m.b.x = 2.0f*(q2q3 + q1q4);
+    m.b.y = 1.0f-2.0f*(q2q2 + q4q4);
+    m.b.z = 2.0f*(q3q4 - q1q2);
+    m.c.x = 2.0f*(q2q4 - q1q3);
+    m.c.y = 2.0f*(q3q4 + q1q2);
+    m.c.z = 1.0f-2.0f*(q2q2 + q3q3);
+}
+
+// return the rotation matrix equivalent for this quaternion
+template <typename T>
+void QuaternionT<T>::rotation_matrix(Matrix3f &m) const
+{
+    const T q3q3 = q3 * q3;
+    const T q3q4 = q3 * q4;
+    const T q2q2 = q2 * q2;
+    const T q2q3 = q2 * q3;
+    const T q2q4 = q2 * q4;
+    const T q1q2 = q1 * q2;
+    const T q1q3 = q1 * q3;
+    const T q1q4 = q1 * q4;
+    const T q4q4 = q4 * q4;
 
     m.a.x = 1.0f-2.0f*(q3q3 + q4q4);
     m.a.y = 2.0f*(q2q3 - q1q4);
@@ -46,19 +72,20 @@ void Quaternion::rotation_matrix(Matrix3f &m) const
 }
 
 // return the rotation matrix equivalent for this quaternion after normalization
-void Quaternion::rotation_matrix_norm(Matrix3f &m) const
-{
-    const float q1q1 = q1 * q1;
-    const float q1q2 = q1 * q2;
-    const float q1q3 = q1 * q3;
-    const float q1q4 = q1 * q4;
-    const float q2q2 = q2 * q2;
-    const float q2q3 = q2 * q3;
-    const float q2q4 = q2 * q4;
-    const float q3q3 = q3 * q3;
-    const float q3q4 = q3 * q4;
-    const float q4q4 = q4 * q4;
-    const float invs = 1.0f / (q1q1 + q2q2 + q3q3 + q4q4);
+template <typename T>
+void QuaternionT<T>::rotation_matrix_norm(Matrix3<T> &m) const
+{
+    const T q1q1 = q1 * q1;
+    const T q1q2 = q1 * q2;
+    const T q1q3 = q1 * q3;
+    const T q1q4 = q1 * q4;
+    const T q2q2 = q2 * q2;
+    const T q2q3 = q2 * q3;
+    const T q2q4 = q2 * q4;
+    const T q3q3 = q3 * q3;
+    const T q3q4 = q3 * q4;
+    const T q4q4 = q4 * q4;
+    const T invs = 1.0f / (q1q1 + q2q2 + q3q3 + q4q4);
 
     m.a.x = ( q2q2 - q3q3 - q4q4 + q1q1)*invs;
     m.a.y = 2.0f*(q2q3 - q1q4)*invs;
@@ -74,44 +101,45 @@ void Quaternion::rotation_matrix_norm(Matrix3f &m) const
 // return the rotation matrix equivalent for this quaternion
 // Thanks to Martin John Baker
 // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
-void Quaternion::from_rotation_matrix(const Matrix3f &m)
-{
-    const float &m00 = m.a.x;
-    const float &m11 = m.b.y;
-    const float &m22 = m.c.z;
-    const float &m10 = m.b.x;
-    const float &m01 = m.a.y;
-    const float &m20 = m.c.x;
-    const float &m02 = m.a.z;
-    const float &m21 = m.c.y;
-    const float &m12 = m.b.z;
-    float &qw = q1;
-    float &qx = q2;
-    float &qy = q3;
-    float &qz = q4;
-
-    const float tr = m00 + m11 + m22;
+template <typename T>
+void QuaternionT<T>::from_rotation_matrix(const Matrix3<T> &m)
+{
+    const T &m00 = m.a.x;
+    const T &m11 = m.b.y;
+    const T &m22 = m.c.z;
+    const T &m10 = m.b.x;
+    const T &m01 = m.a.y;
+    const T &m20 = m.c.x;
+    const T &m02 = m.a.z;
+    const T &m21 = m.c.y;
+    const T &m12 = m.b.z;
+    T &qw = q1;
+    T &qx = q2;
+    T &qy = q3;
+    T &qz = q4;
+
+    const T tr = m00 + m11 + m22;
 
     if (tr > 0) {
-        const float S = sqrtf(tr+1) * 2;
+        const T S = sqrtF(tr+1) * 2;
         qw = 0.25f * S;
         qx = (m21 - m12) / S;
         qy = (m02 - m20) / S;
         qz = (m10 - m01) / S;
     } else if ((m00 > m11) && (m00 > m22)) {
-        const float S = sqrtf(1.0f + m00 - m11 - m22) * 2.0f;
+        const T S = sqrtF(1.0f + m00 - m11 - m22) * 2.0f;
         qw = (m21 - m12) / S;
         qx = 0.25f * S;
         qy = (m01 + m10) / S;
         qz = (m02 + m20) / S;
     } else if (m11 > m22) {
-        const float S = sqrtf(1.0f + m11 - m00 - m22) * 2.0f;
+        const T S = sqrtF(1.0f + m11 - m00 - m22) * 2.0f;
         qw = (m02 - m20) / S;
         qx = (m01 + m10) / S;
         qy = 0.25f * S;
         qz = (m12 + m21) / S;
     } else {
-        const float S = sqrtf(1.0f + m22 - m00 - m11) * 2.0f;
+        const T S = sqrtF(1.0f + m22 - m00 - m11) * 2.0f;
         qw = (m10 - m01) / S;
         qx = (m02 + m20) / S;
         qy = (m12 + m21) / S;
@@ -120,7 +148,8 @@ void Quaternion::from_rotation_matrix(const Matrix3f &m)
 }
 
 // create a quaternion from a given rotation
-void Quaternion::from_rotation(enum Rotation rotation)
+template <typename T>
+void QuaternionT<T>::from_rotation(enum Rotation rotation)
 {
     // the constants below can be calculated using the following formula:
     //     Matrix3f m_from_rot;
@@ -379,10 +408,11 @@ void Quaternion::from_rotation(enum Rotation rotation)
 }
 
 // rotate this quaternion by the given rotation
-void Quaternion::rotate(enum Rotation rotation)
+template <typename T>
+void QuaternionT<T>::rotate(enum Rotation rotation)
 {
     // create quaternion from rotation matrix
-    Quaternion q_from_rot;
+    QuaternionT<T> q_from_rot;
     q_from_rot.from_rotation(rotation);
 
     // rotate this quaternion
@@ -390,22 +420,24 @@ void Quaternion::rotate(enum Rotation rotation)
 }
 
 // convert a vector from earth to body frame
-void Quaternion::earth_to_body(Vector3f &v) const
+template <typename T>
+void QuaternionT<T>::earth_to_body(Vector3<T> &v) const
 {
-    Matrix3f m;
+    Matrix3<T> m;
     rotation_matrix(m);
     v = m * v;
 }
 
 // create a quaternion from Euler angles
-void Quaternion::from_euler(float roll, float pitch, float yaw)
+template <typename T>
+void QuaternionT<T>::from_euler(T roll, T pitch, T yaw)
 {
-    const float cr2 = cosf(roll*0.5f);
-    const float cp2 = cosf(pitch*0.5f);
-    const float cy2 = cosf(yaw*0.5f);
-    const float sr2 = sinf(roll*0.5f);
-    const float sp2 = sinf(pitch*0.5f);
-    const float sy2 = sinf(yaw*0.5f);
+    const T cr2 = cosF(roll*0.5);
+    const T cp2 = cosF(pitch*0.5);
+    const T cy2 = cosF(yaw*0.5);
+    const T sr2 = sinF(roll*0.5);
+    const T sp2 = sinF(pitch*0.5);
+    const T sy2 = sinF(yaw*0.5);
 
     q1 = cr2*cp2*cy2 + sr2*sp2*sy2;
     q2 = sr2*cp2*cy2 - cr2*sp2*sy2;
@@ -415,18 +447,20 @@ void Quaternion::from_euler(float roll, float pitch, float yaw)
 
 // create a quaternion from Euler angles applied in yaw, roll, pitch order
 // instead of the normal yaw, pitch, roll order
-void Quaternion::from_vector312(float roll, float pitch, float yaw)
+template <typename T>
+void QuaternionT<T>::from_vector312(T roll, T pitch, T yaw)
 {
-    Matrix3f m;
+    Matrix3<T> m;
     m.from_euler312(roll, pitch, yaw);
 
     from_rotation_matrix(m);
 }
 
 // create a quaternion from its axis-angle representation
-void Quaternion::from_axis_angle(Vector3f v)
+template <typename T>
+void QuaternionT<T>::from_axis_angle(Vector3<T> v)
 {
-    const float theta = v.length();
+    const T theta = v.length();
     if (is_zero(theta)) {
         q1 = 1.0f;
         q2=q3=q4=0.0f;
@@ -438,7 +472,8 @@ void Quaternion::from_axis_angle(Vector3f v)
 
 // create a quaternion from its axis-angle representation
 // the axis vector must be length 1, theta is in radians
-void Quaternion::from_axis_angle(const Vector3f &axis, float theta)
+template <typename T>
+void QuaternionT<T>::from_axis_angle(const Vector3<T> &axis, T theta)
 {
     // axis must be a unit vector as there is no check for length
     if (is_zero(theta)) {
@@ -446,28 +481,30 @@ void Quaternion::from_axis_angle(const Vector3f &axis, float theta)
         q2=q3=q4=0.0f;
         return;
     }
-    const float st2 = sinf(theta/2.0f);
+    const T st2 = sinF(theta/2.0f);
 
-    q1 = cosf(theta/2.0f);
+    q1 = cosF(theta/2.0f);
     q2 = axis.x * st2;
     q3 = axis.y * st2;
     q4 = axis.z * st2;
 }
 
 // rotate by the provided axis angle
-void Quaternion::rotate(const Vector3f &v)
+template <typename T>
+void QuaternionT<T>::rotate(const Vector3<T> &v)
 {
-    Quaternion r;
+    QuaternionT<T> r;
     r.from_axis_angle(v);
     (*this) *= r;
 }
 
 // convert this quaternion to a rotation vector where the direction of the vector represents
 // the axis of rotation and the length of the vector represents the angle of rotation
-void Quaternion::to_axis_angle(Vector3f &v) const
+template <typename T>
+void QuaternionT<T>::to_axis_angle(Vector3<T> &v) const
 {
-    const float l = sqrtf(sq(q2)+sq(q3)+sq(q4));
-    v = Vector3f(q2,q3,q4);
+    const T l = sqrtF(sq(q2)+sq(q3)+sq(q4));
+    v = Vector3<T>(q2,q3,q4);
     if (!is_zero(l)) {
         v /= l;
         v *= wrap_PI(2.0f * atan2f(l,q1));
@@ -476,9 +513,10 @@ void Quaternion::to_axis_angle(Vector3f &v) const
 
 // create a quaternion from its axis-angle representation
 // only use with small angles.  I.e. length of v should less than 0.17 radians (i.e. 10 degrees)
-void Quaternion::from_axis_angle_fast(Vector3f v)
+template <typename T>
+void QuaternionT<T>::from_axis_angle_fast(Vector3<T> v)
 {
-    const float theta = v.length();
+    const T theta = v.length();
     if (is_zero(theta)) {
         q1 = 1.0f;
         q2=q3=q4=0.0f;
@@ -490,11 +528,12 @@ void Quaternion::from_axis_angle_fast(Vector3f v)
 
 // create a quaternion from its axis-angle representation
 // theta should less than 0.17 radians (i.e. 10 degrees)
-void Quaternion::from_axis_angle_fast(const Vector3f &axis, float theta)
+template <typename T>
+void QuaternionT<T>::from_axis_angle_fast(const Vector3<T> &axis, T theta)
 {
-    const float t2 = theta/2.0f;
-    const float sqt2 = sq(t2);
-    const float st2 = t2-sqt2*t2/6.0f;
+    const T t2 = theta/2.0f;
+    const T sqt2 = sq(t2);
+    const T st2 = t2-sqt2*t2/6.0f;
 
     q1 = 1.0f-(sqt2/2.0f)+sq(sqt2)/24.0f;
     q2 = axis.x * st2;
@@ -504,28 +543,29 @@ void Quaternion::from_axis_angle_fast(const Vector3f &axis, float theta)
 
 // rotate by the provided axis angle
 // only use with small angles.  I.e. length of v should less than 0.17 radians (i.e. 10 degrees)
-void Quaternion::rotate_fast(const Vector3f &v)
+template <typename T>
+void QuaternionT<T>::rotate_fast(const Vector3<T> &v)
 {
-    const float theta = v.length();
+    const T theta = v.length();
     if (is_zero(theta)) {
         return;
     }
-    const float t2 = theta/2.0f;
-    const float sqt2 = sq(t2);
-    float st2 = t2-sqt2*t2/6.0f;
+    const T t2 = theta/2.0f;
+    const T sqt2 = sq(t2);
+    T st2 = t2-sqt2*t2/6.0f;
     st2 /= theta;
 
     //"rotation quaternion"
-    const float w2 = 1.0f-(sqt2/2.0f)+sq(sqt2)/24.0f;
-    const float x2 = v.x * st2;
-    const float y2 = v.y * st2;
-    const float z2 = v.z * st2;
+    const T w2 = 1.0f-(sqt2/2.0f)+sq(sqt2)/24.0f;
+    const T x2 = v.x * st2;
+    const T y2 = v.y * st2;
+    const T z2 = v.z * st2;
 
     //copy our quaternion
-    const float w1 = q1;
-    const float x1 = q2;
-    const float y1 = q3;
-    const float z1 = q4;
+    const T w1 = q1;
+    const T x1 = q2;
+    const T y1 = q3;
+    const T z1 = q4;
 
     //do the multiply into our quaternion
     q1 = w1*w2 - x1*x2 - y1*y2 - z1*z2;
@@ -535,25 +575,37 @@ void Quaternion::rotate_fast(const Vector3f &v)
 }
 
 // get euler roll angle
-float Quaternion::get_euler_roll() const
+template <typename T>
+T QuaternionT<T>::get_euler_roll() const
 {
     return (atan2f(2.0f*(q1*q2 + q3*q4), 1.0f - 2.0f*(q2*q2 + q3*q3)));
 }
 
 // get euler pitch angle
-float Quaternion::get_euler_pitch() const
+template <typename T>
+T QuaternionT<T>::get_euler_pitch() const
 {
     return safe_asin(2.0f*(q1*q3 - q4*q2));
 }
 
 // get euler yaw angle
-float Quaternion::get_euler_yaw() const
+template <typename T>
+T QuaternionT<T>::get_euler_yaw() const
 {
     return atan2f(2.0f*(q1*q4 + q2*q3), 1.0f - 2.0f*(q3*q3 + q4*q4));
 }
 
 // create eulers from a quaternion
-void Quaternion::to_euler(float &roll, float &pitch, float &yaw) const
+template <typename T>
+void QuaternionT<T>::to_euler(double &roll, double &pitch, double &yaw) const
+{
+    roll = get_euler_roll();
+    pitch = get_euler_pitch();
+    yaw = get_euler_yaw();
+}
+
+template <typename T>
+void QuaternionT<T>::to_euler(float &roll, float &pitch, float &yaw) const
 {
     roll = get_euler_roll();
     pitch = get_euler_pitch();
@@ -561,37 +613,42 @@ void Quaternion::to_euler(float &roll, float &pitch, float &yaw) const
 }
 
 // create eulers from a quaternion
-Vector3f Quaternion::to_vector312(void) const
+template <typename T>
+Vector3<T> QuaternionT<T>::to_vector312(void) const
 {
-    Matrix3f m;
+    Matrix3<T> m;
     rotation_matrix(m);
     return m.to_euler312();
 }
 
-float Quaternion::length(void) const
+template <typename T>
+T QuaternionT<T>::length(void) const
 {
-    return sqrtf(sq(q1) + sq(q2) + sq(q3) + sq(q4));
+    return sqrtF(sq(q1) + sq(q2) + sq(q3) + sq(q4));
 }
 
 // return the reverse rotation of this quaternion
-Quaternion Quaternion::inverse(void) const
+template <typename T>
+QuaternionT<T> QuaternionT<T>::inverse(void) const
 {
-    return Quaternion(q1, -q2, -q3, -q4);
+    return QuaternionT<T>(q1, -q2, -q3, -q4);
 }
 
 // reverse the rotation of this quaternion
-void Quaternion::invert()
+template <typename T>
+void QuaternionT<T>::invert()
 {
     q2 = -q2;
     q3 = -q3;
     q4 = -q4;
 }
 
-void Quaternion::normalize(void)
+template <typename T>
+void QuaternionT<T>::normalize(void)
 {
-    const float quatMag = length();
+    const T quatMag = length();
     if (!is_zero(quatMag)) {
-        const float quatMagInv = 1.0f/quatMag;
+        const T quatMagInv = 1.0f/quatMag;
         q1 *= quatMagInv;
         q2 *= quatMagInv;
         q3 *= quatMagInv;
@@ -599,18 +656,19 @@ void Quaternion::normalize(void)
     }
 }
 
-Quaternion Quaternion::operator*(const Quaternion &v) const
+template <typename T>
+QuaternionT<T> QuaternionT<T>::operator*(const QuaternionT<T> &v) const
 {
-    Quaternion ret;
-    const float &w1 = q1;
-    const float &x1 = q2;
-    const float &y1 = q3;
-    const float &z1 = q4;
+    QuaternionT<T> ret;
+    const T &w1 = q1;
+    const T &x1 = q2;
+    const T &y1 = q3;
+    const T &z1 = q4;
 
-    const float w2 = v.q1;
-    const float x2 = v.q2;
-    const float y2 = v.q3;
-    const float z2 = v.q4;
+    const T w2 = v.q1;
+    const T x2 = v.q2;
+    const T y2 = v.q3;
+    const T z2 = v.q4;
 
     ret.q1 = w1*w2 - x1*x2 - y1*y2 - z1*z2;
     ret.q2 = w1*x2 + x1*w2 + y1*z2 - z1*y2;
@@ -624,7 +682,8 @@ Quaternion Quaternion::operator*(const Quaternion &v) const
 // (*this) to a rotation matrix then multiplying it to the argument `v`.
 //
 // 15 multiplies and 15 add / subtracts. Caches 3 floats
-Vector3f Quaternion::operator*(const Vector3f &v) const
+template <typename T>
+Vector3<T> QuaternionT<T>::operator*(const Vector3<T> &v) const
 {
     // This uses the formula
     //
@@ -633,10 +692,10 @@ Vector3f Quaternion::operator*(const Vector3f &v) const
     // where "x" is the cross product (explicitly inlined for performance below), 
     // "q1" is the scalar part and "qv" is the vector part of this quaternion
 
-    Vector3f ret = v;
+    Vector3<T> ret = v;
 
     // Compute and cache "qv x v1"
-    float uv[] = {q3 * v.z - q4 * v.y, q4 * v.x - q2 * v.z, q2 * v.y - q3 * v.x};
+    T uv[] = {q3 * v.z - q4 * v.y, q4 * v.x - q2 * v.z, q2 * v.y - q3 * v.x};
 
     uv[0] += uv[0];
     uv[1] += uv[1];
@@ -647,17 +706,18 @@ Vector3f Quaternion::operator*(const Vector3f &v) const
     return ret;
 }
 
-Quaternion &Quaternion::operator*=(const Quaternion &v)
+template <typename T>
+QuaternionT<T> &QuaternionT<T>::operator*=(const QuaternionT<T> &v)
 {
-    const float w1 = q1;
-    const float x1 = q2;
-    const float y1 = q3;
-    const float z1 = q4;
+    const T w1 = q1;
+    const T x1 = q2;
+    const T y1 = q3;
+    const T z1 = q4;
 
-    const float w2 = v.q1;
-    const float x2 = v.q2;
-    const float y2 = v.q3;
-    const float z2 = v.q4;
+    const T w2 = v.q1;
+    const T x2 = v.q2;
+    const T y2 = v.q3;
+    const T z2 = v.q4;
 
     q1 = w1*w2 - x1*x2 - y1*y2 - z1*z2;
     q2 = w1*x2 + x1*w2 + y1*z2 - z1*y2;
@@ -667,18 +727,19 @@ Quaternion &Quaternion::operator*=(const Quaternion &v)
     return *this;
 }
 
-Quaternion Quaternion::operator/(const Quaternion &v) const
+template <typename T>
+QuaternionT<T> QuaternionT<T>::operator/(const QuaternionT<T> &v) const
 {
-    Quaternion ret;
-    const float &quat0 = q1;
-    const float &quat1 = q2;
-    const float &quat2 = q3;
-    const float &quat3 = q4;
+    QuaternionT<T> ret;
+    const T &quat0 = q1;
+    const T &quat1 = q2;
+    const T &quat2 = q3;
+    const T &quat3 = q4;
 
-    const float rquat0 = v.q1;
-    const float rquat1 = v.q2;
-    const float rquat2 = v.q3;
-    const float rquat3 = v.q4;
+    const T rquat0 = v.q1;
+    const T rquat1 = v.q2;
+    const T rquat2 = v.q3;
+    const T rquat3 = v.q4;
 
     ret.q1 = (rquat0*quat0 + rquat1*quat1 + rquat2*quat2 + rquat3*quat3);
     ret.q2 = (rquat0*quat1 - rquat1*quat0 - rquat2*quat3 + rquat3*quat2);
@@ -688,26 +749,32 @@ Quaternion Quaternion::operator/(const Quaternion &v) const
 }
 
 // angular difference in radians between quaternions
-Quaternion Quaternion::angular_difference(const Quaternion &v) const
+template <typename T>
+QuaternionT<T> QuaternionT<T>::angular_difference(const QuaternionT<T> &v) const
 {
     return v.inverse() * *this;
 }
 
 // absolute (e.g. always positive) earth-frame roll-pitch difference (in radians) between this Quaternion and another
-float Quaternion::roll_pitch_difference(const Quaternion &v) const
+template <typename T>
+T QuaternionT<T>::roll_pitch_difference(const QuaternionT<T> &v) const
 {
     // convert Quaternions to rotation matrices
-    Matrix3f m, vm;
+    Matrix3<T> m, vm;
     rotation_matrix(m);
     v.rotation_matrix(vm);
 
     // rotate earth frame vertical vector by each rotation matrix
-    const Vector3f z_unit_vec{0,0,1};
-    const Vector3f z_unit_m = m.mul_transpose(z_unit_vec);
-    const Vector3f z_unit_vm = vm.mul_transpose(z_unit_vec);
-    const Vector3f vec_diff = z_unit_vm - z_unit_m;
-    const float vec_len_div2 = constrain_float(vec_diff.length() * 0.5f, 0.0f, 1.0f);
+    const Vector3<T> z_unit_vec{0,0,1};
+    const Vector3<T> z_unit_m = m.mul_transpose(z_unit_vec);
+    const Vector3<T> z_unit_vm = vm.mul_transpose(z_unit_vec);
+    const Vector3<T> vec_diff = z_unit_vm - z_unit_m;
+    const T vec_len_div2 = constrain_float(vec_diff.length() * 0.5, 0.0, 1.0);
 
     // calculate and return angular difference
-    return (2.0f * asinf(vec_len_div2));
+    return (2.0 * asinf(vec_len_div2));
 }
+
+// define for float and double
+template class QuaternionT<float>;
+template class QuaternionT<double>;

libraries/AP_Math/quaternion.h

@@ -23,32 +23,33 @@
 #endif
 #include <math.h>
 
-class Quaternion {
+template <typename T>
+class QuaternionT {
 public:
-    float        q1, q2, q3, q4;
+    T        q1, q2, q3, q4;
 
     // constructor creates a quaternion equivalent
     // to roll=0, pitch=0, yaw=0
-    Quaternion()
+    QuaternionT<T>()
     {
         q1 = 1;
         q2 = q3 = q4 = 0;
     }
 
     // setting constructor
-    Quaternion(const float _q1, const float _q2, const float _q3, const float _q4) :
+    QuaternionT<T>(const T _q1, const T _q2, const T _q3, const T _q4) :
         q1(_q1), q2(_q2), q3(_q3), q4(_q4)
     {
     }
 
     // setting constructor
-    Quaternion(const float _q[4]) :
+    QuaternionT<T>(const T _q[4]) :
         q1(_q[0]), q2(_q[1]), q3(_q[2]), q4(_q[3])
     {
     }
 
     // function call operator
-    void operator()(const float _q1, const float _q2, const float _q3, const float _q4)
+    void operator()(const T _q1, const T _q2, const T _q3, const T _q4)
     {
         q1 = _q1;
         q2 = _q2;
@@ -64,12 +65,13 @@ public:
 
     // return the rotation matrix equivalent for this quaternion
     void        rotation_matrix(Matrix3f &m) const;
+    void        rotation_matrix(Matrix3d &m) const;
 
     // return the rotation matrix equivalent for this quaternion after normalization
-    void        rotation_matrix_norm(Matrix3f &m) const;
+    void        rotation_matrix_norm(Matrix3<T> &m) const;
 
     // return the rotation matrix equivalent for this quaternion
-    void		from_rotation_matrix(const Matrix3f &m);
+    void		from_rotation_matrix(const Matrix3<T> &m);
 
     // create a quaternion from a given rotation
     void        from_rotation(enum Rotation rotation);
@@ -78,58 +80,59 @@ public:
     void        rotate(enum Rotation rotation);
 
     // convert a vector from earth to body frame
-    void        earth_to_body(Vector3f &v) const;
+    void        earth_to_body(Vector3<T> &v) const;
 
     // create a quaternion from Euler angles
-    void        from_euler(float roll, float pitch, float yaw);
+    void        from_euler(T roll, T pitch, T yaw);
 
     // create a quaternion from Euler angles applied in yaw, roll, pitch order
     // instead of the normal yaw, pitch, roll order
-    void        from_vector312(float roll, float pitch, float yaw);
+    void        from_vector312(T roll, T pitch, T yaw);
 
     // convert this quaternion to a rotation vector where the direction of the vector represents
     // the axis of rotation and the length of the vector represents the angle of rotation
-    void        to_axis_angle(Vector3f &v) const;
+    void        to_axis_angle(Vector3<T> &v) const;
 
     // create a quaternion from a rotation vector where the direction of the vector represents
     // the axis of rotation and the length of the vector represents the angle of rotation
-    void        from_axis_angle(Vector3f v);
+    void        from_axis_angle(Vector3<T> v);
 
     // create a quaternion from its axis-angle representation
     // the axis vector must be length 1. the rotation angle theta is in radians
-    void        from_axis_angle(const Vector3f &axis, float theta);
+    void        from_axis_angle(const Vector3<T> &axis, T theta);
 
     // rotate by the provided rotation vector
-    void        rotate(const Vector3f &v);
+    void        rotate(const Vector3<T> &v);
 
     // create a quaternion from a rotation vector
     // only use with small angles.  I.e. length of v should less than 0.17 radians (i.e. 10 degrees)
-    void        from_axis_angle_fast(Vector3f v);
+    void        from_axis_angle_fast(Vector3<T> v);
 
     // create a quaternion from its axis-angle representation
     // the axis vector must be length 1, theta should less than 0.17 radians (i.e. 10 degrees)
-    void        from_axis_angle_fast(const Vector3f &axis, float theta);
+    void        from_axis_angle_fast(const Vector3<T> &axis, T theta);
 
     // rotate by the provided rotation vector
     // only use with small angles.  I.e. length of v should less than 0.17 radians (i.e. 10 degrees)
-    void        rotate_fast(const Vector3f &v);
+    void        rotate_fast(const Vector3<T> &v);
 
     // get euler roll angle
-    float       get_euler_roll() const;
+    T       get_euler_roll() const;
 
     // get euler pitch angle
-    float       get_euler_pitch() const;
+    T       get_euler_pitch() const;
 
     // get euler yaw angle
-    float       get_euler_yaw() const;
+    T       get_euler_yaw() const;
 
     // create eulers from a quaternion
     void        to_euler(float &roll, float &pitch, float &yaw) const;
+    void        to_euler(double &roll, double &pitch, double &yaw) const;
 
     // create eulers from a quaternion
-    Vector3f    to_vector312(void) const;
+    Vector3<T>    to_vector312(void) const;
 
-    float length(void) const;
+    T length(void) const;
     void normalize();
 
     // initialise the quaternion to no rotation
@@ -139,39 +142,52 @@ public:
         q2 = q3 = q4 = 0.0f;
     }
 
-    Quaternion inverse(void) const;
+    QuaternionT<T> inverse(void) const;
 
     // reverse the rotation of this quaternion
     void invert();
 
     // allow a quaternion to be used as an array, 0 indexed
-    float & operator[](uint8_t i)
+    T & operator[](uint8_t i)
     {
-        float *_v = &q1;
+        T *_v = &q1;
 #if MATH_CHECK_INDEXES
         assert(i < 4);
 #endif
         return _v[i];
     }
 
-    const float & operator[](uint8_t i) const
+    const T & operator[](uint8_t i) const
     {
-        const float *_v = &q1;
+        const T *_v = &q1;
 #if MATH_CHECK_INDEXES
         assert(i < 4);
 #endif
         return _v[i];
     }
 
-    Quaternion operator*(const Quaternion &v) const;
-
-    Vector3f operator*(const Vector3f &v) const;
-    Quaternion &operator*=(const Quaternion &v);
-    Quaternion operator/(const Quaternion &v) const;
+    QuaternionT<T> operator*(const QuaternionT<T> &v) const;
+    Vector3<T> operator*(const Vector3<T> &v) const;
+    QuaternionT<T> &operator*=(const QuaternionT<T> &v);
+    QuaternionT<T> operator/(const QuaternionT<T> &v) const;
 
     // angular difference between quaternions
-    Quaternion angular_difference(const Quaternion &v) const;
+    QuaternionT<T> angular_difference(const QuaternionT<T> &v) const;
 
     // absolute (e.g. always positive) earth-frame roll-pitch difference (in radians) between this Quaternion and another
-    float roll_pitch_difference(const Quaternion &v) const;
+    T roll_pitch_difference(const QuaternionT<T> &v) const;
+
+    // double/float conversion
+    QuaternionT<double> todouble(void) const {
+        return QuaternionT<double>(q1,q2,q3,q4);
+    }
+    QuaternionT<float> tofloat(void) const {
+        return QuaternionT<float>(q1,q2,q3,q4);
+    }
 };
+
+typedef QuaternionT<float> Quaternion;
+typedef QuaternionT<double> QuaternionD;
+
+
+

libraries/AP_Math/tests/test_math.cpp

@@ -624,11 +624,11 @@ TEST(MathTest, RANDOM16)
 
 TEST(MathTest, VELCORRECTION)
 {
-    static constexpr Vector3f pos{1.0f, 1.0f, 0.0f};
-    static constexpr Matrix3f rot(0.0f, -1.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
-    static constexpr Vector3f rate{-1.0f, -1.0f, -1.0f};
-    EXPECT_TRUE(Vector3f(1.0f, 1.0f, 0.0f) == get_vel_correction_for_sensor_offset(pos, rot, rate));
-    EXPECT_TRUE(Vector3f() == get_vel_correction_for_sensor_offset(Vector3f(), rot, rate));
+    static constexpr Vector3F pos{1.0f, 1.0f, 0.0f};
+    static constexpr Matrix3F rot(0.0f, -1.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
+    static constexpr Vector3F rate{-1.0f, -1.0f, -1.0f};
+    EXPECT_TRUE(Vector3F(1.0f, 1.0f, 0.0f) == get_vel_correction_for_sensor_offset(pos, rot, rate));
+    EXPECT_TRUE(Vector3F() == get_vel_correction_for_sensor_offset(Vector3f(), rot, rate));
 }
 
 TEST(MathTest, LOWPASSALPHA)

libraries/AP_Math/tests/test_math_double.cpp

@@ -224,9 +224,9 @@ TEST(MathTest, NormDouble)
     double norm_5 = norm(3,4);
     double norm_6 = norm(4.0,3.0,12.0);
 
-    EXPECT_DOUBLE_EQ(norm_1, 4.317406177520752);
+    EXPECT_DOUBLE_EQ(norm_1, 4.3174066289845809);
     EXPECT_DOUBLE_EQ(norm_2, 4.0);
-    EXPECT_DOUBLE_EQ(norm_3, 5.3000001907348633);
+    EXPECT_DOUBLE_EQ(norm_3, 5.2999999999999998);
     EXPECT_DOUBLE_EQ(norm_4, 0.0);
     EXPECT_DOUBLE_EQ(norm_5, 5.0);
     EXPECT_DOUBLE_EQ(norm_6, 13.0);

libraries/AP_Math/vector2.cpp

@@ -21,22 +21,22 @@
 #include "AP_Math.h"
 
 template <typename T>
-float Vector2<T>::length_squared() const
+T Vector2<T>::length_squared() const
 {
-    return (float)(x*x + y*y);
+    return (T)(x*x + y*y);
 }
 
 template <typename T>
-float Vector2<T>::length(void) const
+T Vector2<T>::length(void) const
 {
     return norm(x, y);
 }
 
 // limit vector to a given length. returns true if vector was limited
 template <typename T>
-bool Vector2<T>::limit_length(float max_length)
+bool Vector2<T>::limit_length(T max_length)
 {
-    const float len = length();
+    const T len = length();
     if ((len > max_length) && is_positive(len)) {
         x *= (max_length / len);
         y *= (max_length / len);
@@ -142,24 +142,24 @@ bool Vector2<T>::operator !=(const Vector2<T> &v) const
 }
 
 template <typename T>
-float Vector2<T>::angle(const Vector2<T> &v2) const
+T Vector2<T>::angle(const Vector2<T> &v2) const
 {
-    const float len = this->length() * v2.length();
+    const T len = this->length() * v2.length();
     if (len <= 0) {
         return 0.0f;
     }
-    const float cosv = ((*this)*v2) / len;
+    const T cosv = ((*this)*v2) / len;
     if (cosv >= 1) {
         return 0.0f;
     }
     if (cosv <= -1) {
         return M_PI;
     }
-    return acosf(cosv);
+    return acosF(cosv);
 }
 
 template <typename T>
-float Vector2<T>::angle(void) const
+T Vector2<T>::angle(void) const
 {
     return M_PI_2 + atan2f(-x, y);
 }
@@ -174,16 +174,16 @@ bool Vector2<T>::segment_intersection(const Vector2<T>& seg1_start, const Vector
     const Vector2<T> r1 = seg1_end - seg1_start;
     const Vector2<T> r2 = seg2_end - seg2_start;
     const Vector2<T> ss2_ss1 = seg2_start - seg1_start;
-    const float r1xr2 = r1 % r2;
-    const float q_pxr = ss2_ss1 % r1;
+    const T r1xr2 = r1 % r2;
+    const T q_pxr = ss2_ss1 % r1;
     if (fabsf(r1xr2) < FLT_EPSILON) {
         // either collinear or parallel and non-intersecting
         return false;
     } else {
         // t = (q - p) * s / (r * s)
         // u = (q - p) * r / (r * s)
-        const float t = (ss2_ss1 % r2) / r1xr2;
-        const float u = q_pxr / r1xr2;
+        const T t = (ss2_ss1 % r2) / r1xr2;
+        const T u = q_pxr / r1xr2;
         if ((u >= 0) && (u <= 1) && (t >= 0) && (t <= 1)) {
             // lines intersect
             // t can be any non-negative value because (p, p + r) is a ray
@@ -201,7 +201,7 @@ bool Vector2<T>::segment_intersection(const Vector2<T>& seg1_start, const Vector
 // returns true if they intersect and intersection argument is updated with intersection closest to seg_start
 // solution adapted from http://stackoverflow.com/questions/1073336/circle-line-segment-collision-detection-algorithm
 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)
+bool Vector2<T>::circle_segment_intersection(const Vector2<T>& seg_start, const Vector2<T>& seg_end, const Vector2<T>& circle_center, T radius, Vector2<T>& intersection)
 {
     // calculate segment start and end as offsets from circle's center
     const Vector2<T> seg_start_local = seg_start - circle_center;
@@ -209,16 +209,16 @@ bool Vector2<T>::circle_segment_intersection(const Vector2<T>& seg_start, const
     // calculate vector from start to end
     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));
-    const float c = sq(seg_start_local.x) + sq(seg_start_local.y) - sq(radius);
+    const T a = sq(seg_end_minus_start.x) + sq(seg_end_minus_start.y);
+    const T b = 2 * ((seg_end_minus_start.x * seg_start_local.x) + (seg_end_minus_start.y * seg_start_local.y));
+    const T c = sq(seg_start_local.x) + sq(seg_start_local.y) - sq(radius);
 
     // check for invalid data
     if (::is_zero(a) || isnan(a) || isnan(b) || isnan(c)) {
        return false;
     }
 
-    const float delta = sq(b) - (4.0f * a * c);
+    const T delta = sq(b) - (4.0f * a * c);
 
     if (isnan(delta)) {
        return false;
@@ -229,9 +229,9 @@ bool Vector2<T>::circle_segment_intersection(const Vector2<T>& seg_start, const
         return false;
     }
 
-    const float delta_sqrt = sqrtf(delta);
-    const float t1 = (-b + delta_sqrt) / (2.0f * a);
-    const float t2 = (-b - delta_sqrt) / (2.0f * a);
+    const T delta_sqrt = sqrtF(delta);
+    const T t1 = (-b + delta_sqrt) / (2.0f * a);
+    const T t2 = (-b - delta_sqrt) / (2.0f * a);
 
     // Three hit cases:
     //          -o->             --|-->  |            |  --|->
@@ -302,10 +302,10 @@ Vector2<T> Vector2<T>::projected(const Vector2<T> &v)
 
 // extrapolate position given bearing (in degrees) and distance
 template <typename T>
-void Vector2<T>::offset_bearing(float bearing, float distance)
+void Vector2<T>::offset_bearing(T bearing, T distance)
 {
-    x += cosf(radians(bearing)) * distance;
-    y += sinf(radians(bearing)) * distance;
+    x += cosF(radians(bearing)) * distance;
+    y += sinF(radians(bearing)) * distance;
 }
 
 // given a position pos_delta and a velocity v1 produce a vector
@@ -333,7 +333,7 @@ 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();
+    const T l2 = (v - w).length_squared();
     if (l2 < FLT_EPSILON) {
         // v == w case
         return v;
@@ -342,7 +342,7 @@ Vector2<T> Vector2<T>::closest_point(const Vector2<T> &p, const Vector2<T> &v, c
     // 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;
+    const T t = ((p - v) * (w - v)) / l2;
     if (t <= 0) {
         return v;
     } else if (t >= 1) {
@@ -361,12 +361,12 @@ 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();
+    const T l2 = w.length_squared();
     if (l2 < FLT_EPSILON) {
         // v == w case
         return w;
     }
-    const float t = (p * w) / l2;
+    const T t = (p * w) / l2;
     if (t <= 0) {
         return Vector2<T>(0,0);
     } else if (t >= 1) {
@@ -379,9 +379,9 @@ Vector2<T> Vector2<T>::closest_point(const Vector2<T> &p, const Vector2<T> &w)
 // closest distance between a line segment and a point
 // https://stackoverflow.com/questions/2824478/shortest-distance-between-two-line-segments
 template <typename T>
-float Vector2<T>::closest_distance_between_line_and_point_squared(const Vector2<T> &w1,
-                                                                  const Vector2<T> &w2,
-                                                                  const Vector2<T> &p)
+T Vector2<T>::closest_distance_between_line_and_point_squared(const Vector2<T> &w1,
+                                                              const Vector2<T> &w2,
+                                                              const Vector2<T> &p)
 {
     return closest_distance_between_radial_and_point_squared(w2-w1, p-w1);
 }
@@ -390,28 +390,28 @@ float Vector2<T>::closest_distance_between_line_and_point_squared(const Vector2<
 // p is a point
 // returns the closest distance between the line segment and the point
 template <typename T>
-float Vector2<T>::closest_distance_between_line_and_point(const Vector2<T> &w1,
-                                                          const Vector2<T> &w2,
-                                                          const Vector2<T> &p)
+T Vector2<T>::closest_distance_between_line_and_point(const Vector2<T> &w1,
+                                                      const Vector2<T> &w2,
+                                                      const Vector2<T> &p)
 {
-    return sqrtf(closest_distance_between_line_and_point_squared(w1, w2, p));
+    return sqrtF(closest_distance_between_line_and_point_squared(w1, w2, p));
 }
 
 // a1->a2 and b2->v2 define two line segments
 // returns the square of the closest distance between the two line segments
 // see https://stackoverflow.com/questions/2824478/shortest-distance-between-two-line-segments
 template <typename T>
-float Vector2<T>::closest_distance_between_lines_squared(const Vector2<T> &a1,
-                                                         const Vector2<T> &a2,
-                                                         const Vector2<T> &b1,
-                                                         const Vector2<T> &b2)
-{
-    const float dist1 = Vector2<T>::closest_distance_between_line_and_point_squared(b1,b2,a1);
-    const float dist2 = Vector2<T>::closest_distance_between_line_and_point_squared(b1,b2,a2);
-    const float dist3 = Vector2<T>::closest_distance_between_line_and_point_squared(a1,a2,b1);
-    const float dist4 = Vector2<T>::closest_distance_between_line_and_point_squared(a1,a2,b2);
-    const float m1 = MIN(dist1,dist2);
-    const float m2 = MIN(dist3,dist4);
+T Vector2<T>::closest_distance_between_lines_squared(const Vector2<T> &a1,
+                                                     const Vector2<T> &a2,
+                                                     const Vector2<T> &b1,
+                                                     const Vector2<T> &b2)
+{
+    const T dist1 = Vector2<T>::closest_distance_between_line_and_point_squared(b1,b2,a1);
+    const T dist2 = Vector2<T>::closest_distance_between_line_and_point_squared(b1,b2,a2);
+    const T dist3 = Vector2<T>::closest_distance_between_line_and_point_squared(a1,a2,b1);
+    const T dist4 = Vector2<T>::closest_distance_between_line_and_point_squared(a1,a2,b2);
+    const T m1 = MIN(dist1,dist2);
+    const T m2 = MIN(dist3,dist4);
     return MIN(m1,m2);
 }
 
@@ -419,8 +419,8 @@ float Vector2<T>::closest_distance_between_lines_squared(const Vector2<T> &a1,
 // p is a point
 // returns the square of the closest distance between the radial and the point
 template <typename T>
-float Vector2<T>::closest_distance_between_radial_and_point_squared(const Vector2<T> &w,
-                                                                   const Vector2<T> &p)
+T Vector2<T>::closest_distance_between_radial_and_point_squared(const Vector2<T> &w,
+                                                                const Vector2<T> &p)
 {
     const Vector2<T> closest = closest_point(p, w);
     return (closest - p).length_squared();
@@ -430,20 +430,20 @@ float Vector2<T>::closest_distance_between_radial_and_point_squared(const Vector
 // p is a point
 // returns the closest distance between the radial and the point
 template <typename T>
-float Vector2<T>::closest_distance_between_radial_and_point(const Vector2<T> &w,
+T Vector2<T>::closest_distance_between_radial_and_point(const Vector2<T> &w,
                                                             const Vector2<T> &p)
 {
-    return sqrtf(closest_distance_between_radial_and_point_squared(w,p));
+    return sqrtF(closest_distance_between_radial_and_point_squared(w,p));
 }
 
 // rotate vector by angle in radians
 template <typename T>
-void Vector2<T>::rotate(float angle_rad)
+void Vector2<T>::rotate(T angle_rad)
 {
-    const float cs = cosf(angle_rad);
-    const float sn = sinf(angle_rad);
-    float rx = x * cs - y * sn;
-    float ry = x * sn + y * cs;
+    const T cs = cosF(angle_rad);
+    const T sn = sinF(angle_rad);
+    T rx = x * cs - y * sn;
+    T ry = x * sn + y * cs;
     x = rx;
     y = ry;
 }
@@ -457,3 +457,4 @@ template bool Vector2<long>::operator ==(const Vector2<long> &v) const;
 template bool Vector2<long>::operator !=(const Vector2<long> &v) const;
 template bool Vector2<int>::operator ==(const Vector2<int> &v) const;
 template bool Vector2<int>::operator !=(const Vector2<int> &v) const;
+

libraries/AP_Math/vector2.h

@@ -35,6 +35,7 @@
 
 #include <cmath>
 #include <AP_Common/AP_Common.h>
+#include "ftype.h"
 
 template <typename T>
 struct Vector2
@@ -92,11 +93,11 @@ struct Vector2
 
     // computes the angle between this vector and another vector
     // returns 0 if the vectors are parallel, and M_PI if they are antiparallel
-    float angle(const Vector2<T> &v2) const;
+    T angle(const Vector2<T> &v2) const;
 
     // computes the angle of this vector in radians, from 0 to 2pi,
     // from a unit vector(1,0); a (1,1) vector's angle is +M_PI/4
-    float angle(void) const;
+    T angle(void) const;
 
     // check if any elements are NAN
     bool is_nan(void) const WARN_IF_UNUSED;
@@ -133,13 +134,13 @@ struct Vector2
     }
 
     // gets the length of this vector squared
-    float length_squared() const;
+    T length_squared() const;
 
     // gets the length of this vector
-    float length(void) const;
+    T length(void) const;
 
     // limit vector to a given length. returns true if vector was limited
-    bool limit_length(float max_length);
+    bool limit_length(T max_length);
 
     // normalizes this vector
     void normalize();
@@ -157,10 +158,10 @@ struct Vector2
     Vector2<T> projected(const Vector2<T> &v);
 
     // adjust position by a given bearing (in degrees) and distance
-    void offset_bearing(float bearing, float distance);
+    void offset_bearing(T bearing, T distance);
 
     // rotate vector by angle in radians
-    void rotate(float angle_rad);
+    void rotate(T angle_rad);
 
     /*
       conversion to/from double
@@ -194,20 +195,20 @@ struct Vector2
     // w1 and w2 define a line segment
     // p is a point
     // returns the square of the closest distance between the line segment and the point
-    static float closest_distance_between_line_and_point_squared(const Vector2<T> &w1,
+    static T closest_distance_between_line_and_point_squared(const Vector2<T> &w1,
                                                                  const Vector2<T> &w2,
                                                                  const Vector2<T> &p);
 
     // w1 and w2 define a line segment
     // p is a point
     // returns the closest distance between the line segment and the point
-    static float closest_distance_between_line_and_point(const Vector2<T> &w1,
+    static T closest_distance_between_line_and_point(const Vector2<T> &w1,
                                                          const Vector2<T> &w2,
                                                          const Vector2<T> &p);
 
     // a1->a2 and b2->v2 define two line segments
     // returns the square of the closest distance between the two line segments
-    static float closest_distance_between_lines_squared(const Vector2<T> &a1,
+    static T closest_distance_between_lines_squared(const Vector2<T> &a1,
                                                         const Vector2<T> &a2,
                                                         const Vector2<T> &b1,
                                                         const Vector2<T> &b2);
@@ -215,13 +216,13 @@ struct Vector2
     // 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
-    static float closest_distance_between_radial_and_point_squared(const Vector2<T> &w,
+    static T closest_distance_between_radial_and_point_squared(const Vector2<T> &w,
                                                                    const Vector2<T> &p);
 
     // 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,
+    static T closest_distance_between_radial_and_point(const Vector2<T> &w,
                                                            const Vector2<T> &p);
 
     // find the intersection between two line segments
@@ -231,22 +232,22 @@ struct Vector2
 
     // find the intersection between a line segment and a circle
     // returns true if they intersect and intersection argument is updated with intersection closest to seg_start
-    static bool circle_segment_intersection(const Vector2<T>& seg_start, const Vector2<T>& seg_end, const Vector2<T>& circle_center, float radius, Vector2<T>& intersection) WARN_IF_UNUSED;
+    static bool circle_segment_intersection(const Vector2<T>& seg_start, const Vector2<T>& seg_end, const Vector2<T>& circle_center, T radius, Vector2<T>& intersection) WARN_IF_UNUSED;
 
     // check if a point falls on the line segment from seg_start to seg_end
     static bool point_on_segment(const Vector2<T>& point,
                                  const Vector2<T>& seg_start,
                                  const Vector2<T>& seg_end) WARN_IF_UNUSED {
-        const float expected_run = seg_end.x-seg_start.x;
-        const float intersection_run = point.x-seg_start.x;
+        const T expected_run = seg_end.x-seg_start.x;
+        const T intersection_run = point.x-seg_start.x;
         // check slopes are identical:
         if (fabsf(expected_run) < FLT_EPSILON) {
             if (fabsf(intersection_run) > FLT_EPSILON) {
                 return false;
             }
         } else {
-            const float expected_slope = (seg_end.y-seg_start.y)/expected_run;
-            const float intersection_slope = (point.y-seg_start.y)/intersection_run;
+            const T expected_slope = (seg_end.y-seg_start.y)/expected_run;
+            const T intersection_slope = (point.y-seg_start.y)/intersection_run;
             if (fabsf(expected_slope - intersection_slope) > FLT_EPSILON) {
                 return false;
             }
@@ -279,4 +280,4 @@ typedef Vector2<uint16_t>       Vector2ui;
 typedef Vector2<int32_t>        Vector2l;
 typedef Vector2<uint32_t>       Vector2ul;
 typedef Vector2<float>          Vector2f;
-typedef Vector2<double>          Vector2d;
+typedef Vector2<double>         Vector2d;

libraries/AP_Math/vector3.cpp

@@ -31,8 +31,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
     case ROTATION_NONE:
         return;
     case ROTATION_YAW_45: {
-        tmp = HALF_SQRT_2*(float)(x - y);
-        y   = HALF_SQRT_2*(float)(x + y);
+        tmp = HALF_SQRT_2*(ftype)(x - y);
+        y   = HALF_SQRT_2*(ftype)(x + y);
         x = tmp;
         return;
     }
@@ -41,8 +41,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
         return;
     }
     case ROTATION_YAW_135: {
-        tmp = -HALF_SQRT_2*(float)(x + y);
-        y   =  HALF_SQRT_2*(float)(x - y);
+        tmp = -HALF_SQRT_2*(ftype)(x + y);
+        y   =  HALF_SQRT_2*(ftype)(x - y);
         x = tmp;
         return;
     }
@@ -50,8 +50,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
         x = -x; y = -y;
         return;
     case ROTATION_YAW_225: {
-        tmp = HALF_SQRT_2*(float)(y - x);
-        y   = -HALF_SQRT_2*(float)(x + y);
+        tmp = HALF_SQRT_2*(ftype)(y - x);
+        y   = -HALF_SQRT_2*(ftype)(x + y);
         x = tmp;
         return;
     }
@@ -60,8 +60,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
         return;
     }
     case ROTATION_YAW_315: {
-        tmp = HALF_SQRT_2*(float)(x + y);
-        y   = HALF_SQRT_2*(float)(y - x);
+        tmp = HALF_SQRT_2*(ftype)(x + y);
+        y   = HALF_SQRT_2*(ftype)(y - x);
         x = tmp;
         return;
     }
@@ -70,8 +70,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
         return;
     }
     case ROTATION_ROLL_180_YAW_45: {
-        tmp = HALF_SQRT_2*(float)(x + y);
-        y   = HALF_SQRT_2*(float)(x - y);
+        tmp = HALF_SQRT_2*(ftype)(x + y);
+        y   = HALF_SQRT_2*(ftype)(x - y);
         x = tmp; z = -z;
         return;
     }
@@ -80,8 +80,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
         return;
     }
     case ROTATION_ROLL_180_YAW_135: {
-        tmp = HALF_SQRT_2*(float)(y - x);
-        y   = HALF_SQRT_2*(float)(y + x);
+        tmp = HALF_SQRT_2*(ftype)(y - x);
+        y   = HALF_SQRT_2*(ftype)(y + x);
         x = tmp; z = -z;
         return;
     }
@@ -90,8 +90,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
         return;
     }
     case ROTATION_ROLL_180_YAW_225: {
-        tmp = -HALF_SQRT_2*(float)(x + y);
-        y   =  HALF_SQRT_2*(float)(y - x);
+        tmp = -HALF_SQRT_2*(ftype)(x + y);
+        y   =  HALF_SQRT_2*(ftype)(y - x);
         x = tmp; z = -z;
         return;
     }
@@ -100,8 +100,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
         return;
     }
     case ROTATION_ROLL_180_YAW_315: {
-        tmp =  HALF_SQRT_2*(float)(x - y);
-        y   = -HALF_SQRT_2*(float)(x + y);
+        tmp =  HALF_SQRT_2*(ftype)(x - y);
+        y   = -HALF_SQRT_2*(ftype)(x + y);
         x = tmp; z = -z;
         return;
     }
@@ -111,8 +111,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
     }
     case ROTATION_ROLL_90_YAW_45: {
         tmp = z; z = y; y = -tmp;
-        tmp = HALF_SQRT_2*(float)(x - y);
-        y   = HALF_SQRT_2*(float)(x + y);
+        tmp = HALF_SQRT_2*(ftype)(x - y);
+        y   = HALF_SQRT_2*(ftype)(x + y);
         x = tmp;
         return;
     }
@@ -123,8 +123,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
     }
     case ROTATION_ROLL_90_YAW_135: {
         tmp = z; z = y; y = -tmp;
-        tmp = -HALF_SQRT_2*(float)(x + y);
-        y   =  HALF_SQRT_2*(float)(x - y);
+        tmp = -HALF_SQRT_2*(ftype)(x + y);
+        y   =  HALF_SQRT_2*(ftype)(x - y);
         x = tmp;
         return;
     }
@@ -134,8 +134,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
     }
     case ROTATION_ROLL_270_YAW_45: {
         tmp = z; z = -y; y = tmp;
-        tmp = HALF_SQRT_2*(float)(x - y);
-        y   = HALF_SQRT_2*(float)(x + y);
+        tmp = HALF_SQRT_2*(ftype)(x - y);
+        y   = HALF_SQRT_2*(ftype)(x + y);
         x = tmp;
         return;
     }
@@ -146,8 +146,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
     }
     case ROTATION_ROLL_270_YAW_135: {
         tmp = z; z = -y; y = tmp;
-        tmp = -HALF_SQRT_2*(float)(x + y);
-        y   =  HALF_SQRT_2*(float)(x - y);
+        tmp = -HALF_SQRT_2*(ftype)(x + y);
+        y   =  HALF_SQRT_2*(ftype)(x - y);
         x = tmp;
         return;
     }
@@ -221,32 +221,32 @@ void Vector3<T>::rotate(enum Rotation rotation)
         return;
     }
     case ROTATION_ROLL_90_PITCH_68_YAW_293: {
-        float tmpx = x;
-        float tmpy = y;
-        float tmpz = z;
+        T tmpx = x;
+        T tmpy = y;
+        T tmpz = z;
         x =  0.143039f * tmpx +  0.368776f * tmpy + -0.918446f * tmpz;
         y = -0.332133f * tmpx + -0.856289f * tmpy + -0.395546f * tmpz;
         z = -0.932324f * tmpx +  0.361625f * tmpy +  0.000000f * tmpz;
         return;
     }
     case ROTATION_PITCH_315: {
-        tmp = HALF_SQRT_2*(float)(x - z);
-        z   = HALF_SQRT_2*(float)(x + z);
+        tmp = HALF_SQRT_2*(ftype)(x - z);
+        z   = HALF_SQRT_2*(ftype)(x + z);
         x = tmp;
         return;
     }
     case ROTATION_ROLL_90_PITCH_315: {
         tmp = z; z = y; y = -tmp;
-        tmp = HALF_SQRT_2*(float)(x - z);
-        z   = HALF_SQRT_2*(float)(x + z);
+        tmp = HALF_SQRT_2*(ftype)(x - z);
+        z   = HALF_SQRT_2*(ftype)(x + z);
         x = tmp;
         return;
     }
     case ROTATION_PITCH_7: {
-        const float sin_pitch = 0.12186934340514748f; // sinf(pitch);
-        const float cos_pitch = 0.992546151641322f; // cosf(pitch);
-        float tmpx = x;
-        float tmpz = z;
+        const T sin_pitch = 0.12186934340514748f; // sinF(pitch);
+        const T cos_pitch = 0.992546151641322f; // cosF(pitch);
+        T tmpx = x;
+        T tmpz = z;
         x =  cos_pitch * tmpx + sin_pitch * tmpz;
         z = -sin_pitch * tmpx + cos_pitch * tmpz;
         return;
@@ -284,12 +284,12 @@ void Vector3<T>::rotate_inverse(enum Rotation rotation)
 
 // rotate vector by angle in radians in xy plane leaving z untouched
 template <typename T>
-void Vector3<T>::rotate_xy(float angle_rad)
+void Vector3<T>::rotate_xy(T angle_rad)
 {
-    const float cs = cosf(angle_rad);
-    const float sn = sinf(angle_rad);
-    float rx = x * cs - y * sn;
-    float ry = x * sn + y * cs;
+    const T cs = cosF(angle_rad);
+    const T sn = sinF(angle_rad);
+    T rx = x * cs - y * sn;
+    T ry = x * sn + y * cs;
     x = rx;
     y = ry;
 }
@@ -310,16 +310,16 @@ T Vector3<T>::operator *(const Vector3<T> &v) const
 }
 
 template <typename T>
-float Vector3<T>::length(void) const
+T Vector3<T>::length(void) const
 {
     return norm(x, y, z);
 }
 
 // limit xy component vector to a given length. returns true if vector was limited
 template <typename T>
-bool Vector3<T>::limit_length_xy(float max_length)
+bool Vector3<T>::limit_length_xy(T max_length)
 {
-    const float length_xy = norm(x, y);
+    const T length_xy = norm(x, y);
     if ((length_xy > max_length) && is_positive(length_xy)) {
         x *= (max_length / length_xy);
         y *= (max_length / length_xy);
@@ -411,17 +411,17 @@ bool Vector3<T>::operator !=(const Vector3<T> &v) const
 }
 
 template <typename T>
-float Vector3<T>::angle(const Vector3<T> &v2) const
+T Vector3<T>::angle(const Vector3<T> &v2) const
 {
-    const float len = this->length() * v2.length();
+    const T len = this->length() * v2.length();
     if (len <= 0) {
         return 0.0f;
     }
-    const float cosv = ((*this)*v2) / len;
+    const T cosv = ((*this)*v2) / len;
     if (fabsf(cosv) >= 1) {
         return 0.0f;
     }
-    return acosf(cosv);
+    return acosF(cosv);
 }
 
 // multiplication of transpose by a vector
@@ -445,21 +445,21 @@ Matrix3<T> Vector3<T>::mul_rowcol(const Vector3<T> &v2) const
 
 // extrapolate position given bearing and pitch (in degrees) and distance
 template <typename T>
-void Vector3<T>::offset_bearing(float bearing, float pitch, float distance)
+void Vector3<T>::offset_bearing(T bearing, T pitch, T distance)
 {
-    y += cosf(radians(pitch)) * sinf(radians(bearing)) * distance;
-    x += cosf(radians(pitch)) * cosf(radians(bearing)) * distance;
-    z += sinf(radians(pitch)) * distance;
+    y += cosF(radians(pitch)) * sinF(radians(bearing)) * distance;
+    x += cosF(radians(pitch)) * cosF(radians(bearing)) * distance;
+    z += sinF(radians(pitch)) * distance;
 }
 
 // distance from the tip of this vector to a line segment specified by two vectors
 template <typename T>
-float Vector3<T>::distance_to_segment(const Vector3<T> &seg_start, const Vector3<T> &seg_end) const
+T Vector3<T>::distance_to_segment(const Vector3<T> &seg_start, const Vector3<T> &seg_end) const
 {
     // triangle side lengths
-    const float a = (*this-seg_start).length();
-    const float b = (seg_start-seg_end).length();
-    const float c = (seg_end-*this).length();
+    const T a = (*this-seg_start).length();
+    const T b = (seg_start-seg_end).length();
+    const T c = (seg_end-*this).length();
 
     // protect against divide by zero later
     if (::is_zero(b)) {
@@ -467,23 +467,23 @@ float Vector3<T>::distance_to_segment(const Vector3<T> &seg_start, const Vector3
     }
 
     // semiperimeter of triangle
-    const float s = (a+b+c) * 0.5f;
+    const T s = (a+b+c) * 0.5f;
 
-    float area_squared = s*(s-a)*(s-b)*(s-c);
+    T area_squared = s*(s-a)*(s-b)*(s-c);
     // area must be constrained above 0 because a triangle could have 3 points could be on a line and float rounding could push this under 0
     if (area_squared < 0.0f) {
         area_squared = 0.0f;
     }
-    const float area = safe_sqrt(area_squared);
+    const T area = safe_sqrt(area_squared);
     return 2.0f*area/b;
 }
 
 // Shortest distance between point(p) to a point contained in the line segment defined by w1,w2
 template <typename T>
-float Vector3<T>::closest_distance_between_line_and_point(const Vector3<T> &w1, const Vector3<T> &w2, const Vector3<T> &p)
+T Vector3<T>::closest_distance_between_line_and_point(const Vector3<T> &w1, const Vector3<T> &w2, const Vector3<T> &p)
 {    
     const Vector3<T> nearest = point_on_line_closest_to_other_point(w1, w2, p);
-    const float dist = (nearest - p).length();
+    const T dist = (nearest - p).length();
     return dist;
 }
 
@@ -495,18 +495,18 @@ Vector3<T> Vector3<T>::point_on_line_closest_to_other_point(const Vector3<T> &w1
     const Vector3<T> line_vec = w2-w1;
     const Vector3<T> p_vec = p - w1;
     
-    const float line_vec_len = line_vec.length();
+    const T line_vec_len = line_vec.length();
     // protection against divide by zero
     if(::is_zero(line_vec_len)) {
         return {0.0f, 0.0f, 0.0f};
     }
 
-    const float scale = 1/line_vec_len;
+    const T scale = 1/line_vec_len;
     const Vector3<T> unit_vec = line_vec * scale;
     const Vector3<T> scaled_p_vec = p_vec * scale;
 
-    float dot_product = unit_vec * scaled_p_vec;
-    dot_product = constrain_float(dot_product,0.0f,1.0f); 
+    T dot_product = unit_vec * scaled_p_vec;
+    dot_product = constrain_ftype(dot_product,0.0f,1.0f);
  
     const Vector3<T> closest_point = line_vec * dot_product;
     return (closest_point + w1);
@@ -525,15 +525,15 @@ void Vector3<T>::segment_to_segment_closest_point(const Vector3<T>& seg1_start,
 
     const Vector3<T> diff = seg1_start - seg2_start;
 
-    const float a = line1*line1;
-    const float b = line1*line2;
-    const float c = line2*line2;
-    const float d = line1*diff;
-    const float e = line2*diff;
+    const T a = line1*line1;
+    const T b = line1*line2;
+    const T c = line2*line2;
+    const T d = line1*diff;
+    const T e = line2*diff;
 
-    const float discriminant = (a*c) - (b*b);
-    float sN, sD = discriminant;           // default sD = D >= 0
-    float tc, tN, tD = discriminant;       // tc = tN / tD, default tD = D >= 0 
+    const T discriminant = (a*c) - (b*b);
+    T sN, sD = discriminant;           // default sD = D >= 0
+    T tc, tN, tD = discriminant;       // tc = tN / tD, default tD = D >= 0
 
     if (discriminant < FLT_EPSILON) {
         sN = 0.0;         // force using point seg1_start on line 1
@@ -596,8 +596,8 @@ bool Vector3<T>::segment_plane_intersect(const Vector3<T>& seg_start, const Vect
     Vector3<T> u = seg_end - seg_start;
     Vector3<T> w = seg_start - plane_point;
 
-    float D = plane_normal * u;
-    float N = -(plane_normal * w);
+    T D = plane_normal * u;
+    T N = -(plane_normal * w);
 
     if (fabsf(D) < FLT_EPSILON) {
         if (::is_zero(N)) {
@@ -608,7 +608,7 @@ bool Vector3<T>::segment_plane_intersect(const Vector3<T>& seg_start, const Vect
             return false;
         }
     }
-    const float sI = N / D;
+    const T sI = N / D;
     if (sI < 0 || sI > 1) {
         // does not intersect
         return false;

libraries/AP_Math/vector3.h

@@ -60,6 +60,8 @@
 
 #include "rotations.h"
 
+#include "ftype.h"
+
 template <typename T>
 class Matrix3;
 
@@ -164,12 +166,12 @@ public:
     }
 
     // scale a vector3
-    Vector3<T> scale(const float v) const {
+    Vector3<T> scale(const T v) const {
         return *this * v;
     }
     
     // computes the angle between this vector and another vector
-    float angle(const Vector3<T> &v2) const;
+    T angle(const Vector3<T> &v2) const;
 
     // check if any elements are NAN
     bool is_nan(void) const WARN_IF_UNUSED;
@@ -190,7 +192,7 @@ public:
     void rotate_inverse(enum Rotation rotation);
 
     // rotate vector by angle in radians in xy plane leaving z untouched
-    void rotate_xy(float rotation_rad);
+    void rotate_xy(T rotation_rad);
 
     // return xy components of a vector3 as a vector2.
     // this returns a reference to the original vector3 xy data
@@ -208,10 +210,10 @@ public:
     }
 
     // gets the length of this vector
-    float length(void) const;
+    T length(void) const;
 
     // limit xy component vector to a given length. returns true if vector was limited
-    bool limit_length_xy(float max_length);
+    bool limit_length_xy(T max_length);
 
     // normalizes this vector
     void normalize()
@@ -252,18 +254,18 @@ public:
     }
 
     // distance from the tip of this vector to another vector squared (so as to avoid the sqrt calculation)
-    float distance_squared(const Vector3<T> &v) const {
-        const float dist_x = x-v.x;
-        const float dist_y = y-v.y;
-        const float dist_z = z-v.z;
+    T distance_squared(const Vector3<T> &v) const {
+        const T dist_x = x-v.x;
+        const T dist_y = y-v.y;
+        const T dist_z = z-v.z;
         return (dist_x*dist_x + dist_y*dist_y + dist_z*dist_z);
     }
 
     // distance from the tip of this vector to a line segment specified by two vectors
-    float distance_to_segment(const Vector3<T> &seg_start, const Vector3<T> &seg_end) const;
+    T distance_to_segment(const Vector3<T> &seg_start, const Vector3<T> &seg_end) const;
 
     // extrapolate position given bearing and pitch (in degrees) and distance
-    void offset_bearing(float bearing, float pitch, float distance);
+    void offset_bearing(T bearing, T pitch, T distance);
 
     /*
       conversion to/from double
@@ -292,13 +294,14 @@ public:
     }
 
     // Shortest distance between point(p) to a point contained in the line segment defined by w1,w2
-    static float closest_distance_between_line_and_point(const Vector3<T> &w1, const Vector3<T> &w2, const Vector3<T> &p);
+    static T closest_distance_between_line_and_point(const Vector3<T> &w1, const Vector3<T> &w2, const Vector3<T> &p);
 
     // Point in the line segment defined by w1,w2 which is closest to point(p)
     static Vector3<T> point_on_line_closest_to_other_point(const Vector3<T> &w1, const Vector3<T> &w2, const Vector3<T> &p);
 
     // This implementation is borrowed from: http://geomalgorithms.com/a07-_distance.html
     // INPUT: 4 points corresponding to start and end of two line segments
+
     // OUTPUT: closest point on segment 2, from segment 1, gets passed on reference as "closest_point"
     static void segment_to_segment_closest_point(const Vector3<T>& seg1_start, const Vector3<T>& seg1_end, const Vector3<T>& seg2_start, const Vector3<T>& seg2_end, Vector3<T>& closest_point);