From 7a0db340f7363a678963e5761a006ac099d94c54 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 2 Mar 2015 12:56:12 +0100 Subject: added quaternion methods for inverse and vector rotation --- src/lib/mathlib/math/Quaternion.hpp | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) (limited to 'src/lib/mathlib/math') diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index 38400beef..d28966fca 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -115,6 +115,35 @@ public: return Vector<3>(&data[1]); } + /** + * inverse of quaternion + */ + math::Quaternion inverse() { + Quaternion res; + memcpy(res.data,data,sizeof(res.data)); + res.data[1] = -res.data[1]; + res.data[2] = -res.data[2]; + res.data[3] = -res.data[3]; + return res; + } + + + /** + * rotate vector by quaternion + */ + Vector<3> rotate(const Vector<3> &w) { + Quaternion q_w; // extend vector to quaternion + Quaternion q = {data[0],data[1],data[2],data[3]}; + Quaternion q_rotated; // quaternion representation of rotated vector + q_w(0) = 0; + q_w(1) = w.data[0]; + q_w(2) = w.data[1]; + q_w(3) = w.data[2]; + q_rotated = q*q_w*q.inverse(); + Vector<3> res = {q_rotated.data[1],q_rotated.data[2],q_rotated.data[3]}; + return res; + } + /** * set quaternion to rotation defined by euler angles */ -- cgit v1.2.3