Diff: Quaternion.h

Revision:

3:c0137be74db4

Parent:

1:857642c51139

Child:

4:1ced03aa8c75

--- a/Quaternion.h	Thu Mar 12 10:14:52 2015 +0000
+++ b/Quaternion.h	Fri Mar 13 09:10:41 2015 +0000
@@ -18,6 +18,49 @@
         w = _w;
         v = _v;
     }
+    Quaternion(Vector3 row0, Vector3 row1, Vector3 row2) {
+        // from rotation matrix
+        const float m[3][3] = {
+            { row0.x, row0.y, row0.z },
+            { row1.x, row1.y, row1.z },
+            { row2.x, row2.y, row2.z }
+        };
+        
+        const float tr = m[0][0] + m[1][1] + m[2][2];
+    
+        if (tr > 0)
+        {
+          const float S = sqrt(tr+1.0) * 2;
+          w = 0.25 * S;
+          v.x = (m[2][1] - m[1][2]) / S;
+          v.y = (m[0][2] - m[2][0]) / S;
+          v.z = (m[1][0] - m[0][1]) / S;
+        }
+        else if ((m[0][0] < m[1][1])&(m[0][0] < m[2][2]))
+        {
+          const float S = sqrt(1.0 + m[0][0] - m[1][1] - m[2][2]) * 2;
+          w = (m[2][1] - m[1][2]) / S;
+          v.x = 0.25 * S;
+          v.y = (m[0][1] + m[1][0]) / S;
+          v.z = (m[0][2] + m[2][0]) / S;
+        }
+        else if (m[1][1] < m[2][2])
+        {
+          const float S = sqrt(1.0 + m[1][1] - m[0][0] - m[2][2]) * 2;
+          w = (m[0][2] - m[2][0]) / S;
+          v.x = (m[0][1] + m[1][0]) / S;
+          v.y = 0.25 * S;
+          v.z = (m[1][2] + m[2][1]) / S;
+        }
+        else
+        {
+          const float S = sqrt(1.0 + m[2][2] - m[0][0] - m[1][1]) * 2;
+          w = (m[1][0] - m[0][1]) / S;
+          v.x = (m[0][2] + m[2][0]) / S;
+          v.y = (m[1][2] + m[2][1]) / S;
+          v.z = 0.25 * S;
+        }
+    }
     Quaternion(float theta_x, float theta_y, float theta_z)
     {
         float cos_z_2 = cosf(0.5f*theta_z);