From e7afa628d1ad752331106da3ef83f845da51a6de Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 4 May 2021 21:12:23 +1000 Subject: [PATCH] AP_Math: allow for double EKF build --- libraries/AP_Math/AP_Math.cpp | 16 +- libraries/AP_Math/AP_Math.h | 26 +- .../examples/matrix_alg/matrix_alg.cpp | 10 +- libraries/AP_Math/ftype.h | 53 +++ libraries/AP_Math/matrix3.cpp | 48 +-- libraries/AP_Math/matrix3.h | 14 +- libraries/AP_Math/quaternion.cpp | 345 +++++++++++------- libraries/AP_Math/quaternion.h | 86 +++-- libraries/AP_Math/tests/test_math.cpp | 10 +- libraries/AP_Math/tests/test_math_double.cpp | 4 +- libraries/AP_Math/vector2.cpp | 111 +++--- libraries/AP_Math/vector2.h | 37 +- libraries/AP_Math/vector3.cpp | 150 ++++---- libraries/AP_Math/vector3.h | 27 +- 14 files changed, 553 insertions(+), 384 deletions(-) create mode 100644 libraries/AP_Math/ftype.h diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp index 41d2e8693d..a3a0551758 100644 --- a/libraries/AP_Math/AP_Math.cpp +++ b/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(const float amt, const float low, const float high, uint32_t line); +template double constrain_value_line(const double amt, const double low, const double high, uint32_t line); template 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 diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index 7af6e999bd..e00a8f9056 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/libraries/AP_Math/AP_Math.h @@ -20,6 +20,18 @@ #include "location.h" #include "control.h" +#if HAL_WITH_EKF_DOUBLE +typedef Vector2 Vector2F; +typedef Vector3 Vector3F; +typedef Matrix3 Matrix3F; +typedef QuaternionD QuaternionF; +#else +typedef Vector2 Vector2F; +typedef Vector3 Vector3F; +typedef Matrix3 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 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 -float sq(const T val) +ftype sq(const T val) { - float v = static_cast(val); + ftype v = static_cast(val); return v*v; } @@ -189,7 +202,7 @@ float sq(const T val) * dimension. */ template -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 -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 @@ -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/ diff --git a/libraries/AP_Math/examples/matrix_alg/matrix_alg.cpp b/libraries/AP_Math/examples/matrix_alg/matrix_alg.cpp index 6670bb4581..bb254a1af2 100644 --- a/libraries/AP_Math/examples/matrix_alg/matrix_alg.cpp +++ b/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; } diff --git a/libraries/AP_Math/ftype.h b/libraries/AP_Math/ftype.h new file mode 100644 index 0000000000..f8b53094e3 --- /dev/null +++ b/libraries/AP_Math/ftype.h @@ -0,0 +1,53 @@ +#pragma once + +/* + allow for builds with either single or double precision EKF + */ + +#include + +/* + 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 diff --git a/libraries/AP_Math/matrix3.cpp b/libraries/AP_Math/matrix3.cpp index 6ee6f4b39d..ed16beaff2 100644 --- a/libraries/AP_Math/matrix3.cpp +++ b/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 -void Matrix3::from_euler(float roll, float pitch, float yaw) +void Matrix3::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::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 -void Matrix3::to_euler(float *roll, float *pitch, float *yaw) const +void Matrix3::to_euler(T *roll, T *pitch, T *yaw) const { if (pitch != nullptr) { *pitch = -safe_asin(c.x); @@ -80,7 +80,7 @@ void Matrix3::from_rotation(enum Rotation rotation) template Vector3 Matrix3::to_euler312() const { - return Vector3(asinf(c.y), + return Vector3(asinF(c.y), atan2f(-c.x, c.z), atan2f(-a.y, b.y)); } @@ -89,14 +89,14 @@ Vector3 Matrix3::to_euler312() const fill the matrix from Euler angles in radians in 312 convention */ template -void Matrix3::from_euler312(float roll, float pitch, float yaw) +void Matrix3::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::rotate(const Vector3 &g) template void Matrix3::normalize(void) { - const float error = a * b; + const T error = a * b; const Vector3 t0 = a - (b * (0.5f * error)); const Vector3 t1 = b - (a * (0.5f * error)); const Vector3 t2 = t0 % t1; @@ -238,15 +238,15 @@ void Matrix3::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 -void Matrix3::from_axis_angle(const Vector3 &v, float theta) +void Matrix3::from_axis_angle(const Vector3 &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 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; diff --git a/libraries/AP_Math/matrix3.h b/libraries/AP_Math/matrix3.h index e3dae99946..3b2d8ed8c9 100644 --- a/libraries/AP_Math/matrix3.h +++ b/libraries/AP_Math/matrix3.h @@ -37,9 +37,14 @@ // #pragma once +#include "ftype.h" + #include "vector3.h" #include "vector2.h" +template +class Vector3; + // 3x3 matrix with elements of type T template 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 &v, float theta); + void from_axis_angle(const Vector3 &v, T theta); // normalize a rotation matrix void normalize(void); + // double/float conversion Matrix3 todouble(void) const { return Matrix3(a.todouble(), b.todouble(), c.todouble()); } diff --git a/libraries/AP_Math/quaternion.cpp b/libraries/AP_Math/quaternion.cpp index 1a34d5e0cf..98a376a59b 100644 --- a/libraries/AP_Math/quaternion.cpp +++ b/libraries/AP_Math/quaternion.cpp @@ -22,17 +22,43 @@ #include // return the rotation matrix equivalent for this quaternion -void Quaternion::rotation_matrix(Matrix3f &m) const +template +void QuaternionT::rotation_matrix(Matrix3d &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; + 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 +void QuaternionT::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 +template +void QuaternionT::rotation_matrix_norm(Matrix3 &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); + 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) +template +void QuaternionT::from_rotation_matrix(const Matrix3 &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 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 float tr = m00 + m11 + m22; + 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 +void QuaternionT::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 +void QuaternionT::rotate(enum Rotation rotation) { // create quaternion from rotation matrix - Quaternion q_from_rot; + QuaternionT 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 +void QuaternionT::earth_to_body(Vector3 &v) const { - Matrix3f m; + Matrix3 m; rotation_matrix(m); v = m * v; } // create a quaternion from Euler angles -void Quaternion::from_euler(float roll, float pitch, float yaw) +template +void QuaternionT::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 +void QuaternionT::from_vector312(T roll, T pitch, T yaw) { - Matrix3f m; + Matrix3 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 +void QuaternionT::from_axis_angle(Vector3 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 +void QuaternionT::from_axis_angle(const Vector3 &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 +void QuaternionT::rotate(const Vector3 &v) { - Quaternion r; + QuaternionT 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 +void QuaternionT::to_axis_angle(Vector3 &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(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 +void QuaternionT::from_axis_angle_fast(Vector3 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 +void QuaternionT::from_axis_angle_fast(const Vector3 &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 +void QuaternionT::rotate_fast(const Vector3 &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 +T QuaternionT::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 +T QuaternionT::get_euler_pitch() const { return safe_asin(2.0f*(q1*q3 - q4*q2)); } // get euler yaw angle -float Quaternion::get_euler_yaw() const +template +T QuaternionT::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 +void QuaternionT::to_euler(double &roll, double &pitch, double &yaw) const +{ + roll = get_euler_roll(); + pitch = get_euler_pitch(); + yaw = get_euler_yaw(); +} + +template +void QuaternionT::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 +Vector3 QuaternionT::to_vector312(void) const { - Matrix3f m; + Matrix3 m; rotation_matrix(m); return m.to_euler312(); } -float Quaternion::length(void) const +template +T QuaternionT::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 +QuaternionT QuaternionT::inverse(void) const { - return Quaternion(q1, -q2, -q3, -q4); + return QuaternionT(q1, -q2, -q3, -q4); } // reverse the rotation of this quaternion -void Quaternion::invert() +template +void QuaternionT::invert() { q2 = -q2; q3 = -q3; q4 = -q4; } -void Quaternion::normalize(void) +template +void QuaternionT::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 +QuaternionT QuaternionT::operator*(const QuaternionT &v) const { - Quaternion ret; - const float &w1 = q1; - const float &x1 = q2; - const float &y1 = q3; - const float &z1 = q4; + QuaternionT 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 +Vector3 QuaternionT::operator*(const Vector3 &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 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 +QuaternionT &QuaternionT::operator*=(const QuaternionT &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 +QuaternionT QuaternionT::operator/(const QuaternionT &v) const { - Quaternion ret; - const float &quat0 = q1; - const float &quat1 = q2; - const float &quat2 = q3; - const float &quat3 = q4; + QuaternionT 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 +QuaternionT QuaternionT::angular_difference(const QuaternionT &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 +T QuaternionT::roll_pitch_difference(const QuaternionT &v) const { // convert Quaternions to rotation matrices - Matrix3f m, vm; + Matrix3 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 z_unit_vec{0,0,1}; + const Vector3 z_unit_m = m.mul_transpose(z_unit_vec); + const Vector3 z_unit_vm = vm.mul_transpose(z_unit_vec); + const Vector3 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; +template class QuaternionT; diff --git a/libraries/AP_Math/quaternion.h b/libraries/AP_Math/quaternion.h index 126c3755e8..793c0b83ad 100644 --- a/libraries/AP_Math/quaternion.h +++ b/libraries/AP_Math/quaternion.h @@ -23,32 +23,33 @@ #endif #include -class Quaternion { +template +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() { q1 = 1; q2 = q3 = q4 = 0; } // setting constructor - Quaternion(const float _q1, const float _q2, const float _q3, const float _q4) : + QuaternionT(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(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 &m) const; // return the rotation matrix equivalent for this quaternion - void from_rotation_matrix(const Matrix3f &m); + void from_rotation_matrix(const Matrix3 &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 &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 &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 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 &axis, T theta); // rotate by the provided rotation vector - void rotate(const Vector3f &v); + void rotate(const Vector3 &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 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 &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 &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 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 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 operator*(const QuaternionT &v) const; + Vector3 operator*(const Vector3 &v) const; + QuaternionT &operator*=(const QuaternionT &v); + QuaternionT operator/(const QuaternionT &v) const; // angular difference between quaternions - Quaternion angular_difference(const Quaternion &v) const; + QuaternionT angular_difference(const QuaternionT &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 &v) const; + + // double/float conversion + QuaternionT todouble(void) const { + return QuaternionT(q1,q2,q3,q4); + } + QuaternionT tofloat(void) const { + return QuaternionT(q1,q2,q3,q4); + } }; + +typedef QuaternionT Quaternion; +typedef QuaternionT QuaternionD; + + + diff --git a/libraries/AP_Math/tests/test_math.cpp b/libraries/AP_Math/tests/test_math.cpp index c2972daaab..c4d091218d 100644 --- a/libraries/AP_Math/tests/test_math.cpp +++ b/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) diff --git a/libraries/AP_Math/tests/test_math_double.cpp b/libraries/AP_Math/tests/test_math_double.cpp index a744937c97..194508e5a4 100644 --- a/libraries/AP_Math/tests/test_math_double.cpp +++ b/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); diff --git a/libraries/AP_Math/vector2.cpp b/libraries/AP_Math/vector2.cpp index 2251ba1eb9..7bd72ce7d4 100644 --- a/libraries/AP_Math/vector2.cpp +++ b/libraries/AP_Math/vector2.cpp @@ -21,22 +21,22 @@ #include "AP_Math.h" template -float Vector2::length_squared() const +T Vector2::length_squared() const { - return (float)(x*x + y*y); + return (T)(x*x + y*y); } template -float Vector2::length(void) const +T Vector2::length(void) const { return norm(x, y); } // limit vector to a given length. returns true if vector was limited template -bool Vector2::limit_length(float max_length) +bool Vector2::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::operator !=(const Vector2 &v) const } template -float Vector2::angle(const Vector2 &v2) const +T Vector2::angle(const Vector2 &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 -float Vector2::angle(void) const +T Vector2::angle(void) const { return M_PI_2 + atan2f(-x, y); } @@ -174,16 +174,16 @@ bool Vector2::segment_intersection(const Vector2& seg1_start, const Vector const Vector2 r1 = seg1_end - seg1_start; const Vector2 r2 = seg2_end - seg2_start; const Vector2 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::segment_intersection(const Vector2& 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 -bool Vector2::circle_segment_intersection(const Vector2& seg_start, const Vector2& seg_end, const Vector2& circle_center, float radius, Vector2& intersection) +bool Vector2::circle_segment_intersection(const Vector2& seg_start, const Vector2& seg_end, const Vector2& circle_center, T radius, Vector2& intersection) { // calculate segment start and end as offsets from circle's center const Vector2 seg_start_local = seg_start - circle_center; @@ -209,16 +209,16 @@ bool Vector2::circle_segment_intersection(const Vector2& seg_start, const // calculate vector from start to end const Vector2 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::circle_segment_intersection(const Vector2& 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 Vector2::projected(const Vector2 &v) // extrapolate position given bearing (in degrees) and distance template -void Vector2::offset_bearing(float bearing, float distance) +void Vector2::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 Vector2 Vector2::closest_point(const Vector2 &p, const Vector2 &v, const Vector2 &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 Vector2::closest_point(const Vector2 &p, const Vector2 &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 Vector2 Vector2::closest_point(const Vector2 &p, const Vector2 &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(0,0); } else if (t >= 1) { @@ -379,9 +379,9 @@ Vector2 Vector2::closest_point(const Vector2 &p, const Vector2 &w) // closest distance between a line segment and a point // https://stackoverflow.com/questions/2824478/shortest-distance-between-two-line-segments template -float Vector2::closest_distance_between_line_and_point_squared(const Vector2 &w1, - const Vector2 &w2, - const Vector2 &p) +T Vector2::closest_distance_between_line_and_point_squared(const Vector2 &w1, + const Vector2 &w2, + const Vector2 &p) { return closest_distance_between_radial_and_point_squared(w2-w1, p-w1); } @@ -390,28 +390,28 @@ float Vector2::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 -float Vector2::closest_distance_between_line_and_point(const Vector2 &w1, - const Vector2 &w2, - const Vector2 &p) +T Vector2::closest_distance_between_line_and_point(const Vector2 &w1, + const Vector2 &w2, + const Vector2 &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 -float Vector2::closest_distance_between_lines_squared(const Vector2 &a1, - const Vector2 &a2, - const Vector2 &b1, - const Vector2 &b2) +T Vector2::closest_distance_between_lines_squared(const Vector2 &a1, + const Vector2 &a2, + const Vector2 &b1, + const Vector2 &b2) { - const float dist1 = Vector2::closest_distance_between_line_and_point_squared(b1,b2,a1); - const float dist2 = Vector2::closest_distance_between_line_and_point_squared(b1,b2,a2); - const float dist3 = Vector2::closest_distance_between_line_and_point_squared(a1,a2,b1); - const float dist4 = Vector2::closest_distance_between_line_and_point_squared(a1,a2,b2); - const float m1 = MIN(dist1,dist2); - const float m2 = MIN(dist3,dist4); + const T dist1 = Vector2::closest_distance_between_line_and_point_squared(b1,b2,a1); + const T dist2 = Vector2::closest_distance_between_line_and_point_squared(b1,b2,a2); + const T dist3 = Vector2::closest_distance_between_line_and_point_squared(a1,a2,b1); + const T dist4 = Vector2::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::closest_distance_between_lines_squared(const Vector2 &a1, // p is a point // returns the square of the closest distance between the radial and the point template -float Vector2::closest_distance_between_radial_and_point_squared(const Vector2 &w, - const Vector2 &p) +T Vector2::closest_distance_between_radial_and_point_squared(const Vector2 &w, + const Vector2 &p) { const Vector2 closest = closest_point(p, w); return (closest - p).length_squared(); @@ -430,20 +430,20 @@ float Vector2::closest_distance_between_radial_and_point_squared(const Vector // p is a point // returns the closest distance between the radial and the point template -float Vector2::closest_distance_between_radial_and_point(const Vector2 &w, +T Vector2::closest_distance_between_radial_and_point(const Vector2 &w, const Vector2 &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 -void Vector2::rotate(float angle_rad) +void Vector2::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::operator ==(const Vector2 &v) const; template bool Vector2::operator !=(const Vector2 &v) const; template bool Vector2::operator ==(const Vector2 &v) const; template bool Vector2::operator !=(const Vector2 &v) const; + diff --git a/libraries/AP_Math/vector2.h b/libraries/AP_Math/vector2.h index 0b087268fd..f5ffec11f7 100644 --- a/libraries/AP_Math/vector2.h +++ b/libraries/AP_Math/vector2.h @@ -35,6 +35,7 @@ #include #include +#include "ftype.h" template 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 &v2) const; + T angle(const Vector2 &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 projected(const Vector2 &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 &w1, + static T closest_distance_between_line_and_point_squared(const Vector2 &w1, const Vector2 &w2, const Vector2 &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 &w1, + static T closest_distance_between_line_and_point(const Vector2 &w1, const Vector2 &w2, const Vector2 &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 &a1, + static T closest_distance_between_lines_squared(const Vector2 &a1, const Vector2 &a2, const Vector2 &b1, const Vector2 &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 &w, + static T closest_distance_between_radial_and_point_squared(const Vector2 &w, const Vector2 &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 &w, + static T closest_distance_between_radial_and_point(const Vector2 &w, const Vector2 &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& seg_start, const Vector2& seg_end, const Vector2& circle_center, float radius, Vector2& intersection) WARN_IF_UNUSED; + static bool circle_segment_intersection(const Vector2& seg_start, const Vector2& seg_end, const Vector2& circle_center, T radius, Vector2& 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& point, const Vector2& seg_start, const Vector2& 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 Vector2ui; typedef Vector2 Vector2l; typedef Vector2 Vector2ul; typedef Vector2 Vector2f; -typedef Vector2 Vector2d; +typedef Vector2 Vector2d; diff --git a/libraries/AP_Math/vector3.cpp b/libraries/AP_Math/vector3.cpp index 71ffc08944..0457579010 100644 --- a/libraries/AP_Math/vector3.cpp +++ b/libraries/AP_Math/vector3.cpp @@ -31,8 +31,8 @@ void Vector3::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::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::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::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::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::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::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::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::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::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::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::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::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::rotate_inverse(enum Rotation rotation) // rotate vector by angle in radians in xy plane leaving z untouched template -void Vector3::rotate_xy(float angle_rad) +void Vector3::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::operator *(const Vector3 &v) const } template -float Vector3::length(void) const +T Vector3::length(void) const { return norm(x, y, z); } // limit xy component vector to a given length. returns true if vector was limited template -bool Vector3::limit_length_xy(float max_length) +bool Vector3::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::operator !=(const Vector3 &v) const } template -float Vector3::angle(const Vector3 &v2) const +T Vector3::angle(const Vector3 &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 Vector3::mul_rowcol(const Vector3 &v2) const // extrapolate position given bearing and pitch (in degrees) and distance template -void Vector3::offset_bearing(float bearing, float pitch, float distance) +void Vector3::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 -float Vector3::distance_to_segment(const Vector3 &seg_start, const Vector3 &seg_end) const +T Vector3::distance_to_segment(const Vector3 &seg_start, const Vector3 &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::distance_to_segment(const Vector3 &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 -float Vector3::closest_distance_between_line_and_point(const Vector3 &w1, const Vector3 &w2, const Vector3 &p) +T Vector3::closest_distance_between_line_and_point(const Vector3 &w1, const Vector3 &w2, const Vector3 &p) { const Vector3 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 Vector3::point_on_line_closest_to_other_point(const Vector3 &w1 const Vector3 line_vec = w2-w1; const Vector3 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 unit_vec = line_vec * scale; const Vector3 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 closest_point = line_vec * dot_product; return (closest_point + w1); @@ -525,15 +525,15 @@ void Vector3::segment_to_segment_closest_point(const Vector3& seg1_start, const Vector3 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::segment_plane_intersect(const Vector3& seg_start, const Vect Vector3 u = seg_end - seg_start; Vector3 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::segment_plane_intersect(const Vector3& 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; diff --git a/libraries/AP_Math/vector3.h b/libraries/AP_Math/vector3.h index c5ad55ea5f..9f42a8f0c6 100644 --- a/libraries/AP_Math/vector3.h +++ b/libraries/AP_Math/vector3.h @@ -60,6 +60,8 @@ #include "rotations.h" +#include "ftype.h" + template class Matrix3; @@ -164,12 +166,12 @@ public: } // scale a vector3 - Vector3 scale(const float v) const { + Vector3 scale(const T v) const { return *this * v; } // computes the angle between this vector and another vector - float angle(const Vector3 &v2) const; + T angle(const Vector3 &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 &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 &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 &seg_start, const Vector3 &seg_end) const; + T distance_to_segment(const Vector3 &seg_start, const Vector3 &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 &w1, const Vector3 &w2, const Vector3 &p); + static T closest_distance_between_line_and_point(const Vector3 &w1, const Vector3 &w2, const Vector3 &p); // Point in the line segment defined by w1,w2 which is closest to point(p) static Vector3 point_on_line_closest_to_other_point(const Vector3 &w1, const Vector3 &w2, const Vector3 &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& seg1_start, const Vector3& seg1_end, const Vector3& seg2_start, const Vector3& seg2_end, Vector3& closest_point);