SolidCraft: an open-source parametric 3D CAD application built in pure Rust. Desktop, web, and automation through MCP.
//! Bounded, chronological checks of a joint trajectory. The motion is sampled, so this is not
//! continuous collision detection. A joint drive decides on the exact bodies whether they pass
//! into each other at a sample ([`ContactGeometry::overlaps`]) and closes in on where they first
//! touch through placements known to be clear ([`ContactGeometry::gap`]); gaps and interference
//! depths are measured on the bodies' contact meshes.
use solidcraft_geom::prelude::*;
use std::cell::Cell;
use std::collections::{BTreeMap, BTreeSet, HashMap};
use std::sync::Arc;
use solidcore_record::Vec3;
use solidcraft_doc::joints;
use solidcraft_doc::{Document, IDENTITY, Mat, apply_point, mat_mul};
use solidcraft_geom::{Aabb3, Bvh, Mesh};
use crate::params::bad;
use crate::{Result, Session};
const MAX_SAMPLES: usize = 4096;
const MAX_PLACEMENTS: usize = 4096;
const MAX_BODIES: usize = 256;
const MAX_TRIANGLES: usize = 50_000;
const MAX_WORK: usize = 50_000_000;
/// Most exact intersections one joint drive works out: one per pair of bodies whose exact boxes
/// meet, at each sample and bisection step.
const MAX_INTERSECTIONS: usize = 2048;
fn limit(message: &str) -> crate::EngineError {
bad("contact.sweep", message)
}
#[derive(Clone)]
struct CachedBody {
path: Vec<u64>,
offset: Mat,
/// The body itself, for the exact checks, and its exact box in its own frame.
body: solidcraft_kernel::Body,
exact_box: Aabb3,
mesh: Arc<Mesh>,
size: f64,
/// How far its mesh may stray from its faces: 0 when every face is flat (the mesh is the
/// shape), else the chord tolerance it was tessellated to.
chord: f64,
}
/// Bodies [`ContactMeshes`] keeps; it starts over when full.
const MAX_CACHED_MESHES: usize = 4 * MAX_BODIES;
/// A body's contact mesh and what is measured on it, made once per body content.
#[derive(Clone)]
pub(crate) struct ContactMesh {
mesh: Arc<Mesh>,
chord: f64,
/// Its thinnest width across its facet directions (the sweep's step is a quarter of it).
thickness: f64,
}
/// Contact meshes by the body's geometry ([`solidcraft_doc::ModelBody::geometry_key`]), shared
/// by a session and its scratch copies: driving a joint again meshes no body that did not change.
#[derive(Default)]
pub(crate) struct ContactMeshes(HashMap<u64, ContactMesh>);
/// Tessellate `body` for contact checks and measure how thin it is.
fn contact_mesh(body: &solidcraft_doc::ModelBody) -> Result<ContactMesh> {
let tolerance = (body.body.size() * 1e-3).max(1e-3);
let mesh = body.body.mesh(&solidcraft_kernel::MeshTolerance::chord(tolerance))?;
if mesh.triangles.len() > MAX_TRIANGLES || mesh.positions.len().saturating_add(mesh.triangles.len().saturating_mul(4)) > 200_000 {
return Err(limit("body exceeds contact sweep tessellation budget"));
}
let extent = mesh.bounds().size();
// Whole-body widths along bounded distinct facet normals catch tilted
// plates without interpreting skinny mesh triangles as thin material.
// Local thin walls of a wider body remain a sampling limitation.
let mut thickness = extent.x.min(extent.y).min(extent.z);
let mut directions: Vec<Vec3> = Vec::new();
for triangle in &mesh.triangles {
let Some([a, b, c]) = mesh.tri(triangle) else {
return Err(limit("invalid contact tessellation"));
};
let Some(direction) = (b - a).cross(c - a).normalized() else { continue };
if directions.len() < 32 && !directions.iter().any(|d| d.dot(direction).abs() > 1.0 - 1e-6) {
directions.push(direction);
}
}
for direction in directions {
let (mut low, mut high) = (f64::INFINITY, f64::NEG_INFINITY);
for point in &mesh.positions {
let projected = point.dot(direction);
low = low.min(projected);
high = high.max(projected);
}
thickness = thickness.min(high - low);
}
if !thickness.is_finite() || thickness <= 1e-9 {
return Err(limit("body is too thin for a bounded contact sweep"));
}
// Flat faces tessellate exactly; only planes known from how the face was
// made count (an unknown face is taken as curved).
let faces = body.body.face_count();
let flat = faces <= 1024 && (0..faces).all(|i| body.body.face_plane(i).is_some());
let chord = if flat { 0.0 } else { tolerance };
Ok(ContactMesh { mesh: Arc::new(mesh), chord, thickness })
}
/// A member of a contact set: (occurrence id, name).
pub(super) type Member = (u64, String);
/// Two set members whose bodies pass into each other: their names, and the bodies (indices in
/// [`ContactGeometry`]) that do.
pub(super) struct Overlap {
pub(super) a: String,
pub(super) b: String,
bodies: (usize, usize),
}
#[derive(Clone)]
pub(super) struct ContactGeometry {
bodies: Vec<CachedBody>,
/// Body indices, and the set members (occurrence id, name) they are under.
pairs: Vec<(usize, usize, Member, Member)>,
step: f64,
}
impl ContactGeometry {
pub(super) fn new(s: &Session) -> Result<Self> {
let assembly = &s.doc.assembly;
let sets: Vec<Vec<u64>> = if assembly.contact_all {
vec![s.doc.occurrences.iter().map(|o| o.id).collect()]
} else {
assembly.contact_sets.iter().filter(|set| !set.suppressed).map(|set| set.occurrences.clone()).collect()
};
Self::of_sets(s, &sets)
}
/// The geometry of the bodies under `sets` (occurrence ids), paired as contact sets pair
/// them: bodies under different members of one set.
pub(super) fn of_sets(s: &Session, sets: &[Vec<u64>]) -> Result<Self> {
let mut out = Self { bodies: Vec::new(), pairs: Vec::new(), step: f64::INFINITY };
if sets.is_empty() {
return Ok(out);
}
let state = s.model.state();
let mut meshes: HashMap<String, (Arc<Mesh>, f64)> = HashMap::new();
let mut stack = vec![(0, Vec::<u64>::new())];
let mut count = 0;
while let Some((component, path)) = stack.pop() {
count += 1;
if count > MAX_PLACEMENTS || path.len() > 64 {
return Err(limit("assembly exceeds contact sweep placement budget"));
}
for occurrence in s.doc.occurrences.iter().filter(|o| o.parent == component) {
let mut child = path.clone();
child.push(occurrence.id);
stack.push((occurrence.component, child));
if stack.len() > MAX_PLACEMENTS {
return Err(limit("assembly exceeds contact sweep placement budget"));
}
}
if !sets.iter().any(|set| path.iter().any(|id| set.contains(id))) {
continue;
}
for body in state.bodies.iter().filter(|b| s.doc.model_body_component(b) == component) {
if body.body.is_mesh() {
return Err(limit("contact sweep does not support mesh bodies; disable their contact set to move them"));
}
if out.bodies.len() >= MAX_BODIES {
return Err(limit("too many bodies for a contact sweep"));
}
let (mesh, chord) = if let Some(cached) = meshes.get(&body.name) {
cached.clone()
} else {
let key = body.geometry_key();
let known = key.and_then(|k| s.contact_meshes.lock().ok().and_then(|c| c.0.get(&k).cloned()));
let made = match known {
Some(made) => made,
None => {
let made = contact_mesh(body)?;
if let (Some(k), Ok(mut cache)) = (key, s.contact_meshes.lock()) {
if cache.0.len() >= MAX_CACHED_MESHES {
cache.0.clear();
}
cache.0.insert(k, made.clone());
}
made
}
};
out.step = out.step.min(made.thickness * 0.25);
meshes.insert(body.name.clone(), (made.mesh.clone(), made.chord));
(made.mesh, made.chord)
};
out.bodies.push(CachedBody {
path: path.clone(),
offset: s.doc.model_body_offset(body).copied().unwrap_or(IDENTITY),
body: body.body.clone(),
exact_box: body.body.exact_bounds()?,
mesh,
size: body.body.size(),
chord,
});
}
}
let mut pairs = BTreeSet::new();
for set in sets {
let tagged: Vec<(usize, u64)> =
out.bodies.iter().enumerate().filter_map(|(i, b)| b.path.iter().rev().find(|id| set.contains(id)).map(|id| (i, *id))).collect();
for (i, (a, oa)) in tagged.iter().enumerate() {
for (b, ob) in tagged.iter().skip(i + 1) {
if oa != ob {
pairs.insert((*a, *b, *oa, *ob));
}
}
}
}
let name = |id| s.doc.occurrences.iter().find(|o| o.id == id).map(|o| o.name.clone()).unwrap_or_default();
out.pairs = pairs.into_iter().map(|(a, b, oa, ob)| (a, b, (oa, name(oa)), (ob, name(ob)))).collect();
Ok(out)
}
pub(super) fn has_pairs(&self) -> bool {
!self.pairs.is_empty()
}
/// Keeps only the pairs of set members (occurrence ids) `keep` takes.
pub(super) fn keep_pairs(&mut self, keep: impl Fn(u64, u64) -> bool) {
self.pairs.retain(|(_, _, a, b)| keep(a.0, b.0));
}
/// Move the pairs of set members `take` picks into a geometry of their own (with the same
/// bodies), leaving the rest here.
pub(super) fn take_pairs(&mut self, take: impl Fn(u64, u64) -> bool) -> ContactGeometry {
let (taken, kept) = std::mem::take(&mut self.pairs).into_iter().partition(|(_, _, a, b)| take(a.0, b.0));
self.pairs = kept;
ContactGeometry { bodies: self.bodies.clone(), pairs: taken, step: self.step }
}
/// The diagonal of the box the bodies fill in `s`: past it, a part moving in a straight
/// line meets nothing it has not met.
pub(super) fn reach(&self, s: &Session) -> Result<f64> {
Ok(self.bounds(s)?.size().len())
}
/// How many mesh points are placed for each check.
pub(super) fn vertex_count(&self) -> usize {
self.bodies.iter().map(|b| b.mesh.positions.len()).fold(0, usize::saturating_add)
}
/// How many pairs of set members are checked against each other.
pub(super) fn pair_count(&self) -> usize {
self.pairs.iter().map(|(_, _, a, b)| (a.0.min(b.0), a.0.max(b.0))).collect::<BTreeSet<_>>().len()
}
fn frame(body: &CachedBody, s: &Session) -> Result<Mat> {
Self::frame_in(body, &s.doc)
}
fn frame_in(body: &CachedBody, doc: &Document) -> Result<Mat> {
let mut frame = IDENTITY;
for id in &body.path {
let occurrence = doc.occurrences.iter().find(|o| o.id == *id).ok_or_else(|| limit("contact occurrence disappeared"))?;
frame = mat_mul(&frame, &occurrence.transform);
}
Ok(mat_mul(&frame, &body.offset))
}
fn bounds(&self, s: &Session) -> Result<Aabb3> {
let mut bounds = Aabb3::EMPTY;
for body in &self.bodies {
let frame = Self::frame(body, s)?;
for point in &body.mesh.positions {
bounds.add(apply_point(&frame, *point));
}
}
Ok(bounds)
}
pub(super) fn samples(&self, start: &Session, end: &Session) -> Result<usize> {
let a = joints::joint_values(&start.doc.assembly);
let b = joints::joint_values(&end.doc.assembly);
let bounds = self.bounds(start)?.union(&self.bounds(end)?);
let state = start.model.state();
let (mut linear, mut angular, mut radius) = (0.0, 0.0, 0.0_f64);
for joint in start.doc.assembly.joints.iter().filter(|j| !j.suppressed) {
for (index, _) in joint.kind.dofs().iter().enumerate() {
let delta = (a.get(&joint.id).and_then(|q| q.get(index)).copied().unwrap_or(0.0)
- b.get(&joint.id).and_then(|q| q.get(index)).copied().unwrap_or(0.0))
.abs();
if joint.kind.is_angle(index) {
angular += delta;
if delta == 0.0 {
continue;
}
for session in [start, end] {
for origin in [&joint.a, &joint.b] {
let frame = session.doc.origin_world(&state, origin)?;
let pivot = apply_point(&frame, Vec3::ZERO);
let far = Vec3::new(
(bounds.min.x - pivot.x).abs().max((bounds.max.x - pivot.x).abs()),
(bounds.min.y - pivot.y).abs().max((bounds.max.y - pivot.y).abs()),
(bounds.min.z - pivot.z).abs().max((bounds.max.z - pivot.z).abs()),
);
radius = radius.max(far.len());
}
}
} else {
linear += delta;
}
}
}
let required = ((linear + angular * (radius + linear)) / self.step).max(angular / 1_f64.to_radians()).ceil().max(1.0);
if !required.is_finite() || required > MAX_SAMPLES as f64 {
return Err(limit("motion exceeds contact sweep sampling budget; drive the joint in smaller increments"));
}
let vertices: usize = self.bodies.iter().map(|body| body.mesh.positions.len()).sum();
if (required as usize).saturating_mul(vertices.saturating_add(self.pairs.len())) > MAX_WORK {
return Err(limit("motion exceeds contact sweep transform budget; simplify the contact set or use smaller increments"));
}
Ok(required as usize)
}
/// Each body's mesh placed where `s` has it.
fn placed(&self, s: &Session) -> Result<Vec<Mesh>> {
self.bodies
.iter()
.map(|body| {
let frame = Self::frame(body, s)?;
let positions = body.mesh.positions.iter().map(|p| apply_point(&frame, *p)).collect();
Ok(Mesh { positions, triangles: body.mesh.triangles.clone(), ..Mesh::default() })
})
.collect()
}
/// The pairs of bodies of set members that pass into each other in `s` (every pair, or the
/// first found when `all` is false), decided on the exact bodies: their intersection holds
/// volume. Bodies that only touch, such as a cam and its follower, share none. Only bodies
/// whose exact boxes meet are intersected; `intersections` counts them, refused past the
/// drive's budget.
pub(super) fn overlaps(&self, s: &Session, intersections: &mut usize, all: bool) -> Result<Vec<Overlap>> {
let frames = self.frames(s)?;
let boxes: Vec<Aabb3> = self.bodies.iter().zip(&frames).map(|(b, f)| placed_box(&b.exact_box, f)).collect();
let mut placed: Vec<Option<solidcraft_kernel::Body>> = vec![None; self.bodies.len()];
let mut out = Vec::new();
for (a, b, oa, ob) in &self.pairs {
let (Some(xa), Some(xb)) = (boxes.get(*a), boxes.get(*b)) else {
return Err(limit("invalid contact geometry pair"));
};
if !xa.meets(xb) {
continue;
}
*intersections = intersections.saturating_add(1);
if *intersections > MAX_INTERSECTIONS {
return Err(limit("motion exceeds contact sweep intersection budget; simplify the contact set or use smaller increments"));
}
let undecided = |e: &dyn std::fmt::Display| {
bad("contact.sweep", format!("whether {} and {} pass into each other could not be decided ({e})", oa.1, ob.1))
};
let (pa, pb) = (self.placed_body(*a, &frames, &mut placed)?, self.placed_body(*b, &frames, &mut placed)?);
let Some(shared) = solidcraft_kernel::boolean(&pa, &pb, solidcraft_kernel::BoolOp::Intersect).map_err(|e| undecided(&e))? else {
continue;
};
if solidcraft_kernel::measure_mass(&shared).map_err(|e| undecided(&e))?.volume > 0.0 {
out.push(Overlap { a: oa.1.clone(), b: ob.1.clone(), bodies: (*a, *b) });
if !all {
break;
}
}
}
Ok(out)
}
/// The least gap between the bodies of `pairs` in `s`, where they are clear of each other: their
/// exact distance ([`solidcraft_kernel::body_distance`]), less the meshes' chord tolerances
/// where it could only be measured on the meshes, so the gap is never more than the bodies'.
/// Between bodies that pass into each other it means nothing (a distance does not tell them
/// from touching ones): it is asked only of placements known to be clear.
pub(super) fn gap(&self, s: &Session, pairs: &[Overlap]) -> Result<f64> {
let frames = self.frames(s)?;
let mut placed: Vec<Option<solidcraft_kernel::Body>> = vec![None; self.bodies.len()];
let mut least = f64::INFINITY;
for pair in pairs {
let (a, b) = pair.bodies;
let (Some(ca), Some(cb), Some(fa), Some(fb)) = (self.bodies.get(a), self.bodies.get(b), frames.get(a), frames.get(b)) else {
return Err(limit("invalid contact geometry pair"));
};
let (pa, pb) = (self.placed_body(a, &frames, &mut placed)?, self.placed_body(b, &frames, &mut placed)?);
let mesh = |c: &CachedBody, f: &Mat| Mesh {
positions: c.mesh.positions.iter().map(|p| apply_point(f, *p)).collect(),
triangles: c.mesh.triangles.clone(),
..Mesh::default()
};
let (ma, mb) = (mesh(ca, fa), mesh(cb, fb));
let (sa, sb) = (solidcraft_kernel::DistanceShape::new(&pa, &ma, ca.chord), solidcraft_kernel::DistanceShape::new(&pb, &mb, cb.chord));
let Some(d) = solidcraft_kernel::body_distance(&sa, &sb, f64::INFINITY)? else {
return Err(limit("invalid contact geometry pair"));
};
let gap = if d.exact { d.distance } else { d.distance - ca.chord - cb.chord };
least = least.min(gap.max(0.0));
}
Ok(least)
}
/// The furthest a point of the bodies of `pairs` moves toward the other body of its pair
/// from where `from` ([`ContactGeometry::frames`]) placed them to where `to` has them
/// (bounded by the two bodies' own furthest moves, each by the corners of its exact box,
/// which a rigid move takes furthest).
pub(super) fn pair_travel(&self, from: &[Mat], to: &Session, pairs: &[Overlap]) -> Result<f64> {
let moved = |i: usize| -> Result<f64> {
let (Some(body), Some(fa)) = (self.bodies.get(i), from.get(i)) else {
return Err(limit("invalid contact geometry pair"));
};
let fb = Self::frame(body, to)?;
let local = &body.exact_box;
let mut most = 0.0_f64;
for c in 0..8 {
let pick = |bit: usize, lo: f64, hi: f64| if c & bit == 0 { lo } else { hi };
let corner = Vec3::new(pick(1, local.min.x, local.max.x), pick(2, local.min.y, local.max.y), pick(4, local.min.z, local.max.z));
most = most.max(apply_point(fa, corner).dist(apply_point(&fb, corner)));
}
Ok(most)
};
let mut most = 0.0_f64;
for pair in pairs {
most = most.max(moved(pair.bodies.0)? + moved(pair.bodies.1)?);
}
Ok(most)
}
/// Where `s` places each body.
pub(super) fn frames(&self, s: &Session) -> Result<Vec<Mat>> {
self.bodies.iter().map(|b| Self::frame(b, s)).collect()
}
/// Body `i` placed by `frames`, made once per check.
fn placed_body(&self, i: usize, frames: &[Mat], placed: &mut [Option<solidcraft_kernel::Body>]) -> Result<solidcraft_kernel::Body> {
if let Some(Some(done)) = placed.get(i) {
return Ok(done.clone());
}
let (Some(body), Some(frame)) = (self.bodies.get(i), frames.get(i)) else {
return Err(limit("invalid contact geometry pair"));
};
let moved = solidcraft_kernel::transform_matrix(&body.body, *frame)?;
if let Some(slot) = placed.get_mut(i) {
*slot = Some(moved.clone());
}
Ok(moved)
}
/// The pairs of set members whose bodies pass through each other in `s`, with how deep the
/// first two bodies found go (every pair, or the first found when `all` is false). `work`
/// adds up the cost, refused past `budget`.
pub(super) fn hits(&self, s: &Session, work: &mut usize, budget: usize, all: bool) -> Result<Vec<(Member, Member, f64)>> {
let mut out: Vec<(Member, Member, f64)> = Vec::new();
// The member pairs found so far (lower id first).
let mut found: BTreeSet<(u64, u64)> = BTreeSet::new();
let meshes = self.placed(s)?;
// Each mesh's box once, not once per pair it is in.
let boxes: Vec<Aabb3> = meshes.iter().map(Mesh::bounds).collect();
for (i, (_, _, oa, ob)) in self.pairs.iter().enumerate() {
// Members already found colliding need no more of their bodies tested.
if found.contains(&(oa.0.min(ob.0), oa.0.max(ob.0))) {
continue;
}
if let Some(depth) = self.depth(i, &meshes, &boxes, work, budget)? {
found.insert((oa.0.min(ob.0), oa.0.max(ob.0)));
out.push((oa.clone(), ob.clone(), depth));
if !all {
break;
}
}
}
Ok(out)
}
/// How deep the bodies of each pair pass into each other in `s` (0 where they don't), in
/// the order of the pairs. `work` adds up the cost, refused past `budget`.
pub(super) fn depths(&self, s: &Session, work: &mut usize, budget: usize) -> Result<Vec<f64>> {
let meshes = self.placed(s)?;
let boxes: Vec<Aabb3> = meshes.iter().map(Mesh::bounds).collect();
(0..self.pairs.len()).map(|i| Ok(self.depth(i, &meshes, &boxes, work, budget)?.unwrap_or(0.0))).collect()
}
/// The set members pair `i` is between.
pub(super) fn members(&self, i: usize) -> Option<(Member, Member)> {
self.pairs.get(i).map(|(_, _, a, b)| (a.clone(), b.clone()))
}
/// How far the meshes of pair `i` may stray from their faces (see [`ContactGeometry::chord`]).
pub(super) fn pair_chord(&self, i: usize) -> f64 {
let chord = |b: usize| self.bodies.get(b).map_or(0.0, |b| b.chord);
self.pairs.get(i).map_or(0.0, |(a, b, _, _)| chord(*a).max(chord(*b)))
}
/// Pair `i` alone (with the same bodies).
pub(super) fn only(&self, i: usize) -> ContactGeometry {
ContactGeometry { bodies: self.bodies.clone(), pairs: self.pairs.get(i).cloned().into_iter().collect(), step: self.step }
}
/// How deep the bodies of pair `i` (placed as `meshes`, in `boxes`) pass into each other,
/// when more than a touch.
fn depth(&self, i: usize, meshes: &[Mesh], boxes: &[Aabb3], work: &mut usize, budget: usize) -> Result<Option<f64>> {
let Some((a, b, _, _)) = self.pairs.get(i) else { return Err(limit("invalid contact geometry pair")) };
let (Some(ma), Some(mb), Some(ba), Some(bb)) = (meshes.get(*a), meshes.get(*b), self.bodies.get(*a), self.bodies.get(*b)) else {
return Err(limit("invalid contact geometry pair"));
};
let (Some(aabb), Some(bbb)) = (boxes.get(*a), boxes.get(*b)) else {
return Err(limit("invalid contact geometry pair"));
};
// Boxes apart, or only touching (parts end to end): `mesh_overlap` finds nothing there,
// so it costs nothing (#5968).
let tol = 1e-4 * ba.size.max(bb.size).max(1.0);
if aabb.max.x < bbb.min.x + tol
|| bbb.max.x < aabb.min.x + tol
|| aabb.max.y < bbb.min.y + tol
|| bbb.max.y < aabb.min.y + tol
|| aabb.max.z < bbb.min.z + tol
|| bbb.max.z < aabb.min.z + tol
{
return Ok(None);
}
let cost = ma
.positions
.len()
// 4 sample points and 3 edges to probe per triangle of `a` against each triangle of `b` (#3323), and the other way round.
.saturating_add(ma.triangles.len().saturating_mul(7))
.saturating_mul(mb.triangles.len())
.saturating_add(mb.positions.len().saturating_add(mb.triangles.len().saturating_mul(7)).saturating_mul(ma.triangles.len()));
*work = work.saturating_add(cost);
if *work > budget {
return Err(limit("motion exceeds contact sweep collision budget; simplify the contact set or use smaller increments"));
}
Ok(super::mesh_overlap(ma, mb, tol))
}
}
/// How near two set members come: the bodies under them nearest each other.
pub(super) struct Gap {
pub(super) a: Member,
pub(super) b: Member,
/// 0 where they touch or pass into each other.
pub(super) mm: f64,
/// The nearest points (world), on a's body and on b's.
pub(super) at: (Vec3, Vec3),
}
impl ContactGeometry {
/// The gap between each pair of set members whose bodies come within `cutoff` of each
/// other in `s` (all pairs with an infinite cutoff), nearest first. Mesh distances: exact
/// for flat faces, within [`ContactGeometry::chord`] for curved ones. `work` adds up the
/// cost, refused past `budget`.
pub(super) fn gaps(&self, s: &Session, cutoff: f64, work: &mut usize, budget: usize) -> Result<Vec<Gap>> {
let meshes = self.placed(s)?;
let tris: Vec<Vec<[Vec3; 3]>> = meshes.iter().map(|m| m.triangles.iter().filter_map(|t| m.tri(t)).collect()).collect();
let boxes: Vec<Aabb3> = meshes.iter().map(Mesh::bounds).collect();
let mut trees: Vec<Option<Bvh>> = vec![None; meshes.len()];
let mut nearest: BTreeMap<(u64, u64), Gap> = BTreeMap::new();
for (a, b, oa, ob) in &self.pairs {
let key = (oa.0.min(ob.0), oa.0.max(ob.0));
let limit = nearest.get(&key).map_or(cutoff, |g| g.mm.min(cutoff));
let (Some(ta), Some(tb), Some(ba), Some(bb)) = (tris.get(*a), tris.get(*b), boxes.get(*a), boxes.get(*b)) else {
return Err(limit_error("invalid contact geometry pair"));
};
if box_distance(ba, bb) > limit {
continue;
}
if trees.get(*b).is_some_and(Option::is_none)
&& let Some(slot) = trees.get_mut(*b)
{
*work = work.saturating_add(tb.len());
*slot = Some(Bvh::new(&tb.iter().map(tri_box).collect::<Vec<_>>()));
}
let Some(Some(tree)) = trees.get(*b) else { return Err(limit_error("invalid contact geometry pair")) };
let Some((mm, pa, pb)) = mesh_gap(ta, tb, tree, ba, bb, limit, work, budget)? else { continue };
nearest.insert(key, Gap { a: oa.clone(), b: ob.clone(), mm, at: (pa, pb) });
}
let mut out: Vec<Gap> = nearest.into_values().collect();
out.sort_by(|x, y| x.mm.total_cmp(&y.mm));
Ok(out)
}
/// The most the gaps can be off: the largest chord tolerance of a curved body (0 when every
/// body is flat-faced, and the mesh distances are exact).
pub(super) fn chord(&self) -> f64 {
self.bodies.iter().map(|b| b.chord).fold(0.0, f64::max)
}
/// The size of the largest body (at least 1 mm): what tolerances scale with.
pub(super) fn scale(&self) -> f64 {
self.bodies.iter().map(|b| b.size).fold(1.0, f64::max)
}
/// The furthest any point of the bodies moves between `from` and `to` (bounded by the
/// corners of each body's box, which a rigid move takes furthest).
pub(super) fn travel(&self, from: &Document, to: &Document) -> Result<f64> {
let mut most = 0.0_f64;
for body in &self.bodies {
let (fa, fb) = (Self::frame_in(body, from)?, Self::frame_in(body, to)?);
let local = body.mesh.bounds();
for c in 0..8 {
let pick = |bit: usize, lo: f64, hi: f64| if c & bit == 0 { lo } else { hi };
let corner = Vec3::new(pick(1, local.min.x, local.max.x), pick(2, local.min.y, local.max.y), pick(4, local.min.z, local.max.z));
most = most.max(apply_point(&fa, corner).dist(apply_point(&fb, corner)));
}
}
Ok(most)
}
}
/// The box holding `local` placed by `frame` (its corners' box: a rigid motion keeps the box's
/// contents inside it).
fn placed_box(local: &Aabb3, frame: &Mat) -> Aabb3 {
let mut world = Aabb3::EMPTY;
for c in 0..8 {
let pick = |bit: usize, lo: f64, hi: f64| if c & bit == 0 { lo } else { hi };
world.add(apply_point(
frame,
Vec3::new(pick(1, local.min.x, local.max.x), pick(2, local.min.y, local.max.y), pick(4, local.min.z, local.max.z)),
));
}
world
}
fn limit_error(message: &str) -> crate::EngineError {
limit(message)
}
fn tri_box(t: &[Vec3; 3]) -> Aabb3 {
let mut b = Aabb3::EMPTY;
for p in t {
b.add(*p);
}
b
}
/// The distance between two boxes (0 where they overlap).
fn box_distance(a: &Aabb3, b: &Aabb3) -> f64 {
let axis = |a0: f64, a1: f64, b0: f64, b1: f64| (b0 - a1).max(a0 - b1).max(0.0);
let (x, y, z) = (axis(a.min.x, a.max.x, b.min.x, b.max.x), axis(a.min.y, a.max.y, b.min.y, b.max.y), axis(a.min.z, a.max.z, b.min.z, b.max.z));
(x * x + y * y + z * z).sqrt()
}
/// The gap between meshes `a` and `b` (their triangles; `tree` over b's) and their nearest
/// points, when within `limit` (None: farther). 0 where their surfaces cross, and where one
/// lies wholly inside the other.
#[allow(clippy::too_many_arguments)]
fn mesh_gap(
a: &[[Vec3; 3]],
b: &[[Vec3; 3]],
tree: &Bvh,
ba: &Aabb3,
bb: &Aabb3,
limit: f64,
work: &mut usize,
budget: usize,
) -> Result<Option<(f64, Vec3, Vec3)>> {
let mut best: Option<(f64, Vec3, Vec3)> = None;
// An upper bound to prune with when there is no limit: a vertex of each near the other.
let mut bound = limit;
if !bound.is_finite() {
let centre = bb.center();
let near = |list: &[[Vec3; 3]], to: Vec3| list.iter().map(|t| t[0]).min_by(|p, q| p.dist(to).total_cmp(&q.dist(to)));
if let Some(pa) = near(a, centre)
&& let Some(pb) = near(b, pa)
{
*work = work.saturating_add(a.len() + b.len());
bound = pa.dist(pb);
best = Some((bound, pa, pb));
}
}
for ta in a {
let box_a = tri_box(ta);
if box_distance(&box_a, bb) > bound {
continue;
}
// Both callbacks read the bound the other tightens.
let (visits, shared) = (Cell::new(0usize), Cell::new(bound));
tree.visit(
|node| {
visits.set(visits.get() + 1);
box_distance(node, &box_a) <= shared.get()
},
|j| {
let Some(tb) = b.get(j as usize) else { return };
visits.set(visits.get() + 8);
let (d, pa, pb) = tri_distance(ta, tb);
if d <= shared.get() && best.is_none_or(|(x, _, _)| d < x) {
shared.set(d);
best = Some((d, pa, pb));
}
},
);
bound = shared.get();
let visits = visits.get();
*work = work.saturating_add(visits);
if *work > budget {
return Err(limit_error("gaps exceed the work budget; check fewer parts or leave out report_gaps"));
}
if bound <= 0.0 {
return Ok(best);
}
}
// Wholly inside, no surface crossing: a vertex of one inside the other.
let overlap =
ba.min.x <= bb.max.x && bb.min.x <= ba.max.x && ba.min.y <= bb.max.y && bb.min.y <= ba.max.y && ba.min.z <= bb.max.z && bb.min.z <= ba.max.z;
if overlap {
*work = work.saturating_add(a.len() + b.len());
for (p, other) in [(a.first().map(|t| t[0]), b), (b.first().map(|t| t[0]), a)] {
if let Some(p) = p
&& super::inside(p, other)
{
return Ok(Some((0.0, p, p)));
}
}
}
Ok(best.filter(|(d, _, _)| *d <= limit))
}
/// The distance between triangles `a` and `b`, with their nearest points (0 where they cross).
fn tri_distance(a: &[Vec3; 3], b: &[Vec3; 3]) -> (f64, Vec3, Vec3) {
for (x, y) in [(a, b), (b, a)] {
for i in 0..3 {
if let (Some(p), Some(q)) = (x.get(i), x.get((i + 1) % 3))
&& let Some(hit) = segment_hits_triangle(*p, *q, y)
{
return (0.0, hit, hit);
}
}
}
let mut best = (f64::INFINITY, a[0], b[0]);
let mut keep = |pa: Vec3, pb: Vec3| {
let d = pa.dist(pb);
if d < best.0 {
best = (d, pa, pb);
}
};
for p in a {
keep(*p, super::closest(*p, b[0], b[1], b[2]));
}
for q in b {
keep(super::closest(*q, a[0], a[1], a[2]), *q);
}
for i in 0..3 {
for j in 0..3 {
let (pa, pb) = segment_closest(a[i], a[(i + 1) % 3], b[j], b[(j + 1) % 3]);
keep(pa, pb);
}
}
best
}
/// Where the segment `p`→`q` hits triangle `t` (endpoints count), if it crosses its plane.
pub(super) fn segment_hits_triangle(p: Vec3, q: Vec3, t: &[Vec3; 3]) -> Option<Vec3> {
let d = q - p;
let (e1, e2) = (t[1] - t[0], t[2] - t[0]);
let h = d.cross(e2);
let det = e1.dot(h);
// Parallel (or degenerate): the vertex and edge distances answer it.
if det.abs() < 1e-12 * e1.len().max(1e-300) * e2.len().max(1e-300) * d.len().max(1e-300) {
return None;
}
let f = 1.0 / det;
let sv = p - t[0];
let u = f * sv.dot(h);
if !(0.0..=1.0).contains(&u) {
return None;
}
let qv = sv.cross(e1);
let v = f * d.dot(qv);
if v < 0.0 || u + v > 1.0 {
return None;
}
let along = f * e2.dot(qv);
(0.0..=1.0).contains(&along).then(|| p + d * along)
}
/// The nearest points of segments p1q1 and p2q2.
fn segment_closest(p1: Vec3, q1: Vec3, p2: Vec3, q2: Vec3) -> (Vec3, Vec3) {
let (d1, d2, r) = (q1 - p1, q2 - p2, p1 - p2);
let (a, e, f) = (d1.dot(d1), d2.dot(d2), d2.dot(r));
let eps = 1e-24;
let (s, t) = if a <= eps && e <= eps {
(0.0, 0.0)
} else if a <= eps {
(0.0, (f / e).clamp(0.0, 1.0))
} else {
let c = d1.dot(r);
if e <= eps {
((-c / a).clamp(0.0, 1.0), 0.0)
} else {
let b = d1.dot(d2);
let denom = a * e - b * b;
let mut s = if denom > eps * a * e { ((b * f - c * e) / denom).clamp(0.0, 1.0) } else { 0.0 };
let mut t = (b * s + f) / e;
if t < 0.0 {
t = 0.0;
s = (-c / a).clamp(0.0, 1.0);
} else if t > 1.0 {
t = 1.0;
s = ((b - c) / a).clamp(0.0, 1.0);
}
(s, t)
}
};
(p1 + d1 * s, p2 + d2 * t)
}
#[cfg(test)]
#[path = "contact_sweep_tests.rs"]
mod tests;