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

New algorithm to extract euler angles from rotation matrix

Browse source
This commit is contained in:
Enrico Turri 2019-01-11 09:23:35 +01:00
parent 2f6bf3e3f2
commit a0245c69a6
2 changed files with 67 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

58
src/libslic3r/Geometry.cpp
View file

@ -1181,6 +1181,61 @@ Transform3d assemble_transform(const Vec3d& translation, const Vec3d& rotation,
Vec3d extract_euler_angles(const Eigen::Matrix<double, 3, 3, Eigen::DontAlign>& rotation_matrix) Vec3d extract_euler_angles(const Eigen::Matrix<double, 3, 3, Eigen::DontAlign>& rotation_matrix)
{ {
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
#if ENABLE_NEW_EULER_ANGLES
bool x_only = (rotation_matrix(0, 0) == 1.0) && (rotation_matrix(0, 1) == 0.0) && (rotation_matrix(0, 2) == 0.0) && (rotation_matrix(1, 0) == 0.0) && (rotation_matrix(2, 0) == 0.0);
bool y_only = (rotation_matrix(0, 1) == 0.0) && (rotation_matrix(1, 0) == 0.0) && (rotation_matrix(1, 1) == 1.0) && (rotation_matrix(1, 2) == 0.0) && (rotation_matrix(2, 1) == 0.0);
bool z_only = (rotation_matrix(0, 2) == 0.0) && (rotation_matrix(1, 2) == 0.0) && (rotation_matrix(2, 0) == 0.0) && (rotation_matrix(2, 1) == 0.0) && (rotation_matrix(2, 2) == 1.0);
// bool xy_only = (rotation_matrix(0, 1) == 0.0); // Rx * Ry
bool yx_only = (rotation_matrix(1, 0) == 0.0); // Ry * Rx
// bool xz_only = (rotation_matrix(0, 2) == 0.0); // Rx * Rz
// bool zx_only = (rotation_matrix(2, 0) == 0.0); // Rz * Rx
// bool yz_only = (rotation_matrix(1, 2) == 0.0); // Ry * Rz
// bool zy_only = (rotation_matrix(2, 1) == 0.0); // Rz * Ry
Vec3d angles = Vec3d::Zero();
if (x_only || y_only || z_only)
{
angles = rotation_matrix.eulerAngles(0, 1, 2);
if (x_only && (std::abs(angles(1)) == (double)PI) && (std::abs(angles(2)) == (double)PI))
{
angles(0) -= (double)PI;
angles(1) = 0.0;
angles(2) = 0.0;
}
}
else
{
double cy_abs = ::sqrt(sqr(rotation_matrix(0, 0)) + sqr(rotation_matrix(1, 0)));
angles(0) = ::atan2(rotation_matrix(2, 1), rotation_matrix(2, 2));
angles(1) = ::atan2(-rotation_matrix(2, 0), cy_abs);
angles(2) = ::atan2(rotation_matrix(1, 0), rotation_matrix(0, 0));
if (yx_only && (angles(2) == (double)PI))
{
angles(0) -= (double)PI;
angles(1) = (double)PI - angles(1);
angles(2) = 0.0;
}
}
// debug check
Geometry::Transformation t;
t.set_rotation(angles);
if (!t.get_matrix().matrix().block(0, 0, 3, 3).isApprox(rotation_matrix))
{
std::cout << "something went wrong in extracting euler angles from matrix" << std::endl;
// Eigen::Matrix<double, 3, 3, Eigen::DontAlign> m = t.get_matrix().matrix().block(0, 0, 3, 3);
// for (int r = 0; r < 3; ++r)
// {
// for (int c = 0; c < 3; ++c)
// {
// std::cout << r << ", " << c << ": " << m(r, c) << " - " << rotation_matrix(r, c) << std::endl;
// }
// }
}
#else
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
auto y_only = [](const Eigen::Matrix<double, 3, 3, Eigen::DontAlign>& matrix) -> bool { 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); 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);
}; };
@ -1210,6 +1265,9 @@ Vec3d extract_euler_angles(const Eigen::Matrix<double, 3, 3, Eigen::DontAlign>&
angles(1) = ::atan2(-rotation_matrix(2, 0), cy_abs); 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)); angles(2) = (angles(1) >= 0.0) ? ::atan2(rotation_matrix(1, 2), rotation_matrix(1, 1)) : ::atan2(-rotation_matrix(1, 2), rotation_matrix(1, 1));
} }
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
#endif // ENABLE_NEW_EULER_ANGLES
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
return angles; return angles;
} }

9
src/libslic3r/Technologies.hpp
View file

@ -49,3 +49,12 @@
#endif // _technologies_h_ #endif // _technologies_h_
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
//====================
// 1.42.0.alpha3 techs
//====================
#define ENABLE_1_42_0_ALPHA3 1
// Changed algorithm to extract euler angles from rotation matrix
#define ENABLE_NEW_EULER_ANGLES (1 && ENABLE_1_42_0_ALPHA3)
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@