Quaternion: added direct efficient body z-axis calculation with test

matrix/Quaternion.hpp

@@ -440,6 +440,27 @@ public:
         return Vector3<Type>(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<Type> dcm_z()
+    {
+        Quaternion &q = *this;
+        Vector3<Type> 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
      */

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;