Code cleanup

This commit is contained in:
Enrico Turri 2019-01-14 08:56:01 +01:00
parent 1d81469d97
commit 4affd6ace7
2 changed files with 19 additions and 25 deletions

View File

@ -1181,7 +1181,6 @@ Transform3d assemble_transform(const Vec3d& translation, const Vec3d& rotation,
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);
@ -1218,24 +1217,23 @@ Vec3d extract_euler_angles(const Eigen::Matrix<double, 3, 3, Eigen::DontAlign>&
}
}
// 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;
// }
// }
}
// // 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 {
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);
};
@ -1265,9 +1263,7 @@ Vec3d extract_euler_angles(const Eigen::Matrix<double, 3, 3, Eigen::DontAlign>&
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));
}
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
#endif // ENABLE_NEW_EULER_ANGLES
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
return angles;
}

View File

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