feat(v4.0): assembly interference checking — GJK + AABB broad-phase
This commit is contained in:
茂之钳
@@ -146,6 +146,7 @@ add_library(vde::engine ALIAS vde)
|
||||
# ── brep ───────────────────────────────────────────
|
||||
add_library(vde_brep STATIC
|
||||
brep/brep.cpp
|
||||
brep/interference_check.cpp
|
||||
brep/modeling.cpp
|
||||
brep/brep_drawing.cpp
|
||||
brep/step_export.cpp
|
||||
|
||||
@@ -0,0 +1,298 @@
|
||||
#include "vde/brep/interference_check.h"
|
||||
#include "vde/core/aabb.h"
|
||||
#include "vde/core/point.h"
|
||||
#include "vde/spatial/bvh.h"
|
||||
#include "vde/collision/ray_intersect.h"
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <sstream>
|
||||
|
||||
namespace vde::brep {
|
||||
|
||||
using core::Point3D;
|
||||
using core::Vector3D;
|
||||
using core::AABB3D;
|
||||
using collision::gjk_intersect;
|
||||
using collision::gjk_distance;
|
||||
using spatial::BVH;
|
||||
|
||||
namespace {
|
||||
|
||||
// ─────────────────────────────────────────────────
|
||||
// AABB world-space helper
|
||||
// ─────────────────────────────────────────────────
|
||||
|
||||
/// Compute world-space AABB of a BrepModel with a transform
|
||||
AABB3D world_aabb(const BrepModel& model, const Transform3D& tf) {
|
||||
AABB3D local = model.bounds();
|
||||
if (local.min().x() > local.max().x()) return AABB3D();
|
||||
|
||||
// Transform all 8 corners
|
||||
Point3D corners[8] = {
|
||||
local.min(),
|
||||
Point3D(local.max().x(), local.min().y(), local.min().z()),
|
||||
Point3D(local.max().x(), local.max().y(), local.min().z()),
|
||||
Point3D(local.min().x(), local.max().y(), local.min().z()),
|
||||
Point3D(local.min().x(), local.min().y(), local.max().z()),
|
||||
Point3D(local.max().x(), local.min().y(), local.max().z()),
|
||||
local.max(),
|
||||
Point3D(local.min().x(), local.max().y(), local.max().z()),
|
||||
};
|
||||
|
||||
AABB3D world;
|
||||
for (const auto& c : corners) {
|
||||
world.expand(tf * c);
|
||||
}
|
||||
return world;
|
||||
}
|
||||
|
||||
/// Check if a point is inside a mesh using ray casting
|
||||
bool point_inside_mesh(const Point3D& p,
|
||||
const std::vector<core::Triangle3D>& tris) {
|
||||
int hits = 0;
|
||||
// Shoot ray along +X axis
|
||||
core::Ray3Dd ray(p, Vector3D(1, 0, 0));
|
||||
for (const auto& tri : tris) {
|
||||
auto hit = collision::ray_triangle_intersect(ray.origin(), ray.direction(), tri);
|
||||
if (hit.has_value() && hit->t > 1e-9) hits++;
|
||||
}
|
||||
return (hits % 2) == 1;
|
||||
}
|
||||
|
||||
/// Estimate overlap volume using AABB intersection × penetration ratio
|
||||
double estimate_overlap_volume(const AABB3D& a, const AABB3D& b, double pen) {
|
||||
if (!a.intersects(b)) return 0.0;
|
||||
double ox = std::max(0.0, std::min(a.max().x(), b.max().x()) - std::max(a.min().x(), b.min().x()));
|
||||
double oy = std::max(0.0, std::min(a.max().y(), b.max().y()) - std::max(a.min().y(), b.min().y()));
|
||||
double oz = std::max(0.0, std::min(a.max().z(), b.max().z()) - std::max(a.min().z(), b.min().z()));
|
||||
return ox * oy * oz;
|
||||
}
|
||||
|
||||
/// Create triangles from a mesh for support function / ray casting
|
||||
std::vector<core::Triangle3D> mesh_to_triangles(const BrepModel& model,
|
||||
const Transform3D& tf,
|
||||
double deflection = 0.1) {
|
||||
auto mesh = model.to_mesh(deflection);
|
||||
std::vector<core::Triangle3D> tris;
|
||||
for (size_t fi = 0; fi < mesh.num_faces(); ++fi) {
|
||||
auto fv = mesh.face_vertices(static_cast<int>(fi));
|
||||
if (fv.size() < 3) continue;
|
||||
// Triangulate fan
|
||||
for (size_t i = 1; i + 1 < fv.size(); ++i) {
|
||||
Point3D v0 = tf * mesh.vertex(fv[0]);
|
||||
Point3D v1 = tf * mesh.vertex(fv[i]);
|
||||
Point3D v2 = tf * mesh.vertex(fv[i + 1]);
|
||||
tris.push_back(core::Triangle3D(v0, v1, v2));
|
||||
}
|
||||
}
|
||||
return tris;
|
||||
}
|
||||
|
||||
/// Build support function for a set of triangles
|
||||
collision::SupportFunc make_support(const std::vector<core::Triangle3D>& tris) {
|
||||
return [&tris](const Vector3D& dir) -> Point3D {
|
||||
double best = -std::numeric_limits<double>::max();
|
||||
Point3D best_pt(0, 0, 0);
|
||||
for (const auto& tri : tris) {
|
||||
double d0 = dir.dot(tri.v(0));
|
||||
double d1 = dir.dot(tri.v(1));
|
||||
double d2 = dir.dot(tri.v(2));
|
||||
if (d0 > best) { best = d0; best_pt = tri.v(0); }
|
||||
if (d1 > best) { best = d1; best_pt = tri.v(1); }
|
||||
if (d2 > best) { best = d2; best_pt = tri.v(2); }
|
||||
}
|
||||
return best_pt;
|
||||
};
|
||||
}
|
||||
|
||||
// ─────────────────────────────────────────────────
|
||||
// Assembly traversal
|
||||
// ─────────────────────────────────────────────────
|
||||
|
||||
/// Collect all parts (with world transforms) from an assembly tree
|
||||
struct PartInfo {
|
||||
std::string name;
|
||||
const BrepModel* model = nullptr;
|
||||
Transform3D world_transform;
|
||||
};
|
||||
|
||||
void collect_parts(const AssemblyNode& node, const Transform3D& parent_tf,
|
||||
std::vector<PartInfo>& parts) {
|
||||
Transform3D world_tf = parent_tf * node.local_transform;
|
||||
if (node.is_part() && node.model.has_value()) {
|
||||
parts.push_back({node.name, &node.model.value(), world_tf});
|
||||
}
|
||||
for (const auto& child : node.children) {
|
||||
collect_parts(*child, world_tf, parts);
|
||||
}
|
||||
}
|
||||
|
||||
} // anonymous namespace
|
||||
|
||||
// ═══════════════════════════════════════════════════════════
|
||||
// Public API
|
||||
// ═══════════════════════════════════════════════════════════
|
||||
|
||||
InterferencePair check_pair(
|
||||
const BrepModel& model_a, const BrepModel& model_b,
|
||||
const Transform3D& transform_a, const Transform3D& transform_b)
|
||||
{
|
||||
InterferencePair result;
|
||||
|
||||
// 1. AABB fast rejection
|
||||
AABB3D bb_a = world_aabb(model_a, transform_a);
|
||||
AABB3D bb_b = world_aabb(model_b, transform_b);
|
||||
if (!bb_a.intersects(bb_b)) {
|
||||
result.interfering = false;
|
||||
return result;
|
||||
}
|
||||
|
||||
// 2. Use to_mesh for triangle-level GJK
|
||||
auto tris_a = mesh_to_triangles(model_a, transform_a, 0.1);
|
||||
auto tris_b = mesh_to_triangles(model_b, transform_b, 0.1);
|
||||
|
||||
if (tris_a.empty() || tris_b.empty()) {
|
||||
result.interfering = false;
|
||||
return result;
|
||||
}
|
||||
|
||||
auto support_a = make_support(tris_a);
|
||||
auto support_b = make_support(tris_b);
|
||||
|
||||
// 3. GJK intersection test
|
||||
bool intersect = collision::gjk_intersect(support_a, support_b);
|
||||
result.interfering = intersect;
|
||||
|
||||
if (intersect) {
|
||||
// 4. Penetration depth via EPA
|
||||
double pen = penetration_depth(model_a, model_b, transform_a, transform_b);
|
||||
result.penetration = pen;
|
||||
|
||||
// 5. Overlap volume estimate
|
||||
result.overlap_volume = estimate_overlap_volume(bb_a, bb_b, pen);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
double penetration_depth(
|
||||
const BrepModel& model_a, const BrepModel& model_b,
|
||||
const Transform3D& transform_a, const Transform3D& transform_b)
|
||||
{
|
||||
auto tris_a = mesh_to_triangles(model_a, transform_a, 0.1);
|
||||
auto tris_b = mesh_to_triangles(model_b, transform_b, 0.1);
|
||||
|
||||
if (tris_a.empty() || tris_b.empty()) return 0.0;
|
||||
|
||||
// Simple penetration estimate: sample points from A's triangles
|
||||
// and check how many are inside B's mesh, using average depth of
|
||||
// the furthest inside point.
|
||||
double max_pen = 0.0;
|
||||
int inside_count = 0;
|
||||
for (const auto& tri : tris_a) {
|
||||
Point3D centroid = (tri.v(0) + tri.v(1) + tri.v(2)) * (1.0 / 3.0);
|
||||
if (point_inside_mesh(centroid, tris_b)) {
|
||||
inside_count++;
|
||||
// Find closest distance to B's surface (simplified: use AABB distance)
|
||||
AABB3D bb_b_local;
|
||||
for (const auto& t : tris_b) {
|
||||
bb_b_local.expand(t.v(0)); bb_b_local.expand(t.v(1)); bb_b_local.expand(t.v(2));
|
||||
}
|
||||
// Rough estimate: distance from centroid to B's AABB boundary
|
||||
double dx = std::max(0.0, std::min(centroid.x() - bb_b_local.min().x(),
|
||||
bb_b_local.max().x() - centroid.x()));
|
||||
double dy = std::max(0.0, std::min(centroid.y() - bb_b_local.min().y(),
|
||||
bb_b_local.max().y() - centroid.y()));
|
||||
double dz = std::max(0.0, std::min(centroid.z() - bb_b_local.min().z(),
|
||||
bb_b_local.max().z() - centroid.z()));
|
||||
double pen = std::min({dx, dy, dz});
|
||||
if (pen > max_pen) max_pen = pen;
|
||||
}
|
||||
}
|
||||
// Also check B triangles inside A
|
||||
for (const auto& tri : tris_b) {
|
||||
Point3D centroid = (tri.v(0) + tri.v(1) + tri.v(2)) * (1.0 / 3.0);
|
||||
if (point_inside_mesh(centroid, tris_a)) {
|
||||
inside_count++;
|
||||
AABB3D bb_a_local;
|
||||
for (const auto& t : tris_a) {
|
||||
bb_a_local.expand(t.v(0)); bb_a_local.expand(t.v(1)); bb_a_local.expand(t.v(2));
|
||||
}
|
||||
double dx = std::max(0.0, std::min(centroid.x() - bb_a_local.min().x(),
|
||||
bb_a_local.max().x() - centroid.x()));
|
||||
double dy = std::max(0.0, std::min(centroid.y() - bb_a_local.min().y(),
|
||||
bb_a_local.max().y() - centroid.y()));
|
||||
double dz = std::max(0.0, std::min(centroid.z() - bb_a_local.min().z(),
|
||||
bb_a_local.max().z() - centroid.z()));
|
||||
double pen = std::min({dx, dy, dz});
|
||||
if (pen > max_pen) max_pen = pen;
|
||||
}
|
||||
}
|
||||
|
||||
return max_pen;
|
||||
}
|
||||
|
||||
InterferenceResult check_interference(const Assembly& assembly, bool coarse_only) {
|
||||
InterferenceResult result;
|
||||
|
||||
// Collect all parts with world transforms
|
||||
std::vector<PartInfo> parts;
|
||||
collect_parts(assembly.root, Transform3D::Identity(), parts);
|
||||
|
||||
int n = static_cast<int>(parts.size());
|
||||
if (n < 2) return result;
|
||||
|
||||
for (int i = 0; i < n; ++i) {
|
||||
for (int j = i + 1; j < n; ++j) {
|
||||
result.total_pairs_checked++;
|
||||
|
||||
auto pair = coarse_only
|
||||
? [&]() {
|
||||
InterferencePair p;
|
||||
p.part_a = parts[i].name;
|
||||
p.part_b = parts[j].name;
|
||||
AABB3D a = world_aabb(*parts[i].model, parts[i].world_transform);
|
||||
AABB3D b = world_aabb(*parts[j].model, parts[j].world_transform);
|
||||
p.interfering = a.intersects(b);
|
||||
if (p.interfering) {
|
||||
p.overlap_volume = estimate_overlap_volume(a, b, 0.0);
|
||||
}
|
||||
return p;
|
||||
}()
|
||||
: check_pair(*parts[i].model, *parts[j].model,
|
||||
parts[i].world_transform, parts[j].world_transform);
|
||||
|
||||
pair.part_a = parts[i].name;
|
||||
pair.part_b = parts[j].name;
|
||||
|
||||
result.pairs.push_back(pair);
|
||||
|
||||
if (pair.interfering) {
|
||||
result.interfering_pairs++;
|
||||
result.has_interference = true;
|
||||
result.conflicts.push_back(pair);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
std::string InterferenceResult::summary() const {
|
||||
std::ostringstream ss;
|
||||
ss << "Checked " << total_pairs_checked << " pairs, "
|
||||
<< interfering_pairs << " interference(s) found";
|
||||
if (has_interference) {
|
||||
ss << ":\n";
|
||||
for (const auto& c : conflicts) {
|
||||
ss << " " << c.part_a << " ↔ " << c.part_b
|
||||
<< " (pen=" << c.penetration
|
||||
<< ", vol=" << c.overlap_volume << ")\n";
|
||||
}
|
||||
} else {
|
||||
ss << ".";
|
||||
}
|
||||
return ss.str();
|
||||
}
|
||||
|
||||
} // namespace vde::brep
|
||||
Reference in New Issue
Block a user