欧拉角分解旋转矩阵 C++

欧拉角分解旋转矩阵是将旋转矩阵分解为三个绕不同轴向旋转的角度的过程。在C++中，可以使用以下代码实现：

#include <iostream>
#include <cmath>

using namespace std;

// 定义旋转矩阵
typedef double Matrix[3][3];

// 定义欧拉角
typedef double Euler[3];

// 将欧拉角转换为旋转矩阵
void euler2matrix(const Euler &euler, Matrix &matrix) {
    double cx = cos(euler[0]);
    double sx = sin(euler[0]);
    double cy = cos(euler[1]);
    double sy = sin(euler[1]);
    double cz = cos(euler[2]);
    double sz = sin(euler[2]);

    matrix[0][0] = cz * cy;
    matrix[0][1] = sz * cy;
    matrix[0][2] = -sy;
    matrix[1][0] = cz * sy * sx - sz * cx;
    matrix[1][1] = sz * sy * sx + cz * cx;
    matrix[1][2] = cy * sx;
    matrix[2][0] = cz * sy * cx + sz * sx;
    matrix[2][1] = sz * sy * cx - cz * sx;
    matrix[2][2] = cy * cx;
}

// 将旋转矩阵分解为欧拉角
void matrix2euler(const Matrix &matrix, Euler &euler) {
    euler[1] = asin(-matrix[0][2]);
    double cy = cos(euler[1]);
    if (fabs(cy) > 1e-6) {
        euler[0] = atan2(matrix[1][2] / cy, matrix[2][2] / cy);
        euler[2] = atan2(matrix[0][1] / cy, matrix[0][0] / cy);
    } else {
        euler[0] = atan2(-matrix[2][1], matrix[1][1]);
        euler[2] = 0;
    }
}

// 输出欧拉角
void print_euler(const Euler &euler) {
    cout << "Yaw: " << euler[0] << " Pitch: " << euler[1] << " Roll: " << euler[2] << endl;
}

int main() {
    Matrix matrix;
    Euler euler = {0, 0, 0};

    // 将欧拉角转换为旋转矩阵
    euler2matrix(euler, matrix);

    // 输出旋转矩阵
    cout << "Rotation matrix:" << endl;
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            cout << matrix[i][j] << " ";
        }
        cout << endl;
    }

    // 将旋转矩阵分解为欧拉角
    matrix2euler(matrix, euler);

    // 输出欧拉角
    cout << "Euler angles:" << endl;
    print_euler(euler);

    return 0;
}

此代码将欧拉角和旋转矩阵定义为结构体，并提供了将欧拉角转换为旋转矩阵和将旋转矩阵分解为欧拉角的函数。主函数中将欧拉角初始化为0，并输出生成的旋转矩阵和分解出的欧拉角。