OpenCascade/occt
1
0
Fork 0
mirror of https://git.dev.opencascade.org/repos/occt.git synced 2025-04-03 17:56:21 +03:00
Code Packages Releases Activity

0025574: gp_YawPitchRoll Euler Angle computation gives wrong results

Browse Source 
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.
This commit is contained in:
akz 2015-10-13 10:30:52 +03:00 committed by bugmaster
parent 5fce160515
commit 4f5ad41656
6 changed files with 320 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
dox/dev_guides/upgrade/upgrade.md
View File

@ -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.

1
src/IVtkOCC/IVtkOCC_ViewerSelector.cxx
View File

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

234
src/QABugs/QABugs_19.cxx
View File

@ -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);

77
src/gp/gp_EulerSequence.hxx
View File

@ -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,
gp_YawPitchRoll,
gp_Extrinsic_XYZ,
gp_Extrinsic_XZY,
gp_Extrinsic_YZX,
gp_Extrinsic_YXZ,
gp_Extrinsic_ZXY,
gp_Extrinsic_ZYX,
gp_Intrinsic_XYZ,
gp_Intrinsic_XZY,
gp_Intrinsic_YZX,
gp_Intrinsic_YXZ,
gp_Intrinsic_ZXY,
gp_Intrinsic_ZYX,
gp_Extrinsic_XYX,
gp_Extrinsic_XZX,
gp_Extrinsic_YZY,
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
//! Classic Euler angles, alias to Intrinsic_ZXZ
gp_EulerAngles,
//! Yaw Pitch Roll (or nautical) angles, alias to Intrinsic_ZYX
gp_YawPitchRoll,
// Tait-Bryan angles (using three different axes)
gp_Extrinsic_XYZ,
gp_Extrinsic_XZY,
gp_Extrinsic_YZX,
gp_Extrinsic_YXZ,
gp_Extrinsic_ZXY,
gp_Extrinsic_ZYX,
gp_Intrinsic_XYZ,
gp_Intrinsic_XZY,
gp_Intrinsic_YZX,
gp_Intrinsic_YXZ,
gp_Intrinsic_ZXY,
gp_Intrinsic_ZYX,
// Proper Euler angles (using two different axes, first and third the same)
gp_Extrinsic_XYX,
gp_Extrinsic_XZX,
gp_Extrinsic_YZY,
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

24
src/gp/gp_Quaternion.cxx
View File

@ -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);
case gp_Intrinsic_XZY: return Params (1, T, F, F);
case gp_Intrinsic_YZX: return Params (2, F, F, F);
case gp_Intrinsic_YXZ: return Params (2, T, F, F);
case gp_Intrinsic_ZXY: return Params (3, F, F, F);
case gp_Intrinsic_ZYX: return Params (3, T, F, F);
// Conversion of intrinsic angles is made by the same code as for extrinsic,
// using equivalence rule: intrinsic rotation is equivalent to extrinsic
// rotation by the same angles but with inverted order of elemental rotations.
// Swapping of angles (Alpha <-> Gamma) is done inside conversion procedure;
// sequence of axes is inverted by setting appropriate parameters here.
// 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 :

9
tests/bugs/fclasses/bug25574 Normal file
View File

@ -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