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

handle gimbal lock when converting between quaternion and Euler angles (issue #0004062)

Browse Source
This commit is contained in:
wmayer
2019-07-22 16:31:42 +02:00
parent 97054ee4be
commit d1b47718cf
1 changed files with 18 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
src/Base/Rotation.cpp
View File

@@ -651,9 +651,24 @@ void Rotation::getYawPitchRoll(double& y, double& p, double& r) const
double q23 = quat[2]*quat[3];
double qd2 = 2.0*(q13-q02);
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));
// handle gimbal lock
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) {
// south pole
y = 0.0;
p = -D_PI/2.0;
r = -2.0 * atan2(quat[0],quat[3]);
}
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));
}
// convert to degree
y = (y/D_PI)*180;