四元数到滚动俯仰偏航答案

【问题标题】：Quaternion to Roll Pitch Yaw四元数到滚动俯仰偏航
【发布时间】：2013-10-23 01:24:36
【问题描述】：

我在哪里可以找到一个函数（最好用 php 编写），它将四元数旋转转换为 3D 旋转（Roll Pitch Yaw）并且还解决了万向节锁定问题（在万向节锁定角度不返回 NaN）。谢谢

【问题讨论】：

【解决方案1】：

这不是 PHP，而是 C++。转移到 PHP 应该很容易。在rotateX、rotateY 或rotateZ 未分配的情况下，可以看到无效/万向架情况，因为该值将是NaN。

        //Our Quaternion is defined by qx,qy,qz,qw
        double qx;
        double qy;
        double qz;
        double qw;          

        double rotateXa0 = 2.0*(qy*qz + qw*qx);
        double rotateXa1 = qw*qw - qx*qx - qy*qy + qz*qz;
        double rotateX = 0.0;
        if (rotateXa0 != 0.0 && rotateXa1 != 0.0) 
            rotateX = atan2(rotateXa0, rotateXa1);

        double rotateYa0 = -2.0*(qx*qz - qw*qy);
        double rotateY = 0.0;
        if( rotateYa0 >= 1.0 )
            rotateY = M_PI/2.0;
        else if( rotateYa0 <= -1.0 )
            rotateY = -M_PI/2.0;
        else rotateY = asin(rotateYa0);

        double rotateZa0 = 2.0*(qx*qy + qw*qz);
        double rotateZa1 = qw*qw + qx*qx - qy*qy - qz*qz;
        double rotateZ = 0.0;
        if (rotateZa0 != 0.0 && rotateZa1 != 0.0)
            rotateZ = atan2(rotateZa0, rotateZa1);

【讨论】：