From 10b395a7829cad0a0647bcd4f8da35e88c0e9a66 Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 27 Nov 2015 08:52:47 +0100 Subject: [PATCH] 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 : */