C++ version of @evbernardes' algorithm which is implemented in python

quat_to_euler.md

Tait-Bryan, intrinsic only specific implementation of @evbernardes' algorithm (https://github.com/evbernardes/quaternion_to_euler) in C++

Angles CameraBase::calculateAnglesFromQuaternion(DirectX::XMFLOAT4 quaternion)
{
    // Ported from: https://github.com/evbernardes/quaternion_to_euler

    int i=0;
    int j=0;
    int k=0;
    // The first action in the function is to reverse the sequence if the rotation is intrinsic. As we only do intrinsic rotations, we have to make sure i,j and k are reversed
    // So if e.g. the sequence is ZXY, i should be 1, j should be 0 and k should be 2 as the sequence to use is yxz. 
    switch(_rotationSequenceToUse)
    {
        case zyx:
            i = 0;
            j = 1;
            k = 2;
            break;
        case zxy:
            i = 1;
            j = 0;
            k = 2;
            break;
        case xyz:
            i = 2;
            j = 1;
            k = 0;
            break;
        case xzy:
            i = 1;
            j = 2;
            k = 0;
            break;
        case yxz:
            i = 2;
            j = 0;
            k = 1;
            break;
        case yzx:
            i = 0;
            j = 2;
            k = 1;
            break;
    }

    float sign = (float)static_cast<int>((i - j) * (j - k) * (k - i) / 2);

    float angles[3] = {0.0f};

    float quat[4] = {quaternion.x, quaternion.y, quaternion.z, quaternion.w};
    float a = quat[3] - quat[j];
    float b = quat[i] + quat[k] * sign;
    float c = quat[j] + quat[3];
    float d = quat[k] * sign - quat[i];

    float n2 = a*a + b*b + c*c + d*d;

    // always not proper as we only support Tait-Bryan angles/rotations

    angles[1] = std::acos((2.0f * (a*a + b*b) / n2) - 1.0f);
    bool safe1 = abs(angles[1]) >= FLT_EPSILON;
    bool safe2 = abs(angles[1] - DirectX::XM_PI) >= FLT_EPSILON;
    if(safe1 && safe2)
    {
        float half_sum = std::atan2(b, a);
        float half_diff = std::atan2(-d, c);

        angles[0] = half_sum + half_diff;
        angles[2] = half_sum - half_diff;
    }
    else
    {
        // always intrinsic as we rotate a camera
        angles[0] = 0.0f;

        if(!safe1)
        {
            float half_sum = std::atan2(b, a);
            angles[2] = 2.0f * half_sum;
        }
        if(!safe2)
        {
            float half_diff = std::atan2(-d, c);
            angles[2] = -2.0f * half_diff;
        }
    }

    for(int index=0;index<3;index++)
    {
        if(angles[index] < -DirectX::XM_PI)
        {
            angles[index] += DirectX::XM_2PI;
        }
        else
        {
            if(angles[index] > DirectX::XM_PI)
            {
                angles[index] -= DirectX::XM_2PI;
            }
        }
    }
    // always not proper as we only support Tait-Bryan angles/rotations
    angles[2] *= sign;
    angles[1] -= DirectX::XM_PIDIV2;
    // reversal, always intrinsic
    float tmp = angles[0];
    angles[0] = angles[2];
    angles[2] = tmp;

    // angle 1 is pitch, angle 0 is yaw and angle 2 is roll... 
    return Angles(clampAngle(angles[1]), clampAngle(angles[0]), clampAngle(angles[2]));
}