如何用Yaw、Pitch、Roll旋转3D向量？附问题代码求排查

阿华AIGC实验室

2026-5-14

解决3D向量的Yaw/Pitch/Roll旋转问题

我来帮你搞定这个3D向量旋转的问题！你之前的代码没正常工作，大概率是旋转矩阵的元素对应错误或者旋转顺序/坐标系的假设不匹配导致的。下面我会一步步给你讲清楚问题所在，以及正确的无外部库实现方案。

首先要确认的基础问题

1. 角度转弧度的函数必须正确

C标准库的sin()和cos()都是接收弧度参数的，所以你的cos_r和sin_r函数必须先把角度转换成弧度。如果这一步错了，所有旋转计算都会乱掉。先补上正确的实现：

#define M_PI 3.14159265358979323846f

float deg_to_rad(float degrees) {
    return degrees * M_PI / 180.0f;
}

float sin_r(float degrees) {
    return sin(deg_to_rad(degrees));
}

float cos_r(float degrees) {
    return cos(deg_to_rad(degrees));
}

2. 明确旋转顺序和坐标系

我们用最常用的3D场景约定：

Yaw（偏航）：绕Y轴旋转（左右转向）
Pitch（俯仰）：绕X轴旋转（上下抬头/低头）
Roll（翻滚）：绕Z轴旋转（左右倾斜）
右手坐标系：Y轴向上，X轴向右，Z轴向前
旋转顺序：先Yaw，再Pitch，最后Roll（每次绕自身当前轴旋转，也就是局部坐标系旋转）

正确的旋转实现

根据上面的约定，我们需要计算三个旋转矩阵的组合（Roll × Pitch × Yaw），并且用行向量乘以矩阵的方式计算变换后的向量（因为你的代码里是用原向量分量去乘矩阵元素，对应行向量的乘法逻辑）。

修改后的rotate函数如下：

class Vector3 {
public:
    float x, y, z;

    void rotate(float yaw, float pitch, float roll) {
        // 计算各角度的三角函数值
        float cos_yaw = cos_r(yaw);
        float sin_yaw = sin_r(yaw);
        float cos_pitch = cos_r(pitch);
        float sin_pitch = sin_r(pitch);
        float cos_roll = cos_r(roll);
        float sin_roll = sin_r(roll);

        // 计算组合旋转矩阵的元素（行向量乘法：v' = v × M，M = Roll × Pitch × Yaw）
        float m00 = cos_roll * cos_yaw - sin_roll * sin_pitch * sin_yaw;
        float m01 = -sin_roll * cos_pitch;
        float m02 = cos_roll * sin_yaw + sin_roll * sin_pitch * cos_yaw;

        float m10 = sin_roll * cos_yaw + cos_roll * sin_pitch * sin_yaw;
        float m11 = cos_roll * cos_pitch;
        float m12 = sin_roll * sin_yaw - cos_roll * sin_pitch * cos_yaw;

        float m20 = -cos_pitch * sin_yaw;
        float m21 = sin_pitch;
        float m22 = cos_pitch * cos_yaw;

        // 保存原向量值，避免计算时被覆盖
        float orig_x = x;
        float orig_y = y;
        float orig_z = z;

        // 执行行向量与矩阵的乘法
        x = orig_x * m00 + orig_y * m01 + orig_z * m02;
        y = orig_x * m10 + orig_y * m11 + orig_z * m12;
        z = orig_x * m20 + orig_y * m21 + orig_z * m22;
    }
};