Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: use ftype for a few internal trig fns

Browse Source
This commit is contained in:
Andrew Tridgell 2021-07-09 12:20:50 +10:00
parent a6a01cf432
commit afb928081a
4 changed files with 18 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_Math/AP_Math.h
View File

@ -143,13 +143,13 @@ double wrap_360_cd(const double angle);
wrap an angle in radians to -PI ~ PI (equivalent to +- 180 degrees) wrap an angle in radians to -PI ~ PI (equivalent to +- 180 degrees)
*/ */
template <typename T> template <typename T>
float wrap_PI(const T radian); ftype wrap_PI(const T radian);
/* /*
* wrap an angle in radians to 0..2PI * wrap an angle in radians to 0..2PI
*/ */
template <typename T> template <typename T>
float wrap_2PI(const T radian); ftype wrap_2PI(const T radian);
/* /*
* Constrain a value to be within the range: low and high * Constrain a value to be within the range: low and high
@ -179,7 +179,7 @@ inline int64_t constrain_int64(const int64_t amt, const int64_t low, const int64
} }
// degrees -> radians // degrees -> radians
static inline constexpr float radians(float deg) static inline constexpr ftype radians(ftype deg)
{ {
return deg * DEG_TO_RAD; return deg * DEG_TO_RAD;
} }

2
libraries/AP_Math/definitions.h
View File

@ -7,7 +7,7 @@
#ifdef M_PI #ifdef M_PI
# undef M_PI # undef M_PI
#endif #endif
#define M_PI (3.141592653589793f) #define M_PI (3.141592653589793)
#ifdef M_PI_2 #ifdef M_PI_2
# undef M_PI_2 # undef M_PI_2

8
libraries/AP_Math/matrix3.cpp
View File

@ -52,10 +52,10 @@ void Matrix3<T>::to_euler(T *roll, T *pitch, T *yaw) const
*pitch = -safe_asin(c.x); *pitch = -safe_asin(c.x);
} }
if (roll != nullptr) { if (roll != nullptr) {
*roll = atan2f(c.y, c.z); *roll = atan2F(c.y, c.z);
} }
if (yaw != nullptr) { if (yaw != nullptr) {
*yaw = atan2f(b.x, a.x); *yaw = atan2F(b.x, a.x);
} }
} }
@ -81,8 +81,8 @@ 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));
} }
/* /*

20
libraries/AP_Math/quaternion.cpp
View File

@ -481,9 +481,9 @@ void QuaternionT<T>::from_axis_angle(const Vector3<T> &axis, T theta)
q2=q3=q4=0.0f; q2=q3=q4=0.0f;
return; return;
} }
const T st2 = sinF(theta/2.0f); const T st2 = sinF(0.5*theta);
q1 = cosF(theta/2.0f); q1 = cosF(0.5*theta);
q2 = axis.x * st2; q2 = axis.x * st2;
q3 = axis.y * st2; q3 = axis.y * st2;
q4 = axis.z * st2; q4 = axis.z * st2;
@ -507,7 +507,7 @@ void QuaternionT<T>::to_axis_angle(Vector3<T> &v) const
v = Vector3<T>(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));
} }
} }
@ -531,11 +531,11 @@ void QuaternionT<T>::from_axis_angle_fast(Vector3<T> v)
template <typename T> template <typename T>
void QuaternionT<T>::from_axis_angle_fast(const Vector3<T> &axis, T theta) void QuaternionT<T>::from_axis_angle_fast(const Vector3<T> &axis, T theta)
{ {
const T t2 = theta/2.0f; const T t2 = 0.5*theta;
const T sqt2 = sq(t2); const T sqt2 = sq(t2);
const T 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-(0.5*sqt2)+sq(sqt2)/24.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;
@ -550,13 +550,13 @@ void QuaternionT<T>::rotate_fast(const Vector3<T> &v)
if (is_zero(theta)) { if (is_zero(theta)) {
return; return;
} }
const T t2 = theta/2.0f; const T t2 = 0.5*theta;
const T sqt2 = sq(t2); const T sqt2 = sq(t2);
T st2 = t2-sqt2*t2/6.0f; T st2 = t2-sqt2*t2/6.0f;
st2 /= theta; st2 /= theta;
//"rotation quaternion" //"rotation quaternion"
const T w2 = 1.0f-(sqt2/2.0f)+sq(sqt2)/24.0f; const T w2 = 1.0f-(0.5*sqt2)+sq(sqt2)/24.0f;
const T x2 = v.x * st2; const T x2 = v.x * st2;
const T y2 = v.y * st2; const T y2 = v.y * st2;
const T z2 = v.z * st2; const T z2 = v.z * st2;
@ -578,7 +578,7 @@ void QuaternionT<T>::rotate_fast(const Vector3<T> &v)
template <typename T> template <typename T>
T QuaternionT<T>::get_euler_roll() const 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
@ -592,7 +592,7 @@ T QuaternionT<T>::get_euler_pitch() const
template <typename T> template <typename T>
T QuaternionT<T>::get_euler_yaw() const 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
@ -772,7 +772,7 @@ T QuaternionT<T>::roll_pitch_difference(const QuaternionT<T> &v) const
const T vec_len_div2 = constrain_float(vec_diff.length() * 0.5, 0.0, 1.0); 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.0 * asinf(vec_len_div2)); return (2.0 * asinF(vec_len_div2));
} }
// define for float and double // define for float and double