Changeset 103 in flairsrc for trunk/lib/FlairCore/src/RotationMatrix.cpp
 Timestamp:
 Oct 11, 2016, 12:07:16 PM (5 years ago)
 File:

 1 edited
Legend:
 Unmodified
 Added
 Removed

trunk/lib/FlairCore/src/RotationMatrix.cpp
r101 r103 21 21 #include "Quaternion.h" 22 22 #include "math.h" 23 24 using std::max; 23 25 24 26 namespace flair { … … 51 53 } 52 54 53 //from 54 //http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/ 55 //from 3dmgx325 manual 55 56 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 } 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.; 83 92 } 84 93 quaternion.Normalize(); 85 94 } 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 96 96 Quaternion RotationMatrix::ToQuaternion(void) const { 97 97 Quaternion quaternion;
Note: See TracChangeset
for help on using the changeset viewer.