kindred/create
1
1
Fork 0
Code Issues 53 Pull Requests Actions Packages Projects 9 Releases 2 Wiki Activity

[Core] Rotation : improve gimbal lock handling

Browse Source 
Tolerance copied from OCC "gp_Quaternion.cxx"
This commit is contained in:
0penBrain
2021-09-21 19:10:46 +02:00
parent 68fbf89e38
commit e1dc69f56d
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/Base/Rotation.cpp
View File

@@ -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) <= 16 * DBL_EPSILON) { // Tolerance copied from OCC "gp_Quaternion.cxx"
// 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) <= 16 * DBL_EPSILON) { // Tolerance copied from OCC "gp_Quaternion.cxx"
// south pole
y = 0.0;
p = -D_PI/2.0;