diff options
author | Friedemann Ludwig <frieludwig@hotmail.com> | 2015-03-03 20:41:27 +0100 |
---|---|---|
committer | Friedemann Ludwig <frieludwig@hotmail.com> | 2015-03-03 20:41:27 +0100 |
commit | 4a99daf5b61485dba8fb68aa044993d2060edce3 (patch) | |
tree | f849f2f1b7fae1657a1943651eba34867f9cbc32 /src/lib/mathlib/math/Quaternion.hpp | |
parent | 32012e8aac641ee5a10eec14c79346c148bcce90 (diff) | |
parent | cf7f59e1f32977e49be9367b6441076ef77fa884 (diff) | |
download | px4-firmware-4a99daf5b61485dba8fb68aa044993d2060edce3.tar.gz px4-firmware-4a99daf5b61485dba8fb68aa044993d2060edce3.tar.bz2 px4-firmware-4a99daf5b61485dba8fb68aa044993d2060edce3.zip |
Merge remote-tracking branch 'upstream/master' into wobbling_elevator
Diffstat (limited to 'src/lib/mathlib/math/Quaternion.hpp')
-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) { |