[Core] Rotation : fix gimbal lock handling

+ fix Euler representation on GUI
2021-09-12 19:45:28 +02:00
parent 455412ccc7
commit 59a4a4c361
2 changed files with 11 additions and 11 deletions
--- a/src/Base/Rotation.cpp
+++ b/src/Base/Rotation.cpp
@@ -673,13 +673,13 @@ void Rotation::getYawPitchRoll(double& y, double& p, double& r) const
    double qd2 = 2.0*(q13-q02);

    // handle gimbal lock
-    if (fabs(qd2-1.0) < DBL_EPSILON) {
+    if (fabs(qd2-1.0) <= DBL_EPSILON) {
        // north pole
        y = 0.0;
        p = D_PI/2.0;
        r = 2.0 * atan2(quat[0],quat[3]);
    }
-    else if (fabs(qd2+1.0) < DBL_EPSILON) {
+    else if (fabs(qd2+1.0) <= DBL_EPSILON) {
        // south pole
        y = 0.0;
        p = -D_PI/2.0;