484 lines
20 KiB
Rust
484 lines
20 KiB
Rust
//! Independent fixture collision adapter. No toolpath generation code is used.
|
|
//! Surface casts and solid containment are both needed: a surface-only query can
|
|
//! miss a tool that starts wholly inside a closed fixture.
|
|
use crate::{
|
|
JobInput, POST_ERROR, Result, Tool, ToolKind, invalid,
|
|
nc::Segment,
|
|
verify::{Finding, Limits, Report, Status},
|
|
};
|
|
use parry3d_f64::{
|
|
math::{Pose, Vector},
|
|
query::{ShapeCastOptions, ShapeCastStatus, cast_shapes, distance},
|
|
shape::{Cone, Cylinder, Shape, Triangle},
|
|
};
|
|
use solidcraft_geom::Vec3;
|
|
use std::collections::BTreeMap;
|
|
|
|
#[derive(Clone, Copy)]
|
|
struct Bounds {
|
|
min: Vec3,
|
|
max: Vec3,
|
|
}
|
|
impl Bounds {
|
|
fn point(p: Vec3) -> Self {
|
|
Self { min: p, max: p }
|
|
}
|
|
fn include(&mut self, p: Vec3) {
|
|
self.min = Vec3::new(self.min.x.min(p.x), self.min.y.min(p.y), self.min.z.min(p.z));
|
|
self.max = Vec3::new(self.max.x.max(p.x), self.max.y.max(p.y), self.max.z.max(p.z));
|
|
}
|
|
fn overlaps(self, b: Self) -> bool {
|
|
self.min.x <= b.max.x
|
|
&& self.max.x >= b.min.x
|
|
&& self.min.y <= b.max.y
|
|
&& self.max.y >= b.min.y
|
|
&& self.min.z <= b.max.z
|
|
&& self.max.z >= b.min.z
|
|
}
|
|
}
|
|
|
|
struct Component {
|
|
body: usize,
|
|
triangles: Vec<usize>,
|
|
bounds: Bounds,
|
|
closed: bool,
|
|
}
|
|
struct Edge {
|
|
first: usize,
|
|
forward: bool,
|
|
count: usize,
|
|
consistent: bool,
|
|
}
|
|
type Vertex = [u64; 3];
|
|
fn vertex(p: Vec3) -> Vertex {
|
|
[p.x, p.y, p.z].map(|v| if v == 0.0 { 0 } else { v.to_bits() })
|
|
}
|
|
fn root(parents: &mut [usize], mut i: usize) -> usize {
|
|
while let Some(&p) = parents.get(i) {
|
|
if p == i {
|
|
break;
|
|
}
|
|
let next = parents.get(p).copied().unwrap_or(p);
|
|
if let Some(slot) = parents.get_mut(i) {
|
|
*slot = next;
|
|
}
|
|
i = next;
|
|
}
|
|
i
|
|
}
|
|
|
|
/// Parry's primitives have a Y axis. This orthogonal mapping puts SolidCraft's
|
|
/// downward drill tip on +Y without introducing a rounded rotation transform.
|
|
fn vector(p: Vec3) -> Vector {
|
|
Vector::new(p.x, -p.z, p.y)
|
|
}
|
|
struct Part {
|
|
radius: f64,
|
|
low: f64,
|
|
high: f64,
|
|
cone: bool,
|
|
label: &'static str,
|
|
}
|
|
fn parts(t: &Tool) -> Vec<Part> {
|
|
let tip = t.tip_length();
|
|
let mut out = Vec::with_capacity(4);
|
|
if t.kind == ToolKind::TwistDrill {
|
|
out.push(Part { radius: t.diameter / 2.0, low: 0.0, high: tip, cone: true, label: "cutter" });
|
|
}
|
|
if t.cutting_length > tip {
|
|
out.push(Part { radius: t.diameter / 2.0, low: tip, high: t.cutting_length, cone: false, label: "cutter" });
|
|
}
|
|
if t.overall_length > t.cutting_length {
|
|
out.push(Part { radius: t.shank_diameter / 2.0, low: t.cutting_length, high: t.overall_length, cone: false, label: "shank" });
|
|
}
|
|
out.push(Part { radius: t.holder_diameter / 2.0, low: t.overall_length, high: t.overall_length + t.holder_length, cone: false, label: "holder" });
|
|
out
|
|
}
|
|
impl Part {
|
|
fn center(&self, p: Vec3) -> Vec3 {
|
|
p + Vec3::new(0.0, 0.0, (self.low + self.high) / 2.0)
|
|
}
|
|
fn bounds(&self, s: &Segment, margin: f64) -> Bounds {
|
|
let radius = self.radius + margin;
|
|
Bounds {
|
|
min: Vec3::new(s.from.x.min(s.to.x) - radius, s.from.y.min(s.to.y) - radius, s.from.z.min(s.to.z) + self.low - margin),
|
|
max: Vec3::new(s.from.x.max(s.to.x) + radius, s.from.y.max(s.to.y) + radius, s.from.z.max(s.to.z) + self.high + margin),
|
|
}
|
|
}
|
|
fn cast(&self, s: &Segment, tri: &[Vec3; 3], margin: f64) -> Status {
|
|
let center = vector(self.center(s.from));
|
|
let position = Pose::translation(center.x, center.y, center.z);
|
|
let triangle = Triangle::new(vector(tri[0]), vector(tri[1]), vector(tri[2]));
|
|
let cylinder = Cylinder::new((self.high - self.low) / 2.0, self.radius);
|
|
let cone = Cone::new((self.high - self.low) / 2.0, self.radius);
|
|
let shape: &dyn Shape = if self.cone { &cone } else { &cylinder };
|
|
let query = |distance| {
|
|
cast_shapes(
|
|
&position,
|
|
vector(s.to - s.from),
|
|
shape,
|
|
&Pose::IDENTITY,
|
|
Vector::ZERO,
|
|
&triangle,
|
|
ShapeCastOptions {
|
|
max_time_of_impact: 1.0,
|
|
target_distance: distance,
|
|
stop_at_penetration: true,
|
|
compute_impact_geometry_on_penetration: true,
|
|
},
|
|
)
|
|
};
|
|
// Third-party numerical errors or panics never certify clearance.
|
|
std::panic::catch_unwind(std::panic::AssertUnwindSafe(|| {
|
|
// A zero-velocity shape cast may report no impact even for initial
|
|
// penetration. Check the initial pose independently before sweeping.
|
|
let initial = match distance(&position, shape, &Pose::IDENTITY, &triangle).map(|d| d.distance) {
|
|
Ok(d) if d.is_finite() && d <= 1e-10 => return Status::Fail,
|
|
Ok(d) if d.is_finite() && d > margin => Status::Pass,
|
|
_ => Status::Inconclusive,
|
|
};
|
|
match query(margin) {
|
|
Ok(None) => initial,
|
|
Ok(Some(_)) => match query(0.0) {
|
|
Ok(Some(hit))
|
|
if matches!(hit.status, ShapeCastStatus::Converged | ShapeCastStatus::PenetratingOrWithinTargetDist)
|
|
&& hit.time_of_impact.is_finite()
|
|
&& (0.0..=1.0).contains(&hit.time_of_impact) =>
|
|
{
|
|
Status::Fail
|
|
}
|
|
_ => Status::Inconclusive,
|
|
},
|
|
Err(_) => Status::Inconclusive,
|
|
}
|
|
}))
|
|
.unwrap_or(Status::Inconclusive)
|
|
}
|
|
}
|
|
|
|
fn finding(report: &mut Report, status: Status, s: &Segment, part: &Part, index: usize, detail: &str) {
|
|
if status == Status::Fail || report.status == Status::Pass {
|
|
report.status = status;
|
|
}
|
|
report.finding(Finding {
|
|
severity: if status == Status::Fail { crate::verify::FindingSeverity::Error } else { crate::verify::FindingSeverity::Unresolved },
|
|
record: None,
|
|
source: None,
|
|
kind: if status == Status::Fail { format!("fixture_{}_collision", part.label) } else { "fixture_clearance_unresolved".into() },
|
|
line: Some(s.line),
|
|
at: s.from,
|
|
detail: format!("{} of tool {} at motion {}: {detail}", part.label, s.tool, index + 1),
|
|
});
|
|
}
|
|
|
|
pub(crate) async fn fixtures<F: std::future::Future<Output = ()>>(
|
|
input: &JobInput,
|
|
segments: &[Segment],
|
|
limits: Limits,
|
|
cancelled: &impl Fn() -> bool,
|
|
yield_control: &mut impl FnMut() -> F,
|
|
report: &mut Report,
|
|
) -> Result<()> {
|
|
let tris = &input.geometry.fixtures;
|
|
if tris.is_empty() {
|
|
return Ok(());
|
|
}
|
|
let mut parents: Vec<_> = (0..tris.len()).collect();
|
|
let mut edges: BTreeMap<(usize, [Vertex; 2]), Edge> = BTreeMap::new();
|
|
let bodies: Vec<usize> = if input.geometry.fixture_groups.is_empty() {
|
|
vec![0; tris.len()]
|
|
} else {
|
|
input.geometry.fixture_groups.iter().enumerate().flat_map(|(body, &count)| std::iter::repeat_n(body, count)).collect()
|
|
};
|
|
let mut degenerate = Vec::new();
|
|
for (i, tri) in tris.iter().enumerate() {
|
|
if i.is_multiple_of(1024) {
|
|
yield_control().await;
|
|
if cancelled() {
|
|
return invalid("simulation cancelled");
|
|
}
|
|
}
|
|
report.work += 3;
|
|
if report.work > limits.work {
|
|
return Ok(());
|
|
}
|
|
if (tri[1] - tri[0]).cross(tri[2] - tri[0]).len() < 1e-12 {
|
|
degenerate.push(i);
|
|
}
|
|
for (a, b) in [(tri[0], tri[1]), (tri[1], tri[2]), (tri[2], tri[0])] {
|
|
let a = vertex(a);
|
|
let b = vertex(b);
|
|
let forward = a < b;
|
|
let key = (bodies.get(i).copied().unwrap_or(0), if forward { [a, b] } else { [b, a] });
|
|
if let Some(e) = edges.get_mut(&key) {
|
|
let a = root(&mut parents, e.first);
|
|
let b = root(&mut parents, i);
|
|
if let Some(slot) = parents.get_mut(b) {
|
|
*slot = a;
|
|
}
|
|
e.count += 1;
|
|
e.consistent &= e.forward != forward;
|
|
} else {
|
|
edges.insert(key, Edge { first: i, forward, count: 1, consistent: true });
|
|
}
|
|
}
|
|
}
|
|
let mut components: BTreeMap<usize, Component> = BTreeMap::new();
|
|
for (i, tri) in tris.iter().enumerate() {
|
|
let key = root(&mut parents, i);
|
|
let c = components.entry(key).or_insert_with(|| Component {
|
|
body: bodies.get(i).copied().unwrap_or(0),
|
|
triangles: Vec::new(),
|
|
bounds: Bounds::point(tri[0]),
|
|
closed: true,
|
|
});
|
|
c.triangles.push(i);
|
|
for p in tri {
|
|
c.bounds.include(*p);
|
|
}
|
|
if i.is_multiple_of(1024) {
|
|
yield_control().await;
|
|
if cancelled() {
|
|
return invalid("simulation cancelled");
|
|
}
|
|
}
|
|
}
|
|
for (index, e) in edges.values().enumerate() {
|
|
if index.is_multiple_of(1024) {
|
|
yield_control().await;
|
|
if cancelled() {
|
|
return invalid("simulation cancelled");
|
|
}
|
|
}
|
|
if (e.count != 2 || !e.consistent)
|
|
&& let Some(c) = components.get_mut(&root(&mut parents, e.first))
|
|
{
|
|
c.closed = false;
|
|
}
|
|
}
|
|
for i in degenerate {
|
|
if let Some(c) = components.get_mut(&root(&mut parents, i)) {
|
|
c.closed = false;
|
|
}
|
|
}
|
|
let mut body_closed = BTreeMap::new();
|
|
for c in components.values() {
|
|
let closed = body_closed.entry(c.body).or_insert(true);
|
|
*closed &= c.closed;
|
|
}
|
|
let margin = POST_ERROR + limits.uncertainty;
|
|
for (index, s) in segments.iter().enumerate() {
|
|
yield_control().await;
|
|
if cancelled() {
|
|
return invalid("simulation cancelled");
|
|
}
|
|
let tool = input.tools.iter().find(|t| t.id == s.tool).ok_or_else(|| crate::Error("unknown simulation tool".into()))?;
|
|
for part in parts(tool) {
|
|
let swept = part.bounds(s, margin);
|
|
let mut windings: BTreeMap<usize, f64> = BTreeMap::new();
|
|
for c in components.values() {
|
|
report.work += 1;
|
|
if report.work.is_multiple_of(1024) {
|
|
yield_control().await;
|
|
if cancelled() {
|
|
return invalid("simulation cancelled");
|
|
}
|
|
}
|
|
if report.work > limits.work {
|
|
return Ok(());
|
|
}
|
|
if !swept.overlaps(c.bounds) {
|
|
continue;
|
|
}
|
|
let center = part.center(s.from);
|
|
let containment = Bounds::point(center).overlaps(c.bounds);
|
|
let mut solid_angle = 0.0;
|
|
let mut status = if c.closed { Status::Pass } else { Status::Inconclusive };
|
|
for &i in &c.triangles {
|
|
report.work += 1;
|
|
if report.work.is_multiple_of(1024) {
|
|
yield_control().await;
|
|
if cancelled() {
|
|
return invalid("simulation cancelled");
|
|
}
|
|
}
|
|
if report.work > limits.work {
|
|
return Ok(());
|
|
}
|
|
let Some(tri) = tris.get(i) else {
|
|
return invalid("invalid fixture triangle index");
|
|
};
|
|
let mut bounds = Bounds::point(tri[0]);
|
|
bounds.include(tri[1]);
|
|
bounds.include(tri[2]);
|
|
if swept.overlaps(bounds) {
|
|
let cast = part.cast(s, tri, margin);
|
|
if cast == Status::Fail {
|
|
status = Status::Fail;
|
|
break;
|
|
}
|
|
if cast == Status::Inconclusive {
|
|
status = Status::Inconclusive;
|
|
}
|
|
}
|
|
if c.closed && containment {
|
|
let [a, b, d] = tri.map(|v| v - center);
|
|
let denominator = a.len() * b.len() * d.len() + a.dot(b) * d.len() + b.dot(d) * a.len() + d.dot(a) * b.len();
|
|
solid_angle += 2.0 * a.dot(b.cross(d)).atan2(denominator);
|
|
}
|
|
}
|
|
if status != Status::Fail && c.closed && containment {
|
|
*windings.entry(c.body).or_default() += solid_angle / (4.0 * std::f64::consts::PI);
|
|
}
|
|
if status != Status::Pass {
|
|
finding(
|
|
report,
|
|
status,
|
|
s,
|
|
&part,
|
|
index,
|
|
if status == Status::Fail {
|
|
"swept shape contacts the fixture surface"
|
|
} else {
|
|
"clearance is within the error margin, the query did not converge, or fixture topology is open/degenerate"
|
|
},
|
|
);
|
|
}
|
|
}
|
|
for (body, winding) in &windings {
|
|
let status = if !body_closed.get(body).copied().unwrap_or(false) {
|
|
Status::Inconclusive
|
|
} else if winding.is_finite() && winding.abs() > 1.0 - 1e-6 {
|
|
Status::Fail
|
|
} else if !winding.is_finite() || winding.abs() > 1e-6 {
|
|
Status::Inconclusive
|
|
} else {
|
|
Status::Pass
|
|
};
|
|
if status != Status::Pass {
|
|
finding(report, status, s, &part, index, "initial tool volume is inside a fixture or its solid winding is unresolved");
|
|
}
|
|
}
|
|
}
|
|
}
|
|
Ok(())
|
|
}
|
|
|
|
#[cfg(test)]
|
|
#[allow(clippy::unwrap_used)]
|
|
mod tests {
|
|
use super::*;
|
|
use crate::{corpus, nc, verify};
|
|
|
|
fn box_mesh(min: Vec3, max: Vec3) -> Vec<[Vec3; 3]> {
|
|
let p = [
|
|
min,
|
|
Vec3::new(max.x, min.y, min.z),
|
|
Vec3::new(max.x, max.y, min.z),
|
|
Vec3::new(min.x, max.y, min.z),
|
|
Vec3::new(min.x, min.y, max.z),
|
|
Vec3::new(max.x, min.y, max.z),
|
|
max,
|
|
Vec3::new(min.x, max.y, max.z),
|
|
];
|
|
[[0, 3, 2, 1], [4, 5, 6, 7], [0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [3, 0, 4, 7]]
|
|
.into_iter()
|
|
.flat_map(|q| [[p[q[0]], p[q[1]], p[q[2]]], [p[q[0]], p[q[2]], p[q[3]]]])
|
|
.collect()
|
|
}
|
|
fn run(mesh: Vec<[Vec3; 3]>, motion: &str) -> Report {
|
|
run_groups(mesh, vec![], motion)
|
|
}
|
|
fn run_groups(mesh: Vec<[Vec3; 3]>, groups: Vec<usize>, motion: &str) -> Report {
|
|
let mut input = corpus::fixture("face").unwrap();
|
|
input.geometry.fixtures = mesh;
|
|
input.geometry.fixture_groups = groups;
|
|
let src = format!("G21 G90 G17 G94 G54\nG0 X0 Y0 Z15\nT1 M6\n{motion}\nM30");
|
|
let parsed = nc::parse(&src, &input.tools).unwrap();
|
|
verify::simulate(&input, &parsed, Limits::default()).unwrap()
|
|
}
|
|
#[test]
|
|
fn near_fixture_is_clear_below_wide_holder() {
|
|
// Old whole-tool AABB overlaps this fixture. Cutter/shank radius is 3;
|
|
// the 10-radius holder starts at Z65 and cannot touch this low block.
|
|
let r = run(box_mesh(Vec3::new(4.5, 0.0, 16.0), Vec3::new(5.0, 1.0, 17.0)), "G0 X1");
|
|
assert_eq!(r.status, Status::Pass, "{:?}", r.findings);
|
|
}
|
|
#[test]
|
|
fn thin_fixture_between_endpoints_is_a_collision() {
|
|
let r = run(box_mesh(Vec3::new(5.0, -1.0, 16.0), Vec3::new(5.01, 1.0, 17.0)), "G0 X10");
|
|
assert_eq!(r.status, Status::Fail);
|
|
assert!(r.findings.iter().any(|f| f.kind == "fixture_cutter_collision" && f.line == Some(4)));
|
|
}
|
|
#[test]
|
|
fn holder_and_shank_collisions_are_distinguished() {
|
|
for (z, kind) in [(40.0, "shank"), (70.0, "holder")] {
|
|
let r = run(box_mesh(Vec3::new(5.0, -1.0, z), Vec3::new(5.1, 1.0, z + 1.0)), "G0 X10");
|
|
assert_eq!(r.status, Status::Fail);
|
|
assert!(r.findings.iter().any(|f| f.kind == format!("fixture_{kind}_collision")));
|
|
}
|
|
}
|
|
#[test]
|
|
fn contained_tool_is_not_missed_and_winding_is_irrelevant() {
|
|
let cube = box_mesh(Vec3::new(-20.0, -20.0, 10.0), Vec3::new(20.0, 20.0, 100.0));
|
|
for reverse in [false, true] {
|
|
let mesh = cube.iter().map(|t| if reverse { [t[2], t[1], t[0]] } else { *t }).collect();
|
|
let r = run(mesh, "G0 X1");
|
|
assert_eq!(r.status, Status::Fail, "{:?}", r.findings);
|
|
}
|
|
}
|
|
#[test]
|
|
fn overlapping_components_with_opposite_winding_do_not_cancel() {
|
|
let mut a = box_mesh(Vec3::new(-20.0, -20.0, 10.0), Vec3::new(20.0, 20.0, 100.0));
|
|
a.extend(box_mesh(Vec3::new(-21.0, -21.0, 9.0), Vec3::new(21.0, 21.0, 101.0)).into_iter().map(|t| [t[2], t[1], t[0]]));
|
|
assert_eq!(run_groups(a, vec![12, 12], "G0 X1").status, Status::Fail);
|
|
}
|
|
#[test]
|
|
fn cavity_in_one_fixture_body_remains_empty() {
|
|
let mut a = box_mesh(Vec3::new(-20.0, -20.0, 10.0), Vec3::new(20.0, 20.0, 100.0));
|
|
a.extend(box_mesh(Vec3::new(-15.0, -15.0, 11.0), Vec3::new(15.0, 15.0, 99.0)).into_iter().map(|t| [t[2], t[1], t[0]]));
|
|
assert_eq!(run_groups(a, vec![24], "G0 X1").status, Status::Pass);
|
|
}
|
|
#[test]
|
|
fn malformed_fixture_group_counts_are_rejected() {
|
|
let mut input = corpus::fixture("face").unwrap();
|
|
input.geometry.fixture_groups = vec![usize::MAX, 1];
|
|
assert!(input.validate().is_err());
|
|
input.geometry.fixture_groups = vec![0];
|
|
assert!(input.validate().is_err());
|
|
}
|
|
#[test]
|
|
fn open_containing_shell_is_inconclusive() {
|
|
let mut cube = box_mesh(Vec3::new(-20.0, -20.0, 10.0), Vec3::new(20.0, 20.0, 100.0));
|
|
cube.pop();
|
|
assert_eq!(run(cube, "G0 X1").status, Status::Inconclusive);
|
|
}
|
|
#[test]
|
|
fn cylinder_corners_and_margin_are_resolved() {
|
|
let part = Part { radius: 1.0, low: 0.0, high: 2.0, cone: false, label: "cutter" };
|
|
let s = Segment { from: Vec3::ZERO, to: Vec3::ZERO, rapid: true, feed: 0.0, tool: 1, line: 4 };
|
|
let triangle = |x, y| [Vec3::new(x, y, 0.5), Vec3::new(x + 0.05, y, 1.5), Vec3::new(x, y + 0.05, 1.5)];
|
|
assert_eq!(part.cast(&s, &triangle(0.8, 0.8), 0.0035), Status::Pass);
|
|
assert_eq!(part.cast(&s, &triangle(0.5, 0.5), 0.0035), Status::Fail);
|
|
assert_ne!(part.cast(&s, &triangle(1.001, 0.0), 0.0035), Status::Pass);
|
|
}
|
|
#[test]
|
|
fn drill_point_shape_has_clearance_outside_its_cone() {
|
|
let part = Part { radius: 1.0, low: 0.0, high: 1.0, cone: true, label: "cutter" };
|
|
let s = Segment { from: Vec3::ZERO, to: Vec3::ZERO, rapid: false, feed: 100.0, tool: 1, line: 4 };
|
|
let clear = [Vec3::new(0.8, 0.0, 0.1), Vec3::new(0.9, 0.0, 0.1), Vec3::new(0.8, 0.1, 0.2)];
|
|
assert_eq!(part.cast(&s, &clear, 0.0035), Status::Pass);
|
|
let contact = clear.map(|p| p + Vec3::new(0.0, 0.0, 0.8));
|
|
assert_eq!(part.cast(&s, &contact, 0.0035), Status::Fail);
|
|
}
|
|
#[test]
|
|
fn fixture_budget_and_cancellation_never_pass() {
|
|
let mut input = corpus::fixture("face").unwrap();
|
|
input.geometry.fixtures = box_mesh(Vec3::new(4.5, 0.0, 16.0), Vec3::new(5.0, 1.0, 17.0));
|
|
let parsed = nc::parse("G21 G90 G17 G94 G54\nG0 X0 Y0 Z15\nT1 M6\nG0 X1\nM30", &input.tools).unwrap();
|
|
let r = verify::simulate(&input, &parsed, Limits { work: 1, ..Limits::default() }).unwrap();
|
|
assert_eq!(r.status, Status::Inconclusive);
|
|
assert!(verify::simulate_cancellable(&input, &parsed, Limits::default(), || true).is_err());
|
|
}
|
|
}
|