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

Timestamp:

Oct 11, 2016, 12:07:16 PM (8 years ago)

Author:

Sanahuja Guillaume

Message:

maj quaternion 3dmgx3

Location:

trunk/lib/FlairCore/src

Files:

• OneAxisRotation_impl.cpp (modified) (1 diff)
• RotationMatrix.cpp (modified) (2 diffs)

trunk/lib/FlairCore/src/OneAxisRotation_impl.cpp

@@ -49 +49 @@
 
 void OneAxisRotation_impl::ComputeRotation(RotationMatrix &matrix) const {
-  printf("not yet implemented\n");
+  Printf("not yet implemented\n");
 }
 

trunk/lib/FlairCore/src/RotationMatrix.cpp

@@ -21 +21 @@
 #include "Quaternion.h"
 #include "math.h"
+
+using std::max;
 
 namespace flair {
@@ -51 +53 @@
 }
 
-//from
-//http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
+//from 3dmgx3-25 manual
 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;
-    }
+  float test1 = m[0][0] + m[1][1] + m[2][2];
+  float test2 = m[0][0] - m[1][1] - m[2][2];
+  float test3 = -m[0][0] + m[1][1] - m[2][2];
+  float test4 = -m[0][0] - m[1][1] + m[2][2];
+
+  if(test1==max(max(test1,test2),max(test3,test4))) {
+    float s=2*sqrtf(1+test1);
+    quaternion.q0 = s/4.;
+    quaternion.q1 = ( m[1][2] - m[2][1] ) / s;
+    quaternion.q2 = ( m[2][0] - m[0][2] ) / s;
+    quaternion.q3 = ( m[0][1] - m[1][0] ) / s;
+  }
+
+  if(test2==max(max(test1,test2),max(test3,test4))) {
+    float s=2*sqrtf(1+test2);
+    quaternion.q0 = ( m[2][1] - m[1][2] ) / s;
+    quaternion.q1 = -s/4.;
+    quaternion.q2 = -( m[1][0] + m[0][1] ) / s;
+    quaternion.q3 = -( m[0][2] + m[2][0] ) / s;
+  }
+
+  if(test3==max(max(test1,test2),max(test3,test4))) {
+    float s=2*sqrtf(1+test3);
+    quaternion.q0 = (m[0][2] - m[2][0] ) / s;
+    quaternion.q1 = -( m[1][0] + m[0][1] ) / s;
+    quaternion.q2 = -s/4.;
+    quaternion.q3 = -( m[2][1] + m[1][2] ) / s;
+  }
+
+  if(test4==max(max(test1,test2),max(test3,test4))) {
+    float s=2*sqrtf(1+test4);
+    quaternion.q0 = (m[1][0] - m[0][1] ) / s;
+    quaternion.q1 = -( m[0][2] + m[2][0] ) / s;
+    quaternion.q2 = -( m[2][1] + m[1][2] ) / s;
+    quaternion.q3 = -s/4.;
   }
   quaternion.Normalize();
 }
-/*
-void RotationMatrix::ToQuaternion(Quaternion &quaternion) const {
-                quaternion.q0 = 0.5f * sqrtf(1.0f + m[0][0] + m[1][1] + m[2][2]);
-                quaternion.q1 = 0.5f * sqrtf(1.0f + m[0][0] - m[1][1] - m[2][2]);
-                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;