Changeset 101 in flairsrc for trunk/lib/FlairCore/src/RotationMatrix.cpp
 Timestamp:
 10/05/16 17:20:41 (7 years ago)
 File:

 1 edited
Legend:
 Unmodified
 Added
 Removed

trunk/lib/FlairCore/src/RotationMatrix.cpp
r100 r101 51 51 } 52 52 53 //from 54 //http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/ 55 void 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 /* 53 87 void RotationMatrix::ToQuaternion(Quaternion &quaternion) const { 54 88 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]); 57 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]); 58 94 } 59 95 */ 60 96 Quaternion RotationMatrix::ToQuaternion(void) const { 61 97 Quaternion quaternion;
Note:
See TracChangeset
for help on using the changeset viewer.