use std::collections::BinaryHeap;
use crate::{
Result, boolean,
distance::{self, Selection},
math::{Aabb, Point3, Transform, Vec3},
measure,
mesh::{
Mesh, MeshOptions,
bvh::{Bvh, moved_box, on_triangle, ray_triangle, triangles},
tessellate,
},
tol,
topo::Brep,
};
const MESHES: [MeshOptions; 3] = [
MeshOptions {
linear: 0.01,
angular: 0.2,
},
MeshOptions {
linear: 0.05,
angular: 0.5,
},
MeshOptions {
linear: 0.2,
angular: 0.8,
},
];
pub const TOUCHING_MM3: f64 = 1e-9;
const ROUGH_VOLUME: f64 = 1e-10;
const SLIVER_MM3: f64 = 1e-6;
pub struct Body {
brep: Brep,
mesh: Mesh,
part: Vec<u32>,
solids: u32,
bvh: Bvh,
deflection: f64,
bbox: Aabb,
volume: std::sync::OnceLock<f64>,
}
impl Body {
pub fn new(brep: Brep) -> Result<Body> {
for f in brep.face_ids() {
if let crate::geom::Surface::Nurbs(n) = &brep.face(f).surface {
n.prepare();
}
}
let mut last = None;
for opt in MESHES {
match tessellate(&brep, &opt) {
Ok(mesh) => {
let bvh = Bvh::new(&mesh);
let mut bbox = Aabb::EMPTY;
for p in &mesh.positions {
bbox.add(*p);
}
return Ok(Body {
part: vec![0; mesh.triangles.len()],
solids: 1,
brep,
mesh,
bvh,
deflection: opt.linear,
bbox,
volume: std::sync::OnceLock::new(),
});
}
Err(e) => last = Some(e),
}
}
Err(last.expect("MESHES is not empty"))
}
pub fn brep(&self) -> &Brep {
&self.brep
}
pub fn joined(parts: &[&Body]) -> Body {
let mut brep = Brep::default();
let mut mesh = Mesh::default();
let mut part = Vec::new();
let mut solids = 0;
let mut bbox = Aabb::EMPTY;
let mut deflection: f64 = 0.0;
for b in parts {
brep.append(&b.brep);
let base = mesh.positions.len() as u32;
mesh.positions.extend_from_slice(&b.mesh.positions);
mesh.triangles
.extend(b.mesh.triangles.iter().map(|t| t.map(|i| i + base)));
part.extend(b.part.iter().map(|k| solids + k));
solids += b.solids;
bbox.union(&b.bbox);
deflection = deflection.max(b.deflection);
}
Body {
bvh: Bvh::new(&mesh),
brep,
mesh,
part,
solids,
deflection,
bbox,
volume: std::sync::OnceLock::new(),
}
}
pub fn moved(&self, t: &Transform) -> Body {
let mut brep = self.brep.clone();
brep.transform(t);
let mut mesh = self.mesh.clone();
for p in &mut mesh.positions {
*p = t.point(*p);
}
let mut bbox = Aabb::EMPTY;
for p in &mesh.positions {
bbox.add(*p);
}
let volume = std::sync::OnceLock::new();
if let Some(&v) = self.volume.get() {
let _ = volume.set(v);
}
Body {
bvh: Bvh::new(&mesh),
brep,
mesh,
part: self.part.clone(),
solids: self.solids,
deflection: self.deflection,
bbox,
volume,
}
}
pub fn points(&self) -> &[Point3] {
&self.mesh.positions
}
fn clear_of(&self, p: Point3, r: f64) -> bool {
let mut near = Vec::new();
self.bvh.within(&Aabb { min: p, max: p }, r, &mut near);
near.iter().all(|&i| {
let t = self.triangle(i);
on_triangle(p, &t).dist(p) > r
})
}
fn volume(&self) -> f64 {
*self.volume.get_or_init(|| measure::volume(&self.brep))
}
fn triangle(&self, i: usize) -> [Point3; 3] {
let t = self.mesh.triangles[i];
[0, 1, 2].map(|k| self.mesh.positions[t[k] as usize])
}
fn holds(&self, p: Point3) -> bool {
const RAYS: [Vec3; 3] = [
Vec3::new(0.5773502691896258, 0.5773502691896258, 0.5773502691896258),
Vec3::new(-0.2672612419124244, 0.5345224838248488, 0.8017837257372732),
Vec3::new(0.8164965809277261, -0.4082482904638631, 0.4082482904638631),
];
let reach = 2.0 * (self.bbox.max - self.bbox.min).norm() + 1.0;
let mut out = Vec::new();
let votes = RAYS
.iter()
.filter(|&&d| {
self.bvh.along(p, d, (0.0, reach), 0.0, &mut out);
let hits = out
.iter()
.filter(|&&i| {
let [a, b, c] = self.triangle(i);
ray_triangle(p, d, a, b, c).is_some_and(|s| s > 0.0)
})
.count();
hits % 2 == 1
})
.count();
votes >= 2
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub enum Overlap {
Apart,
Touching,
Shared(f64),
}
impl Overlap {
pub fn volume(&self) -> f64 {
match self {
Overlap::Shared(v) => *v,
_ => 0.0,
}
}
}
pub fn overlap(a: &Body, b: &Body, rel: &Transform) -> Result<Overlap> {
let margin = 2.0 * (a.deflection + b.deflection);
let moved = moved_box(&b.bbox, rel);
let boxes_meet = crate::mesh::bvh::box_gap(&a.bbox, &moved) <= margin;
let near = boxes_meet
&& a.bvh.any_within(&b.bvh, rel, margin, |i, j| {
let tb = b.triangle(j).map(|p| rel.point(p));
triangles(&a.triangle(i), &tb) <= margin
});
if !near {
if !boxes_meet {
return Ok(Overlap::Apart);
}
if let Some(&p) = b.mesh.positions.first()
&& a.holds(rel.point(p))
{
return Ok(Overlap::Shared(b.volume()));
}
if let Some(&p) = a.mesh.positions.first()
&& b.holds(rel.inverse().point(p))
{
return Ok(Overlap::Shared(a.volume()));
}
return Ok(Overlap::Apart);
}
let back = rel.inverse();
let reach = moved_box(&a.bbox, &back);
for f in b.brep.face_ids() {
if crate::mesh::bvh::box_gap(&measure::face_bbox(&b.brep, f), &reach) <= margin {
measure::prime_face_volume(&b.brep, f);
}
}
let mut placed = b.brep.clone();
placed.transform(rel);
let shared = boolean::intersection(&a.brep, &placed)?;
let mut v = measure::volume_to(&shared, ROUGH_VOLUME);
if v < SLIVER_MM3 {
v = measure::volume(&shared);
}
Ok(if v > TOUCHING_MM3 {
Overlap::Shared(v)
} else {
Overlap::Touching
})
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub enum Meeting {
Shared(f64),
Touching,
Apart(f64),
}
pub fn meeting(a: &Body, b: &Body, rel: &Transform) -> Result<Meeting> {
if let Overlap::Shared(v) = overlap(a, b, rel)? {
return Ok(Meeting::Shared(v));
}
let mut placed = b.brep.clone();
placed.transform(rel);
let gap = distance::closest(
&a.brep,
&Selection::body(&a.brep),
&placed,
&Selection::body(&placed),
)?
.distance;
Ok(if gap <= tol::LINEAR {
Meeting::Touching
} else {
Meeting::Apart(gap)
})
}
pub fn between(a: &Brep, b: &Brep) -> Result<Meeting> {
meeting(
&Body::new(a.clone())?,
&Body::new(b.clone())?,
&Transform::IDENTITY,
)
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Sweep {
pub inside: f64,
pub cell: f64,
pub slack: f64,
}
impl Default for Sweep {
fn default() -> Sweep {
Sweep {
inside: 0.0,
cell: 0.1,
slack: 0.0,
}
}
}
struct Seen {
uv: [[f64; 2]; 3],
w: [f64; 3],
lo: [f64; 2],
hi: [f64; 2],
front: bool,
part: u32,
own: (f64, f64),
slope: Option<[f64; 2]>,
}
impl Seen {
fn new(t: &[Point3; 3], part: u32, [u, v, d]: [Vec3; 3]) -> Seen {
let at = |p: Point3| {
let x = p - Point3::new(0.0, 0.0, 0.0);
([x.dot(u), x.dot(v)], x.dot(d))
};
let (c, w): (Vec<[f64; 2]>, Vec<f64>) = t.iter().map(|&p| at(p)).unzip();
let uv = [c[0], c[1], c[2]];
let lo = [0, 1].map(|k| uv[0][k].min(uv[1][k]).min(uv[2][k]));
let hi = [0, 1].map(|k| uv[0][k].max(uv[1][k]).max(uv[2][k]));
let (e1, e2) = (t[1] - t[0], t[2] - t[0]);
let facing = e1.cross(e2).dot(d);
let span = e1.norm().max(e2.norm());
let w = [w[0], w[1], w[2]];
let own = (w[0].min(w[1]).min(w[2]), w[0].max(w[1]).max(w[2]));
let [p, q, r] = uv;
let (f1, f2) = ([q[0] - p[0], q[1] - p[1]], [r[0] - p[0], r[1] - p[1]]);
let det = f1[0] * f2[1] - f1[1] * f2[0];
let reach = (hi[0] - lo[0]).max(hi[1] - lo[1]);
let slope = (det.abs() > 1e-12 * reach * reach.max(1.0)).then(|| {
let (dw1, dw2) = (w[1] - w[0], w[2] - w[0]);
[
(dw1 * f2[1] - dw2 * f1[1]) / det,
(dw2 * f1[0] - dw1 * f2[0]) / det,
]
});
Seen {
uv,
w,
lo,
hi,
front: facing >= -1e-9 * span * span,
part,
own,
slope,
}
}
fn meets(&self, cell: &Cell) -> bool {
if self.lo[0] > cell.hi[0]
|| self.hi[0] < cell.lo[0]
|| self.lo[1] > cell.hi[1]
|| self.hi[1] < cell.lo[1]
{
return false;
}
let corners = [
[cell.lo[0], cell.lo[1]],
[cell.hi[0], cell.lo[1]],
[cell.lo[0], cell.hi[1]],
[cell.hi[0], cell.hi[1]],
];
(0..3).all(|k| {
let (p, q) = (self.uv[k], self.uv[(k + 1) % 3]);
let n = [q[1] - p[1], p[0] - q[0]];
if n[0] == 0.0 && n[1] == 0.0 {
return true;
}
let side = |c: [f64; 2]| n[0] * (c[0] - p[0]) + n[1] * (c[1] - p[1]);
let tri = self.uv.map(side);
let (t0, t1) = (
tri[0].min(tri[1]).min(tri[2]),
tri[0].max(tri[1]).max(tri[2]),
);
let box_side = corners.map(side);
let (b0, b1) = (
box_side.iter().copied().fold(f64::INFINITY, f64::min),
box_side.iter().copied().fold(f64::NEG_INFINITY, f64::max),
);
b0 <= t1 && b1 >= t0
})
}
fn depth_over(&self, cell: &Cell) -> (f64, f64) {
let Some([a, b]) = self.slope else {
return self.own;
};
let p = self.uv[0];
let base = self.w[0] - a * p[0] - b * p[1];
let (au, bv) = (
(a * cell.lo[0], a * cell.hi[0]),
(b * cell.lo[1], b * cell.hi[1]),
);
let lo = base + au.0.min(au.1) + bv.0.min(bv.1);
let hi = base + au.0.max(au.1) + bv.0.max(bv.1);
(self.own.0.max(lo), self.own.1.min(hi))
}
fn crossing(&self, x: f64, y: f64) -> Option<f64> {
let [p, q, r] = self.uv;
let (e1, e2) = ([q[0] - p[0], q[1] - p[1]], [r[0] - p[0], r[1] - p[1]]);
let det = e1[0] * e2[1] - e1[1] * e2[0];
if det.abs() <= 1e-300 {
return None;
}
let (dx, dy) = (x - p[0], y - p[1]);
let s = (dx * e2[1] - dy * e2[0]) / det;
let t = (e1[0] * dy - e1[1] * dx) / det;
(s >= 0.0 && t >= 0.0 && s + t <= 1.0)
.then(|| self.w[0] + s * (self.w[1] - self.w[0]) + t * (self.w[2] - self.w[0]))
}
}
struct Cell {
lo: [f64; 2],
hi: [f64; 2],
a: Vec<u32>,
b: Vec<u32>,
soonest: f64,
}
impl PartialEq for Cell {
fn eq(&self, other: &Cell) -> bool {
self.cmp(other).is_eq()
}
}
impl Eq for Cell {}
impl PartialOrd for Cell {
fn partial_cmp(&self, other: &Cell) -> Option<std::cmp::Ordering> {
Some(self.cmp(other))
}
}
impl Ord for Cell {
fn cmp(&self, other: &Cell) -> std::cmp::Ordering {
other
.soonest
.total_cmp(&self.soonest)
.then(other.lo[0].total_cmp(&self.lo[0]))
.then(other.lo[1].total_cmp(&self.lo[1]))
}
}
pub fn swept_contact(a: &Body, b: &Body, d: Vec3, how: &Sweep) -> Option<f64> {
swept_contacts(&[a], &[b], d, how)[0]
}
pub fn swept_contacts(moving: &[&Body], fixed: &[&Body], d: Vec3, how: &Sweep) -> Vec<Option<f64>> {
let Some(frame) = across(d) else {
return vec![None; fixed.len()];
};
let d = frame[2];
let boxed = |bodies: &[&Body]| {
let mut all = Aabb::EMPTY;
for body in bodies {
all.union(&body.bbox);
}
all
};
let (reach, from) = (boxed(fixed), boxed(moving));
let mut sa = Vec::new();
let mut first_part = 0;
for body in moving {
let mut near = Vec::new();
body.bvh.along_line(&reach, d, &mut near);
sa.extend(
near.iter()
.map(|&i| Seen::new(&body.triangle(i), first_part + body.part[i], frame)),
);
first_part += body.solids;
}
let ra = moving
.iter()
.map(|b| 1.01 * b.deflection)
.fold(how.inside, f64::max);
let clear_of_moving = |p: Point3| moving.iter().all(|b| b.clear_of(p, ra));
fixed
.iter()
.map(|body| {
let mut near = Vec::new();
body.bvh.along_line(&from, d, &mut near);
let sb: Vec<Seen> = near
.iter()
.map(|&i| Seen::new(&body.triangle(i), body.part[i], frame))
.collect();
let rb = (1.01 * body.deflection).max(how.inside);
first_contact(&sa, &sb, frame, (ra, rb), how, &clear_of_moving, &|p| {
body.clear_of(p, rb)
})
})
.collect()
}
fn across(d: Vec3) -> Option<[Vec3; 3]> {
let d = d.unit()?;
let u = if d.x.abs() < 0.9 { Vec3::X } else { Vec3::Y };
let u = (u - d * u.dot(d)).unit()?;
Some([u, d.cross(u), d])
}
fn first_contact(
sa: &[Seen],
sb: &[Seen],
[u, v, d]: [Vec3; 3],
(ra, rb): (f64, f64),
how: &Sweep,
clear_a: &dyn Fn(Point3) -> bool,
clear_b: &dyn Fn(Point3) -> bool,
) -> Option<f64> {
if sb.is_empty() {
return None;
}
let shadow = |s: &[Seen], of: &[u32]| {
let mut lo = [f64::INFINITY; 2];
let mut hi = [f64::NEG_INFINITY; 2];
for &i in of {
let t = &s[i as usize];
for k in 0..2 {
lo[k] = lo[k].min(t.lo[k]);
hi[k] = hi[k].max(t.hi[k]);
}
}
(lo, hi)
};
let all_b: Vec<u32> = (0..sb.len() as u32).collect();
let (lb, hb) = shadow(sb, &all_b);
let all_a: Vec<u32> = (0..sa.len() as u32)
.filter(|&i| {
let t = &sa[i as usize];
t.lo[0] <= hb[0] && t.hi[0] >= lb[0] && t.lo[1] <= hb[1] && t.hi[1] >= lb[1]
})
.collect();
if all_a.is_empty() {
return None;
}
let (la, ha) = shadow(sa, &all_a);
let lo = [la[0].max(lb[0]), la[1].max(lb[1])];
let hi = [ha[0].min(hb[0]), ha[1].min(hb[1])];
if hi[0] <= lo[0] || hi[1] <= lo[1] {
return None;
}
let make = |lo: [f64; 2], hi: [f64; 2], pa: &[u32], pb: &[u32]| -> Option<Cell> {
let mut cell = Cell {
lo,
hi,
a: Vec::new(),
b: Vec::new(),
soonest: 0.0,
};
let mut ka = Vec::new();
let (mut a_near, mut a_far) = (f64::INFINITY, f64::NEG_INFINITY);
for &i in pa {
let t = &sa[i as usize];
if t.meets(&cell) {
ka.push(i);
if t.front {
let (x, y) = t.depth_over(&cell);
a_near = a_near.min(x);
a_far = a_far.max(y);
}
}
}
if ka.is_empty() || a_near > a_far {
return None;
}
let mut kb = Vec::new();
let mut ahead = f64::INFINITY;
for &i in pb {
let t = &sb[i as usize];
if t.meets(&cell) {
kb.push(i);
let (near, far) = t.depth_over(&cell);
if far > a_near {
ahead = ahead.min(near);
}
}
}
if !ahead.is_finite() {
return None;
}
cell.a = ka;
cell.b = kb;
cell.soonest = (ahead - a_far).max(0.0);
Some(cell)
};
let mut queue = BinaryHeap::new();
queue.extend(make(lo, hi, &all_a, &all_b));
let mut best = f64::INFINITY;
while let Some(cell) = queue.pop() {
if cell.soonest >= best - how.slack {
break;
}
let size = (cell.hi[0] - cell.lo[0]).max(cell.hi[1] - cell.lo[1]);
if size > how.cell {
let halves = |k: usize| {
let (lo, hi) = (cell.lo[k], cell.hi[k]);
if hi - lo > how.cell {
let mid = 0.5 * (lo + hi);
vec![(lo, mid), (mid, hi)]
} else {
vec![(lo, hi)]
}
};
for (x0, x1) in halves(0) {
for (y0, y1) in halves(1) {
queue.extend(make([x0, y0], [x1, y1], &cell.a, &cell.b));
}
}
continue;
}
let x = cell.lo[0] + (0.5 + 0.0618) * (cell.hi[0] - cell.lo[0]);
let y = cell.lo[1] + (0.5 - 0.0414) * (cell.hi[1] - cell.lo[1]);
let (Some(on_a), Some(on_b)) = (stretches(sa, &cell.a, x, y), stretches(sb, &cell.b, x, y))
else {
continue;
};
let place = Point3::new(0.0, 0.0, 0.0) + u * x + v * y;
if let Some(t) = ray_contact(
&on_a,
&on_b,
|at, travel| {
let p = place + d * at;
clear_b(p) && clear_a(p - d * travel)
},
2.0 * (ra + rb),
) {
best = best.min(t);
}
}
best.is_finite().then_some(best)
}
fn stretches(s: &[Seen], tris: &[u32], x: f64, y: f64) -> Option<Vec<(f64, f64)>> {
let mut ws: Vec<(u32, f64)> = tris
.iter()
.filter_map(|&i| {
let t = &s[i as usize];
Some((t.part, t.crossing(x, y)?))
})
.collect();
ws.sort_by(|p, q| p.0.cmp(&q.0).then(p.1.total_cmp(&q.1)));
ws.dedup_by(|p, q| p.0 == q.0 && p.1 - q.1 <= 1e-9);
let mut out: Vec<(f64, f64)> = Vec::new();
for run in ws.chunk_by(|p, q| p.0 == q.0) {
if !run.len().is_multiple_of(2) {
return None;
}
out.extend(run.chunks(2).map(|c| (c[0].1, c[1].1)));
}
out.sort_by(|p, q| p.0.total_cmp(&q.0));
let mut merged: Vec<(f64, f64)> = Vec::new();
for (lo, hi) in out {
match merged.last_mut() {
Some(last) if lo <= last.1 + 1e-9 => last.1 = last.1.max(hi),
_ => merged.push((lo, hi)),
}
}
Some(merged)
}
fn ray_contact(
on_a: &[(f64, f64)],
on_b: &[(f64, f64)],
certain: impl Fn(f64, f64) -> bool,
first: f64,
) -> Option<f64> {
let mut tries: Vec<(f64, f64, f64)> = Vec::new();
for &(p0, lead) in on_a {
for &(q0, q1) in on_b {
if q1 <= lead {
continue;
}
let from = q0.max(lead);
let mut depth = first;
while from + depth < q1 && depth <= lead - p0 {
tries.push((from - lead, from + depth - lead, from + 0.5 * depth));
depth *= 2.0;
}
}
}
tries.sort_by(|x, y| x.0.total_cmp(&y.0).then(x.1.total_cmp(&y.1)));
let shared = |t: f64| -> f64 {
let mut total = 0.0;
for &(p0, p1) in on_a {
for &(q0, q1) in on_b {
total += ((p1 + t).min(q1) - (p0 + t).max(q0)).max(0.0);
}
}
total
};
let start = shared(0.0);
tries
.into_iter()
.find(|&(contact, travel, at)| {
shared(travel) - start >= 0.5 * (travel - contact) && certain(at, travel)
})
.map(|t| t.0)
}
#[cfg(test)]
mod tests {
use super::*;
use crate::{
math::{Frame, Vec2},
profile::Profile,
sweep::extrude,
};
fn solid(lo: [f64; 3], hi: [f64; 3]) -> Brep {
let c = Vec2::new(0.5 * (lo[0] + hi[0]), 0.5 * (lo[1] + hi[1]));
let frame = Frame::new(Vec3::new(0.0, 0.0, lo[2]), Vec3::Z, Vec3::X).unwrap();
let p = Profile::rect(c, hi[0] - lo[0], hi[1] - lo[1]);
extrude(&p, &frame, hi[2] - lo[2], "k").unwrap()
}
fn block(lo: [f64; 3], hi: [f64; 3]) -> Body {
Body::new(solid(lo, hi)).unwrap()
}
fn moved(x: f64, y: f64, z: f64) -> Transform {
Transform::translation(Vec3::new(x, y, z))
}
#[test]
fn blocks_apart_touching_and_sharing() {
let a = block([0.0, 0.0, 0.0], [10.0, 10.0, 10.0]);
let b = block([0.0, 0.0, 0.0], [4.0, 4.0, 4.0]);
assert_eq!(
overlap(&a, &b, &moved(11.0, 3.0, 3.0)).unwrap(),
Overlap::Apart
);
assert_eq!(
overlap(&a, &b, &moved(10.0, 3.0, 3.0)).unwrap(),
Overlap::Touching
);
let v = overlap(&a, &b, &moved(8.5, 3.0, 3.0)).unwrap().volume();
assert!((v - 24.0).abs() < 1e-9, "{v}");
}
#[test]
fn a_meeting_measures_a_gap_the_meshes_cannot_see() {
let a = block([0.0, 0.0, 0.0], [10.0, 10.0, 10.0]);
let b = block([0.0, 0.0, 0.0], [4.0, 4.0, 4.0]);
let Meeting::Apart(gap) = meeting(&a, &b, &moved(10.0 + 1e-5, 3.0, 3.0)).unwrap() else {
panic!("a clear pair read as meeting");
};
assert!((gap - 1e-5).abs() < 1e-9, "{gap}");
assert_eq!(
meeting(&a, &b, &moved(10.0, 3.0, 3.0)).unwrap(),
Meeting::Touching
);
let Meeting::Apart(far) = meeting(&a, &b, &moved(13.0, 3.0, 3.0)).unwrap() else {
panic!("a clear pair read as meeting");
};
assert!((far - 3.0).abs() < 1e-9, "{far}");
let shared = between(
&solid([0.0, 0.0, 0.0], [10.0, 10.0, 10.0]),
&solid([8.5, 3.0, 3.0], [12.5, 7.0, 7.0]),
)
.unwrap();
let Meeting::Shared(v) = shared else {
panic!("an overlapping pair read as {shared:?}");
};
assert!((v - 24.0).abs() < 1e-9, "{v}");
}
#[test]
fn a_block_buried_whole_shares_its_volume() {
let a = block([0.0, 0.0, 0.0], [10.0, 10.0, 10.0]);
let b = block([0.0, 0.0, 0.0], [4.0, 4.0, 4.0]);
let inside = moved(3.0, 3.0, 3.0);
for v in [
overlap(&a, &b, &inside).unwrap().volume(),
overlap(&b, &a, &inside.inverse()).unwrap().volume(),
] {
assert!((v - 64.0).abs() < 1e-9, "{v}");
}
}
#[test]
fn a_block_in_a_notch_inside_its_box_but_clear_of_it_is_apart() {
let lower = solid([0.0, 0.0, 0.0], [10.0, 5.0, 4.0]);
let upper = solid([0.0, 5.0, 0.0], [5.0, 10.0, 4.0]);
let l = Body::new(boolean::union(&lower, &upper).unwrap()).unwrap();
let cube = block([0.0, 0.0, 0.0], [2.0, 2.0, 2.0]);
assert_eq!(
overlap(&l, &cube, &moved(6.5, 6.5, 1.0)).unwrap(),
Overlap::Apart
);
}
#[test]
fn a_swept_block_runs_into_a_wall_after_its_gap_and_never_backing_away() {
let a = block([0.0, 0.0, 0.0], [2.0, 2.0, 2.0]);
let wall = block([5.0, -1.0, -1.0], [6.0, 3.0, 3.0]);
let ahead = swept_contact(&a, &wall, Vec3::X, &Sweep::default()).unwrap();
assert!((ahead - 3.0).abs() < 1e-9, "{ahead}");
assert_eq!(swept_contact(&a, &wall, -Vec3::X, &Sweep::default()), None);
assert_eq!(swept_contact(&a, &wall, Vec3::Y, &Sweep::default()), None);
}
#[test]
fn a_swept_shaft_slides_on_through_a_bore_its_own_size_but_not_sideways() {
let plate = solid([-5.0, -5.0, 0.0], [5.0, 5.0, 4.0]);
let axis = Frame::new(Vec3::new(0.0, 0.0, -3.0), Vec3::Z, Vec3::X).unwrap();
let hole = crate::primitive::cylinder(&axis, 2.0, 10.0, "h").unwrap();
let plate = Body::new(crate::boolean::difference(&plate, &hole).unwrap()).unwrap();
let mut shaft = crate::primitive::cylinder(&axis, 2.0, 5.0, "s").unwrap();
shaft.transform(&Transform::rotation(Vec3::ZERO, Vec3::Z, 0.137));
let shaft = Body::new(shaft).unwrap();
assert_eq!(
swept_contact(&shaft, &plate, Vec3::Z, &Sweep::default()),
None
);
assert_eq!(
swept_contact(&shaft, &plate, -Vec3::Z, &Sweep::default()),
None
);
let hit = swept_contact(&shaft, &plate, Vec3::X, &Sweep::default()).unwrap();
assert!(hit < 1e-6, "{hit}");
let tilted = Vec3::new(0.05, 0.0, 1.0);
assert!(swept_contact(&shaft, &plate, tilted, &Sweep::default()).is_some_and(|t| t < 1e-6));
}
#[test]
fn a_swept_move_counts_only_what_it_adds_to_an_overlap_in_place() {
let a = block([0.0, 0.0, 0.0], [3.0, 2.0, 2.0]);
let b = block([2.0, 0.0, 0.0], [6.0, 2.0, 2.0]);
assert_eq!(swept_contact(&a, &b, -Vec3::X, &Sweep::default()), None);
let on = swept_contact(&a, &b, Vec3::X, &Sweep::default()).unwrap();
assert!(on < 1e-9, "{on}");
}
#[test]
fn a_swept_ball_runs_into_a_slanted_block_where_its_face_first_touches_it() {
let c = Vec3::new(1.0, 1.2, 0.9);
let f = Frame::new(c, Vec3::Z, Vec3::X).unwrap();
let ball = Body::new(crate::primitive::sphere(&f, 1.3, "s").unwrap()).unwrap();
let turn = 0.4_f64;
let mut slab = solid([4.0, -2.0, -2.0], [5.0, 4.0, 4.0]);
slab.transform(&Transform::rotation(
Vec3::new(4.5, 1.0, 1.0),
Vec3::Z,
turn,
));
let slab = Body::new(slab).unwrap();
let d = Vec3::new(1.0, 0.2, 0.1).unit().unwrap();
let n = Vec3::new(-turn.cos(), -turn.sin(), 0.0);
let p = Vec3::new(4.5 - 0.5 * turn.cos(), 1.0 - 0.5 * turn.sin(), 1.0);
let want = ((c - p).dot(n) - 1.3) / -n.dot(d);
let t = swept_contact(&ball, &slab, d, &Sweep::default()).unwrap();
assert!((t - want).abs() < 0.02, "{t} against {want}");
}
#[test]
fn a_host_and_its_insert_joined_move_as_one_and_meet_as_one() {
let plate = solid([-5.0, -5.0, 0.0], [5.0, 5.0, 4.0]);
let mouth = Frame::new(Vec3::new(0.0, 0.0, 2.0), Vec3::Z, Vec3::X).unwrap();
let pocket = crate::primitive::cylinder(&mouth, 2.0, 3.0, "h").unwrap();
let plate = Body::new(crate::boolean::difference(&plate, &pocket).unwrap()).unwrap();
let insert = Body::new(crate::primitive::cylinder(&mouth, 2.0, 2.0, "i").unwrap()).unwrap();
let both = Body::joined(&[&plate, &insert]);
let above = block([-1.0, -1.0, 7.0], [1.0, 1.0, 8.0]);
let t = swept_contact(&both, &above, Vec3::Z, &Sweep::default()).unwrap();
assert!((t - 3.0).abs() < 1e-9, "{t}");
let beside = block([6.0, -1.0, 0.0], [8.0, 1.0, 9.0]);
assert_eq!(
swept_contact(&both, &beside, Vec3::Z, &Sweep::default()),
None
);
}
#[test]
fn a_pin_modelled_tight_in_its_hole_slides_along_it_but_not_across() {
let plate = solid([-5.0, -5.0, 0.0], [5.0, 5.0, 4.0]);
let axis = Frame::new(Vec3::new(0.0, 0.0, -1.0), Vec3::Z, Vec3::X).unwrap();
let hole = crate::primitive::cylinder(&axis, 2.0, 6.0, "h").unwrap();
let plate = Body::new(crate::boolean::difference(&plate, &hole).unwrap()).unwrap();
let low = Frame::new(Vec3::new(0.0, 0.0, 1.0), Vec3::Z, Vec3::X).unwrap();
let pin = Body::new(crate::primitive::cylinder(&low, 2.1, 2.0, "p").unwrap()).unwrap();
assert_eq!(
swept_contact(&pin, &plate, Vec3::Z, &Sweep::default()),
None
);
assert_eq!(
swept_contact(&pin, &plate, -Vec3::Z, &Sweep::default()),
None
);
assert!(swept_contact(&pin, &plate, Vec3::X, &Sweep::default()).is_some_and(|t| t < 1e-6));
}
#[test]
fn a_hair_of_interference_only_touches_but_a_fifth_of_a_millimetre_runs_in() {
let a = block([0.0, 0.0, 0.0], [2.0, 2.0, 2.0]);
let how = Sweep {
inside: 0.025,
..Sweep::default()
};
let hair = block([1.97, -1.0, 3.0], [4.0, 3.0, 5.0]);
assert_eq!(swept_contact(&a, &hair, Vec3::Z, &how), None);
assert!(swept_contact(&a, &hair, Vec3::Z, &Sweep::default()).is_some());
let fifth = block([1.8, -1.0, 3.0], [4.0, 3.0, 5.0]);
let t = swept_contact(&a, &fifth, Vec3::Z, &how).unwrap();
assert!((t - 1.0).abs() < 1e-9, "{t}");
}
#[test]
fn a_body_moved_meets_what_its_new_place_meets_without_meshing_again() {
let cube = block([0.0, 0.0, 0.0], [2.0, 2.0, 2.0]);
let wall = block([5.0, -3.0, -1.0], [6.0, 3.0, 3.0]);
let slid = cube.moved(&Transform::translation(Vec3::new(1.0, 0.0, 0.0)));
assert_eq!(slid.points().len(), cube.points().len());
let t = swept_contact(&slid, &wall, Vec3::X, &Sweep::default()).unwrap();
assert!((t - 2.0).abs() < 1e-9, "{t}");
let turned = cube.moved(
&Transform::rotation(Vec3::ZERO, Vec3::Z, std::f64::consts::FRAC_PI_2)
.then(&Transform::translation(Vec3::new(2.0, 0.0, 0.0))),
);
let t = swept_contact(&turned, &wall, Vec3::X, &Sweep::default()).unwrap();
assert!((t - 3.0).abs() < 1e-9, "{t}");
}
#[test]
fn one_body_against_many_meets_each_after_its_own_gap() {
let cube = block([0.0, 0.0, 0.0], [2.0, 2.0, 2.0]);
let near = block([3.0, -1.0, -1.0], [4.0, 3.0, 1.0]);
let far = block([5.0, -1.0, 1.5], [6.0, 3.0, 3.0]);
let behind = block([-3.0, -1.0, -1.0], [-2.0, 3.0, 3.0]);
let got = swept_contacts(
&[&cube],
&[&near, &far, &behind],
Vec3::X,
&Sweep::default(),
);
assert!((got[0].unwrap() - 1.0).abs() < 1e-9, "{got:?}");
assert!((got[1].unwrap() - 3.0).abs() < 1e-9, "{got:?}");
assert_eq!(got[2], None);
}
#[test]
fn bodies_moved_together_keep_each_solid_apart() {
let plate = solid([-5.0, -5.0, 0.0], [5.0, 5.0, 4.0]);
let mouth = Frame::new(Vec3::new(0.0, 0.0, 2.0), Vec3::Z, Vec3::X).unwrap();
let pocket = crate::primitive::cylinder(&mouth, 2.0, 3.0, "h").unwrap();
let plate = Body::new(crate::boolean::difference(&plate, &pocket).unwrap()).unwrap();
let insert = Body::new(crate::primitive::cylinder(&mouth, 2.0, 2.0, "i").unwrap()).unwrap();
let above = block([-1.0, -1.0, 7.0], [1.0, 1.0, 8.0]);
let got = swept_contacts(&[&plate, &insert], &[&above], Vec3::Z, &Sweep::default());
assert!((got[0].unwrap() - 3.0).abs() < 1e-9, "{got:?}");
}
}