From 70be93d1126cb088f07da7146ddff31449c44db6 Mon Sep 17 00:00:00 2001 From: Lukas Matena Date: Fri, 23 Sep 2022 10:46:43 +0200 Subject: [PATCH] Fixed issue with Euler angles: the function to extract Euler angles did not work reliably in some of the corner cases. The bug was not present in 2.5.0 release. --- src/libslic3r/Geometry.cpp | 61 +++++++------------------- src/libslic3r/Geometry.hpp | 4 +- src/libslic3r/Model.cpp | 2 +- src/slic3r/GUI/Gizmos/GLGizmoScale.cpp | 2 +- src/slic3r/GUI/Selection.cpp | 8 ++-- tests/libslic3r/test_geometry.cpp | 24 ++++++++++ 6 files changed, 48 insertions(+), 53 deletions(-) diff --git a/src/libslic3r/Geometry.cpp b/src/libslic3r/Geometry.cpp index 621289a618..a5aaa38992 100644 --- a/src/libslic3r/Geometry.cpp +++ b/src/libslic3r/Geometry.cpp @@ -374,46 +374,17 @@ Transform3d scale_transform(const Vec3d& scale) return transform; } -Vec3d extract_euler_angles(const Eigen::Matrix& rotation_matrix) +Vec3d extract_rotation(const Eigen::Matrix& rotation_matrix) { - // reference: http://eecs.qmul.ac.uk/~gslabaugh/publications/euler.pdf - Vec3d angles1 = Vec3d::Zero(); - Vec3d angles2 = Vec3d::Zero(); - if (std::abs(std::abs(rotation_matrix(2, 0)) - 1.0) < 1e-5) { - angles1.z() = 0.0; - if (rotation_matrix(2, 0) < 0.0) { // == -1.0 - angles1.y() = 0.5 * double(PI); - angles1.x() = angles1.z() + ::atan2(rotation_matrix(0, 1), rotation_matrix(0, 2)); - } - else { // == 1.0 - angles1.y() = - 0.5 * double(PI); - angles1.x() = - angles1.y() + ::atan2(- rotation_matrix(0, 1), - rotation_matrix(0, 2)); - } - angles2 = angles1; - } - else { - angles1.y() = -::asin(rotation_matrix(2, 0)); - const double inv_cos1 = 1.0 / ::cos(angles1.y()); - angles1.x() = ::atan2(rotation_matrix(2, 1) * inv_cos1, rotation_matrix(2, 2) * inv_cos1); - angles1.z() = ::atan2(rotation_matrix(1, 0) * inv_cos1, rotation_matrix(0, 0) * inv_cos1); - - angles2.y() = double(PI) - angles1.y(); - const double inv_cos2 = 1.0 / ::cos(angles2.y()); - angles2.x() = ::atan2(rotation_matrix(2, 1) * inv_cos2, rotation_matrix(2, 2) * inv_cos2); - angles2.z() = ::atan2(rotation_matrix(1, 0) * inv_cos2, rotation_matrix(0, 0) * inv_cos2); - } - - // The following euristic is the best found up to now (in the sense that it works fine with the greatest number of edge use-cases) - // but there are other use-cases were it does not - // We need to improve it - const double min_1 = angles1.cwiseAbs().minCoeff(); - const double min_2 = angles2.cwiseAbs().minCoeff(); - const bool use_1 = (min_1 < min_2) || (is_approx(min_1, min_2) && (angles1.norm() <= angles2.norm())); - - return use_1 ? angles1 : angles2; + // The extracted "rotation" is a triplet of numbers such that Geometry::rotation_transform + // returns the original transform. Because of the chosen order of rotations, the triplet + // is not equivalent to Euler angles in the usual sense. + Vec3d angles = rotation_matrix.eulerAngles(2,1,0); + std::swap(angles(0), angles(2)); + return angles; } -Vec3d extract_euler_angles(const Transform3d& transform) +Vec3d extract_rotation(const Transform3d& transform) { // use only the non-translational part of the transform Eigen::Matrix m = transform.matrix().block(0, 0, 3, 3); @@ -421,7 +392,7 @@ Vec3d extract_euler_angles(const Transform3d& transform) m.col(0).normalize(); m.col(1).normalize(); m.col(2).normalize(); - return extract_euler_angles(m); + return extract_rotation(m); } #if ENABLE_WORLD_COORDINATE @@ -430,7 +401,7 @@ Transform3d Transformation::get_offset_matrix() const return assemble_transform(get_offset()); } -static Transform3d extract_rotation(const Transform3d& trafo) +static Transform3d extract_rotation_matrix(const Transform3d& trafo) { Matrix3d rotation; Matrix3d scale; @@ -464,12 +435,12 @@ static bool contains_skew(const Transform3d& trafo) Vec3d Transformation::get_rotation() const { - return extract_euler_angles(extract_rotation(m_matrix)); + return extract_rotation(extract_rotation_matrix(m_matrix)); } Transform3d Transformation::get_rotation_matrix() const { - return extract_rotation(m_matrix); + return extract_rotation_matrix(m_matrix); } #else bool Transformation::Flags::needs_update(bool dont_translate, bool dont_rotate, bool dont_scale, bool dont_mirror) const @@ -532,7 +503,7 @@ void Transformation::set_rotation(Axis axis, double rotation) #if ENABLE_WORLD_COORDINATE auto [curr_rotation, scale] = extract_rotation_scale(m_matrix); - Vec3d angles = extract_euler_angles(curr_rotation); + Vec3d angles = extract_rotation(curr_rotation); angles[axis] = rotation; const Vec3d offset = get_offset(); @@ -569,7 +540,7 @@ void Transformation::set_scaling_factor(const Vec3d& scaling_factor) assert(scaling_factor.x() > 0.0 && scaling_factor.y() > 0.0 && scaling_factor.z() > 0.0); const Vec3d offset = get_offset(); - m_matrix = extract_rotation(m_matrix) * scale_transform(scaling_factor); + m_matrix = extract_rotation_matrix(m_matrix) * scale_transform(scaling_factor); m_matrix.translation() = offset; #else set_scaling_factor(X, scaling_factor.x()); @@ -704,7 +675,7 @@ void Transformation::set_from_transform(const Transform3d& transform) m3x3.col(2).normalize(); // rotation - set_rotation(extract_euler_angles(m3x3)); + set_rotation(extract_rotation(m3x3)); // forces matrix recalculation matrix m_matrix = get_matrix(); @@ -833,7 +804,7 @@ Transformation Transformation::volume_to_bed_transformation(const Transformation for (int i = 0; i < 3; ++i) scale(i) = pts.col(i).dot(qs.col(i)) / pts.col(i).dot(pts.col(i)); - out.set_rotation(Geometry::extract_euler_angles(volume_rotation_trafo)); + out.set_rotation(Geometry::extract_rotation(volume_rotation_trafo)); out.set_scaling_factor(Vec3d(std::abs(scale.x()), std::abs(scale.y()), std::abs(scale.z()))); out.set_mirror(Vec3d(scale.x() > 0 ? 1. : -1, scale.y() > 0 ? 1. : -1, scale.z() > 0 ? 1. : -1)); } diff --git a/src/libslic3r/Geometry.hpp b/src/libslic3r/Geometry.hpp index d0ae84d86f..495b88c17a 100644 --- a/src/libslic3r/Geometry.hpp +++ b/src/libslic3r/Geometry.hpp @@ -375,11 +375,11 @@ Transform3d scale_transform(const Vec3d& scale); // Returns the euler angles extracted from the given rotation matrix // Warning -> The matrix should not contain any scale or shear !!! -Vec3d extract_euler_angles(const Eigen::Matrix& rotation_matrix); +Vec3d extract_rotation(const Eigen::Matrix& rotation_matrix); // Returns the euler angles extracted from the given affine transform // Warning -> The transform should not contain any shear !!! -Vec3d extract_euler_angles(const Transform3d& transform); +Vec3d extract_rotation(const Transform3d& transform); class Transformation { diff --git a/src/libslic3r/Model.cpp b/src/libslic3r/Model.cpp index 97d8ea04b2..9a2a0bb839 100644 --- a/src/libslic3r/Model.cpp +++ b/src/libslic3r/Model.cpp @@ -1917,7 +1917,7 @@ void ModelVolume::rotate(double angle, Axis axis) void ModelVolume::rotate(double angle, const Vec3d& axis) { - set_rotation(get_rotation() + Geometry::extract_euler_angles(Eigen::Quaterniond(Eigen::AngleAxisd(angle, axis)).toRotationMatrix())); + set_rotation(get_rotation() + Geometry::extract_rotation(Eigen::Quaterniond(Eigen::AngleAxisd(angle, axis)).toRotationMatrix())); } void ModelVolume::mirror(Axis axis) diff --git a/src/slic3r/GUI/Gizmos/GLGizmoScale.cpp b/src/slic3r/GUI/Gizmos/GLGizmoScale.cpp index 0f68cb7619..572e011e5e 100644 --- a/src/slic3r/GUI/Gizmos/GLGizmoScale.cpp +++ b/src/slic3r/GUI/Gizmos/GLGizmoScale.cpp @@ -285,7 +285,7 @@ void GLGizmoScale3D::on_render() #else m_bounding_box = v.bounding_box(); m_transform = v.world_matrix(); - angles = Geometry::extract_euler_angles(m_transform); + angles = Geometry::extract_rotation(m_transform); // consider rotation+mirror only components of the transform for offsets offsets_transform = Geometry::assemble_transform(Vec3d::Zero(), angles, Vec3d::Ones(), v.get_instance_mirror()); m_offsets_transform = Geometry::assemble_transform(Vec3d::Zero(), v.get_volume_rotation(), Vec3d::Ones(), v.get_volume_mirror()); diff --git a/src/slic3r/GUI/Selection.cpp b/src/slic3r/GUI/Selection.cpp index 0b2a13040a..c6ddd1fa3e 100644 --- a/src/slic3r/GUI/Selection.cpp +++ b/src/slic3r/GUI/Selection.cpp @@ -958,7 +958,7 @@ void Selection::rotate(const Vec3d& rotation, TransformationType transformation_ else { // extracts rotations from the composed transformation const Vec3d new_rotation = transformation_type.world() ? - Geometry::extract_euler_angles(Geometry::assemble_transform(Vec3d::Zero(), rotation) * m_cache.volumes_data[i].get_instance_rotation_matrix()) : + Geometry::extract_rotation(Geometry::assemble_transform(Vec3d::Zero(), rotation) * m_cache.volumes_data[i].get_instance_rotation_matrix()) : transformation_type.absolute() ? rotation : rotation + m_cache.volumes_data[i].get_instance_rotation(); if (rot_axis_max == 2 && transformation_type.joint()) { // Only allow rotation of multiple instances as a single rigid body when rotating around the Z axis. @@ -979,7 +979,7 @@ void Selection::rotate(const Vec3d& rotation, TransformationType transformation_ v.set_volume_rotation(m_cache.volumes_data[i].get_volume_rotation() + rotation); else { const Transform3d m = Geometry::assemble_transform(Vec3d::Zero(), rotation); - const Vec3d new_rotation = Geometry::extract_euler_angles(m * m_cache.volumes_data[i].get_volume_rotation_matrix()); + const Vec3d new_rotation = Geometry::extract_rotation(m * m_cache.volumes_data[i].get_volume_rotation_matrix()); v.set_volume_rotation(new_rotation); } } @@ -989,7 +989,7 @@ void Selection::rotate(const Vec3d& rotation, TransformationType transformation_ else if (m_mode == Volume) { // extracts rotations from the composed transformation const Transform3d m = Geometry::assemble_transform(Vec3d::Zero(), rotation); - const Vec3d new_rotation = Geometry::extract_euler_angles(m * m_cache.volumes_data[i].get_volume_rotation_matrix()); + const Vec3d new_rotation = Geometry::extract_rotation(m * m_cache.volumes_data[i].get_volume_rotation_matrix()); if (transformation_type.joint()) { const Vec3d local_pivot = m_cache.volumes_data[i].get_instance_full_matrix().inverse() * m_cache.dragging_center; const Vec3d offset = m * (m_cache.volumes_data[i].get_volume_position() - local_pivot); @@ -1050,7 +1050,7 @@ void Selection::flattening_rotate(const Vec3d& normal) voldata.get_instance_scaling_factor().cwiseInverse(), voldata.get_instance_mirror()) * normal).normalized(); // Additional rotation to align tnormal with the down vector in the world coordinate space. auto extra_rotation = Eigen::Quaterniond().setFromTwoVectors(tnormal, -Vec3d::UnitZ()); - v.set_instance_rotation(Geometry::extract_euler_angles(extra_rotation.toRotationMatrix() * m_cache.volumes_data[i].get_instance_rotation_matrix())); + v.set_instance_rotation(Geometry::extract_rotation(extra_rotation.toRotationMatrix() * m_cache.volumes_data[i].get_instance_rotation_matrix())); #endif // ENABLE_WORLD_COORDINATE } diff --git a/tests/libslic3r/test_geometry.cpp b/tests/libslic3r/test_geometry.cpp index 41ef69aaaf..239edd4f74 100644 --- a/tests/libslic3r/test_geometry.cpp +++ b/tests/libslic3r/test_geometry.cpp @@ -764,3 +764,27 @@ TEST_CASE("Convex polygon intersection test prusa polygons", "[Geometry][Rotcali REQUIRE(res == ref); } } + + +TEST_CASE("Euler angles roundtrip", "[Geometry]") { + std::vector euler_angles_vec = {{M_PI/2., -M_PI, 0.}, + {M_PI, -M_PI, 0.}, + {M_PI, -M_PI, 2*M_PI}, + {0., 0., M_PI}, + {M_PI, M_PI/2., 0.}, + {0.2, 0.3, -0.5}}; + + // Also include all combinations of zero and +-pi/2: + for (double x : {0., M_PI/2., -M_PI/2.}) + for (double y : {0., M_PI/2., -M_PI/2.}) + for (double z : {0., M_PI/2., -M_PI/2.}) + euler_angles_vec.emplace_back(x, y, z); + + for (Vec3d& euler_angles : euler_angles_vec) { + Transform3d trafo1 = Geometry::rotation_transform(euler_angles); + euler_angles = Geometry::extract_rotation(trafo1); + Transform3d trafo2 = Geometry::rotation_transform(euler_angles); + + REQUIRE(trafo1.isApprox(trafo2)); + } +}