[fixed] Possible small bug in Rotation::getYawPitchRoll(...) code

[FCuser2019]

I suspected that the last described problem was caused by the execution of the code

Code: Select all

else {
        y = atan2(2.0*(q01+q23),(q00+q33)-(q11+q22));
        p = qd2 > 1.0 ? D_PI/2.0 : (qd2 < -1.0 ? -D_PI/2.0 : asin (qd2));
        r = atan2(2.0*(q12+q03),(q22+q33)-(q00+q11));
    }

even in gimbal lock condition. To verify this I extracted from Rotation.cpp only the lines of code needed to test the methods setYawPitchRoll(...) and getYawPitchRoll(...) and I added some printf in getYawPitchRoll(...) to see which part of the code is executed when (yaw=10° , pitch=+90° , roll=20°)...

As I suspected the code that treats the normal condition is executed instead of the code that treats the gimbal lock condition! OK, so the problem is in the 2 tests if(fabs(qd2-1.0) < DBL_EPSILON) and if(fabs(qd2+1.0) < DBL_EPSILON).
I replaced DBL_EPSILON with a double toll variable:

Code: Select all

void Rotation::getYawPitchRoll(double& y, double& p, double& r) const
{
    double q00 = quat[0]*quat[0];
    double q11 = quat[1]*quat[1];
    double q22 = quat[2]*quat[2];
    double q33 = quat[3]*quat[3];
    double q01 = quat[0]*quat[1];
    double q02 = quat[0]*quat[2];
    double q03 = quat[0]*quat[3];
    double q12 = quat[1]*quat[2];
    double q13 = quat[1]*quat[3];
    double q23 = quat[2]*quat[3];
    double qd2 = 2.0*(q13-q02);

    double toll=1.0e-15; 
    printf ("toll=%g\n",toll);

    // handle gimbal lock
    if (fabs(qd2-1.0) < toll) {
        // north pole
        printf("gimbal lock +90°\n");
        y = 0.0;
        p = M_PI/2.0;
        r = 2.0 * atan2(quat[0],quat[3]);
    }
    else if (fabs(qd2+1.0) < toll) {
        // south pole
        printf("gimbal lock -90°\n");
        y = 0.0;
        p = -M_PI/2.0;
        r = -2.0 * atan2(quat[0],quat[3]);
    }
    else {
        printf("normal condition\n");
        y = atan2(2.0*(q01+q23),(q00+q33)-(q11+q22));
        p = qd2 > 1.0 ? M_PI/2.0 : (qd2 < -1.0 ? -M_PI/2.0 : asin (qd2));
        r = atan2(2.0*(q12+q03),(q22+q33)-(q00+q11));
    }

    // convert to degree
    y = (y/M_PI)*180;
    p = (p/M_PI)*180;
    r = (r/M_PI)*180;
}

I redid the test with toll=1e-15 and now when (yaw=10° , pitch=+90° , roll=20°) the correct part of the code is executed.

Recently a new piece of code has been merged in Rotation.cpp (from Realthunder, from OCCT gp/gp_Quaternion.cxx) that implements the conversion between quaternions and generalized Euler angles, in Rotation::getEulerAngles(...) a tolerance 16*DBL_EPSILON is used. I tested this by setting in my code toll=16*DBL_EPSILON and also in this case the correct part of the code is executed.

[openBrain]

I've just realised that the Python console also displays wrong angles after setting
(yaw=10° , pitch=+90° , roll=20°):

Found the problem.
One of my more epic fixes PR submitted : https://github.com/FreeCAD/FreeCAD/pull/5027
Feel free to ask if you need details you can't get by looking at the PR diff

[FCuser2019]

Many thanks openBrain for your solution to the problem.
I will try it later

[FCuser2019]

Hello, sorry for the late reply (busy start to the week).

To test whether changing the test condition from < to <= also solves the problem for possible other problematic angles besides (yaw=10° , pitch=+90° , roll=20°), I modified my test code (now test2.cpp, Rotation2.cpp and Rotation2.h) so as to test all combinations of angles with:

yaw from -179° to +179° step +1°
pitch= -90° , +90°
roll from -179° to +179° step +1°

the <= test reduces the number of bad cases but, unfortunately, there are other angle combinations for which the normal condition branch of code is still executed instead of the gimbal lock branch.

Probably to solve this problem there is no alternative to solutions like the one used in Rotation::getEulerAngles(...), i.e. to use a tolerance greater than DBL_EPSILON in the tests.

Out of curiosity, I did some tests:

using if(fabs(qd2-1.0) < toll) , if(fabs(qd2+1.0) < toll) with:
toll= DBL_EPSILON --> test2 displays problematic angle combinations
toll= 2*DBL_EPSILON --> test2 displays problematic angle combinations
toll= 3*DBL_EPSILON --> test2 does NOT display problematic angle combinations
toll= 4*DBL_EPSILON --> test2 does NOT display problematic angle combinations

using if(fabs(qd2-1.0) <= toll) , if(fabs(qd2+1.0) <= toll) with:
toll= DBL_EPSILON --> test2 displays problematic angle combinations
toll= 2*DBL_EPSILON --> test2 does NOT display problematic angle combinations
toll= 3*DBL_EPSILON --> test2 does NOT display problematic angle combinations
toll= 4*DBL_EPSILON --> test2 does NOT display problematic angle combinations

obviously this is just an empirical test, the optimal value for the tolerance (if any) should be determined by a mathematical proof that is valid all the time (and not only for combinations of integer angles between -179° and +179°), unfortunately I don't have the necessary knowledge for this study.
As I have already said, the developers of OCCT in Rotation::getEulerAngles(...) use a tolerance of 16*DBL_EPSILON, perhaps they have done the necessary theoretical studies to determine this value.

However, while we wait to see what value to use for the tolerance, the modification proposed by openBrain reduces the set of problematic angle combinations.

[openBrain]

As I have already said, the developers of OCCT in Rotation::getEulerAngles(...) use a tolerance of 16*DBL_EPSILON, perhaps they have done the necessary theoretical studies to determine this value.

Looks like I missed this information. Could you have a pointer to OCC code regarding this ?

[FCuser2019]

Hello openBrain, you can find the code for Rotation::getEulerAngles(...) at the end of https://github.com/FreeCAD/FreeCAD/blob ... tation.cpp
(here the forum topic https://forum.freecadweb.org/viewtopic. ... 10#p485189)

I have examined in more detail the code in Rotation::getEulerAngles(...) that handles the gimbal lock condition (the else branch):

Code: Select all

    {
        double cy = sqrt (M(o.i, o.i) * M(o.i, o.i) + M(o.j, o.i) * M(o.j, o.i));
        if (cy > 16 * DBL_EPSILON) 
        {
            theAlpha = atan2 (M(o.k, o.j), M(o.k, o.k));
            theGamma = atan2 (M(o.j, o.i), M(o.i, o.i));
        } 
        else 
        {
            theAlpha = atan2 (-M(o.j, o.k), M(o.j, o.j));
            theGamma = 0.;
        }
        theBeta = atan2 (-M(o.k, o.i), cy);
    }

the condition tested in Rotation::getEulerAngles(...) appears mathematically equivalent to the condition tested in Rotation::getYawPitchRoll(...) but most likely the two conditions are not equivalent from the point of view of rounding errors (there is an extra square root), so perhaps that is why a large 16* DBL_EPSILON tolerance is used in the Rotation::getEulerAngles(...) code.

P.S.
The good news is that this new code added by RealThunder may in future allow all possible 24 rotations to be used in the Placement window instead of just the intrinsic zy'x'' rotation available now.

[openBrain]

Hello openBrain, you can find the code for Rotation::getEulerAngles(...) at the end of https://github.com/FreeCAD/FreeCAD/blob ... tation.cpp

OK, it's not OCC code. It's code by @realthunder using OCC API.
Let's ask :

Ping

@RT, could you please explain if this "16*DBL_EPSILON" is based on some rationals ? Thx

[realthunder]

@RT, could you please explain if this "16*DBL_EPSILON" is based on some rationals ? Thx

As mentioned in the code comments, this piece of code is copied and modified from OCC gp_Quaternion.cxx. It is done so to avoid dependency of OCC in FreeCAD Base module. I didn't touch the functionality part of the code, so I'll just assume OCC chose this tolerance for good reason.

[openBrain]

As mentioned in the code comments, this piece of code is copied and modified from OCC gp_Quaternion.cxx. It is done so to avoid dependency of OCC in FreeCAD Base module. I didn't touch the functionality part of the code, so I'll just assume OCC chose this tolerance for good reason.

Thanks, created a new PR applying this value.

[FCuser2019]

realthunder, openBrain thanks