diff --git a/matrix/Quaternion.hpp b/matrix/Quaternion.hpp index 38eed54374..75484e939d 100644 --- a/matrix/Quaternion.hpp +++ b/matrix/Quaternion.hpp @@ -440,6 +440,27 @@ public: return Vector3(q(1), q(2), q(3)); } + /** + * Corresponding body z-axis to an attitude quaternion / + * last orthogonal unit basis vector + * + * == last column of the equivalent rotation matrix + * but calculated more efficiently than a full conversion + */ + Vector3 dcm_z() + { + Quaternion &q = *this; + Vector3 R_z; + const Type a = q(0); + const Type b = q(1); + const Type c = q(2); + const Type d = q(3); + R_z(0) = 2 * (a * c + b * d); + R_z(1) = 2 * (c * d - a * b); + R_z(2) = a * a - b * b - c * c + d * d; + return R_z; + } + /** * XXX DEPRECATED, can use assignment or ctor */ diff --git a/test/attitude.cpp b/test/attitude.cpp index 8042bc1a2b..49e59b257b 100644 --- a/test/attitude.cpp +++ b/test/attitude.cpp @@ -68,6 +68,10 @@ int main() // quaternion to dcm Dcmf dcm1(q_check); TEST(isEqual(dcm1, dcm_check)); + // quaternion z-axis unit base vector + Vector3f q_z = q_check.dcm_z(); + Vector3f R_z(dcm_check(0, 2), dcm_check(1, 2), dcm_check(2, 2)); + TEST(isEqual(q_z, R_z)); // dcm default ctor Dcmf dcm2;