From 10b395a7829cad0a0647bcd4f8da35e88c0e9a66 Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 27 Nov 2015 08:52:47 +0100 Subject: [PATCH 1/3] added more quaternion methods --- matrix/Matrix.hpp | 12 +++--- matrix/Quaternion.hpp | 76 ++++++++++++++++++++++++++++++++++--- matrix/Vector.hpp | 2 +- matrix/helper_functions.hpp | 30 +++++++++++++++ test/attitude.cpp | 60 +++++++++++++++++++++++++++++ 5 files changed, 167 insertions(+), 13 deletions(-) create mode 100644 matrix/helper_functions.hpp diff --git a/matrix/Matrix.hpp b/matrix/Matrix.hpp index 2e32fc29b2..bad33f7a9a 100644 --- a/matrix/Matrix.hpp +++ b/matrix/Matrix.hpp @@ -361,8 +361,8 @@ public: Matrix abs() { Matrix r; - for (int i=0; i max_val) { max_val = val; @@ -386,8 +386,8 @@ public: Type min() { Type min_val = (*this)(0,0); - for (int i=0; i() { Quaternion &q = *this; - Type cosPhi_2 = Type(cos(euler.phi() / 2.0)); - Type cosTheta_2 = Type(cos(euler.theta() / 2.0)); - Type cosPsi_2 = Type(cos(euler.psi() / 2.0)); - Type sinPhi_2 = Type(sin(euler.phi() / 2.0)); - Type sinTheta_2 = Type(sin(euler.theta() / 2.0)); - Type sinPsi_2 = Type(sin(euler.psi() / 2.0)); + Type cosPhi_2 = Type(cos(euler.phi() / (Type)2.0)); + Type cosTheta_2 = Type(cos(euler.theta() / (Type)2.0)); + Type cosPsi_2 = Type(cos(euler.psi() / (Type)2.0)); + Type sinPhi_2 = Type(sin(euler.phi() / (Type)2.0)); + Type sinTheta_2 = Type(sin(euler.theta() / (Type)2.0)); + Type sinPsi_2 = Type(sin(euler.psi() / (Type)2.0)); q(0) = cosPhi_2 * cosTheta_2 * cosPsi_2 + sinPhi_2 * sinTheta_2 * sinPsi_2; q(1) = sinPhi_2 * cosTheta_2 * cosPsi_2 - @@ -120,6 +121,69 @@ public: v(3) = w(2,0); return Q * v * Type(0.5); } + + void invert() { + Quaternion &q = *this; + q(1) *= -1; + q(2) *= -1; + q(3) *= -1; + } + + Quaternion inversed() { + Quaternion &q = *this; + Quaternion ret; + ret(0) = q(0); + ret(1) = -q(1); + ret(2) = -q(2); + ret(3) = -q(3); + return ret; + } + + void rotate(const Vector &vec) { + Quaternion res; + res.from_axis_angle(vec); + (*this) = (*this) * res; + } + + void from_axis_angle(Vector vec) { + Quaternion &q = *this; + Type theta = vec.norm(); + if(theta < (Type)1e-10) { + q(0) = (Type)1.0; + q(1)=q(2)=q(3)=0; + return; + } + vec /= theta; + from_axis_angle(vec,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); + q(1) = axis(0) * magnitude; + q(2) = axis(1) * magnitude; + q(3) = axis(2) * magnitude; + } + + 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) { + vec = vec / axis_magnitude; + vec = vec * wrap_pi((Type)2.0 * atan2f(axis_magnitude,q(0))); + } + return vec; + } }; typedef Quaternion Quatf; diff --git a/matrix/Vector.hpp b/matrix/Vector.hpp index 4c75ce5694..67f6e98f6a 100644 --- a/matrix/Vector.hpp +++ b/matrix/Vector.hpp @@ -52,7 +52,7 @@ public: Type dot(const MatrixM1 & b) const { const Vector &a(*this); Type r = 0; - for (int i = 0; i + + +namespace matrix +{ +template +float wrap_pi(Type x) +{ + if (!isfinite(x)) { + return x; + } + + while (x >= (Type)M_PI) { + x -= (Type)(2.0 * M_PI); + + } + + while (x < (Type)(-M_PI)) { + x += (Type)(2.0 * M_PI); + + } + + return x; +} + + +}; \ No newline at end of file diff --git a/test/attitude.cpp b/test/attitude.cpp index 58f8f93fc8..4946498634 100644 --- a/test/attitude.cpp +++ b/test/attitude.cpp @@ -133,6 +133,66 @@ int main() q_check *= q_check; assert(q_prod_check == q_check); + // quaternion inverse + q = q_check.inversed(); + assert(fabsf(q_check(0) - q(0)) < eps); + assert(fabsf(q_check(1) + q(1)) < eps); + assert(fabsf(q_check(2) + q(2)) < eps); + assert(fabsf(q_check(3) + q(3)) < eps); + + q = q_check; + q.invert(); + assert(fabsf(q_check(0) - q(0)) < eps); + assert(fabsf(q_check(1) + q(1)) < eps); + assert(fabsf(q_check(2) + q(2)) < eps); + assert(fabsf(q_check(3) + q(3)) < eps); + + // rotate quaternion (nonzero rotation) + Quatf qI(1.0f, 0.0f, 0.0f, 0.0f); + Vector rot; + rot(0) = 1.0f; + rot(1) = rot(2) = 0.0f; + qI.rotate(rot); + Quatf q_true(cosf(1.0f / 2), sinf(1.0f / 2), 0.0f, 0.0f); + assert(fabsf(qI(0) - q_true(0)) < eps); + assert(fabsf(qI(1) - q_true(1)) < eps); + assert(fabsf(qI(2) - q_true(2)) < eps); + assert(fabsf(qI(3) - q_true(3)) < eps); + + // rotate quaternion (zero rotation) + qI = Quatf(1.0f, 0.0f, 0.0f, 0.0f); + rot(0) = 0.0f; + rot(1) = rot(2) = 0.0f; + qI.rotate(rot); + q_true = Quatf(cosf(0.0f), sinf(0.0f), 0.0f, 0.0f); + assert(fabsf(qI(0) - q_true(0)) < eps); + assert(fabsf(qI(1) - q_true(1)) < eps); + assert(fabsf(qI(2) - q_true(2)) < eps); + assert(fabsf(qI(3) - q_true(3)) < eps); + + // get rotation axis from quaternion (nonzero rotation) + q = Quatf(cosf(1.0f / 2), 0.0f, sinf(1.0f / 2), 0.0f); + rot = q.to_axis_angle(); + assert(fabsf(rot(0)) < eps); + assert(fabsf(rot(1) -1.0f) < eps); + assert(fabsf(rot(2)) < eps); + + // get rotation axis from quaternion (zero rotation) + q = Quatf(1.0f, 0.0f, 0.0f, 0.0f); + rot = q.to_axis_angle(); + assert(fabsf(rot(0)) < eps); + assert(fabsf(rot(1)) < eps); + assert(fabsf(rot(2)) < eps); + + // from axis angle (zero rotation) + rot(0) = rot(1) = rot(2) = 0.0f; + q.from_axis_angle(rot, 0.0f); + q_true = Quatf(1.0f, 0.0f, 0.0f, 0.0f); + assert(fabsf(q(0) - q_true(0)) < eps); + assert(fabsf(q(1) - q_true(1)) < eps); + assert(fabsf(q(2) - q_true(2)) < eps); + assert(fabsf(q(3) - q_true(3)) < eps); + }; /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ From c3c6a0a9df19ffa782e095e4ec3993bac738d935 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 10 Dec 2015 16:40:30 +0100 Subject: [PATCH 2/3] fix euler calculation --- matrix/Euler.hpp | 27 ++++++++++++--------------- 1 file changed, 12 insertions(+), 15 deletions(-) diff --git a/matrix/Euler.hpp b/matrix/Euler.hpp index 74e364940f..c055b0d827 100644 --- a/matrix/Euler.hpp +++ b/matrix/Euler.hpp @@ -48,22 +48,19 @@ public: Euler(const Dcm & dcm) : Vector() { - Type psi_val = Type(atan(dcm(1, 0)/ dcm(0, 0))); - Type theta_val = Type(asin(-dcm(2,0))); - Type phi_val = Type(atan(dcm(2, 1)/ dcm(2, 2))); + theta() = (Type)asin(-dcm(2,0)); - // protection against NaN if dcm(0,0) or dcm(2,2) == 0 - psi() = 0; - theta() = 0; - phi() = 0; - if (psi() >= -M_PI_2 && psi() <= M_PI_2) { - psi() = psi_val; - } - if (theta() >= -M_PI_2 && theta() <= M_PI_2) { - theta() = theta_val; - } - if (phi() >= -M_PI_2 && phi() <= M_PI_2) { - phi() = phi_val; + if (fabs(theta() - (Type)M_PI_2) < 1.0e-3) { + phi() = (Type)0.0; + psi() = (Type)atan2(dcm(1,2) - dcm(0,1), dcm(0,2) + dcm(1,1)) + theta(); + + } else if (fabs(theta() + (Type)M_PI_2) < (Type)1.0e-3) { + phi() = (Type)0.0; + psi() = (Type)atan2(dcm(1,2) - dcm(0,1), dcm(0,2) + dcm(1,1)) - theta(); + + } else { + phi() = (Type)atan2(dcm(2,1), dcm(2,2)); + psi() = (Type)atan2(dcm(1,0), dcm(0,0)); } } From cc1658db15d720e05d577787363823be0f2bd161 Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 22 Dec 2015 11:23:18 +0100 Subject: [PATCH 3/3] fixed wrong type conversion --- matrix/Euler.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/matrix/Euler.hpp b/matrix/Euler.hpp index c055b0d827..346acf2c51 100644 --- a/matrix/Euler.hpp +++ b/matrix/Euler.hpp @@ -54,7 +54,7 @@ public: phi() = (Type)0.0; psi() = (Type)atan2(dcm(1,2) - dcm(0,1), dcm(0,2) + dcm(1,1)) + theta(); - } else if (fabs(theta() + (Type)M_PI_2) < (Type)1.0e-3) { + } else if ((Type)fabs(theta() + (Type)M_PI_2) < (Type)1.0e-3) { phi() = (Type)0.0; psi() = (Type)atan2(dcm(1,2) - dcm(0,1), dcm(0,2) + dcm(1,1)) - theta();