Quaternion to Roll Pitch Yaw

0
votes

Where can I find a function (preferably written in php), that converts Quaternion rotation to 3D rotation (Roll Pitch Yaw) AND also solves gimbal lock problem (does not return NaN at gimbal lock angles). Thanks

1
quaternions

1 Answers

0
votes

This is not PHP, but C++. It should be easy to transfer to PHP. Invalid/gimbal cases can be seen where either rotateX, rotateY or rotateZ is not assinged because the value would be 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);