From 39f72610e24965261b868fc6212c6a1235b44d75 Mon Sep 17 00:00:00 2001 From: Gustavo Jose de Sousa Date: Wed, 4 May 2016 10:33:57 -0300 Subject: [PATCH] AP_Math: quaternion: fix style Remove trailing spaces and run astyle. --- libraries/AP_Math/quaternion.cpp | 67 +++++++++++++++++++------------- libraries/AP_Math/quaternion.h | 31 ++++++++++----- 2 files changed, 60 insertions(+), 38 deletions(-) diff --git a/libraries/AP_Math/quaternion.cpp b/libraries/AP_Math/quaternion.cpp index b2a27bffba..2ffa7ae54f 100644 --- a/libraries/AP_Math/quaternion.cpp +++ b/libraries/AP_Math/quaternion.cpp @@ -70,21 +70,21 @@ void Quaternion::from_rotation_matrix(const Matrix3f &m) float 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)) { + qy = (m02 - m20) / S; + qz = (m10 - m01) / S; + } else if ((m00 > m11) && (m00 > m22)) { float S = sqrtf(1.0f + m00 - m11 - m22) * 2; qw = (m21 - m12) / S; qx = 0.25f * S; - qy = (m01 + m10) / S; - qz = (m02 + m20) / S; - } else if (m11 > m22) { + qy = (m01 + m10) / S; + qz = (m02 + m20) / S; + } else if (m11 > m22) { float S = sqrtf(1.0f + m11 - m00 - m22) * 2; qw = (m02 - m20) / S; - qx = (m01 + m10) / S; + qx = (m01 + m10) / S; qy = 0.25f * S; - qz = (m12 + m21) / S; - } else { + qz = (m12 + m21) / S; + } else { float S = sqrtf(1.0f + m22 - m00 - m11) * 2; qw = (m10 - m01) / S; qx = (m02 + m20) / S; @@ -126,9 +126,10 @@ void Quaternion::from_vector312(float roll ,float pitch, float yaw) from_rotation_matrix(m); } -void Quaternion::from_axis_angle(Vector3f v) { +void Quaternion::from_axis_angle(Vector3f v) +{ float theta = v.length(); - if(theta < 1.0e-12f) { + if (theta < 1.0e-12f) { q1 = 1.0f; q2=q3=q4=0.0f; return; @@ -137,8 +138,9 @@ void Quaternion::from_axis_angle(Vector3f v) { from_axis_angle(v,theta); } -void Quaternion::from_axis_angle(const Vector3f &axis, float theta) { - if(theta < 1.0e-12f) { +void Quaternion::from_axis_angle(const Vector3f &axis, float theta) +{ + if (theta < 1.0e-12f) { q1 = 1.0f; q2=q3=q4=0.0f; } @@ -150,24 +152,27 @@ void Quaternion::from_axis_angle(const Vector3f &axis, float theta) { q4 = axis.z * st2; } -void Quaternion::rotate(const Vector3f &v) { +void Quaternion::rotate(const Vector3f &v) +{ Quaternion r; r.from_axis_angle(v); (*this) *= r; } -void Quaternion::to_axis_angle(Vector3f &v) { +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 (l >= 1.0e-12f) { v /= l; v *= wrap_PI(2.0f * atan2f(l,q1)); } } -void Quaternion::from_axis_angle_fast(Vector3f v) { +void Quaternion::from_axis_angle_fast(Vector3f v) +{ float theta = v.length(); - if(theta < 1.0e-12f) { + if (theta < 1.0e-12f) { q1 = 1.0f; q2=q3=q4=0.0f; } @@ -175,7 +180,8 @@ void Quaternion::from_axis_angle_fast(Vector3f v) { from_axis_angle_fast(v,theta); } -void Quaternion::from_axis_angle_fast(const Vector3f &axis, float theta) { +void Quaternion::from_axis_angle_fast(const Vector3f &axis, float theta) +{ float t2 = theta/2.0f; float sqt2 = sq(t2); float st2 = t2-sqt2*t2/6.0f; @@ -186,9 +192,12 @@ void Quaternion::from_axis_angle_fast(const Vector3f &axis, float theta) { q4 = axis.z * st2; } -void Quaternion::rotate_fast(const Vector3f &v) { +void Quaternion::rotate_fast(const Vector3f &v) +{ float theta = v.length(); - if(theta < 1.0e-12f) return; + if (theta < 1.0e-12f) { + return; + } float t2 = theta/2.0f; float sqt2 = sq(t2); float st2 = t2-sqt2*t2/6.0f; @@ -241,7 +250,7 @@ void Quaternion::to_euler(float &roll, float &pitch, float &yaw) const // create eulers from a quaternion Vector3f Quaternion::to_vector312(void) const -{ +{ Matrix3f m; rotation_matrix(m); return m.to_euler312(); @@ -260,17 +269,17 @@ Quaternion Quaternion::inverse(void) const void Quaternion::normalize(void) { float quatMag = length(); - if (quatMag > 1e-16f) - { + if (quatMag > 1e-16f) { float quatMagInv = 1.0f/quatMag; q1 *= quatMagInv; q2 *= quatMagInv; q3 *= quatMagInv; q4 *= quatMagInv; - } + } } -Quaternion Quaternion::operator*(const Quaternion &v) const { +Quaternion Quaternion::operator*(const Quaternion &v) const +{ Quaternion ret; const float &w1 = q1; const float &x1 = q2; @@ -290,7 +299,8 @@ Quaternion Quaternion::operator*(const Quaternion &v) const { return ret; } -Quaternion &Quaternion::operator*=(const Quaternion &v) { +Quaternion &Quaternion::operator*=(const Quaternion &v) +{ float w1 = q1; float x1 = q2; float y1 = q3; @@ -309,7 +319,8 @@ Quaternion &Quaternion::operator*=(const Quaternion &v) { return *this; } -Quaternion Quaternion::operator/(const Quaternion &v) const { +Quaternion Quaternion::operator/(const Quaternion &v) const +{ Quaternion ret; const float &quat0 = q1; const float &quat1 = q2; diff --git a/libraries/AP_Math/quaternion.h b/libraries/AP_Math/quaternion.h index 2487dd60ad..37845c7663 100644 --- a/libraries/AP_Math/quaternion.h +++ b/libraries/AP_Math/quaternion.h @@ -23,26 +23,31 @@ #include #endif -class Quaternion -{ +class Quaternion { public: float q1, q2, q3, q4; // constructor creates a quaternion equivalent // to roll=0, pitch=0, yaw=0 - Quaternion() { - q1 = 1; q2 = q3 = q4 = 0; + Quaternion() + { + q1 = 1; + q2 = q3 = q4 = 0; } // setting constructor Quaternion(const float _q1, const float _q2, const float _q3, const float _q4) : - q1(_q1), q2(_q2), q3(_q3), q4(_q4) { + q1(_q1), q2(_q2), q3(_q3), q4(_q4) + { } // function call operator - void operator ()(const float _q1, const float _q2, const float _q3, const float _q4) + void operator()(const float _q1, const float _q2, const float _q3, const float _q4) { - q1 = _q1; q2 = _q2; q3 = _q3; q4 = _q4; + q1 = _q1; + q2 = _q2; + q3 = _q3; + q4 = _q4; } // check if any elements are NAN @@ -97,12 +102,17 @@ public: void normalize(); // initialise the quaternion to no rotation - void initialise() { q1 = 1.0f; q2 = q3 = q4 = 0.0f; } + void initialise() + { + q1 = 1.0f; + q2 = q3 = q4 = 0.0f; + } Quaternion inverse(void) const; // allow a quaternion to be used as an array, 0 indexed - float & operator[](uint8_t i) { + float & operator[](uint8_t i) + { float *_v = &q1; #if MATH_CHECK_INDEXES assert(i < 4); @@ -110,7 +120,8 @@ public: return _v[i]; } - const float & operator[](uint8_t i) const { + const float & operator[](uint8_t i) const + { const float *_v = &q1; #if MATH_CHECK_INDEXES assert(i < 4);