四元数与欧拉角

欧拉角：

（x，y，z）关于xyz轴旋转多少度，注意：欧拉角有父子层级，因此会产生万向锁，并且同一旋转有多种表达方式

四元数

欧拉角和四元数相互转化代码

Quaternion EulerToQuaternion(float pitch, float yaw, float roll) {
    float cy = cos(yaw * 0.5);
    float sy = sin(yaw * 0.5);
    float cp = cos(pitch * 0.5);
    float sp = sin(pitch * 0.5);
    float cr = cos(roll * 0.5);
    float sr = sin(roll * 0.5);

    return Quaternion(
        cr * cp * cy + sr * sp * sy,
        sr * cp * cy - cr * sp * sy,
        cr * sp * cy + sr * cp * sy,
        cr * cp * sy - sr * sp * cy
    );
}

EulerAngle QuaternionToEuler(const Quaternion& q) {
    EulerAngle angles;

    // 计算俯仰角(pitch)
    float sinp = 2.0 * (q.w * q.y - q.z * q.x);
    if (fabs(sinp) >= 1)
        angles.pitch = copysign(M_PI / 2, sinp); // 使用90度
    else
        angles.pitch = asin(sinp);

    // 计算偏航角(yaw)
    float siny_cosp = 2.0 * (q.w * q.z + q.x * q.y);
    float cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z);
    angles.yaw = atan2(siny_cosp, cosy_cosp);

    // 计算滚转角(roll)
    float sinr_cosp = 2.0 * (q.w * q.x + q.y * q.z);
    float cosr_cosp = 1.0 - 2.0 * (q.x * q.x + q.y * q.y);
    angles.roll = atan2(sinr_cosp, cosr_cosp);

    return angles;
}