3DPrinting/PrusaSlicer-NonPlainar
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Modified extract_euler_angles to cope with gimball lock

Browse Source
This commit is contained in:
Enrico Turri 2019-01-09 11:48:23 +01:00
parent d7610f5e80
commit 5d01441450
1 changed files with 17 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
src/libslic3r/Geometry.cpp
View File

@ -1181,22 +1181,34 @@ Transform3d assemble_transform(const Vec3d& translation, const Vec3d& rotation,
Vec3d extract_euler_angles(const Eigen::Matrix<double, 3, 3, Eigen::DontAlign>& rotation_matrix)
{
auto y_only = [](const Eigen::Matrix<double, 3, 3, Eigen::DontAlign>& 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;