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

AP_Math: use is_zero and float constants

This commit is contained in:
Leonard Hall 2016-06-24 16:14:52 +09:00 committed by Randy Mackay
parent 7f933140c7
commit e0eecd56b2
1 changed files with 12 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_Math/quaternion.cpp
View File

@ -99,19 +99,19 @@ void Quaternion::from_rotation_matrix(const Matrix3f &m)
qy = (m02 - m20) / S;
qz = (m10 - m01) / S;
} else if ((m00 > m11) && (m00 > m22)) {
float S = sqrtf(1.0f + m00 - m11 - m22) * 2;
float 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) {
float S = sqrtf(1.0f + m11 - m00 - m22) * 2;
float 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 {
float S = sqrtf(1.0f + m22 - m00 - m11) * 2;
float S = sqrtf(1.0f + m22 - m00 - m11) * 2.0f;
qw = (m10 - m01) / S;
qx = (m02 + m20) / S;
qy = (m12 + m21) / S;
@ -155,7 +155,7 @@ void Quaternion::from_vector312(float roll ,float pitch, float yaw)
void Quaternion::from_axis_angle(Vector3f v)
{
float theta = v.length();
if (theta < 1.0e-12f) {
if (is_zero(theta)) {
q1 = 1.0f;
q2=q3=q4=0.0f;
return;
@ -166,7 +166,8 @@ void Quaternion::from_axis_angle(Vector3f v)
void Quaternion::from_axis_angle(const Vector3f &axis, float theta)
{
if (theta < 1.0e-12f) {
// axis must be a unit vector as there is no check for length
if (is_zero(theta)) {
q1 = 1.0f;
q2=q3=q4=0.0f;
}
@ -189,7 +190,7 @@ void Quaternion::to_axis_angle(Vector3f &v)
{
float l = sqrt(sq(q2)+sq(q3)+sq(q4));
v = Vector3f(q2,q3,q4);
if (l >= 1.0e-12f) {
if (!is_zero(l)) {
v /= l;
v *= wrap_PI(2.0f * atan2f(l,q1));
}
@ -198,7 +199,7 @@ void Quaternion::to_axis_angle(Vector3f &v)
void Quaternion::from_axis_angle_fast(Vector3f v)
{
float theta = v.length();
if (theta < 1.0e-12f) {
if (is_zero(theta)) {
q1 = 1.0f;
q2=q3=q4=0.0f;
}
@ -221,7 +222,7 @@ void Quaternion::from_axis_angle_fast(const Vector3f &axis, float theta)
void Quaternion::rotate_fast(const Vector3f &v)
{
float theta = v.length();
if (theta < 1.0e-12f) {
if (is_zero(theta)) {
return;
}
float t2 = theta/2.0f;
@ -251,7 +252,7 @@ void Quaternion::rotate_fast(const Vector3f &v)
// get euler roll angle
float Quaternion::get_euler_roll() const
{
return (atan2f(2.0f*(q1*q2 + q3*q4), 1 - 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
@ -263,7 +264,7 @@ float Quaternion::get_euler_pitch() const
// get euler yaw angle
float Quaternion::get_euler_yaw() const
{
return atan2f(2.0f*(q1*q4 + q2*q3), 1 - 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
@ -295,7 +296,7 @@ Quaternion Quaternion::inverse(void) const
void Quaternion::normalize(void)
{
float quatMag = length();
if (quatMag > 1e-16f) {
if (!is_zero(quatMag)) {
float quatMagInv = 1.0f/quatMag;
q1 *= quatMagInv;
q2 *= quatMagInv;