feat(v4.3): mass properties, clearance, assembly mass props
CI / Build & Test (push) Failing after 29s
CI / Release Build (push) Failing after 25s
Build & Test / build-and-test (push) Has been cancelled
Build & Test / python-bindings (push) Has been cancelled

- MassProperties struct: volume, centroid, inertia tensor, gyration radii
- mass_properties(): tetrahedral decomposition for inertia tensor
- assembly_mass_properties(): recursive assembly traversal with parallel axis
- clearance(): AABB-accelerated gap analysis
- 11 new tests, 21/21 passing
This commit is contained in:
茂之钳
2026-07-25 07:12:18 +00:00
parent 1b24bff3b1
commit 3d435a4801
3 changed files with 276 additions and 20 deletions
+138
View File
@@ -195,4 +195,142 @@ Point3D centroid(const BrepModel& body) {
return sum / total_area;
}
// ── mass_properties ─────────────────────────────────────────
MassProperties mass_properties(const BrepModel& body) {
auto mesh = body.to_mesh(0.05);
MassProperties mp;
// Tetrahedral decomposition: each triangle + origin
double total_vol = 0.0;
Point3D weighted_centroid(0, 0, 0);
Eigen::Matrix3d I_ref = Eigen::Matrix3d::Zero();
auto sq = [](const Point3D& a, const Point3D& b) -> double {
return a.x()*b.x() + a.y()*b.y() + a.z()*b.z();
};
for (size_t fi = 0; fi < mesh.num_faces(); ++fi) {
auto fv = mesh.face_vertices(static_cast<int>(fi));
if (fv.size() < 3) continue;
Point3D v0 = mesh.vertex(fv[0]);
Point3D v1 = mesh.vertex(fv[1]);
Point3D v2 = mesh.vertex(fv[2]);
double tet_vol = std::abs((v0.cross(v1)).dot(v2)) / 6.0;
if (tet_vol < 1e-15) continue;
Point3D tet_centroid = (v0 + v1 + v2) * (1.0 / 3.0);
weighted_centroid += tet_centroid * tet_vol;
total_vol += tet_vol;
// Inertia of tet about origin: formula from Tonon (2004)
double diag_sum = v0.squaredNorm() + v1.squaredNorm() + v2.squaredNorm();
double diag = tet_vol * diag_sum / 10.0;
I_ref(0,0) += diag;
I_ref(1,1) += diag;
I_ref(2,2) += diag;
double off_sum = sq(v0,v1) + sq(v1,v2) + sq(v2,v0);
double off = -tet_vol * off_sum / 20.0;
I_ref(0,1) += off; I_ref(1,0) += off;
I_ref(0,2) += off; I_ref(2,0) += off;
I_ref(1,2) += off; I_ref(2,1) += off;
}
if (total_vol < 1e-15) return mp;
mp.volume = total_vol;
mp.centroid = weighted_centroid / total_vol;
// Parallel axis: I_cm = I_ref - m*[(c·c)I - c⊗c]
double m = total_vol;
double cx = mp.centroid.x(), cy = mp.centroid.y(), cz = mp.centroid.z();
Eigen::Matrix3d shift;
shift(0,0) = m * (cy*cy + cz*cz);
shift(1,1) = m * (cx*cx + cz*cz);
shift(2,2) = m * (cx*cx + cy*cy);
shift(0,1) = shift(1,0) = -m * cx * cy;
shift(0,2) = shift(2,0) = -m * cx * cz;
shift(1,2) = shift(2,1) = -m * cy * cz;
mp.inertia = I_ref - shift;
auto gyrate = [&](int i) -> double {
double val = mp.inertia(i,i) / m;
return val > 0 ? std::sqrt(val) : 0.0;
};
mp.gyration_radii = core::Vector3D(gyrate(0), gyrate(1), gyrate(2));
return mp;
}
// ── assembly_mass_properties ────────────────────────────────
MassProperties assembly_mass_properties(const AssemblyNode& node,
const core::Transform3D& parent_tf) {
MassProperties result;
core::Transform3D world_tf = parent_tf * node.local_transform;
if (node.is_part() && node.model.has_value()) {
auto mp = mass_properties(*node.model);
result.volume = mp.volume;
result.centroid = world_tf * mp.centroid;
result.inertia = world_tf.linear() * mp.inertia * world_tf.linear().transpose();
if (result.volume > 1e-15) {
for (int i = 0; i < 3; ++i)
result.gyration_radii(i) = std::sqrt(std::max(0.0, result.inertia(i,i) / result.volume));
}
}
for (const auto& child : node.children) {
auto child_mp = assembly_mass_properties(*child, world_tf);
if (child_mp.volume < 1e-15) continue;
double total_m = result.volume + child_mp.volume;
result.centroid = (result.centroid * result.volume +
child_mp.centroid * child_mp.volume) / total_m;
auto shift = [](const Eigen::Matrix3d& I, double m,
const core::Point3D& old_cm,
const core::Point3D& new_cm) -> Eigen::Matrix3d {
core::Vector3D d = old_cm - new_cm;
Eigen::Matrix3d s;
s(0,0)=m*(d.y()*d.y()+d.z()*d.z());
s(1,1)=m*(d.x()*d.x()+d.z()*d.z());
s(2,2)=m*(d.x()*d.x()+d.y()*d.y());
s(0,1)=s(1,0)=-m*d.x()*d.y();
s(0,2)=s(2,0)=-m*d.x()*d.z();
s(1,2)=s(2,1)=-m*d.y()*d.z();
return I + s;
};
result.inertia = shift(result.inertia, result.volume, result.centroid, result.centroid)
+ shift(child_mp.inertia, child_mp.volume, child_mp.centroid, result.centroid);
result.volume = total_m;
if (result.volume > 1e-15) {
for (int i = 0; i < 3; ++i)
result.gyration_radii(i) = std::sqrt(std::max(0.0, result.inertia(i,i) / result.volume));
}
}
return result;
}
// ── clearance ──────────────────────────────────────────────
double clearance(const BrepModel& a, const BrepModel& b) {
auto bb_a = a.bounds();
auto bb_b = b.bounds();
if (!bb_a.intersects(bb_b)) {
double dx = 0, dy = 0, dz = 0;
if (bb_a.max().x() < bb_b.min().x()) dx = bb_b.min().x() - bb_a.max().x();
else if (bb_b.max().x() < bb_a.min().x()) dx = bb_a.min().x() - bb_b.max().x();
if (bb_a.max().y() < bb_b.min().y()) dy = bb_b.min().y() - bb_a.max().y();
else if (bb_b.max().y() < bb_a.min().y()) dy = bb_a.min().y() - bb_b.max().y();
if (bb_a.max().z() < bb_b.min().z()) dz = bb_b.min().z() - bb_a.max().z();
else if (bb_b.max().z() < bb_a.min().z()) dz = bb_a.min().z() - bb_b.max().z();
return std::sqrt(dx*dx + dy*dy + dz*dz);
}
return distance(a, b);
}
} // namespace vde::brep