Changeset 101 in flair-src for trunk/lib/FlairCore

Timestamp:

Oct 5, 2016, 5:20:41 PM (8 years ago)

Author:

Sanahuja Guillaume

Message:

maj quaternion 3dmgx3

File:

• trunk/lib/FlairCore/src/RotationMatrix.cpp (modified) (2 diffs)

trunk/lib/FlairCore/src/RotationMatrix.cpp

@@ -51 +51 @@
 }
 
+//from
+//http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
+void RotationMatrix::ToQuaternion(Quaternion &quaternion) const {
+  float trace = m[0][0] + m[1][1] + m[2][2];
+  if( trace > 0 ) {
+    float s = 0.5f / sqrtf(trace+ 1.0f);
+    quaternion.q0 = 0.25f / s;
+    quaternion.q1 = ( m[2][1] - m[1][2] ) * s;
+    quaternion.q2 = ( m[0][2] - m[2][0] ) * s;
+    quaternion.q3 = ( m[1][0] - m[0][1] ) * s;
+  } else {
+    if ( m[0][0] > m[1][1] && m[0][0] > m[2][2] ) {
+      float s = 2.0f * sqrtf( 1.0f + m[0][0] - m[1][1] - m[2][2]);
+      quaternion.q0 = (m[2][1] - m[1][2] ) / s;
+      quaternion.q1 = 0.25f * s;
+      quaternion.q2 = (m[0][1] + m[1][0] ) / s;
+      quaternion.q3 = (m[0][2] + m[2][0] ) / s;
+    } else if (m[1][1] > m[2][2]) {
+      float s = 2.0f * sqrtf( 1.0f + m[1][1] - m[0][0] - m[2][2]);
+      quaternion.q0 = (m[0][2] - m[2][0] ) / s;
+      quaternion.q1 = (m[0][1] + m[1][0] ) / s;
+      quaternion.q2 = 0.25f * s;
+      quaternion.q3 = (m[1][2] + m[2][1] ) / s;
+    } else {
+      float s = 2.0f * sqrtf( 1.0f + m[2][2] - m[0][0] - m[1][1] );
+      quaternion.q0 = (m[1][0] - m[0][1] ) / s;
+      quaternion.q1 = (m[0][2] + m[2][0] ) / s;
+      quaternion.q2 = (m[1][2] + m[2][1] ) / s;
+      quaternion.q3 = 0.25f * s;
+    }
+  }
+  quaternion.Normalize();
+}
+/*
 void RotationMatrix::ToQuaternion(Quaternion &quaternion) const {
                 quaternion.q0 = 0.5f * sqrtf(1.0f + m[0][0] + m[1][1] + m[2][2]);
@@ -56 +90 @@
                 quaternion.q2 = 0.5f * sqrtf(1.0f - m[0][0] + m[1][1] - m[2][2]);
                 quaternion.q3 = 0.5f * sqrtf(1.0f - m[0][0] - m[1][1] + m[2][2]);
+                //Printf("%f %f %f\n", m[0][0] , m[1][1] , m[2][2]);
+                //Printf("%f %f %f\n",1.0f + m[0][0] - m[1][1] - m[2][2],1.0f - m[0][0] + m[1][1] - m[2][2],1.0f - m[0][0] - m[1][1] + m[2][2]);
 }
-
+*/
 Quaternion RotationMatrix::ToQuaternion(void) const {
   Quaternion quaternion;