0025574: gp_YawPitchRoll Euler Angle computation gives wrong results

Conversion of gp_Quaternion to and from intrinsic Tait-Bryan angles (including gp_YawPitchRoll) is fixed.

Before that fix the sequence of rotation axes was opposite to intended; e.g. gp_YawPitchRoll (equivalent to gp_Intrinsic_ZYX) actually was defining intrinsic rotations around X, then Y, then Z.
Now this is fixed, and rotations are made in correct order.

Comments to gp_EulerSequence enumeration are restored (lost due to CDL extraction).

Test bugs fclasses bug25574 is added to check correctness of Euler sequences, including cases from #25574 and #25946.

dox/dev_guides/upgrade/upgrade.md

@@ -585,3 +585,13 @@ Verson numbers of BinOCAF and XmlOCAF formats are incremented; new files cannot
 
 For loading OCAF files saved by previous versions and containing attribute TPrsStd_AISPresentation it is necessary that environment variable CSF_MIGRATION_TYPES should be defined, pointing to file src/StdResources/MigrationSheet.txt.
 When using documents loaded from a file, make sure to call method TPrsStd_AISViewer::New() prior to accessing TPrsStd_AISPresentation attributes in this document (that method will create them).
+
+
+@subsection Correction of interpretation of Euler angles in gp_Quaternion
+
+Conversion of gp_Quaternion to and from intrinsic Tait-Bryan angles (including gp_YawPitchRoll) is fixed.
+
+Before that fix the sequence of rotation axes was opposite to intended; e.g. gp_YawPitchRoll (equivalent to gp_Intrinsic_ZYX) actually was defining intrinsic rotations around X, then Y, then Z.
+Now this is fixed, and rotations are made in correct order.
+
+Applications that use gp_Quaternion to convert Yaw-Pitch-Roll angles (or other intrinsic Tait-Bryan sequences) may need to be updated to take this change into account.

src/IVtkOCC/IVtkOCC_ViewerSelector.cxx

@@ -16,7 +16,6 @@
 #include <IVtkOCC_ViewerSelector.hxx>
 #include <Select3D_SensitiveBox.hxx>
 #include <TColgp_Array1OfPnt2d.hxx>
-#include <gp_Quaternion.hxx>
 #include <Graphic3d_Camera.hxx>
 
 

src/QABugs/QABugs_19.cxx

@@ -29,6 +29,7 @@
 
 #include <gp_Pnt2d.hxx>
 #include <gp_Ax1.hxx>
+#include <gp_Quaternion.hxx>
 #include <GCE2d_MakeSegment.hxx>
 #include <Geom2d_TrimmedCurve.hxx>
 #include <DrawTrSurf.hxx>
@@ -3594,6 +3595,237 @@ static Standard_Integer OCC24923(
   return 0;
 }
 
+//=======================================================================
+//function : OCC25574
+//purpose  : check implementation of Euler angles in gp_Quaternion
+//=======================================================================
+
+static Standard_Integer OCC25574 (Draw_Interpretor& theDI, Standard_Integer /*argc*/, const char** /*argv*/)
+{
+  Standard_Boolean isTestOk = Standard_True;
+
+  // Check consistency of Get and Set operations for Euler angles
+  gp_Quaternion aQuat;
+  aQuat.Set(0.06766916507860499, 0.21848101129786085, 0.11994599260380681,0.9660744746954637);
+  Standard_Real alpha,beta,gamma;
+  gp_Mat aRinv = aQuat.GetMatrix().Inverted();
+  gp_Mat aI;
+  aI.SetIdentity();
+  const char* names[] = { "Extrinsic_XYZ", "Extrinsic_XZY", "Extrinsic_YZX", "Extrinsic_YXZ", "Extrinsic_ZXY", "Extrinsic_ZYX", 
+                          "Intrinsic_XYZ", "Intrinsic_XZY", "Intrinsic_YZX", "Intrinsic_YXZ", "Intrinsic_ZXY", "Intrinsic_ZYX", 
+                          "Extrinsic_XYX", "Extrinsic_XZX", "Extrinsic_YZY", "Extrinsic_YXY", "Extrinsic_ZYZ", "Extrinsic_ZXZ",
+                          "Intrinsic_XYX", "Intrinsic_XZX", "Intrinsic_YZY", "Intrinsic_YXY", "Intrinsic_ZXZ", "Intrinsic_ZYZ" };
+  for (int i = gp_Extrinsic_XYZ; i <= gp_Intrinsic_ZYZ; i++)
+  {
+    aQuat.GetEulerAngles (gp_EulerSequence(i), alpha, beta, gamma);
+
+    gp_Quaternion aQuat2;
+    aQuat2.SetEulerAngles (gp_EulerSequence(i), alpha, beta, gamma);
+
+    gp_Mat aR = aQuat2.GetMatrix();
+    gp_Mat aDiff = aR * aRinv - aI;
+    if (aDiff.Determinant() > 1e-5)
+    {
+      theDI << "Error: Euler angles conversion incorrect for sequence " << names[i - gp_Extrinsic_XYZ] << "\n";
+      isTestOk = Standard_False;
+    }
+  }
+
+  // Check conversion between intrinsic and extrinsic rotations
+  // Any extrinsic rotation is equivalent to an intrinsic rotation
+  // by the same angles but with inverted order of elemental rotations, and vice versa
+  // For instance:
+  //    Extrinsic_XZY = Incrinsic_XZY
+  //    R = X(A)Z(B)Y(G) --> R = Y(G)Z(B)X(A)
+  alpha = 0.1517461713131;
+  beta = 1.5162198410141;
+  gamma = 1.9313156236541;
+  Standard_Real alpha2, beta2, gamma2;
+  gp_EulerSequence pairs[][2] = { {gp_Extrinsic_XYZ, gp_Intrinsic_ZYX},
+                                  {gp_Extrinsic_XZY, gp_Intrinsic_YZX},
+                                  {gp_Extrinsic_YZX, gp_Intrinsic_XZY},
+                                  {gp_Extrinsic_YXZ, gp_Intrinsic_ZXY},
+                                  {gp_Extrinsic_ZXY, gp_Intrinsic_YXZ},
+                                  {gp_Extrinsic_ZYX, gp_Intrinsic_XYZ} };
+  for (int i = 0; i < 6; i++)
+  {
+    aQuat.SetEulerAngles(pairs[i][0],  alpha,  beta,  gamma);
+    aQuat.GetEulerAngles(pairs[i][1], gamma2, beta2, alpha2);
+
+    if (Abs(alpha - alpha2) > 1e-5 || Abs(beta - beta2) > 1e-5 || Abs(gamma - gamma2) > 1e-5)
+    {
+      theDI << "Error: intrinsic and extrinsic conversion incorrect for sequence " << names[i] << "\n";
+      isTestOk = Standard_False;
+    }
+  }
+
+  // Check correspondence of enumeration and actual rotation it defines, 
+  // by rotation by one axis and checking that it does not change a point on that axis
+  for (int i = gp_Extrinsic_XYZ; i <= gp_Intrinsic_ZYZ; i++)
+  {
+    // Iterate over rotations R(A)R(B)R(G) for each Euler angle Alpha, Beta, Gamma
+    // There are three ordered axes corresponding to three rotations.
+    // Each rotation applyed with current angle around current axis.
+    for (int j=0; j < 3; j++)
+    {
+      // note that current axis index is obtained by parsing of enumeration name!
+      int anAxis = names[i - gp_Extrinsic_XYZ][10 + j] - 'X';
+      Standard_ASSERT_RETURN (anAxis >=0 && anAxis <= 2, "Incorrect parsing of enumeration name", 1);
+
+      // Set 90 degrees to current Euler angle
+      double anAngles[3] = {0., 0., 0.};
+      anAngles[j] = 0.5 * M_PI;
+
+      gp_Quaternion q2;
+      q2.SetEulerAngles (gp_EulerSequence(i), anAngles[0], anAngles[1], anAngles[2]);
+
+      // Set point on axis corresponding to current rotation
+      // We will apply rotation around this axis
+      gp_XYZ v (0., 0., 0.);
+      v.SetCoord (anAxis + 1, 1.);
+
+      // Apply rotation to point
+      gp_Trsf aT;
+      aT.SetRotation (q2);
+      gp_XYZ v2 = v;
+      aT.Transforms (v2);
+
+      // Check that point is still on origin position
+      if ((v - v2).SquareModulus() > Precision::SquareConfusion())
+      {
+        // avoid reporting small coordinates
+        for (int k=1; k <= 3; k++) 
+          if (Abs (v2.Coord(k)) < Precision::Confusion())
+            v2.SetCoord (k, 0.);
+
+        isTestOk = Standard_False;
+        theDI << "Error: Euler sequence " << names[i - gp_Extrinsic_XYZ] << " is incorrect:\n";
+        theDI << "rotating by angle 90 deg around " << (anAxis == 0 ? "X" : anAxis == 1 ? "Y" : "Z") <<
+                      " converts vector (" << v.X() << ", " << v.Y() << ", " << v.Z() << ") to ("
+                        << v2.X() << ", " << v2.Y() << ", " << v2.Z() << ")\n";
+      }
+    }
+  }
+
+  // Check correspondence of enumeration and actual rotation it defines, 
+  // by comparing cumulative rotation matrix with sequence of rotations by axes
+  const Standard_Real anAngle[3] = { 0.1, 0.2, 0.3 };
+  for (int i = gp_Extrinsic_XYZ; i <= gp_Intrinsic_ZYZ; i++)
+  {
+    // Sequence of rotations
+    gp_Mat aR[3];
+    for (int j=0; j < 3; j++)
+    {
+      // note that current axis index is obtained by parsing of enumeration name!
+      int anAxis = names[i - gp_Extrinsic_XYZ][10 + j] - 'X';
+      Standard_ASSERT_RETURN (anAxis >=0 && anAxis <= 2, "Incorrect parsing of enumeration name", 1);
+
+      // Set point on axis corresponding to current rotation
+      // We will apply rotation around this axis
+      gp_XYZ v (0., 0., 0.);
+      v.SetCoord (anAxis + 1, 1.);
+      aR[j].SetRotation (v, anAngle[j]);
+    }
+
+    // construct cumulative transformation (differently for extrinsic and intrinsic rotations);
+    // note that we parse first symbol of the enum name to identify its type
+    gp_Mat aRot;
+    if (names[i - gp_Extrinsic_XYZ][0] == 'E') // extrinsic
+    {
+      aRot = aR[2] * aR[1] * aR[0];
+    }
+    else // intrinsic
+    {
+      aRot = aR[0] * aR[1] * aR[2];
+    }
+
+    // set the same angles in quaternion
+    gp_Quaternion aQuat;
+    aQuat.SetEulerAngles (gp_EulerSequence(i), anAngle[0], anAngle[1], anAngle[2]);
+
+    gp_Mat aRQ = aQuat.GetMatrix();
+    gp_Mat aDiff = aRQ * aRot.Inverted() - aI;
+    if (aDiff.Determinant() > 1e-5)
+    {
+      theDI << "Error: Euler angles conversion does not correspond to sequential rotations for " << names[i - gp_Extrinsic_XYZ] << "\n";
+      isTestOk = Standard_False;
+    }
+  }
+
+  // similar checkfor YawPitchRoll sequence as defined in description of #25574
+  {
+    // Start with world coordinate system
+    gp_Ax2 world;
+
+    // Perform three rotations using the yaw-pitch-roll convention.
+    // This means: rotate around the original z axis with angle alpha,
+    // then rotate around the new y axis with angle beta,
+    // then rotate around the new x axis with angle gamma.
+    alpha = 0.0 / 180.0 * M_PI;
+    beta = -35.0 / 180.0 * M_PI;
+    gamma = 90.0 / 180.0 * M_PI;
+
+    const gp_Quaternion rotationZ(world.Direction(), alpha);
+    const gp_Vec rotY = rotationZ.Multiply(world.YDirection());
+    const gp_Vec rotX = rotationZ.Multiply(world.XDirection());
+
+    const gp_Quaternion rotationY(rotY, beta);
+    const gp_Vec rotZ = rotationY.Multiply(world.Direction());
+    const gp_Vec rotRotX = rotationY.Multiply(rotX);
+
+    const gp_Quaternion rotationX(rotRotX, gamma);
+    const gp_Vec rotRotZ = rotationX.Multiply(rotZ);
+
+    gp_Ax2 result(gp_Pnt(0.0, 0.0, 0.0), rotRotZ, rotRotX);
+
+    // Now compute the Euler angles
+    gp_Trsf transformation;
+    transformation.SetDisplacement(gp_Ax2(), result);
+
+    Standard_Real computedAlpha;
+    Standard_Real computedBeta;
+    Standard_Real computedGamma;
+
+    transformation.GetRotation().GetEulerAngles(gp_YawPitchRoll, computedAlpha, computedBeta, computedGamma);
+
+    // We expect now to get the same angles as we have used for our rotations
+    if (Abs(alpha - computedAlpha) > 1e-5 || Abs(beta - computedBeta) > 1e-5 || Abs(gamma - computedGamma) > 1e-5)
+    {
+      theDI << "Error: unexpected values of Euler angles for YawPitchRoll sequence:\n";
+      theDI << "alpha: " << alpha / M_PI * 180.0 << " and computed alpha: "
+            << computedAlpha / M_PI * 180.0 << "\n";
+      theDI << "beta: " << beta / M_PI * 180.0 << " and computed beta: "
+            << computedBeta / M_PI * 180.0 << "\n";
+      theDI << "gamma: " << gamma / M_PI * 180.0 << " and computed gamma: "
+            << computedGamma / M_PI * 180.0 << "\n";
+      isTestOk = Standard_False;
+    }
+  }
+
+  // test from #25946
+  {
+    gp_Quaternion q;
+    q.Set(0.06766916507860499, 0.21848101129786085, 0.11994599260380681,0.9660744746954637);
+
+    q.GetEulerAngles(gp_Intrinsic_ZYX, alpha,beta, gamma);
+    Standard_Real alpha2,beta2,gamma2;
+    q.GetEulerAngles(gp_Extrinsic_XYZ, alpha2,beta2,gamma2);
+
+    // gp_Intrinsic_ZYX and gp_Extrinsic_XYZ should produce the same values of angles but in opposite order
+    if (Abs(alpha - gamma2) > 1e-5 || Abs(beta - beta2) > 1e-5 || Abs(gamma - alpha2) > 1e-5)
+    {
+      theDI << "Error: Euler angles computed for gp_Intrinsic_ZYX and gp_Extrinsic_XYZ do not match:\n";
+      theDI << "alpha: " << alpha / M_PI * 180.0 << " and " << alpha2 / M_PI * 180.0 << "\n";
+      theDI << "beta: " << beta / M_PI * 180.0 << " and " << beta2 / M_PI * 180.0 << "\n";
+      theDI << "gamma: " << gamma / M_PI * 180.0 << " and " << gamma2 / M_PI * 180.0 << "\n";
+      isTestOk = Standard_False;
+    }
+  }
+
+  theDI << (isTestOk ? "Test completed" : "Test failed") << "\n";
+  return 0;
+}
+
 #include <TColGeom_Array1OfBSplineCurve.hxx>
 #include <TColStd_Array1OfReal.hxx>
 #include <TColGeom_HArray1OfBSplineCurve.hxx>
@@ -4887,7 +5119,7 @@ void QABugs::Commands_19(Draw_Interpretor& theCommands) {
 
   theCommands.Add ("OCC24537", "OCC24537 [file]", __FILE__, OCC24537, group);
   theCommands.Add ("OCC26750", "OCC26750", __FILE__, OCC26750, group);
-
+  theCommands.Add ("OCC25574", "OCC25574", __FILE__, OCC25574, group);
   theCommands.Add ("OCC26746", "OCC26746 torus [toler NbCheckedPoints] ", __FILE__, OCC26746, group);
 
   theCommands.Add ("BUC26658", "BUC26658 unexpected selection in the context using a selection filter", __FILE__, BUC26658, group);

src/gp/gp_EulerSequence.hxx

@@ -17,35 +17,60 @@
 #ifndef _gp_EulerSequence_HeaderFile
 #define _gp_EulerSequence_HeaderFile
 
+//! Enumerates all 24 possible variants of generalized
+//! Euler angles, defining general 3d rotation by three
+//! rotations around main axes of coordinate system,
+//! in different possible orders.
+//!
+//! The name of the enumeration
+//! corresponds to order of rotations, prefixed by type
+//! of co-ordinate system used:
+//! - Intrinsic: rotations are made around axes of rotating
+//!   co-ordinate system associated with the object
+//! - Extrinsic: rotations are made around axes of fixed
+//!   (static) co-ordinate system
+//!
+//! Two specific values are provided for most frequently used
+//! conventions: classic Euler angles (intrinsic ZXZ) and
+//! yaw-pitch-roll (intrinsic ZYX).
 
 enum gp_EulerSequence
 {
-gp_EulerAngles,
+  //! Classic Euler angles, alias to Intrinsic_ZXZ
-gp_YawPitchRoll,
+  gp_EulerAngles,
-gp_Extrinsic_XYZ,
+
-gp_Extrinsic_XZY,
+  //! Yaw Pitch Roll (or nautical) angles, alias to Intrinsic_ZYX
-gp_Extrinsic_YZX,
+  gp_YawPitchRoll,
-gp_Extrinsic_YXZ,
+
-gp_Extrinsic_ZXY,
+  // Tait-Bryan angles (using three different axes)
-gp_Extrinsic_ZYX,
+  gp_Extrinsic_XYZ,
-gp_Intrinsic_XYZ,
+  gp_Extrinsic_XZY,
-gp_Intrinsic_XZY,
+  gp_Extrinsic_YZX,
-gp_Intrinsic_YZX,
+  gp_Extrinsic_YXZ,
-gp_Intrinsic_YXZ,
+  gp_Extrinsic_ZXY,
-gp_Intrinsic_ZXY,
+  gp_Extrinsic_ZYX,
-gp_Intrinsic_ZYX,
+
-gp_Extrinsic_XYX,
+  gp_Intrinsic_XYZ,
-gp_Extrinsic_XZX,
+  gp_Intrinsic_XZY,
-gp_Extrinsic_YZY,
+  gp_Intrinsic_YZX,
-gp_Extrinsic_YXY,
+  gp_Intrinsic_YXZ,
-gp_Extrinsic_ZYZ,
+  gp_Intrinsic_ZXY,
-gp_Extrinsic_ZXZ,
+  gp_Intrinsic_ZYX,
-gp_Intrinsic_XYX,
+
-gp_Intrinsic_XZX,
+  // Proper Euler angles (using two different axes, first and third the same)
-gp_Intrinsic_YZY,
+  gp_Extrinsic_XYX,
-gp_Intrinsic_YXY,
+  gp_Extrinsic_XZX,
-gp_Intrinsic_ZXZ,
+  gp_Extrinsic_YZY,
-gp_Intrinsic_ZYZ
+  gp_Extrinsic_YXY,
+  gp_Extrinsic_ZYZ,
+  gp_Extrinsic_ZXZ,
+
+  gp_Intrinsic_XYX,
+  gp_Intrinsic_XZX,
+  gp_Intrinsic_YZY,
+  gp_Intrinsic_YXY,
+  gp_Intrinsic_ZXZ,
+  gp_Intrinsic_ZYZ
 };
 
 #endif // _gp_EulerSequence_HeaderFile

src/gp/gp_Quaternion.cxx

@@ -191,6 +191,8 @@ gp_Mat gp_Quaternion::GetMatrix () const
   return aMat;
 }
 
+namespace { // anonymous namespace
+
 //=======================================================================
 //function : translateEulerSequence
 //purpose  : 
@@ -237,12 +239,18 @@ gp_EulerSequence_Parameters translateEulerSequence (const gp_EulerSequence theSe
   case gp_Extrinsic_ZXY: return Params (3, F, F, T);
   case gp_Extrinsic_ZYX: return Params (3, T, F, T);
 
-  case gp_Intrinsic_XYZ: return Params (1, F, F, F);
+  // Conversion of intrinsic angles is made by the same code as for extrinsic,
-  case gp_Intrinsic_XZY: return Params (1, T, F, F);
+  // using equivalence rule: intrinsic rotation is equivalent to extrinsic
-  case gp_Intrinsic_YZX: return Params (2, F, F, F);
+  // rotation by the same angles but with inverted order of elemental rotations.
-  case gp_Intrinsic_YXZ: return Params (2, T, F, F);
+  // Swapping of angles (Alpha <-> Gamma) is done inside conversion procedure;
-  case gp_Intrinsic_ZXY: return Params (3, F, F, F);
+  // sequence of axes is inverted by setting appropriate parameters here.
-  case gp_Intrinsic_ZYX: return Params (3, T, F, F);
+  // Note that proper Euler angles (last block below) are symmetric for sequence of axes.
+  case gp_Intrinsic_XYZ: return Params (3, T, F, F);
+  case gp_Intrinsic_XZY: return Params (2, F, F, F);
+  case gp_Intrinsic_YZX: return Params (1, T, F, F);
+  case gp_Intrinsic_YXZ: return Params (3, F, F, F);
+  case gp_Intrinsic_ZXY: return Params (2, T, F, F);
+  case gp_Intrinsic_ZYX: return Params (1, F, F, F);
 
   case gp_Extrinsic_XYX: return Params (1, F, T, T);
   case gp_Extrinsic_XZX: return Params (1, T, T, T);
@@ -260,10 +268,12 @@ gp_EulerSequence_Parameters translateEulerSequence (const gp_EulerSequence theSe
 
   default:
   case gp_EulerAngles : return Params (3, F, T, F); // = Intrinsic_ZXZ
-  case gp_YawPitchRoll: return Params (3, T, F, F); // = Intrinsic_ZYX
+  case gp_YawPitchRoll: return Params (1, F, F, F); // = Intrinsic_ZYX
   };
 }
 
+} // anonymous namespace
+
 //=======================================================================
 //function : SetEulerAngles
 //purpose  : 

tests/bugs/fclasses/bug25574

@@ -0,0 +1,9 @@
+puts "=========="
+puts "0025574: gp_YawPitchRoll Euler Angle computation gives wrong results"
+puts "=========="
+
+pload QAcommands
+
+puts "Checking conversions of Euler angles in gp_Quaternion"
+OCC25574
+