Changeset 103 in flair-src for trunk/lib/FlairCore/src/RotationMatrix.cpp


Ignore:
Timestamp:
10/11/16 12:07:16 (8 years ago)
Author:
Sanahuja Guillaume
Message:

maj quaternion 3dmgx3

File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/lib/FlairCore/src/RotationMatrix.cpp

    r101 r103  
    2121#include "Quaternion.h"
    2222#include "math.h"
     23
     24using std::max;
    2325
    2426namespace flair {
     
    5153}
    5254
    53 //from
    54 //http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
     55//from 3dmgx3-25 manual
    5556void RotationMatrix::ToQuaternion(Quaternion &quaternion) const {
    56   float trace = m[0][0] + m[1][1] + m[2][2];
    57   if( trace > 0 ) {
    58     float s = 0.5f / sqrtf(trace+ 1.0f);
    59     quaternion.q0 = 0.25f / s;
    60     quaternion.q1 = ( m[2][1] - m[1][2] ) * s;
    61     quaternion.q2 = ( m[0][2] - m[2][0] ) * s;
    62     quaternion.q3 = ( m[1][0] - m[0][1] ) * s;
    63   } else {
    64     if ( m[0][0] > m[1][1] && m[0][0] > m[2][2] ) {
    65       float s = 2.0f * sqrtf( 1.0f + m[0][0] - m[1][1] - m[2][2]);
    66       quaternion.q0 = (m[2][1] - m[1][2] ) / s;
    67       quaternion.q1 = 0.25f * s;
    68       quaternion.q2 = (m[0][1] + m[1][0] ) / s;
    69       quaternion.q3 = (m[0][2] + m[2][0] ) / s;
    70     } else if (m[1][1] > m[2][2]) {
    71       float s = 2.0f * sqrtf( 1.0f + m[1][1] - m[0][0] - m[2][2]);
    72       quaternion.q0 = (m[0][2] - m[2][0] ) / s;
    73       quaternion.q1 = (m[0][1] + m[1][0] ) / s;
    74       quaternion.q2 = 0.25f * s;
    75       quaternion.q3 = (m[1][2] + m[2][1] ) / s;
    76     } else {
    77       float s = 2.0f * sqrtf( 1.0f + m[2][2] - m[0][0] - m[1][1] );
    78       quaternion.q0 = (m[1][0] - m[0][1] ) / s;
    79       quaternion.q1 = (m[0][2] + m[2][0] ) / s;
    80       quaternion.q2 = (m[1][2] + m[2][1] ) / s;
    81       quaternion.q3 = 0.25f * s;
    82     }
     57  float test1 = m[0][0] + m[1][1] + m[2][2];
     58  float test2 = m[0][0] - m[1][1] - m[2][2];
     59  float test3 = -m[0][0] + m[1][1] - m[2][2];
     60  float test4 = -m[0][0] - m[1][1] + m[2][2];
     61
     62  if(test1==max(max(test1,test2),max(test3,test4))) {
     63    float s=2*sqrtf(1+test1);
     64    quaternion.q0 = s/4.;
     65    quaternion.q1 = ( m[1][2] - m[2][1] ) / s;
     66    quaternion.q2 = ( m[2][0] - m[0][2] ) / s;
     67    quaternion.q3 = ( m[0][1] - m[1][0] ) / s;
     68  }
     69
     70  if(test2==max(max(test1,test2),max(test3,test4))) {
     71    float s=2*sqrtf(1+test2);
     72    quaternion.q0 = ( m[2][1] - m[1][2] ) / s;
     73    quaternion.q1 = -s/4.;
     74    quaternion.q2 = -( m[1][0] + m[0][1] ) / s;
     75    quaternion.q3 = -( m[0][2] + m[2][0] ) / s;
     76  }
     77
     78  if(test3==max(max(test1,test2),max(test3,test4))) {
     79    float s=2*sqrtf(1+test3);
     80    quaternion.q0 = (m[0][2] - m[2][0] ) / s;
     81    quaternion.q1 = -( m[1][0] + m[0][1] ) / s;
     82    quaternion.q2 = -s/4.;
     83    quaternion.q3 = -( m[2][1] + m[1][2] ) / s;
     84  }
     85
     86  if(test4==max(max(test1,test2),max(test3,test4))) {
     87    float s=2*sqrtf(1+test4);
     88    quaternion.q0 = (m[1][0] - m[0][1] ) / s;
     89    quaternion.q1 = -( m[0][2] + m[2][0] ) / s;
     90    quaternion.q2 = -( m[2][1] + m[1][2] ) / s;
     91    quaternion.q3 = -s/4.;
    8392  }
    8493  quaternion.Normalize();
    8594}
    86 /*
    87 void RotationMatrix::ToQuaternion(Quaternion &quaternion) const {
    88                 quaternion.q0 = 0.5f * sqrtf(1.0f + m[0][0] + m[1][1] + m[2][2]);
    89                 quaternion.q1 = 0.5f * sqrtf(1.0f + m[0][0] - m[1][1] - m[2][2]);
    90                 quaternion.q2 = 0.5f * sqrtf(1.0f - m[0][0] + m[1][1] - m[2][2]);
    91                 quaternion.q3 = 0.5f * sqrtf(1.0f - m[0][0] - m[1][1] + m[2][2]);
    92                 //Printf("%f %f %f\n", m[0][0] , m[1][1] , m[2][2]);
    93                 //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]);
    94 }
    95 */
     95
    9696Quaternion RotationMatrix::ToQuaternion(void) const {
    9797  Quaternion quaternion;
Note: See TracChangeset for help on using the changeset viewer.