四元数求yaw的代码c语言

以下是一个简单的C语言代码实现四元数求yaw角度的方法：

#include <stdio.h>
#include <math.h>

typedef struct Quaternion {
    float w, x, y, z;
} Quaternion;

float getYaw(Quaternion q) {
    // 计算四元数对应的欧拉角yaw
    float sinr = 2.0f * (q.w * q.y - q.x * q.z);
    float cosr = 1.0f - 2.0f * (q.y * q.y + q.z * q.z);
    float yaw = atan2f(sinr, cosr);

    // 将弧度转换为角度
    return yaw * 180.0f / M_PI;
}

int main() {
    // 创建一个四元数
    Quaternion q = {0.7071f, 0.0f, 0.7071f, 0.0f};

    // 计算四元数对应的yaw角度
    float yaw = getYaw(q);

    // 输出yaw角度
    printf("Yaw: %f\n", yaw);

    return 0;
}

在这个例子中，我们创建了一个名为`Quaternion`的结构体来表示四元数。其中，`w`、`x`、`y`和`z`分别表示四元数的实部和虚部。我们还定义了一个名为`getYaw()`的函数，用于计算四元数对应的yaw角度。在`getYaw()`函数中，我们使用了公式来计算四元数对应的欧拉角yaw，并将弧度转换为角度。最后，在主函数中，我们创建了一个四元数并调用`getYaw()`函数来计算它的yaw角度。