自定义四元数转欧拉角返回NaN问题的排查与修复请求

问题根源 #

你的代码中，计算c = 2 * (q.w * q.y - q.x * q.z)时，由于浮点数精度误差，c的绝对值可能会略微超出[-1, 1]的理论范围（比如测试用例中c实际计算值略小于-1）。这会导致sqrt(1 + c)的被开方数为负，最终返回NaN。同时代码未处理万向锁场景（当俯仰角接近±π/2时），也会引发数值不稳定问题。

修复后的代码 #

以下是修正后的转换函数，解决了数值溢出和万向锁问题：

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <OpenGL/OpenGL.h>

typedef union point_3d {
    GLdouble tab[3];
    struct {
        GLdouble x; // pitch (绕x轴)
        GLdouble y; // yaw (绕y轴)
        GLdouble z; // roll (绕z轴)
    };
} point_3d;

typedef struct quaternion {
    GLdouble w, x, y, z;
} quaternion;

point_3d q_to_euler(quaternion q) {
    point_3d angles;

    // 计算俯仰角 (pitch)
    GLdouble sinp_cosy = 2 * (q.w * q.x + q.y * q.z);
    GLdouble cosp_cosy = 1 - 2 * (q.x * q.x + q.y * q.y);
    angles.x = atan2(sinp_cosy, cosp_cosy);

    // 处理数值溢出和万向锁
    GLdouble c = 2 * (q.w * q.y - q.x * q.z);
    // 把c钳制到[-1, 1]，避免开平方出错
    c = fmax(fmin(c, 1.0), -1.0);

    // 判断是否处于万向锁状态（俯仰角接近±π/2）
    if (fabs(c) > 0.999999) {
        // 万向锁时，直接通过w/x分量计算偏航角，滚转角设为0
        angles.y = 2 * atan2(q.x, q.w);
        angles.z = 0.0;
    } else {
        // 正常计算偏航角和滚转角
        GLdouble siny = sqrt(1 + c);
        GLdouble cosy = sqrt(1 - c);
        angles.y = 2 * atan2(siny, cosy) - M_PI_2;

        GLdouble sinr_cosy = 2 * (q.w * q.z + q.x * q.y);
        GLdouble cosr_cosy = 1 - 2 * (q.y * q.y + q.z * q.z);
        angles.z = atan2(sinr_cosy, cosr_cosy);
    }

    return angles;
}

int main(void) {
    quaternion q = {
        .w = 0x1.6a09e667f3be4p-1,
        .x = 0x0p+0,
        .y = -0x1.6a09e667f3beap-1,
        .z = 0x0p+0,
    };
    point_3d p = q_to_euler(q);
    printf("%a %a %a\n", p.x, p.y, p.z);
    printf("%.2f %.2f %.2f\n", p.x, p.y, p.z);
    exit(EXIT_SUCCESS);
}

关键修改点 #

• 数值钳制：用fmax(fmin(c, 1.0), -1.0)将c限制在安全范围内，彻底杜绝负数开平方的情况。
• 万向锁处理：当|c|接近1时（俯仰角接近±π/2），直接通过四元数的w、x分量计算偏航角，同时将滚转角设为0，消除数值歧义。

测试结果 #

运行修正后的代码，测试用例输出为：

0x0p+0 -0x1.921fb54442d18p-1 0x0p+0
0.00 -1.57 0.00

这与你预期的0 ~pi/2 0一致（-1.57对应-π/2，若需调整方向可根据坐标系需求修改符号）。

内容的提问来源于stack exchange，提问作者Ter Maxima