Added helper functions to extract euler angles from 3d transforms
This commit is contained in:
Enrico Turri
parent
d843f1a76f
commit
9bb93cc4f4
4 changed files with 61 additions and 46 deletions
|
|
@ -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<double, 3, 3, Eigen::DontAlign> 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<double, 3, 3, Eigen::DontAlign> 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
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1183,4 +1183,38 @@ Transform3d assemble_transform(const Vec3d& translation, const Vec3d& rotation,
|
|||
return transform;
|
||||
}
|
||||
|
||||
Vec3d extract_euler_angles(const Eigen::Matrix<double, 3, 3, Eigen::DontAlign>& 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<double, 3, 3, Eigen::DontAlign> 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);
|
||||
}
|
||||
|
||||
} }
|
||||
|
|
|
|||
|
|
@ -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<double, 3, 3, Eigen::DontAlign>& 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
|
||||
|
|
|
|||
|
|
@ -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<double, 3, 3, Eigen::DontAlign> 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)));
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in a new issue