Fix of Place on face:

1) Decreased "gimbal lock" epsilon 10x
2) Got rid of unnecessary back and forth conversions which led to numerical inacurracies

This should fix issues #5172, #5011 and #5398
This commit is contained in:
Vojtech Bubnik authored and Lukas Matena committed 2020-12-09 06:20:41 +01:00
1 parent fe4f1b15c6
commit 2d6b694261
2 files changed
+9 -36

No files matched your search

+1 -1
View File
@@ -1223,7 +1223,7 @@ Vec3d extract_euler_angles(const Eigen::Matrix<double, 3, 3, Eigen::DontAlign>&
// reference: http://www.gregslabaugh.net/publications/euler.pdf
Vec3d angles1 = Vec3d::Zero();
Vec3d angles2 = Vec3d::Zero();
if (is_approx(std::abs(rotation_matrix(2, 0)), 1.0))
if (std::abs(std::abs(rotation_matrix(2, 0)) - 1.0) < 1e-5)
{
angles1(2) = 0.0;
if (rotation_matrix(2, 0) < 0.0) // == -1.0