From b1f76782f6812f502022fbf2e8505656f262f941 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 5 Jul 2016 23:00:35 +0200 Subject: [PATCH] Euler, Quaternion: fix compiler errors for GCC 6.1.1 (#23) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Euler, Quaternion: fix compiler errors for GCC 6.1.1 GCC output: error: implicit conversion from ‘float’ to ‘double’ to match other operand of binary expression [-Werror=double-promotion] * astyle: fix formatting for Euler.hpp & Quaternion.hpp --- matrix/Euler.hpp | 41 +++++++++++++--------- matrix/Quaternion.hpp | 81 +++++++++++++++++++++++++------------------ 2 files changed, 71 insertions(+), 51 deletions(-) diff --git a/matrix/Euler.hpp b/matrix/Euler.hpp index 4178f21e56..7c320eb1f6 100644 --- a/matrix/Euler.hpp +++ b/matrix/Euler.hpp @@ -54,7 +54,7 @@ public: * * @param other vector to copy */ - Euler(const Vector & other) : + Euler(const Vector &other) : Vector(other) { } @@ -64,7 +64,7 @@ public: * * @param other Matrix31 to copy */ - Euler(const Matrix & other) : + Euler(const Matrix &other) : Vector(other) { } @@ -97,19 +97,20 @@ public: * * @param dcm Direction cosine matrix */ - Euler(const Dcm & dcm) : Vector() + Euler(const Dcm &dcm) : Vector() { - Type phi_val = Type(atan2(dcm(2,1), dcm(2,2))); - Type theta_val = Type(asin(-dcm(2,0))); - Type psi_val = Type(atan2(dcm(1,0), dcm(0,0))); + Type phi_val = Type(atan2(dcm(2, 1), dcm(2, 2))); + Type theta_val = Type(asin(-dcm(2, 0))); + Type psi_val = Type(atan2(dcm(1, 0), dcm(0, 0))); Type pi = Type(M_PI); - if (fabs(theta_val - pi/2) < 1.0e-3) { + if (Type(fabs(theta_val - pi / Type(2))) < Type(1.0e-3)) { phi_val = Type(0.0); - psi_val = Type(atan2(dcm(1,2), dcm(0,2))); - } else if (Type(fabs(theta_val + pi/2)) < Type(1.0e-3)) { + psi_val = Type(atan2(dcm(1, 2), dcm(0, 2))); + + } else if (Type(fabs(theta_val + pi / Type(2))) < Type(1.0e-3)) { phi_val = Type(0.0); - psi_val = Type(atan2(-dcm(1,2), -dcm(0,2))); + psi_val = Type(atan2(-dcm(1, 2), -dcm(0, 2))); } phi() = phi_val; @@ -127,29 +128,35 @@ public: * * @param q quaternion */ - Euler(const Quaternion & q) : + Euler(const Quaternion &q) : Vector() { *this = Euler(Dcm(q)); } - inline Type phi() const { + inline Type phi() const + { return (*this)(0); } - inline Type theta() const { + inline Type theta() const + { return (*this)(1); } - inline Type psi() const { + inline Type psi() const + { return (*this)(2); } - inline Type & phi() { + inline Type &phi() + { return (*this)(0); } - inline Type & theta() { + inline Type &theta() + { return (*this)(1); } - inline Type & psi() { + inline Type &psi() + { return (*this)(2); } diff --git a/matrix/Quaternion.hpp b/matrix/Quaternion.hpp index 6965a37e87..fdb46c558f 100644 --- a/matrix/Quaternion.hpp +++ b/matrix/Quaternion.hpp @@ -73,7 +73,7 @@ public: * * @param other Matrix41 to copy */ - Quaternion(const Matrix41 & other) : + Quaternion(const Matrix41 &other) : Vector(other) { } @@ -86,18 +86,18 @@ public: * * @param dcm dcm to set quaternion to */ - Quaternion(const Dcm & dcm) : + Quaternion(const Dcm &dcm) : Vector() { Quaternion &q = *this; - q(0) = Type(0.5 * sqrt(1 + dcm(0, 0) + - dcm(1, 1) + dcm(2, 2))); + q(0) = Type(0.5) * Type(sqrt(Type(1) + dcm(0, 0) + + dcm(1, 1) + dcm(2, 2))); q(1) = Type((dcm(2, 1) - dcm(1, 2)) / - (4 * q(0))); + (Type(4) * q(0))); q(2) = Type((dcm(0, 2) - dcm(2, 0)) / - (4 * q(0))); + (Type(4) * q(0))); q(3) = Type((dcm(1, 0) - dcm(0, 1)) / - (4 * q(0))); + (Type(4) * q(0))); } /** @@ -109,7 +109,7 @@ public: * * @param euler euler angle instance */ - Quaternion(const Euler & euler) : + Quaternion(const Euler &euler) : Vector() { Quaternion &q = *this; @@ -161,10 +161,10 @@ public: { const Quaternion &p = *this; Quaternion r; - r(0) = p(0)*q(0) - p(1)*q(1) - p(2)*q(2) - p(3)*q(3); - r(1) = p(0)*q(1) + p(1)*q(0) - p(2)*q(3) + p(3)*q(2); - r(2) = p(0)*q(2) + p(1)*q(3) + p(2)*q(0) - p(3)*q(1); - r(3) = p(0)*q(3) - p(1)*q(2) + p(2)*q(1) + p(3)*q(0); + r(0) = p(0) * q(0) - p(1) * q(1) - p(2) * q(2) - p(3) * q(3); + r(1) = p(0) * q(1) + p(1) * q(0) - p(2) * q(3) + p(3) * q(2); + r(2) = p(0) * q(2) + p(1) * q(3) + p(2) * q(0) - p(3) * q(1); + r(3) = p(0) * q(3) - p(1) * q(2) + p(2) * q(1) + p(3) * q(0); return r; } @@ -173,7 +173,7 @@ public: * * @param other quaternion to multiply with */ - void operator*=(const Quaternion & other) + void operator*=(const Quaternion &other) { Quaternion &self = *this; self = self * other; @@ -207,7 +207,8 @@ public: * * @param w direction */ - Matrix41 derivative(const Matrix31 & w) const { + Matrix41 derivative(const Matrix31 &w) const + { const Quaternion &q = *this; Type dataQ[] = { q(0), -q(1), -q(2), -q(3), @@ -218,16 +219,17 @@ public: Matrix Q(dataQ); Vector v; v(0) = 0; - v(1) = w(0,0); - v(2) = w(1,0); - v(3) = w(2,0); + v(1) = w(0, 0); + v(2) = w(1, 0); + v(3) = w(2, 0); return Q * v * Type(0.5); } /** * Invert quaternion */ - void invert() { + void invert() + { Quaternion &q = *this; q(1) *= -1; q(2) *= -1; @@ -239,7 +241,8 @@ public: * * @return inverted quaternion */ - Quaternion inversed() { + Quaternion inversed() + { Quaternion &q = *this; Quaternion ret; ret(0) = q(0); @@ -254,7 +257,8 @@ public: * * @param vec rotation vector */ - void rotate(const Vector &vec) { + void rotate(const Vector &vec) + { Quaternion res; res.from_axis_angle(vec); (*this) = (*this) * res; @@ -269,16 +273,19 @@ public: * @param vec rotation vector * @return quaternion representing the rotation */ - void from_axis_angle(Vector vec) { + void from_axis_angle(Vector vec) + { Quaternion &q = *this; Type theta = vec.norm(); - if(theta < (Type)1e-10) { + + if (theta < (Type)1e-10) { q(0) = (Type)1.0; - q(1)=q(2)=q(3)=0; + q(1) = q(2) = q(3) = 0; return; } + vec /= theta; - from_axis_angle(vec,theta); + from_axis_angle(vec, theta); } /** @@ -288,15 +295,18 @@ public: * @param theta scalar describing angle of rotation * @return quaternion representing the rotation */ - void from_axis_angle(const Vector &axis, Type theta) { + void from_axis_angle(const Vector &axis, Type theta) + { Quaternion &q = *this; - if(theta < (Type)1e-10) { - q(0) = (Type)1.0; - q(1)=q(2)=q(3)=0; - } - Type magnitude = sinf(theta/2.0f); - q(0) = cosf(theta/2.0f); + if (theta < (Type)1e-10) { + q(0) = (Type)1.0; + q(1) = q(2) = q(3) = 0; + } + + Type magnitude = sinf(theta / 2.0f); + + q(0) = cosf(theta / 2.0f); q(1) = axis(0) * magnitude; q(2) = axis(1) * magnitude; q(3) = axis(2) * magnitude; @@ -311,17 +321,20 @@ public: * * @return vector, direction representing rotation axis and norm representing angle */ - Vector to_axis_angle() { + Vector to_axis_angle() + { Quaternion &q = *this; Type axis_magnitude = Type(sqrt(q(1) * q(1) + q(2) * q(2) + q(3) * q(3))); Vector vec; vec(0) = q(1); vec(1) = q(2); vec(2) = q(3); - if(axis_magnitude >= (Type)1e-10) { + + if (axis_magnitude >= (Type)1e-10) { vec = vec / axis_magnitude; - vec = vec * wrap_pi((Type)2.0 * atan2f(axis_magnitude,q(0))); + vec = vec * wrap_pi((Type)2.0 * atan2f(axis_magnitude, q(0))); } + return vec; } };