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


Ignore:
Timestamp:
Oct 5, 2016, 5:20:41 PM (5 years ago)
Author:
Sanahuja Guillaume
Message:

maj quaternion 3dmgx3

File:
1 edited

Legend:

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

    r100 r101  
    5151}
    5252
     53//from
     54//http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
     55void 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    }
     83  }
     84  quaternion.Normalize();
     85}
     86/*
    5387void RotationMatrix::ToQuaternion(Quaternion &quaternion) const {
    5488                quaternion.q0 = 0.5f * sqrtf(1.0f + m[0][0] + m[1][1] + m[2][2]);
     
    5690                quaternion.q2 = 0.5f * sqrtf(1.0f - m[0][0] + m[1][1] - m[2][2]);
    5791                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]);
    5894}
    59 
     95*/
    6096Quaternion RotationMatrix::ToQuaternion(void) const {
    6197  Quaternion quaternion;
Note: See TracChangeset for help on using the changeset viewer.