feat(v4.3): mass properties, clearance, assembly mass props
- 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:
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user