diff options
author | Roman Bapst <romanbapst@yahoo.de> | 2015-03-02 12:56:12 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-03-02 22:44:32 +0100 |
commit | 7a0db340f7363a678963e5761a006ac099d94c54 (patch) | |
tree | cf2c69080c6cf86e3fe00aa51bfdf6de405ab500 /src/lib/mathlib | |
parent | a31fccb0b0d2182b20e6d4b001a710f98f1d84ac (diff) | |
download | px4-firmware-7a0db340f7363a678963e5761a006ac099d94c54.tar.gz px4-firmware-7a0db340f7363a678963e5761a006ac099d94c54.tar.bz2 px4-firmware-7a0db340f7363a678963e5761a006ac099d94c54.zip |
added quaternion methods for inverse and vector rotation
Diffstat (limited to 'src/lib/mathlib')
-rw-r--r-- | src/lib/mathlib/math/Quaternion.hpp | 29 |
1 files changed, 29 insertions, 0 deletions
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 @@ -116,6 +116,35 @@ public: } /** + * 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 */ void from_euler(float roll, float pitch, float yaw) { |