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

26
libraries/AP_Math/AP_Math.h
View File

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

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
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;
}

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

14
libraries/AP_Math/matrix3.h
View File

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

345
libraries/AP_Math/quaternion.cpp
View File

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

86
libraries/AP_Math/quaternion.h
View File

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

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

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

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_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);

111
libraries/AP_Math/vector2.cpp
View File

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

37
libraries/AP_Math/vector2.h
View File

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

150
libraries/AP_Math/vector3.cpp
View File

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

27
libraries/AP_Math/vector3.h
View File

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