Open-source manufacturing ERP, MES and QMS. Quoting, MRP, inventory, shop floor, quality and lot/serial traceability on one Postgres schema, with a REST API and MCP server. Self-host or use Carbon Cloud.
TypeScript ai-agentsbill-of-materialserpinventory-managementleanmanufacturingmanufacturing-execution-systemmcp-servermesmrpopen-source-erppostgresqlproduction-planningqmsquality-management-systemreact-routerself-hostedsupabasetraceabilitytypescript
// SPDX-License-Identifier: AGPL-3.0-only
// Carbon (github.com/crbnos/carbon). Modified or adapted versions of this file,
// including ports, remain AGPLv3; serving them over a network requires releasing their source.
//! Collision-query core over the FCL bridge: `contacts_at` (max penetration
//! depth per neighbor), the `classify` early-stop path, path/free-travel sweeps,
//! and the mate/seated/self exemption maps.
use crate::consts::*;
use crate::types::{Component, FastenerInfo};
use nalgebra::Vector3;
use smallvec::SmallVec;
use std::collections::HashMap;
/// Per-query contact list. Almost every sample touches ≤8 others, so the list
/// lives inline on the stack — millions of `contacts_at` calls (2.9M on the
/// 431-part BCU) stop paying a heap alloc each.
pub type Contacts = SmallVec<[(String, f64); 8]>;
/// Global count of `contacts_at` calls (perf diagnostic; read via `contacts_at_calls`).
pub static CONTACTS_AT_CALLS: std::sync::atomic::AtomicUsize =
std::sync::atomic::AtomicUsize::new(0);
/// Snapshot the current `contacts_at` call count.
pub fn contacts_at_calls() -> usize {
CONTACTS_AT_CALLS.load(std::sync::atomic::Ordering::Relaxed)
}
/// Per-query FCL contact budget. Every consumer collapses contacts to
/// max-depth-per-neighbor, so this only bounds pathological deep overlaps; the
/// classify path early-stops well before it.
const NUM_MAX_CONTACTS: usize = 100_000;
/// An allowance map: partner nodeId -> allowed seated interference (mm).
/// `f64::INFINITY` means "the moving part / group member itself" — never counts
/// as touching or blocking.
pub type Exempt = HashMap<String, f64>;
/// One FCL broadphase manager built from a set of static parts, reused across
/// all samples of a sweep — the algorithmic replacement for the old
/// per-sample-per-other pairwise `collide`. The moving part is expected to be
/// absent from `others` (callers pass `others` = the current set minus the
/// moving part), so no self-collision cost is paid.
pub struct Broadphase {
manager: cxx::UniquePtr<collision::Manager>,
index_to_node: Vec<String>,
}
impl Broadphase {
pub fn new(others: &[&Component]) -> Self {
let mut manager = collision::manager_new();
let mut index_to_node = Vec::with_capacity(others.len());
for o in others {
collision::manager_add(manager.pin_mut(), &o.bvh());
index_to_node.push(o.node_id.clone());
}
collision::manager_setup(manager.pin_mut());
Broadphase {
manager,
index_to_node,
}
}
/// `_contacts_at`: (otherName, max_depth) for `part` translated by
/// `translation` against the registered others. One broadphase query.
pub fn contacts_at(&self, part: &Component, translation: &Vector3<f64>) -> Contacts {
CONTACTS_AT_CALLS.fetch_add(1, std::sync::atomic::Ordering::Relaxed);
let bvh = part.bvh();
let cs = collision::manager_collide_single(
&self.manager,
&bvh,
-1,
translation[0],
translation[1],
translation[2],
NUM_MAX_CONTACTS,
);
cs.into_iter()
.map(|c| (self.index_to_node[c.other].clone(), c.depth))
.collect()
}
}
/// `_contacts_at` (one-shot): builds a broadphase over `others` and queries once.
/// Prefer building a `Broadphase` when sweeping many samples.
pub fn contacts_at(
part: &Component,
others: &[&Component],
translation: &Vector3<f64>,
) -> Contacts {
Broadphase::new(others).contacts_at(part, translation)
}
/// A PERSISTENT broadphase manager for a working set of parts — built once and
/// reused across the whole plan. Sweeps exclude the moving part by its index
/// (no per-sweep rebuild, no unregister); parts are set inactive on removal.
/// This is the algorithmic key to matching/beating Python's speed: Python keeps
/// one `DynamicAABBTreeCollisionManager` for the entire greedy loop.
pub struct CollisionWorld {
manager: cxx::UniquePtr<collision::Manager>,
node_to_index: HashMap<String, usize>,
index_to_node: Vec<String>,
}
// SAFETY: the parallel greedy sweeps share ONE `&CollisionWorld` across rayon
// workers, each issuing read-only `contacts_at*` queries. Those bottom out in
// FCL's `DynamicAABBTreeCollisionManager::collide(obj, cdata, cb) const` — a pure
// read (BV overlap + callback; the manager has no `mutable` members and the
// narrowphase result/contact state is call-local), over BVHModels frozen after
// `endModel` (see `SharedBvh`). The only structural mutations (`add`,
// `set_active`) take `&mut self`, so Rust statically forbids them from
// overlapping the shared `&self` queries. `manager_new`/`manager_add`/etc. are
// never called concurrently on one instance.
unsafe impl Sync for CollisionWorld {}
impl CollisionWorld {
pub fn new(parts: &[&Component]) -> Self {
let mut manager = collision::manager_new();
let mut node_to_index = HashMap::new();
let mut index_to_node = Vec::with_capacity(parts.len());
for (i, p) in parts.iter().enumerate() {
collision::manager_add(manager.pin_mut(), &p.bvh());
node_to_index.insert(p.node_id.clone(), i);
index_to_node.push(p.node_id.clone());
}
collision::manager_setup(manager.pin_mut());
CollisionWorld {
manager,
node_to_index,
index_to_node,
}
}
pub fn from_components(parts: &[Component]) -> Self {
let refs: Vec<&Component> = parts.iter().collect();
Self::new(&refs)
}
pub fn index_of(&self, node_id: &str) -> i64 {
self.node_to_index
.get(node_id)
.map(|&i| i as i64)
.unwrap_or(-1)
}
/// Register a new body (a merged/combined mesh) mid-plan. Returns its index.
pub fn add(&mut self, node_id: &str, part: &Component) -> usize {
collision::manager_add(self.manager.pin_mut(), &part.bvh());
collision::manager_setup(self.manager.pin_mut());
let idx = self.index_to_node.len();
self.node_to_index.insert(node_id.to_string(), idx);
self.index_to_node.push(node_id.to_string());
idx
}
/// Toggle a body's participation (removal, or the group unregister protocol).
pub fn set_active(&mut self, node_id: &str, active: bool) {
if let Some(&i) = self.node_to_index.get(node_id) {
collision::manager_set_active(self.manager.pin_mut(), i, active);
}
}
/// `_contacts_at`: (otherName, max_depth) for `part` (moving) translated by
/// `translation`, against the active bodies — one broadphase query, self
/// excluded by index.
pub fn contacts_at(&self, part: &Component, translation: &Vector3<f64>) -> Contacts {
CONTACTS_AT_CALLS.fetch_add(1, std::sync::atomic::Ordering::Relaxed);
let bvh = part.bvh();
let mi = self.index_of(&part.node_id);
let cs = collision::manager_collide_single(
&self.manager,
&bvh,
mi,
translation[0],
translation[1],
translation[2],
NUM_MAX_CONTACTS,
);
cs.into_iter()
.map(|c| (self.index_to_node[c.other].clone(), c.depth))
.collect()
}
/// Resolve an exempt map (name → allowance) into index-space blocking
/// thresholds for `classify`: each finite allowance becomes
/// `allowance + MATE_DEPTH_MARGIN_MM` (the exact skip predicate the Rust
/// consumers apply); infinite allowances pass through as +INF (the backend
/// drops those neighbors entirely — they can never block, near, or touch).
pub fn resolve_exempt(&self, exempt: Option<&Exempt>) -> (Vec<i64>, Vec<f64>) {
let mut idx = Vec::new();
let mut am = Vec::new();
if let Some(ex) = exempt {
for (name, &allow) in ex {
if let Some(&i) = self.node_to_index.get(name) {
idx.push(i as i64);
am.push(if allow == f64::INFINITY {
f64::INFINITY
} else {
allow + MATE_DEPTH_MARGIN_MM
});
}
}
}
(idx, am)
}
/// Threshold-classified `_contacts_at` for the sweep consumers: same
/// (other, depth) shape, but the backend may early-stop each neighbor at
/// the first pair past its predicate threshold (see the bridge doc). On
/// the FCL backend this is EXACTLY `contacts_at` (hints ignored) — parity
/// holds by construction. `want_touch_near=false` when the caller only
/// tests blocking (free_travel / path_blockers).
#[allow(clippy::too_many_arguments)]
pub fn classify(
&self,
part: &Component,
translation: &Vector3<f64>,
skip_nodes: &std::collections::BTreeSet<String>,
ov: &(Vec<i64>, Vec<f64>),
tol: f64,
want_touch_near: bool,
) -> Contacts {
CONTACTS_AT_CALLS.fetch_add(1, std::sync::atomic::Ordering::Relaxed);
let bvh = part.bvh();
let mut skip: SmallVec<[i64; 16]> = SmallVec::with_capacity(skip_nodes.len() + 1);
skip.push(self.index_of(&part.node_id));
for n in skip_nodes {
if let Some(&i) = self.node_to_index.get(n) {
skip.push(i as i64);
}
}
let cs = collision::manager_classify_multi(
&self.manager,
&bvh,
&skip,
&ov.0,
&ov.1,
translation[0],
translation[1],
translation[2],
tol,
want_touch_near,
NUM_MAX_CONTACTS,
);
cs.into_iter()
.map(|c| (self.index_to_node[c.other].clone(), c.depth))
.collect()
}
/// `_contacts_at` with extra bodies culled at the broadphase — the moving
/// part plus any `skip_nodes` (known blockers a sweep no longer needs to
/// re-enumerate). One broadphase query; skipped bodies never narrowphase.
pub fn contacts_at_excluding(
&self,
part: &Component,
translation: &Vector3<f64>,
skip_nodes: &std::collections::BTreeSet<String>,
) -> Contacts {
CONTACTS_AT_CALLS.fetch_add(1, std::sync::atomic::Ordering::Relaxed);
let bvh = part.bvh();
let mut skip: SmallVec<[i64; 16]> = SmallVec::with_capacity(skip_nodes.len() + 1);
skip.push(self.index_of(&part.node_id));
for n in skip_nodes {
if let Some(&i) = self.node_to_index.get(n) {
skip.push(i as i64);
}
}
let cs = collision::manager_collide_single_multi(
&self.manager,
&bvh,
&skip,
translation[0],
translation[1],
translation[2],
NUM_MAX_CONTACTS,
);
cs.into_iter()
.map(|c| (self.index_to_node[c.other].clone(), c.depth))
.collect()
}
}
/// `_blocking_depth`: max blocking penetration over a sample's contacts.
pub fn blocking_depth(contacts: &[(String, f64)], exempt: Option<&Exempt>) -> f64 {
let mut depth = 0.0;
for (other, cd) in contacts {
if let Some(ex) = exempt {
if let Some(&allow) = ex.get(other) {
if *cd <= allow + MATE_DEPTH_MARGIN_MM {
continue;
}
}
}
if *cd > depth {
depth = *cd;
}
}
depth
}
/// `_mate_exempt`: bore-engagement allowances for this part along `direction`.
pub fn mate_exempt(
part: &Component,
direction: &Vector3<f64>,
fasteners: &HashMap<String, FastenerInfo>,
) -> Option<Exempt> {
let info = fasteners.get(&part.node_id)?;
if info.mates.is_empty() && info.sliding.is_empty() {
return None;
}
if direction.dot(&info.axis).abs() > 0.99 {
let mut merged: Exempt = info.sliding.clone();
for (k, v) in &info.mates {
merged.insert(k.clone(), *v); // mates win on collision, per {**sliding, **mates}
}
return Some(merged);
}
None
}
/// `_seated_exempt`: sandwich-squish allowances valid along `direction`.
pub fn seated_exempt(part: &Component, direction: &Vector3<f64>) -> Option<Exempt> {
if part.seated_allowance.is_empty() {
return None;
}
let mut allowed: Exempt = HashMap::new();
for (partner, depth) in &part.seated_allowance {
if let Some(axis) = part.seated_allowance_axes.get(partner) {
if direction.dot(axis).abs() > 0.99 {
allowed.insert(partner.clone(), *depth);
}
}
}
if allowed.is_empty() {
None
} else {
Some(allowed)
}
}
/// `_self_exempt`: merge an infinite allowance for the moving part(s).
pub fn self_exempt(exempt: Option<Exempt>, self_ids: &[&str]) -> Exempt {
let mut merged = exempt.unwrap_or_default();
for id in self_ids {
merged.insert((*id).to_string(), f64::INFINITY);
}
merged
}
/// numpy `np.linspace(start, end, n, endpoint=True)[1:]`.
fn linspace_tail(start: f64, end: f64, n: usize) -> Vec<f64> {
if n <= 1 {
return vec![end];
}
let step = (end - start) / (n as f64 - 1.0);
(1..n).map(|i| start + i as f64 * step).collect()
}
/// Python `int(x)` truncation toward zero, used for the sample-count formula.
fn sample_count(samples: usize, span: f64) -> usize {
let by_spacing = (span / MAX_SAMPLE_SPACING_MM).trunc() as i64 + 1;
samples
.max(by_spacing.max(0) as usize)
.min(MAX_PATH_SAMPLES)
}
/// `_path_is_clear`: dense sampling; None if blocked, else the last touching
/// distance (0.0 for a free flight).
#[allow(clippy::too_many_arguments)]
pub fn path_is_clear(
part: &Component,
world: &CollisionWorld,
direction: &Vector3<f64>,
start: f64,
end: f64,
samples: usize,
tolerance: f64,
base_offset: Option<&Vector3<f64>>,
exempt: Option<Exempt>,
check_until: Option<f64>,
) -> Option<f64> {
if end <= start {
return None;
}
// Compliant squish along the sandwich axis never blocks; explicit
// exemptions keep precedence when larger.
let mut exempt = exempt;
if let Some(seated) = seated_exempt(part, direction) {
let mut merged = seated;
if let Some(ex) = &exempt {
for (k, v) in ex {
let e = merged.entry(k.clone()).or_insert(f64::MIN);
*e = e.max(*v);
}
}
exempt = Some(merged);
}
let n = sample_count(samples, end - start);
let offsets = linspace_tail(start, end, n);
let spacing = (end - start) / (n.max(2) as f64 - 1.0);
let no_skip = std::collections::BTreeSet::new();
let ov = world.resolve_exempt(exempt.as_ref());
let blocked_at = |distance: f64| -> (bool, bool, bool) {
let mut translation = direction * distance;
if let Some(off) = base_offset {
translation += off;
}
let contacts = world.classify(part, &translation, &no_skip, &ov, tolerance, true);
if contacts.is_empty() {
return (false, false, false);
}
let depth = blocking_depth(&contacts, exempt.as_ref());
let touching = contacts.iter().any(|(other, _)| {
!matches!(exempt.as_ref().and_then(|e| e.get(other)), Some(&a) if a == f64::INFINITY)
});
(depth > tolerance, depth > tolerance * 0.5, touching)
};
let mut last_touch = 0.0;
for &s in &offsets {
if let Some(cu) = check_until {
if s > cu {
break;
}
}
let (blocked, near, touching) = blocked_at(s);
if blocked {
return None;
}
if touching {
last_touch = s;
}
if near {
let half = spacing / 2.0;
for probe in [s - half, s + half] {
if probe <= start || probe >= end {
continue;
}
if blocked_at(probe).0 {
return None;
}
}
}
}
Some(last_touch)
}
/// `_free_travel`: furthest clear translation along `direction` from `base_offset`.
#[allow(clippy::too_many_arguments)]
pub fn free_travel(
part: &Component,
world: &CollisionWorld,
direction: &Vector3<f64>,
base_offset: &Vector3<f64>,
cap: f64,
samples: usize,
exempt: Option<&Exempt>,
tolerance: f64,
) -> f64 {
if cap <= 0.0 {
return 0.0;
}
let n = sample_count(samples, cap);
let offsets = linspace_tail(0.0, cap, n);
let no_skip = std::collections::BTreeSet::new();
let ov = world.resolve_exempt(exempt);
let mut clear = 0.0;
for s in offsets {
let translation = direction * s + base_offset;
let contacts = world.classify(part, &translation, &no_skip, &ov, tolerance, false);
if !contacts.is_empty() {
let depth = blocking_depth(&contacts, exempt);
if depth > tolerance {
return clear;
}
}
clear = s;
}
clear
}
/// `_path_blockers`: every present part the removal path cuts through
/// (union over segments). Mirrors Python's unregister-mid-sweep: once a partner
/// is a confirmed blocker its identity is all this returns, so it is culled from
/// every later sample (`contacts_at_excluding`) — otherwise a deep pass-through
/// re-enumerates that blocker's full triangle-contact set at every sample (the
/// dominant cost). Sound: a recorded blocker can't be un-recorded, and FCL
/// enumerates pairs independently, so culling one never hides another's contacts.
#[allow(clippy::too_many_arguments)]
pub fn path_blockers(
part: &Component,
world: &CollisionWorld,
segments: &[(Vector3<f64>, f64)],
samples: usize,
fasteners: &HashMap<String, FastenerInfo>,
extra_exempt: Option<&Exempt>,
tolerance: f64,
) -> std::collections::BTreeSet<String> {
let mut blockers = std::collections::BTreeSet::new();
let mut offset = Vector3::zeros();
for (direction, distance) in segments {
let mut exempt = mate_exempt(part, direction, fasteners);
if let Some(seated) = seated_exempt(part, direction) {
let mut merged = seated;
if let Some(ex) = exempt {
for (k, v) in ex {
merged.insert(k, v);
}
}
exempt = Some(merged);
}
if let Some(extra) = extra_exempt {
let mut merged = exempt.unwrap_or_default();
for (k, v) in extra {
merged.insert(k.clone(), *v);
}
exempt = Some(merged);
}
let count = sample_count(samples, *distance);
let ov = world.resolve_exempt(exempt.as_ref());
for s in linspace_tail(0.0, *distance, count) {
let translation = offset + direction * s;
for (other, depth) in
world.classify(part, &translation, &blockers, &ov, tolerance, false)
{
if let Some(ex) = &exempt {
if let Some(&allow) = ex.get(&other) {
if depth <= allow + MATE_DEPTH_MARGIN_MM {
continue;
}
}
}
if depth > tolerance {
blockers.insert(other);
}
}
}
offset += direction * *distance;
}
blockers
}