#include <math.h>
#include <iostream.h>

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:旋转矩阵到四元数的转换
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;
}