dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

added quaternion methods for inverse and vector rotation

This commit is contained in:
Roman Bapst 2015-03-02 12:56:12 +01:00 committed by Lorenz Meier
parent a31fccb0b0
commit 7a0db340f7
1 changed files with 29 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
src/lib/mathlib/math/Quaternion.hpp
View File

@ -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
*/