From 9bb93cc4f4675ab0e9b0a5d77d3d19f2fc086848 Mon Sep 17 00:00:00 2001 From: Enrico Turri Date: Fri, 12 Oct 2018 12:19:57 +0200 Subject: [PATCH] Added helper functions to extract euler angles from 3d transforms --- src/libslic3r/Format/3mf.cpp | 42 ++++++++++++++--------------------- src/libslic3r/Geometry.cpp | 34 ++++++++++++++++++++++++++++ src/libslic3r/Geometry.hpp | 8 +++++++ src/slic3r/GUI/GLCanvas3D.cpp | 23 ++----------------- 4 files changed, 61 insertions(+), 46 deletions(-) diff --git a/src/libslic3r/Format/3mf.cpp b/src/libslic3r/Format/3mf.cpp index 90817fadc..4d749eb56 100644 --- a/src/libslic3r/Format/3mf.cpp +++ b/src/libslic3r/Format/3mf.cpp @@ -2,6 +2,7 @@ #include "../Model.hpp" #include "../Utils.hpp" #include "../GCode.hpp" +#include "../Geometry.hpp" #include "3mf.hpp" @@ -1253,7 +1254,7 @@ namespace Slic3r { // translation #if ENABLE_MODELINSTANCE_3D_FULL_TRANSFORM - Vec3d offset(transform(0, 3), transform(1, 3), transform(2, 3)); + Vec3d offset = transform.matrix().block(0, 3, 3, 1); #else double offset_x = transform(0, 3); double offset_y = transform(1, 3); @@ -1261,50 +1262,41 @@ namespace Slic3r { #endif // ENABLE_MODELINSTANCE_3D_FULL_TRANSFORM // scale - double sx = ::sqrt(sqr(transform(0, 0)) + sqr(transform(1, 0)) + sqr(transform(2, 0))); - double sy = ::sqrt(sqr(transform(0, 1)) + sqr(transform(1, 1)) + sqr(transform(2, 1))); - double sz = ::sqrt(sqr(transform(0, 2)) + sqr(transform(1, 2)) + sqr(transform(2, 2))); + Eigen::Matrix m3x3 = transform.matrix().block(0, 0, 3, 3); + Vec3d scale(m3x3.col(0).norm(), m3x3.col(1).norm(), m3x3.col(2).norm()); // invalid scale value, return - if ((sx == 0.0) || (sy == 0.0) || (sz == 0.0)) + if ((scale(0) == 0.0) || (scale(1) == 0.0) || (scale(2) == 0.0)) return; #if !ENABLE_MODELINSTANCE_3D_FULL_TRANSFORM // non-uniform scale value, return - if ((std::abs(sx - sy) > 0.00001) || (std::abs(sx - sz) > 0.00001)) + if ((std::abs(scale(0) - scale(1)) > 0.00001) || (std::abs(scale(0) - scale(2)) > 0.00001)) return; #endif // !ENABLE_MODELINSTANCE_3D_FULL_TRANSFORM - double inv_sx = 1.0 / sx; - double inv_sy = 1.0 / sy; - double inv_sz = 1.0 / sz; - - Eigen::Matrix m3x3; - m3x3 << transform(0, 0) * inv_sx, transform(0, 1) * inv_sy, transform(0, 2) * inv_sz, - transform(1, 0) * inv_sx, transform(1, 1) * inv_sy, transform(1, 2) * inv_sz, - transform(2, 0) * inv_sx, transform(2, 1) * inv_sy, transform(2, 2) * inv_sz; + // remove scale + m3x3.col(0).normalize(); + m3x3.col(1).normalize(); + m3x3.col(2).normalize(); #if ENABLE_MODELINSTANCE_3D_FULL_TRANSFORM - Vec3d angles = m3x3.eulerAngles(2, 1, 0); - Vec3d rotation(angles(2), angles(1), angles(0)); + Vec3d rotation = Slic3r::Geometry::extract_euler_angles(m3x3); instance.set_offset(offset); - instance.set_scaling_factor(Vec3d(sx, sy, sz)); + instance.set_scaling_factor(scale); instance.set_rotation(rotation); #else - Eigen::AngleAxisd rotation; - rotation.fromRotationMatrix(m3x3); + Vec3d rotation = Slic3r::Geometry::extract_euler_angles(m3x3); - // invalid rotation axis, we currently handle only rotations around Z axis - if ((rotation.angle() != 0.0) && (rotation.axis() != Vec3d::UnitZ()) && (rotation.axis() != -Vec3d::UnitZ())) + // invalid rotation, we currently handle only rotations around Z axis + if ((rotation(0) != 0.0) || (rotation(1) != 0.0)) return; - double angle_z = (rotation.axis() == Vec3d::UnitZ()) ? rotation.angle() : -rotation.angle(); - instance.offset(0) = offset_x; instance.offset(1) = offset_y; - instance.scaling_factor = sx; - instance.rotation = angle_z; + instance.scaling_factor = scale(0); + instance.rotation = rotation(2); #endif // ENABLE_MODELINSTANCE_3D_FULL_TRANSFORM } diff --git a/src/libslic3r/Geometry.cpp b/src/libslic3r/Geometry.cpp index 5d7b93679..c45f08791 100644 --- a/src/libslic3r/Geometry.cpp +++ b/src/libslic3r/Geometry.cpp @@ -1183,4 +1183,38 @@ Transform3d assemble_transform(const Vec3d& translation, const Vec3d& rotation, return transform; } +Vec3d extract_euler_angles(const Eigen::Matrix& rotation_matrix) +{ + // see: https://www.learnopencv.com/rotation-matrix-to-euler-angles/ + double sy = ::sqrt(sqr(rotation_matrix(0, 0)) + sqr(rotation_matrix(1, 0))); + + Vec3d angles = Vec3d::Zero(); + + if (sy >= 1e-6) + { + angles(0) = ::atan2(rotation_matrix(2, 1), rotation_matrix(2, 2)); + angles(1) = ::atan2(-rotation_matrix(2, 0), sy); + angles(2) = ::atan2(rotation_matrix(1, 0), rotation_matrix(0, 0)); + } + else + { + angles(0) = ::atan2(-rotation_matrix(1, 2), rotation_matrix(1, 1)); + angles(1) = ::atan2(-rotation_matrix(2, 0), sy); + angles(2) = 0.0; + } + + return angles; +} + +Vec3d extract_euler_angles(const Transform3d& transform) +{ + // use only the non-translational part of the transform + Eigen::Matrix m = transform.matrix().block(0, 0, 3, 3); + // remove scale + m.col(0).normalize(); + m.col(1).normalize(); + m.col(2).normalize(); + return extract_euler_angles(m); +} + } } diff --git a/src/libslic3r/Geometry.hpp b/src/libslic3r/Geometry.hpp index 57b9cad02..35f59bdfd 100644 --- a/src/libslic3r/Geometry.hpp +++ b/src/libslic3r/Geometry.hpp @@ -172,6 +172,14 @@ void assemble_transform(Transform3d& transform, const Vec3d& translation = Vec3d // 4) rotate Z // 5) translate Transform3d assemble_transform(const Vec3d& translation = Vec3d::Zero(), const Vec3d& rotation = Vec3d::Zero(), const Vec3d& scale = Vec3d::Ones()); + +// 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); + +// 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); } } #endif diff --git a/src/slic3r/GUI/GLCanvas3D.cpp b/src/slic3r/GUI/GLCanvas3D.cpp index 092548870..2df47519c 100644 --- a/src/slic3r/GUI/GLCanvas3D.cpp +++ b/src/slic3r/GUI/GLCanvas3D.cpp @@ -1413,33 +1413,14 @@ void GLCanvas3D::Selection::rotate(const Vec3d& rotation) for (unsigned int i : m_list) { - Vec3d radius = m * (m_cache.volumes_data[i].get_position() - m_cache.dragging_center); - (*m_volumes)[i]->set_offset(m_cache.dragging_center + radius); - if (single_full_instance) (*m_volumes)[i]->set_rotation(rotation); else { Eigen::Matrix new_rotation_matrix = (m * m_cache.volumes_data[i].get_rotation_matrix()).matrix().block(0, 0, 3, 3); - // extracts euler angles from the composed transformation - // not using Eigen eulerAngles() method because it returns weird results - // see: https://www.learnopencv.com/rotation-matrix-to-euler-angles/ - double sy = ::sqrt(sqr(new_rotation_matrix(0, 0)) + sqr(new_rotation_matrix(1, 0))); - - Vec3d angles = Vec3d::Zero(); - if (sy >= 1e-6) - { - angles(0) = ::atan2(new_rotation_matrix(2, 1), new_rotation_matrix(2, 2)); - angles(1) = ::atan2(-new_rotation_matrix(2, 0), sy); - angles(2) = ::atan2(new_rotation_matrix(1, 0), new_rotation_matrix(0, 0)); - } - else - { - angles(0) = ::atan2(-new_rotation_matrix(1, 2), new_rotation_matrix(1, 1)); - angles(1) = ::atan2(-new_rotation_matrix(2, 0), sy); - angles(2) = 0.0; - } + Vec3d angles = Geometry::extract_euler_angles(new_rotation_matrix); + (*m_volumes)[i]->set_offset(m_cache.dragging_center + m * (m_cache.volumes_data[i].get_position() - m_cache.dragging_center)); (*m_volumes)[i]->set_rotation(Vec3d(angles(0), angles(1), angles(2))); } }