Diff: Quaternion.h

Revision:

6:7ba72ec26bd1

Parent:

5:e31eb7f8925d

Child:

7:9fc4176dde36

--- a/Quaternion.h	Fri Mar 13 16:10:04 2015 +0000
+++ b/Quaternion.h	Wed Mar 18 15:34:12 2015 +0000
@@ -186,6 +186,20 @@
         row2.y = (2*_y*_z)+(2*_w*_x);
         row2.z = 1-(2*(_x*_x))-(2*(_y*_y));
     }
+    
+    Quaternion getAxisAngle() const {
+        Quaternion q1(normalise()); // get normalised version
+        
+        float const angle = 2 * acos(q1.w);
+        double const s = sqrt(1 - q1.w * q1.w); // assuming quaternion normalised then w is less than 1, so term always positive.
+        if (s < 0.001) { // test to avoid divide by zero, s is always positive due to sqrt
+            // if s close to zero then direction of axis not important
+            q1.v = Vector3(1, 0, 0);
+        } else {
+            q1.v = q1.v / s; // normalise axis
+        }
+        return q1;
+    }
 
     const Vector3 getEulerAngles() const {
         double sqw = w*w;