diff --git a/src/libslic3r/Geometry.cpp b/src/libslic3r/Geometry.cpp index fc5b158c7..8faa49ea0 100644 --- a/src/libslic3r/Geometry.cpp +++ b/src/libslic3r/Geometry.cpp @@ -1181,22 +1181,34 @@ Transform3d assemble_transform(const Vec3d& translation, const Vec3d& rotation, Vec3d extract_euler_angles(const Eigen::Matrix& rotation_matrix) { + auto y_only = [](const Eigen::Matrix& matrix) -> bool { + return (matrix(0, 1) == 0.0) && (matrix(1, 0) == 0.0) && (matrix(1, 1) == 1.0) && (matrix(1, 2) == 0.0) && (matrix(2, 1) == 0.0); + }; + // see: https://www.learnopencv.com/rotation-matrix-to-euler-angles/ - double sy = ::sqrt(sqr(rotation_matrix(0, 0)) + sqr(rotation_matrix(1, 0))); + double cy_abs = ::sqrt(sqr(rotation_matrix(0, 0)) + sqr(rotation_matrix(1, 0))); Vec3d angles = Vec3d::Zero(); - if (sy >= 1e-6) + if (cy_abs >= 1e-6) { angles(0) = ::atan2(rotation_matrix(2, 1), rotation_matrix(2, 2)); - angles(1) = ::atan2(-rotation_matrix(2, 0), sy); + angles(1) = ::atan2(-rotation_matrix(2, 0), cy_abs); angles(2) = ::atan2(rotation_matrix(1, 0), rotation_matrix(0, 0)); + + // this is an hack to try to avoid this function to return "strange" values due to gimbal lock + if (y_only(rotation_matrix) && (angles(0) == (double)PI) && (angles(2) == (double)PI)) + { + angles(0) = 0.0; + angles(1) = ::atan2(rotation_matrix(2, 0), cy_abs) - (double)PI; + angles(2) = 0.0; + } } else { angles(0) = 0.0; - angles(1) = ::atan2(-rotation_matrix(2, 0), sy); - angles(2) = (angles(1) >-0.0) ? ::atan2(rotation_matrix(1, 2), rotation_matrix(1, 1)) : ::atan2(-rotation_matrix(1, 2), rotation_matrix(1, 1)); + angles(1) = ::atan2(-rotation_matrix(2, 0), cy_abs); + angles(2) = (angles(1) >= 0.0) ? ::atan2(rotation_matrix(1, 2), rotation_matrix(1, 1)) : ::atan2(-rotation_matrix(1, 2), rotation_matrix(1, 1)); } return angles;