// test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//

#include <iostream>
#include <Eigen/Dense>
#include <cmath>
using namespace Eigen;
int main()
{
    Matrix3d R;
    Vector3d v;
    v << 0, 1, 0;
    R << 0.99296001,0.11836079,0.00459846, 
        -0.11840988,0.99288611,0.01250104, 
        -0.00308611, -0.01295754,0.99991129;
    RowVector3d ret =  R * v ;
    //std::cout << ret << std::endl;
   /* Quaterniond gnss(0.50, 0, 0, 0.086376804);
    auto gnssangle = gnss.matrix().eulerAngles(0, 1, 2);
    std::cout << gnssangle *180/3.14 << std::endl; */
   // Quaterniond ndt(0, 0, -0.75208, 0.659070); //mian xiang qiao
    AngleAxisd rotation_vector;
    
    Quaterniond E(0.99992576,0, 0, -0.001222);
    rotation_vector = E;
    std::cout << "Axis: \n" << rotation_vector.axis() << std::endl;
    auto e = E.matrix().eulerAngles(0, 1, 2).transpose();
    std::cout <<"E: \n"<< e * 180 / 3.14 << std::endl;
    Quaterniond S(0.713180376625,0, 0, -0.700980);
    auto s = S.matrix().eulerAngles(0, 1, 2).transpose();
    std::cout <<"S: \n"<< s * 180 / 3.14 << std::endl;
    Quaterniond W(0.00165279,0, 0, 0.999984);
    rotation_vector = W;
    std::cout << "Axis: \n" << rotation_vector.axis() << std::endl;
    auto w = W.matrix().eulerAngles(0, 1, 2).transpose();
  

    std::cout << "W: \n" << w * 180 / 3.14 << std::endl;
    Quaterniond N(0, 0, 0.7018073567, 0.71236678);

    auto n = N.matrix().eulerAngles(0, 1, 2).transpose();
    std::cout << "N:\n" << n * 180 / 3.14 << std::endl;
    Quaterniond ES(0, 0, -0.39001, 0.920807);

    auto es = ES.matrix().eulerAngles(0, 1, 2).transpose();
    std::cout << "ES:\n" << es * 180 / 3.14 << std::endl;
    Quaterniond WN(0, 0, 0.89513307, 0.445799);

    auto wn = WN.matrix().eulerAngles(0, 1, 2).transpose();
    std::cout << "WN:\n" << wn * 180 / 3.14 << std::endl;
    Quaterniond WS(0, 0, -0.9286255, 0.3710184);

    auto ws = WS.matrix().eulerAngles(0, 1, 2).transpose();
    std::cout << "WS:\n" << ws * 180 / 3.14 << std::endl;
    Quaterniond EN(0, 0, 0.35893411, 0.93336290);

    auto en = EN.matrix().eulerAngles(0, 1, 2).transpose();
    std::cout << "EN:\n" << en * 180 / 3.14 << std::endl;
   // std::cout << ndtangle - gnssangle << std::endl;
    return 0;
    /*
    auto eulerAngle = R.eulerAngles(0, 1, 2);
    float x = 3.12909;
    float y = 3.13609;
    float z = 3.1415926 ;
    std::cout << eulerAngle   << std::endl;
    Eigen::AngleAxisd rollAngle(AngleAxisd(x, Vector3d::UnitX()));
    Eigen::AngleAxisd pitchAngle(AngleAxisd(y, Vector3d::UnitY()));
    Eigen::AngleAxisd yawAngle(AngleAxisd(z, Vector3d::UnitZ()));
   // Eigen::Matrix3d rotation_matrix; 
    R =   rollAngle  * pitchAngle  * yawAngle;
    std::cout << R << std::endl;
    eulerAngle = R.eulerAngles(0, 1, 2);
    std::cout << eulerAngle   << std::endl;
    */
}

// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单
// 调试程序: F5 或调试 >“开始调试”菜单

// 入门使用技巧: 
//   1. 使用解决方案资源管理器窗口添加/管理文件
//   2. 使用团队资源管理器窗口连接到源代码管理
//   3. 使用输出窗口查看生成输出和其他消息
//   4. 使用错误列表窗口查看错误
//   5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目
//   6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件