用C语言模仿simulink中Euler2Quation

在Simulink中，Euler2Quation通常用于将欧拉角转换为旋转矩阵，这是模拟机械系统运动的一种常见需求。在C语言中，你可以通过以下步骤来实现这个功能：

1. **理解欧拉角与旋转矩阵的关系**：欧拉角有多种表示方式，如XYZ、ZYX等，每种组合对应着旋转矩阵的不同形式。你需要明确你的欧拉角是哪种顺序（例如，Z-Y-X）。

2. **创建函数**：创建一个名为`euler2quaternion`或类似名称的函数，接受三个欧拉角作为输入（通常是弧度制），并返回一个4x4的四元数（Quaternion），代表对应的旋转矩阵。

#include <math.h>
typedef struct {
    double x, y, z;
} EulerAngle;

struct Quaternion {
    double w, x, y, z;
};

Quaternion euler2quaternion(EulerAngle eulerAngles, char order) {
    // 确定旋转矩阵的计算公式，这里假设order为Z-Y-X
    double q0, q1, q2, q3;
    
    double t0 = cos(eulerAngles.z * 0.5);
    double t1 = sin(eulerAngles.z * 0.5) * cos(eulerAngles.y * 0.5);
    double t2 = sin(eulerAngles.z * 0.5) * sin(eulerAngles.y * 0.5);

    switch (order) {
        case 'Z':
            q0 = t0;
            q1 = t1;
            q2 = t2 * cos(eulerAngles.x);
            q3 = t2 * sin(eulerAngles.x);
            break;
        // 其他情况，如Y和X，需要相应调整计算公式
        //...
        default:
            printf("Unsupported order! Use Z, Y or X.\n");
            return (Quaternion){0};
    }

    Quaternion result;
    result.w = q0;
    result.x = q1;
    result.y = q2;
    result.z = q3;

    return result;
}

// 使用示例
EulerAngle angles; // 初始化你的欧拉角
char order = 'Z'; // 或者其他欧拉顺序
Quaternion quat = euler2quaternion(angles, order);