Assembly: Port BBS' assembly gizmo

Co-authored-by:  zhou.xu <zhou.xu@bambulab.com>
This commit is contained in:
Noisyfox 2024-11-06 23:09:02 +08:00
parent e3b1e30387
commit 3d45414b71
17 changed files with 715 additions and 104 deletions

View file

@ -717,53 +717,6 @@ Transformation Transformation::volume_to_bed_transformation(const Transformation
return out;
}
TransformationSVD::TransformationSVD(const Transform3d& trafo)
{
const auto &m0 = trafo.matrix().block<3, 3>(0, 0);
mirror = m0.determinant() < 0.0;
Matrix3d m;
if (mirror)
m = m0 * Eigen::DiagonalMatrix<double, 3, 3>(-1.0, 1.0, 1.0);
else
m = m0;
const Eigen::JacobiSVD<Matrix3d> svd(m, Eigen::ComputeFullU | Eigen::ComputeFullV);
u = svd.matrixU();
v = svd.matrixV();
s = svd.singularValues().asDiagonal();
scale = !s.isApprox(Matrix3d::Identity());
anisotropic_scale = ! is_approx(s(0, 0), s(1, 1)) || ! is_approx(s(1, 1), s(2, 2));
rotation = !v.isApprox(u);
if (anisotropic_scale) {
rotation_90_degrees = true;
for (int i = 0; i < 3; ++i) {
const Vec3d row = v.row(i).cwiseAbs();
const size_t num_zeros = is_approx(row[0], 0.) + is_approx(row[1], 0.) + is_approx(row[2], 0.);
const size_t num_ones = is_approx(row[0], 1.) + is_approx(row[1], 1.) + is_approx(row[2], 1.);
if (num_zeros != 2 || num_ones != 1) {
rotation_90_degrees = false;
break;
}
}
// Detect skew by brute force: check if the axes are still orthogonal after transformation
const Matrix3d trafo_linear = trafo.linear();
const std::array<Vec3d, 3> axes = { Vec3d::UnitX(), Vec3d::UnitY(), Vec3d::UnitZ() };
std::array<Vec3d, 3> transformed_axes;
for (int i = 0; i < 3; ++i) {
transformed_axes[i] = trafo_linear * axes[i];
}
skew = std::abs(transformed_axes[0].dot(transformed_axes[1])) > EPSILON ||
std::abs(transformed_axes[1].dot(transformed_axes[2])) > EPSILON ||
std::abs(transformed_axes[2].dot(transformed_axes[0])) > EPSILON;
// This following old code does not work under all conditions. The v matrix can become non diagonal (see SPE-1492)
// skew = ! rotation_90_degrees;
} else
skew = false;
}
// For parsing a transformation matrix from 3MF / AMF.
Transform3d transform3d_from_string(const std::string& transform_str)
{
@ -812,4 +765,78 @@ double rotation_diff_z(const Vec3d &rot_xyz_from, const Vec3d &rot_xyz_to)
return (axis.z() < 0) ? -angle : angle;
}
TransformationSVD::TransformationSVD(const Transform3d& trafo)
{
const auto &m0 = trafo.matrix().block<3, 3>(0, 0);
mirror = m0.determinant() < 0.0;
Matrix3d m;
if (mirror)
m = m0 * Eigen::DiagonalMatrix<double, 3, 3>(-1.0, 1.0, 1.0);
else
m = m0;
const Eigen::JacobiSVD<Matrix3d> svd(m, Eigen::ComputeFullU | Eigen::ComputeFullV);
u = svd.matrixU();
v = svd.matrixV();
s = svd.singularValues().asDiagonal();
scale = !s.isApprox(Matrix3d::Identity());
anisotropic_scale = ! is_approx(s(0, 0), s(1, 1)) || ! is_approx(s(1, 1), s(2, 2));
rotation = !v.isApprox(u);
if (anisotropic_scale) {
rotation_90_degrees = true;
for (int i = 0; i < 3; ++i) {
const Vec3d row = v.row(i).cwiseAbs();
const size_t num_zeros = is_approx(row[0], 0.) + is_approx(row[1], 0.) + is_approx(row[2], 0.);
const size_t num_ones = is_approx(row[0], 1.) + is_approx(row[1], 1.) + is_approx(row[2], 1.);
if (num_zeros != 2 || num_ones != 1) {
rotation_90_degrees = false;
break;
}
}
// Detect skew by brute force: check if the axes are still orthogonal after transformation
const Matrix3d trafo_linear = trafo.linear();
const std::array<Vec3d, 3> axes = { Vec3d::UnitX(), Vec3d::UnitY(), Vec3d::UnitZ() };
std::array<Vec3d, 3> transformed_axes;
for (int i = 0; i < 3; ++i) {
transformed_axes[i] = trafo_linear * axes[i];
}
skew = std::abs(transformed_axes[0].dot(transformed_axes[1])) > EPSILON ||
std::abs(transformed_axes[1].dot(transformed_axes[2])) > EPSILON ||
std::abs(transformed_axes[2].dot(transformed_axes[0])) > EPSILON;
// This following old code does not work under all conditions. The v matrix can become non diagonal (see SPE-1492)
// skew = ! rotation_90_degrees;
} else
skew = false;
}
Transformation mat_around_a_point_rotate(const Transformation &InMat, const Vec3d &pt, const Vec3d &axis, float rotate_theta_radian)
{
auto xyz = InMat.get_offset();
Transformation left;
left.set_offset(-xyz); // at world origin
auto curMat = left * InMat;
auto qua = Eigen::Quaterniond(Eigen::AngleAxisd(rotate_theta_radian, axis));
qua.normalize();
Transform3d cur_matrix;
Transformation rotateMat4;
rotateMat4.set_matrix(cur_matrix.fromPositionOrientationScale(Vec3d(0., 0., 0.), qua, Vec3d(1., 1., 1.)));
curMat = rotateMat4 * curMat; // along_fix_axis
// rotate mat4 along fix pt
Transformation temp_world;
auto qua_world = Eigen::Quaterniond(Eigen::AngleAxisd(0, axis));
qua_world.normalize();
Transform3d cur_matrix_world;
temp_world.set_matrix(cur_matrix_world.fromPositionOrientationScale(pt, qua_world, Vec3d(1., 1., 1.)));
auto temp_xyz = temp_world.get_matrix().inverse() * xyz;
auto new_pos = temp_world.get_matrix() * (rotateMat4.get_matrix() * temp_xyz);
curMat.set_offset(new_pos);
return curMat;
}
}} // namespace Slic3r::Geometry