Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Math: allow for double EKF build

This commit is contained in:
Andrew Tridgell 2021-05-04 21:12:23 +10:00
parent c6c197d4fb
commit e7afa628d1
14 changed files with 553 additions and 384 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_Math/AP_Math.cpp
View File

@ -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 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> template <typename T>
T constrain_value(const T amt, const T low, const T high) 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 * rot_ef_to_bf is a rotation matrix to rotate from earth-frame (NED) to body frame
* angular_rate is rad/sec * 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()) { if (sensor_offset_bf.is_zero()) {
return Vector3f(); return Vector3F();
} }
// correct velocity // correct velocity
const Vector3f vel_offset_body = angular_rate % sensor_offset_bf; const Vector3F vel_offset_body = angular_rate % sensor_offset_bf;
return rot_ef_to_bf.mul_transpose(vel_offset_body) * -1.0f; 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; *f++ = n;
} }
} }
void fill_nanf(double *f, uint16_t count)
{
while (count--) {
*f++ = 0;
}
}
#endif #endif
// Convert 16-bit fixed-point to float // Convert 16-bit fixed-point to float

26
libraries/AP_Math/AP_Math.h
View File

@ -20,6 +20,18 @@
#include "location.h" #include "location.h"
#include "control.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 // define AP_Param types AP_Vector3f and Ap_Matrix3f
AP_PARAMDEFV(Vector3f, Vector3f, AP_PARAM_VECTOR3F); 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); 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_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) 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> 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; return v*v;
} }
@ -189,7 +202,7 @@ float sq(const T val)
* dimension. * dimension.
*/ */
template<typename T, typename... Params> 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...); return sq(first) + sq(parameters...);
} }
@ -199,9 +212,9 @@ float sq(const T first, const Params... parameters)
* dimension. * dimension.
*/ */
template<typename T, typename U, typename... Params> 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> 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 * rot_ef_to_bf is a rotation matrix to rotate from earth-frame (NED) to body frame
* angular_rate is rad/sec * 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 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 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
// fill an array of float with NaN, used to invalidate memory in 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(float *f, uint16_t count);
void fill_nanf(double *f, uint16_t count);
#endif #endif
// from https://embeddedartistry.com/blog/2018/07/12/simple-fixed-point-conversion-in-c/ // from https://embeddedartistry.com/blog/2018/07/12/simple-fixed-point-conversion-in-c/

10
libraries/AP_Math/examples/matrix_alg/matrix_alg.cpp
View File

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

53
libraries/AP_Math/ftype.h Normal file
View File

@ -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

48
libraries/AP_Math/matrix3.cpp
View File

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

14
libraries/AP_Math/matrix3.h
View File

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

345
libraries/AP_Math/quaternion.cpp
View File

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

86
libraries/AP_Math/quaternion.h
View File

@ -23,32 +23,33 @@
#endif #endif
#include <math.h> #include <math.h>
class Quaternion { template <typename T>
class QuaternionT {
public: public:
float q1, q2, q3, q4; T q1, q2, q3, q4;
// constructor creates a quaternion equivalent // constructor creates a quaternion equivalent
// to roll=0, pitch=0, yaw=0 // to roll=0, pitch=0, yaw=0
Quaternion() QuaternionT<T>()
{ {
q1 = 1; q1 = 1;
q2 = q3 = q4 = 0; q2 = q3 = q4 = 0;
} }
// setting constructor // 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) q1(_q1), q2(_q2), q3(_q3), q4(_q4)
{ {
} }
// setting constructor // 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]) q1(_q[0]), q2(_q[1]), q3(_q[2]), q4(_q[3])
{ {
} }
// function call operator // 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; q1 = _q1;
q2 = _q2; q2 = _q2;
@ -64,12 +65,13 @@ public:
// return the rotation matrix equivalent for this quaternion // return the rotation matrix equivalent for this quaternion
void rotation_matrix(Matrix3f &m) const; void rotation_matrix(Matrix3f &m) const;
void rotation_matrix(Matrix3d &m) const;
// return the rotation matrix equivalent for this quaternion after normalization // 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 // 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 // create a quaternion from a given rotation
void from_rotation(enum Rotation rotation); void from_rotation(enum Rotation rotation);
@ -78,58 +80,59 @@ public:
void rotate(enum Rotation rotation); void rotate(enum Rotation rotation);
// convert a vector from earth to body frame // 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 // 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 // create a quaternion from Euler angles applied in yaw, roll, pitch order
// instead of the normal yaw, pitch, roll 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 // 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 // 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 // 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 // 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 // create a quaternion from its axis-angle representation
// the axis vector must be length 1. the rotation angle theta is in radians // 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 // rotate by the provided rotation vector
void rotate(const Vector3f &v); void rotate(const Vector3<T> &v);
// create a quaternion from a rotation vector // 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) // 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 // 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) // 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 // 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) // 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 // get euler roll angle
float get_euler_roll() const; T get_euler_roll() const;
// get euler pitch angle // get euler pitch angle
float get_euler_pitch() const; T get_euler_pitch() const;
// get euler yaw angle // get euler yaw angle
float get_euler_yaw() const; T get_euler_yaw() const;
// create eulers from a quaternion // create eulers from a quaternion
void to_euler(float &roll, float &pitch, float &yaw) const; 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 // 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(); void normalize();
// initialise the quaternion to no rotation // initialise the quaternion to no rotation
@ -139,39 +142,52 @@ public:
q2 = q3 = q4 = 0.0f; q2 = q3 = q4 = 0.0f;
} }
Quaternion inverse(void) const; QuaternionT<T> inverse(void) const;
// reverse the rotation of this quaternion // reverse the rotation of this quaternion
void invert(); void invert();
// allow a quaternion to be used as an array, 0 indexed // 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 #if MATH_CHECK_INDEXES
assert(i < 4); assert(i < 4);
#endif #endif
return _v[i]; 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 #if MATH_CHECK_INDEXES
assert(i < 4); assert(i < 4);
#endif #endif
return _v[i]; return _v[i];
} }
Quaternion operator*(const Quaternion &v) const; QuaternionT<T> operator*(const QuaternionT<T> &v) const;
Vector3<T> operator*(const Vector3<T> &v) const;
Vector3f operator*(const Vector3f &v) const; QuaternionT<T> &operator*=(const QuaternionT<T> &v);
Quaternion &operator*=(const Quaternion &v); QuaternionT<T> operator/(const QuaternionT<T> &v) const;
Quaternion operator/(const Quaternion &v) const;
// angular difference between quaternions // 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 // 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;

10
libraries/AP_Math/tests/test_math.cpp
View File

@ -624,11 +624,11 @@ TEST(MathTest, RANDOM16)
TEST(MathTest, VELCORRECTION) TEST(MathTest, VELCORRECTION)
{ {
static constexpr Vector3f pos{1.0f, 1.0f, 0.0f}; 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 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}; 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(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)); EXPECT_TRUE(Vector3F() == get_vel_correction_for_sensor_offset(Vector3f(), rot, rate));
} }
TEST(MathTest, LOWPASSALPHA) TEST(MathTest, LOWPASSALPHA)

4
libraries/AP_Math/tests/test_math_double.cpp
View File

@ -224,9 +224,9 @@ TEST(MathTest, NormDouble)
double norm_5 = norm(3,4); double norm_5 = norm(3,4);
double norm_6 = norm(4.0,3.0,12.0); 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_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_4, 0.0);
EXPECT_DOUBLE_EQ(norm_5, 5.0); EXPECT_DOUBLE_EQ(norm_5, 5.0);
EXPECT_DOUBLE_EQ(norm_6, 13.0); EXPECT_DOUBLE_EQ(norm_6, 13.0);

95
libraries/AP_Math/vector2.cpp
View File

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

35
libraries/AP_Math/vector2.h
View File

@ -35,6 +35,7 @@
#include <cmath> #include <cmath>
#include <AP_Common/AP_Common.h> #include <AP_Common/AP_Common.h>
#include "ftype.h"
template <typename T> template <typename T>
struct Vector2 struct Vector2
@ -92,11 +93,11 @@ struct Vector2
// computes the angle between this vector and another vector // computes the angle between this vector and another vector
// returns 0 if the vectors are parallel, and M_PI if they are antiparallel // 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, // 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 // 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 // check if any elements are NAN
bool is_nan(void) const WARN_IF_UNUSED; bool is_nan(void) const WARN_IF_UNUSED;
@ -133,13 +134,13 @@ struct Vector2
} }
// gets the length of this vector squared // gets the length of this vector squared
float length_squared() const; T length_squared() const;
// gets the length of this vector // 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 // 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 // normalizes this vector
void normalize(); void normalize();
@ -157,10 +158,10 @@ struct Vector2
Vector2<T> projected(const Vector2<T> &v); Vector2<T> projected(const Vector2<T> &v);
// adjust position by a given bearing (in degrees) and distance // 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 // rotate vector by angle in radians
void rotate(float angle_rad); void rotate(T angle_rad);
/* /*
conversion to/from double conversion to/from double
@ -194,20 +195,20 @@ struct Vector2
// w1 and w2 define a line segment // w1 and w2 define a line segment
// p is a point // p is a point
// returns the square of the closest distance between the line segment and the 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> &w2,
const Vector2<T> &p); const Vector2<T> &p);
// w1 and w2 define a line segment // w1 and w2 define a line segment
// p is a point // p is a point
// returns the closest distance between the line segment and the 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> &w2,
const Vector2<T> &p); const Vector2<T> &p);
// a1->a2 and b2->v2 define two line segments // a1->a2 and b2->v2 define two line segments
// returns the square of the closest distance between the 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> &a2,
const Vector2<T> &b1, const Vector2<T> &b1,
const Vector2<T> &b2); const Vector2<T> &b2);
@ -215,13 +216,13 @@ struct Vector2
// w defines a line segment from the origin // w defines a line segment from the origin
// p is a point // p is a point
// returns the square of the closest distance between the radial and the 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); const Vector2<T> &p);
// w defines a line segment from the origin // w defines a line segment from the origin
// p is a point // p is a point
// returns the closest distance between the radial and the 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); const Vector2<T> &p);
// find the intersection between two line segments // find the intersection between two line segments
@ -231,22 +232,22 @@ struct Vector2
// find the intersection between a line segment and a circle // 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 // 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 // check if a point falls on the line segment from seg_start to seg_end
static bool point_on_segment(const Vector2<T>& point, static bool point_on_segment(const Vector2<T>& point,
const Vector2<T>& seg_start, const Vector2<T>& seg_start,
const Vector2<T>& seg_end) WARN_IF_UNUSED { const Vector2<T>& seg_end) WARN_IF_UNUSED {
const float expected_run = seg_end.x-seg_start.x; const T expected_run = seg_end.x-seg_start.x;
const float intersection_run = point.x-seg_start.x; const T intersection_run = point.x-seg_start.x;
// check slopes are identical: // check slopes are identical:
if (fabsf(expected_run) < FLT_EPSILON) { if (fabsf(expected_run) < FLT_EPSILON) {
if (fabsf(intersection_run) > FLT_EPSILON) { if (fabsf(intersection_run) > FLT_EPSILON) {
return false; return false;
} }
} else { } else {
const float expected_slope = (seg_end.y-seg_start.y)/expected_run; const T expected_slope = (seg_end.y-seg_start.y)/expected_run;
const float intersection_slope = (point.y-seg_start.y)/intersection_run; const T intersection_slope = (point.y-seg_start.y)/intersection_run;
if (fabsf(expected_slope - intersection_slope) > FLT_EPSILON) { if (fabsf(expected_slope - intersection_slope) > FLT_EPSILON) {
return false; return false;
} }

150
libraries/AP_Math/vector3.cpp
View File

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

27
libraries/AP_Math/vector3.h
View File

@ -60,6 +60,8 @@
#include "rotations.h" #include "rotations.h"
#include "ftype.h"
template <typename T> template <typename T>
class Matrix3; class Matrix3;
@ -164,12 +166,12 @@ public:
} }
// scale a vector3 // scale a vector3
Vector3<T> scale(const float v) const { Vector3<T> scale(const T v) const {
return *this * v; return *this * v;
} }
// computes the angle between this vector and another vector // 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 // check if any elements are NAN
bool is_nan(void) const WARN_IF_UNUSED; bool is_nan(void) const WARN_IF_UNUSED;
@ -190,7 +192,7 @@ public:
void rotate_inverse(enum Rotation rotation); void rotate_inverse(enum Rotation rotation);
// rotate vector by angle in radians in xy plane leaving z untouched // 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. // return xy components of a vector3 as a vector2.
// this returns a reference to the original vector3 xy data // this returns a reference to the original vector3 xy data
@ -208,10 +210,10 @@ public:
} }
// gets the length of this vector // 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 // 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 // normalizes this vector
void normalize() 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) // 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 { T distance_squared(const Vector3<T> &v) const {
const float dist_x = x-v.x; const T dist_x = x-v.x;
const float dist_y = y-v.y; const T dist_y = y-v.y;
const float dist_z = z-v.z; const T dist_z = z-v.z;
return (dist_x*dist_x + dist_y*dist_y + dist_z*dist_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 // 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 // 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 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 // 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) // 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); 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 // This implementation is borrowed from: http://geomalgorithms.com/a07-_distance.html
// INPUT: 4 points corresponding to start and end of two line segments // 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" // 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); 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);