struct Quaternion
{
float x;
float y;
float z;
float w;
};
struct rotMatrix
{
float m11, m12, m13;
float m21, m22, m23;
float m31, m32, m33;
};
//fuction:鏃嬭漿鐭╅樀鍒板洓鍏冩暟鐨勮漿鎹?br>void rotMatrixToQuternion(Quaternion &q, rotMatrix &r)
{
float tr = r.m11 + r.m22 +r.m33;
float temp = 0.0;
if(tr > 0.0)
{
temp = 0.5f / sqrtf(tr+1);
q.w = 0.25f / temp;
q.x = (r.m23 - r.m32) * temp;
q.y = (r.m31 - r.m13) * temp;
q.z = (r.m12 - r.m21) * temp;
}
else
{
if(r.m11 > r.m22 && r.m11 > r.m33)
{
temp = 2.0f * sqrtf(1.0f + r.m11 - r.m22 - r.m33);
q.w = (r.m32 - r.m23) / temp;
q.x = 0.25f * temp;
q.y = (r.m12 + r.m21) / temp;
q.z = (r.m13 + r.m31) / temp;
}
else if( r.m22 > r.m33)
{
temp = 2.0f * sqrtf(1.0f + r.m22 - r.m11 - r.m33);
q.w = (r.m13 - r.m31) / temp;
q.x = (r.m12 + r.m21) / temp;
q.y = 0.25f * temp;
q.z = (r.m23 + r.m32) / temp;
}
else
{
temp = 2.0f * sqrtf(1.0f + r.m33 - r.m11 - r.m22);
q.w = (r.m21 - r.m12) / temp;
q.x = (r.m13 + r.m31) / temp;
q.y = (r.m23 + r.m32) / temp;
q.z = 0.25f * temp;
}
}
}
void main()
{
float m1[9] = {0.01551372092999463200, 0.99884343581246959000, -0.04550950666890610900,
0.99922238739871228000 ,-0.01713749902859566800 ,-0.03550952897832390700,
-0.03624837905512174500, -0.04492323298011671700, -0.99833258894743582000};
rotMatrix rm1;
rm1.m11 = m1[0];
rm1.m12 = m1[1];
rm1.m13 = m1[2];
rm1.m21 = m1[3];
rm1.m22 = m1[4];
rm1.m23 = m1[5];
rm1.m31 = m1[6];
rm1.m32 = m1[7];
rm1.m33 = m1[8];
Quaternion q1;
rotMatrixToQuternion(q1, rm1);
cout<<q1.x<<" "<<q1.y<<" "<<q1.z<<" "<<q1.w<<endl;
float m2[9] = {0.01614490437974924100, 0.99884677638989772000, -0.04521569813768747100,
0.99856922398083869000, -0.01380176413826810800, 0.05166252244109931900,
0.05097888759941932700, -0.04598509108593063600, -0.99764047853770654000};
rotMatrix rm2;
rm2.m11 = m2[0];
rm2.m12 = m2[1];
rm2.m13 = m2[2];
rm2.m21 = m2[3];
rm2.m22 = m2[4];
rm2.m23 = m2[5];
rm2.m31 = m2[6];
rm2.m32 = m2[7];
rm2.m33 = m2[8];
Quaternion q2;
rotMatrixToQuternion(q2, rm2);
cout<<q2.x<<" "<<q2.y<<" "<<q2.z<<" "<<q2.w<<endl;
}
noBugnoGain 2008-12-29 11:03 鍙戣〃璇勮
]]>