1use crate::drop::path::{Dilated, Sampling};
21use crate::drop::DropCutter;
22use crate::model::{
23 Diagnostic, Feed, FeedsSpeeds, Heights, LeadKind, LinkingParams, Motion, MotionKind, OperationReport, Point3,
24 RampKind, RetractPolicy, Toolpath,
25};
26use crate::{CamError, Cutter, CutterProfile, MeshSnapshot, Runner, Tolerance, TriangleIndex};
27use std::f64::consts::{PI, TAU};
28
29mod stock;
30use stock::StockMap;
31
32thread_local! {
47 static PRE_OP_STOCK: std::cell::Cell<*const crate::StockEnvelope> = const { std::cell::Cell::new(std::ptr::null()) };
48}
49
50pub fn with_pre_op_stock<R>(stock: Option<&crate::StockEnvelope>, f: impl FnOnce() -> R) -> R {
53 struct Restore(*const crate::StockEnvelope);
54 impl Drop for Restore {
55 fn drop(&mut self) {
56 PRE_OP_STOCK.with(|c| c.set(self.0));
57 }
58 }
59 let previous = PRE_OP_STOCK.with(|c| c.replace(stock.map_or(std::ptr::null(), |s| s as *const _)));
60 let _restore = Restore(previous);
61 f()
62}
63
64pub fn with_current_pre_op_stock<R>(f: impl for<'s> FnOnce(Option<&'s crate::StockEnvelope>) -> R) -> R {
68 let p = PRE_OP_STOCK.with(|c| c.get());
69 let stock = if p.is_null() { None } else { Some(unsafe { &*p }) };
75 f(stock)
76}
77
78fn scope_has_pre_op_stock() -> bool {
80 PRE_OP_STOCK.with(|c| !c.get().is_null())
81}
82
83#[derive(Clone, Debug, PartialEq)]
85pub struct Pass {
86 pub points: Vec<Point3>,
87 pub path_id: Option<u32>,
89}
90
91impl Pass {
92 pub fn new(points: Vec<Point3>, path_id: Option<u32>) -> Self {
93 Self { points, path_id }
94 }
95
96 pub fn start(&self) -> Point3 {
97 self.points[0]
98 }
99
100 pub fn end(&self) -> Point3 {
101 self.points[self.points.len() - 1]
102 }
103
104 fn start_tangent(&self) -> Option<Point3> {
106 self.points.windows(2).find_map(|w| xy_unit(w[1].sub(w[0])))
107 }
108
109 fn end_tangent(&self) -> Option<Point3> {
111 self.points.windows(2).rev().find_map(|w| xy_unit(w[1].sub(w[0])))
112 }
113}
114
115fn xy_unit(d: Point3) -> Option<Point3> {
116 Point3::new(d.x, d.y, 0.0).normalized()
117}
118
119#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
121pub enum LeadSide {
122 #[default]
125 Left,
126 Right,
127}
128
129#[derive(Clone, Copy, Debug, PartialEq)]
133pub struct LinkOptions {
134 pub high_feed: f64,
136 pub radial_allowance: f64,
138 pub axial_allowance: f64,
140 pub chord_tolerance: f64,
142 pub lead_side: LeadSide,
144 pub reorder: bool,
147 pub allow_reverse: bool,
149 pub ramp_length: Option<f64>,
152 pub max_samples: u64,
154 pub material_top: Option<f64>,
161}
162
163impl Default for LinkOptions {
164 fn default() -> Self {
165 Self {
166 high_feed: 5_000.0,
167 radial_allowance: 0.0,
168 axial_allowance: 0.0,
169 chord_tolerance: 0.01,
170 lead_side: LeadSide::Left,
171 reorder: false,
172 allow_reverse: true,
173 ramp_length: None,
174 max_samples: 1_000_000,
175 material_top: None,
176 }
177 }
178}
179
180impl LinkOptions {
181 pub fn validate(&self) -> Result<(), CamError> {
182 let bad = |m: &str| Err(CamError::InvalidInput(format!("linking: {m}")));
183 if !self.high_feed.is_finite() || self.high_feed <= 0.0 {
184 return bad("high feed must be finite and positive");
185 }
186 if !self.radial_allowance.is_finite() || self.radial_allowance < 0.0 || !self.axial_allowance.is_finite() || self.axial_allowance < 0.0 {
187 return bad("allowances must be finite and non-negative");
188 }
189 if !self.chord_tolerance.is_finite() || self.chord_tolerance <= 0.0 {
190 return bad("chord tolerance must be finite and positive");
191 }
192 if let Some(l) = self.ramp_length {
193 if !l.is_finite() || l <= 0.0 {
194 return bad("ramp length must be finite and positive");
195 }
196 }
197 if self.max_samples < 2 {
198 return bad("sample budget too small");
199 }
200 if self.material_top.is_some_and(|t| !t.is_finite()) {
201 return bad("material top must be finite");
202 }
203 Ok(())
204 }
205}
206
207pub struct LinkContext<'a> {
209 pub mesh: &'a MeshSnapshot,
211 pub cutter: &'a Cutter,
212 pub index: Option<&'a dyn TriangleIndex>,
214 pub tolerance: Tolerance,
215 pub heights: &'a Heights,
216 pub feeds: &'a FeedsSpeeds,
217 pub params: &'a LinkingParams,
218 pub options: LinkOptions,
219 pub stock: Option<&'a crate::StockEnvelope>,
227}
228
229#[derive(Clone, Debug, Default, PartialEq)]
231pub struct Linked {
232 pub motions: Vec<Motion>,
233 pub report: OperationReport,
234}
235
236struct Prover<'a> {
238 mesh: &'a MeshSnapshot,
239 index: Option<&'a dyn TriangleIndex>,
240 tolerance: Tolerance,
241 dilated: Dilated<'a>,
242 sampling: Sampling,
243 axial: f64,
244 stock: std::cell::RefCell<Option<StockMap>>,
246}
247
248impl<'a> Prover<'a> {
249 fn new(ctx: &LinkContext<'a>) -> Result<Self, CamError> {
250 let dilated = Dilated { base: ctx.cutter, pad: ctx.options.radial_allowance };
251 DropCutter::from_profile(ctx.mesh, &dilated, ctx.index, ctx.tolerance)?;
253 let floor = ctx.mesh.bounds.min.z - ctx.cutter.total_height() - 1.0;
254 let sampling = Sampling {
255 sample_spacing: 1.0,
256 min_sample_spacing: 1e-6,
257 chord_error: ctx.options.chord_tolerance,
258 max_samples: ctx.options.max_samples,
259 floor_z: floor,
260 ..Sampling::default()
261 };
262 sampling.validate()?;
263 Ok(Self { mesh: ctx.mesh, index: ctx.index, tolerance: ctx.tolerance, dilated, sampling, axial: ctx.options.axial_allowance, stock: std::cell::RefCell::new(None) })
264 }
265
266 fn stock_point(&self, p: Point3) -> f64 {
268 self.stock.borrow().as_ref().map_or(f64::NEG_INFINITY, |s| s.point(p))
269 }
270
271 fn stock_chord(&self, a: Point3, b: Point3) -> f64 {
273 self.stock.borrow().as_ref().map_or(f64::NEG_INFINITY, |s| s.chord(a, b))
274 }
275
276 fn drop(&self) -> Result<DropCutter<'_>, CamError> {
277 DropCutter::from_profile(self.mesh, &self.dilated, self.index, self.tolerance)
278 }
279
280 fn point_bound(&self, p: Point3, report: &mut OperationReport, runner: &mut Runner<'_>) -> Result<f64, CamError> {
282 if report.counts.samples >= self.sampling.max_samples {
283 return Err(CamError::InvalidInput("linking sample work budget exhausted".into()));
284 }
285 let r = self.drop()?.point(p.x, p.y, self.sampling.floor_z, runner)?;
286 report.counts.samples += 1;
287 report.counts.candidates += r.candidate_count;
288 report.counts.contacts += r.contact_count;
289 Ok(r.point.z + self.axial)
290 }
291
292 fn chord_bound(&self, a: Point3, b: Point3, report: &mut OperationReport, runner: &mut Runner<'_>) -> Result<f64, CamError> {
295 if a.distance_xy(b) <= 0.0 {
296 return self.point_bound(a, report, runner);
297 }
298 Ok(self.drop()?.segment_bound(a, b, &self.sampling, report, runner)? + self.axial)
299 }
300
301 fn chord_safe(&self, a: Point3, b: Point3, eps: f64, report: &mut OperationReport, runner: &mut Runner<'_>) -> Result<bool, CamError> {
304 let bound = self.chord_bound(a, b, report, runner)?;
305 Ok(a.z.min(b.z) + eps >= bound)
306 }
307
308 fn polyline_safe(&self, points: &[Point3], eps: f64, report: &mut OperationReport, runner: &mut Runner<'_>) -> Result<bool, CamError> {
309 for w in points.windows(2) {
310 if !self.chord_safe(w[0], w[1], eps, report, runner)? {
311 return Ok(false);
312 }
313 }
314 Ok(true)
315 }
316}
317
318struct Emitter {
321 motions: Vec<Motion>,
322 path_id: Option<u32>,
323}
324
325impl Emitter {
326 fn push(&mut self, mut m: Motion) {
327 if m.path_id.is_none() {
328 m.path_id = self.path_id;
329 }
330 self.motions.push(m);
331 }
332 fn at(&self) -> Option<Point3> {
333 self.motions.iter().rev().find_map(|m| m.end())
334 }
335}
336
337pub fn connect(passes: &[Pass], ctx: &LinkContext<'_>, runner: &mut Runner<'_>) -> Result<Linked, CamError> {
343 connect_with(passes, ctx, &[], runner)
344}
345
346struct KeepOut {
349 points: Vec<Point3>,
350 reach: f64,
351 z: f64,
352}
353
354fn connect_with(passes: &[Pass], ctx: &LinkContext<'_>, keep: &[KeepOut], runner: &mut Runner<'_>) -> Result<Linked, CamError> {
355 if ctx.stock.is_none() && scope_has_pre_op_stock() {
359 return Err(CamError::InvalidInput("linking: LinkContext built without the active pre-operation stock (read it with link::with_current_pre_op_stock)".into()));
360 }
361 ctx.options.validate()?;
362 ctx.params.validate().map_err(CamError::InvalidInput)?;
363 ctx.heights.validate().map_err(CamError::InvalidInput)?;
364 ctx.feeds.validate().map_err(CamError::InvalidInput)?;
365 ctx.tolerance.validate()?;
366 ctx.cutter.validate()?;
367 if passes.is_empty() {
368 return Err(CamError::InvalidInput("linking: no passes to link".into()));
369 }
370 for (i, p) in passes.iter().enumerate() {
371 if p.points.is_empty() {
372 return Err(CamError::InvalidInput(format!("linking: pass {i} is empty")));
373 }
374 if let Some(q) = p.points.iter().find(|q| !q.is_finite()) {
375 return Err(CamError::NonFinite { what: format!("pass {i} point {q:?}") });
376 }
377 }
378 if ctx.mesh.is_empty() {
379 return Err(CamError::EmptyTarget { operation_id: "linking".into() });
380 }
381 let heights = ctx.heights;
382 let eps = ctx.options.chord_tolerance.max(ctx.tolerance.distance_eps);
387 let top = ctx.mesh.bounds.max.z + ctx.options.axial_allowance;
388 if heights.clearance < top + eps {
389 return Err(CamError::InvalidInput("linking: clearance height must be above the full protected mesh and allowance".into()));
390 }
391 if passes.iter().flat_map(|p| &p.points).any(|p| p.z >= heights.clearance) {
392 return Err(CamError::InvalidInput("linking: a pass reaches the clearance height".into()));
393 }
394 let prover = Prover::new(ctx)?;
395 if ctx.options.material_top.is_some() || ctx.stock.is_some() || !keep.is_empty() {
396 let extent = passes.iter().flat_map(|p| p.points.iter().copied()).chain(keep.iter().flat_map(|k| k.points.iter().copied()));
397 let mut map = StockMap::new(extent, ctx.cutter, ctx.options.material_top, ctx.stock, 4_000_000);
398 for k in keep {
399 map.raise(&k.points, k.reach, k.z);
400 }
401 *prover.stock.borrow_mut() = Some(map);
402 }
403 let mut swept = 0usize;
404 let mut report = OperationReport::default();
405 report.counts.triangles = ctx.mesh.triangle_count() as u64;
406 let mut em = Emitter { motions: Vec::new(), path_id: None };
407
408 let order: Vec<(usize, bool)> = if ctx.options.reorder {
409 let start = Point3::new(passes[0].start().x, passes[0].start().y, heights.clearance);
410 order_passes(passes, start, ctx.options.allow_reverse)
411 } else {
412 (0..passes.len()).map(|i| (i, false)).collect()
413 };
414 runner.stage("link", "Link passes", 0, order.len() as u64)?;
415 let mut previous: Option<Pass> = None;
416 for (n, &(index, reversed)) in order.iter().enumerate() {
417 let mut pass = passes[index].clone();
418 if reversed {
419 pass.points.reverse();
420 }
421 em.path_id = pass.path_id;
422 let lead_in = plan_lead(&pass, true, &ctx.params.lead_in, ctx, &prover, eps, &mut report, runner)?;
424 let entry = lead_in.as_ref().and_then(|l| l.first().copied()).unwrap_or(pass.start());
425 match previous.take() {
426 None => {
427 em.push(Motion::rapid(Point3::new(entry.x, entry.y, heights.clearance)));
429 descend(&pass, entry, heights.clearance, ctx, &prover, eps, &mut em, &mut report, runner)?;
430 }
431 Some(prev) => {
432 if let Some(map) = prover.stock.borrow_mut().as_mut() {
434 let from = em.motions[..swept].iter().rev().find_map(|m| m.end());
435 map.sweep(from, &em.motions[swept..]);
436 }
437 swept = em.motions.len();
438 let exit = em.at().unwrap_or(prev.end());
439 link_between(&prev, exit, &pass, entry, ctx, &prover, eps, &mut em, &mut report, runner)?;
440 }
441 }
442 if let Some(lead) = lead_in {
443 let feed = ctx.feeds.lead_feed.unwrap_or(ctx.feeds.cutting_feed);
444 for p in lead.iter().skip(1) {
445 em.push(Motion::new(MotionKind::LeadIn { to: *p }, Feed::Rate(feed)));
446 }
447 }
448 for p in pass.points.iter().skip(1) {
449 em.push(Motion::cut(*p, ctx.feeds.cutting_feed));
450 }
451 if let Some(lead) = plan_lead(&pass, false, &ctx.params.lead_out, ctx, &prover, eps, &mut report, runner)? {
452 let feed = ctx.feeds.lead_feed.unwrap_or(ctx.feeds.cutting_feed);
453 for p in lead.iter().skip(1) {
454 em.push(Motion::new(MotionKind::LeadOut { to: *p }, Feed::Rate(feed)));
455 }
456 }
457 report.counts.passes += 1;
458 previous = Some(pass);
459 runner.work(1, "link", n as u64 + 1, order.len() as u64)?;
460 }
461 let last = em.at().expect("a pass was emitted");
462 em.push(Motion::retract(Point3::new(last.x, last.y, heights.clearance), retract_feed(ctx)));
463 report.counts.motions = em.motions.len() as u64;
464 report.finish();
465 Ok(Linked { motions: em.motions, report })
466}
467
468fn retract_feed(ctx: &LinkContext<'_>) -> Feed {
469 ctx.feeds.retract_feed.map_or(Feed::RAPID, Feed::Rate)
470}
471
472fn feed_height(entry: Point3, from_z: f64, ctx: &LinkContext<'_>, prover: &Prover<'_>, report: &mut OperationReport, runner: &mut Runner<'_>) -> Result<f64, CamError> {
475 let bound = prover.point_bound(entry, report, runner)?.max(prover.stock_point(entry));
477 let z = ctx.heights.feed.max(bound + ctx.params.safe_distance).max(entry.z);
478 Ok(z.min(from_z))
479}
480
481#[allow(clippy::too_many_arguments)]
489fn descend(pass: &Pass, entry: Point3, from_z: f64, ctx: &LinkContext<'_>, prover: &Prover<'_>, eps: f64, em: &mut Emitter, report: &mut OperationReport, runner: &mut Runner<'_>) -> Result<(), CamError> {
490 let zf = feed_height(entry, from_z, ctx, prover, report, runner)?;
491 if zf < from_z {
492 em.push(Motion::rapid(Point3::new(entry.x, entry.y, zf)));
493 }
494 if zf == entry.z {
495 return Ok(());
496 }
497 if zf > entry.z {
498 if let Some(ramp) = plan_ramp(pass, entry, zf, ctx, prover, eps, report, runner)? {
499 for m in ramp {
500 em.push(m);
501 }
502 return Ok(());
503 }
504 }
505 em.push(Motion::plunge(entry, ctx.feeds.plunge_feed));
506 Ok(())
507}
508
509#[allow(clippy::too_many_arguments)]
513fn link_between(prev: &Pass, exit: Point3, next: &Pass, entry: Point3, ctx: &LinkContext<'_>, prover: &Prover<'_>, eps: f64, em: &mut Emitter, report: &mut OperationReport, runner: &mut Runner<'_>) -> Result<(), CamError> {
514 let _ = prev;
515 let heights = ctx.heights;
516 let params = ctx.params;
517 if params.keep_down_distance > 0.0 && exit.distance_xy(entry) <= params.keep_down_distance {
519 if let Some(path) = keep_down_path(exit, entry, ctx, prover, eps, report, runner)? {
520 for p in path {
521 em.push(Motion::link(p, ctx.feeds.cutting_feed));
522 }
523 return Ok(());
524 }
525 report.warn(Diagnostic::new("link-keep-down-unproven", "A keep-down link could not stay within the safe distance of the surface and retracts instead").at(exit));
526 }
527 let vertical = exit.distance_xy(entry) == 0.0;
529 let partial = match params.retract_policy {
530 RetractPolicy::FullRetract => None,
531 RetractPolicy::RetractHeight => Some(heights.retract.max(exit.z).max(entry.z)),
533 RetractPolicy::MinimumRetract => {
534 let stock = if vertical { f64::NEG_INFINITY } else { prover.stock_chord(exit, entry) };
538 let bound = prover.chord_bound(exit, entry, report, runner)?.max(stock);
539 Some((bound + ctx.params.safe_distance).max(exit.z).max(entry.z))
540 }
541 };
542 if let Some(z) = partial {
543 if z < heights.clearance - eps {
544 let a = Point3::new(exit.x, exit.y, z);
545 let b = Point3::new(entry.x, entry.y, z);
546 let proven = (vertical || prover.stock_chord(a, b) <= z) && prover.chord_safe(a, b, eps, report, runner)?;
547 if proven {
548 if z > exit.z {
549 em.push(Motion::retract(a, retract_feed(ctx)));
550 }
551 if a.distance_xy(b) > 0.0 {
552 em.push(Motion::link(b, ctx.options.high_feed));
553 }
554 descend(next, entry, z, ctx, prover, eps, em, report, runner)?;
555 return Ok(());
556 }
557 report.warn(Diagnostic::new("link-fallback-clearance", format!("A retract to Z={z} could not be proven clear of material; the link uses the clearance height")).at(exit));
558 }
559 }
560 em.push(Motion::retract(Point3::new(exit.x, exit.y, heights.clearance), retract_feed(ctx)));
562 if exit.distance_xy(entry) > 0.0 {
563 em.push(Motion::rapid(Point3::new(entry.x, entry.y, heights.clearance)));
564 }
565 descend(next, entry, heights.clearance, ctx, prover, eps, em, report, runner)
566}
567
568fn keep_down_path(exit: Point3, entry: Point3, ctx: &LinkContext<'_>, prover: &Prover<'_>, eps: f64, report: &mut OperationReport, runner: &mut Runner<'_>) -> Result<Option<Vec<Point3>>, CamError> {
574 if prover.stock_chord(exit, entry) > exit.z.min(entry.z) {
576 return Ok(None);
577 }
578 let length = exit.distance_xy(entry);
579 if length <= eps {
580 let bound = prover.point_bound(entry, report, runner)?;
582 return Ok((exit.z.min(entry.z) + eps >= bound).then(|| vec![entry]));
583 }
584 let step = (ctx.cutter.radius() * 0.5).max(eps * 10.0);
585 let n = ((length / step).ceil() as usize).clamp(1, 10_000);
586 let floor = exit.z.min(entry.z);
587 let mut pts: Vec<(Point3, bool)> = Vec::with_capacity(n + 1);
589 pts.push((exit, true));
590 for i in 1..n {
591 let q = exit.lerp(entry, i as f64 / n as f64);
592 let z = prover.point_bound(q, report, runner)?;
593 pts.push((Point3::new(q.x, q.y, z.max(floor)), false));
594 }
595 pts.push((entry, true));
596 let min_len = (eps * 4.0).max(1e-6);
601 let mut i = 0usize;
602 let mut splits = 0usize;
603 while i + 1 < pts.len() {
604 let (a, a_fixed) = pts[i];
605 let (b, b_fixed) = pts[i + 1];
606 let bound = prover.chord_bound(a, b, report, runner)?;
607 if a.z.min(b.z) + eps >= bound {
608 i += 1;
609 continue;
610 }
611 let mut lifted = false;
612 if !a_fixed && a.z < bound {
613 pts[i].0.z = bound;
614 lifted = true;
615 }
616 if !b_fixed && b.z < bound {
617 pts[i + 1].0.z = bound;
618 lifted = true;
619 }
620 if lifted {
621 continue;
622 }
623 if a.distance_xy(b) <= min_len || splits >= 10_000 {
624 return Ok(None);
625 }
626 let m = a.lerp(b, 0.5);
627 let z = prover.point_bound(m, report, runner)?;
628 pts.insert(i + 1, (Point3::new(m.x, m.y, z.max(floor)), false));
629 splits += 1;
630 }
631 if pts.iter().any(|(p, _)| p.z > floor + ctx.params.safe_distance + eps) {
632 return Ok(None);
633 }
634 let pts: Vec<Point3> = pts.into_iter().map(|(p, _)| p).collect();
635 let filtered = crate::filter::filter_points(&pts, ctx.options.chord_tolerance, &[])?;
637 Ok(Some(filtered.points.into_iter().skip(1).collect()))
638}
639
640#[allow(clippy::too_many_arguments)]
644fn plan_lead(pass: &Pass, lead_in: bool, lead: &crate::model::Lead, ctx: &LinkContext<'_>, prover: &Prover<'_>, eps: f64, report: &mut OperationReport, runner: &mut Runner<'_>) -> Result<Option<Vec<Point3>>, CamError> {
645 if lead.kind == LeadKind::None {
646 return Ok(None);
647 }
648 let (anchor, tangent) = if lead_in { (pass.start(), pass.start_tangent()) } else { (pass.end(), pass.end_tangent()) };
649 let Some(t) = tangent else {
650 report.warn(Diagnostic::new("lead-no-tangent", "The pass has no horizontal direction for a lead").at(anchor));
651 return Ok(None);
652 };
653 let name = if lead_in { "lead-in" } else { "lead-out" };
654 let sides = match ctx.options.lead_side {
655 LeadSide::Left => [1.0, -1.0],
656 LeadSide::Right => [-1.0, 1.0],
657 };
658 let mut candidates: Vec<Vec<Point3>> = Vec::new();
659 match lead.kind {
660 LeadKind::None => {}
661 LeadKind::Linear => {
662 let len = lead.radius.max(lead.linear_length);
663 if len > 0.0 {
664 let far = anchor.sub(t.scale(len));
665 candidates.push(if lead_in { vec![far, anchor] } else { vec![anchor, anchor.add(t.scale(len))] });
666 }
667 }
668 LeadKind::Arc => {
669 if lead.radius > 0.0 && lead.sweep_deg > 0.0 {
670 for side in sides {
671 candidates.push(lead_arc(anchor, t, lead.radius, lead.sweep_deg.to_radians(), lead.linear_length, side, lead_in, ctx.options.chord_tolerance));
672 }
673 }
674 }
675 LeadKind::Vertical => {
676 let r = if lead.vertical_radius > 0.0 { lead.vertical_radius } else { lead.radius };
677 if r > 0.0 {
678 candidates.push(vertical_lead(anchor, t, r, lead_in, ctx.options.chord_tolerance));
679 }
680 }
681 }
682 if candidates.is_empty() {
683 report.warn(Diagnostic::new("lead-zero-size", format!("The {name} has no size and was skipped")).at(anchor));
684 return Ok(None);
685 }
686 for c in candidates {
687 if c.iter().any(|p| p.z >= ctx.heights.clearance) {
688 continue;
689 }
690 if prover.polyline_safe(&c, eps, report, runner)? {
691 return Ok(Some(c));
692 }
693 }
694 report.warn(Diagnostic::new("lead-unproven", format!("No {name} could be proven clear of material; the pass starts without one")).at(anchor));
695 Ok(None)
696}
697
698fn lead_arc(anchor: Point3, t: Point3, radius: f64, sweep: f64, linear: f64, side: f64, lead_in: bool, chord_tol: f64) -> Vec<Point3> {
701 let (t, side) = if lead_in { (t, side) } else { (t.scale(-1.0), -side) };
704 let n = Point3::new(-t.y, t.x, 0.0).scale(side);
705 let center = anchor.add(n.scale(radius));
706 let steps = arc_steps(radius, sweep, chord_tol);
707 let mut pts = Vec::with_capacity(steps + 2);
708 let far_at = |a: f64| -> Point3 {
713 let radial = n.scale(-a.cos()).add(t.scale(-a.sin()));
714 center.add(radial.scale(radius))
715 };
716 let far = far_at(sweep);
717 if linear > 0.0 {
718 let dir = n.scale(sweep.sin()).add(t.scale(-sweep.cos()));
720 pts.push(far.add(dir.scale(linear)));
721 }
722 for i in 0..=steps {
723 let a = sweep * (1.0 - i as f64 / steps as f64);
724 pts.push(far_at(a));
725 }
726 *pts.last_mut().expect("at least the anchor") = anchor;
728 if !lead_in {
729 pts.reverse();
730 }
731 pts
732}
733
734fn vertical_lead(anchor: Point3, t: Point3, r: f64, lead_in: bool, chord_tol: f64) -> Vec<Point3> {
737 let t = if lead_in { t } else { t.scale(-1.0) };
738 let center = anchor.add(Point3::new(0.0, 0.0, r));
739 let steps = arc_steps(r, PI / 2.0, chord_tol);
740 let mut pts: Vec<Point3> = (0..=steps)
741 .map(|i| {
742 let a = (PI / 2.0) * (i as f64 / steps as f64);
743 center.add(t.scale(-r * a.cos())).add(Point3::new(0.0, 0.0, -r * a.sin()))
745 })
746 .collect();
747 *pts.last_mut().expect("at least the anchor") = anchor;
748 if !lead_in {
749 pts.reverse();
750 }
751 pts
752}
753
754fn arc_steps(radius: f64, sweep: f64, chord_tol: f64) -> usize {
756 let max_step = if chord_tol >= radius { PI } else { 2.0 * (1.0 - chord_tol / radius).acos() };
757 ((sweep / max_step.max(1e-6)).ceil() as usize).clamp(1, 3600)
758}
759
760#[allow(clippy::too_many_arguments)]
763fn plan_ramp(pass: &Pass, entry: Point3, zf: f64, ctx: &LinkContext<'_>, prover: &Prover<'_>, eps: f64, report: &mut OperationReport, runner: &mut Runner<'_>) -> Result<Option<Vec<Motion>>, CamError> {
764 let ramp = &ctx.params.ramp;
765 if ramp.kind == RampKind::Plunge {
766 return Ok(None);
767 }
768 let feed = ctx.feeds.ramp_feed.unwrap_or(ctx.feeds.cutting_feed);
769 let angle = ramp.angle_deg.to_radians();
770 if angle <= 0.0 {
771 report.warn(Diagnostic::new("ramp-zero-angle", "The ramp angle is zero; plunging instead").at(entry));
772 return Ok(None);
773 }
774 let Some(t) = pass.start_tangent() else {
775 report.warn(Diagnostic::new("ramp-no-tangent", "The pass has no horizontal direction for a ramp; plunging instead").at(entry));
776 return Ok(None);
777 };
778 let descent = zf - entry.z;
779 let proven: Option<Vec<Motion>> = match ramp.kind {
780 RampKind::Helix | RampKind::Smooth => {
781 let d = if ramp.helix_diameter > 0.0 { ramp.helix_diameter } else { ctx.cutter.radius() };
782 let n = Point3::new(-t.y, t.x, 0.0);
785 let r = 0.5 * d;
786 let center = Point3::new(entry.x + n.x * r, entry.y + n.y * r, 0.0);
787 let per_turn = TAU * r * angle.tan();
788 let turns = descent / per_turn;
789 let steps = (arc_steps(r, TAU, ctx.options.chord_tolerance) as f64 * turns).ceil().max(1.0) as usize;
791 let steps = steps.min(100_000);
792 let at = |frac: f64| -> Point3 {
793 let a = -TAU * turns * (1.0 - frac);
796 let radial = n.scale(-a.cos()).add(t.scale(a.sin()));
797 Point3::new(center.x + radial.x * r, center.y + radial.y * r, entry.z + descent * (1.0 - frac))
798 };
799 let mut chords: Vec<Point3> = vec![Point3::new(entry.x, entry.y, zf)];
802 chords.extend((0..=steps).map(|i| at(i as f64 / steps as f64)));
803 if prover.polyline_safe(&chords, eps, report, runner)? {
804 let mut motions = Vec::new();
805 let start = chords[1];
806 motions.push(Motion::link(Point3::new(start.x, start.y, zf), ctx.options.high_feed));
807 let full = turns.floor() as usize;
810 let pitch = -per_turn;
811 let c3 = Point3::new(center.x, center.y, entry.z);
812 for k in 1..=full {
813 let p = at(k as f64 / turns);
814 motions.push(Motion::new(MotionKind::ArcCcw { to: p, center: c3, plane: Default::default(), pitch }, Feed::Rate(feed)));
815 }
816 if turns - full as f64 > 1e-9 {
817 motions.push(Motion::new(MotionKind::ArcCcw { to: entry, center: c3, plane: Default::default(), pitch }, Feed::Rate(feed)));
818 } else if let Some(last) = motions.last_mut() {
819 if let MotionKind::ArcCcw { to, .. } = &mut last.kind {
820 *to = entry;
821 }
822 }
823 let ring_at = |a: f64| -> Point3 {
828 let radial = n.scale(-a.cos()).add(t.scale(a.sin()));
829 Point3::new(center.x + radial.x * r, center.y + radial.y * r, entry.z)
830 };
831 let ring_steps = arc_steps(r, TAU, ctx.options.chord_tolerance).max(2);
832 let ring: Vec<Point3> = (0..=ring_steps).map(|i| if i == ring_steps { entry } else { ring_at(TAU * i as f64 / ring_steps as f64) }).collect();
833 if prover.polyline_safe(&ring, eps, report, runner)? {
834 for to in [ring_at(PI), entry] {
835 motions.push(Motion::new(MotionKind::ArcCcw { to, center: c3, plane: Default::default(), pitch: 0.0 }, Feed::Rate(feed)));
836 }
837 } else {
838 report.warn(Diagnostic::new("ramp-floor-ring-unproven", "The flat turn at the bottom of the helix could not be proven clear of the part; the floor under the helix is left on its slope").at(entry));
839 }
840 Some(motions)
841 } else {
842 None
843 }
844 }
845 RampKind::Profile | RampKind::ZigZag => {
846 let length = ctx.options.ramp_length.unwrap_or(if ramp.helix_diameter > 0.0 { 2.0 * ramp.helix_diameter } else { 4.0 * ctx.cutter.radius() });
847 let track: Vec<Point3> = if ramp.kind == RampKind::Profile {
850 let mut pts = vec![entry];
851 let mut acc = 0.0;
852 for w in pass.points.windows(2) {
853 let seg = w[1].distance_xy(w[0]);
854 if acc + seg >= length {
855 let f = ((length - acc) / seg).clamp(0.0, 1.0);
856 pts.push(w[0].lerp(w[1], f));
857 break;
858 }
859 acc += seg;
860 pts.push(w[1]);
861 }
862 pts
863 } else {
864 vec![entry, entry.add(t.scale(length))]
865 };
866 let track_len: f64 = track.windows(2).map(|w| w[1].distance_xy(w[0])).sum();
867 if track_len <= eps {
868 None
869 } else {
870 let mut legs = (descent / (track_len * angle.tan())).ceil().max(1.0) as usize;
872 if legs % 2 == 1 {
873 legs += 1;
874 }
875 let drop_per_leg = descent / legs as f64;
876 let mut chords = vec![Point3::new(entry.x, entry.y, zf)];
877 let mut z = zf;
878 for leg in 0..legs {
879 let forward = leg % 2 == 0;
880 let pts: Vec<Point3> = if forward { track.iter().skip(1).copied().collect() } else { track.iter().rev().skip(1).copied().collect() };
881 let mut travelled = 0.0;
882 let mut at = if forward { track[0] } else { track[track.len() - 1] };
883 for p in pts {
884 travelled += p.distance_xy(at);
885 at = p;
886 chords.push(Point3::new(p.x, p.y, z - drop_per_leg * travelled / track_len));
887 }
888 z -= drop_per_leg;
889 }
890 if let Some(last) = chords.last_mut() {
891 *last = entry;
892 }
893 if prover.polyline_safe(&chords, eps, report, runner)? {
894 Some(chords.iter().skip(1).map(|p| Motion::new(MotionKind::LeadIn { to: *p }, Feed::Rate(feed))).collect())
895 } else {
896 None
897 }
898 }
899 }
900 RampKind::Plunge => None,
901 };
902 if proven.is_none() {
903 report.warn(Diagnostic::new("ramp-unproven", "The ramp could not be proven clear of material; plunging instead").at(entry));
904 }
905 Ok(proven)
906}
907
908pub fn order_passes(passes: &[Pass], start: Point3, allow_reverse: bool) -> Vec<(usize, bool)> {
911 let mut remaining: Vec<usize> = (0..passes.len()).collect();
912 let mut at = start;
913 let mut out = Vec::with_capacity(passes.len());
914 while !remaining.is_empty() {
915 let mut best: Option<(usize, bool, f64)> = None;
916 for (k, &i) in remaining.iter().enumerate() {
917 let p = &passes[i];
918 let forward = at.distance_xy(p.start());
919 let candidates: [(bool, f64); 2] = [(false, forward), (true, if allow_reverse { at.distance_xy(p.end()) } else { f64::INFINITY })];
920 for (rev, cost) in candidates {
921 let better = match best {
922 None => true,
923 Some((_, _, c)) => cost < c - 1e-12,
924 };
925 if better {
926 best = Some((k, rev, cost));
927 }
928 }
929 }
930 let (k, rev, _) = best.expect("remaining is non-empty");
931 let i = remaining.remove(k);
932 at = if rev { passes[i].start() } else { passes[i].end() };
933 out.push((i, rev));
934 }
935 out
936}
937
938pub fn passes_of(path: &Toolpath) -> Result<Vec<Pass>, CamError> {
943 let mut passes = Vec::new();
944 let mut current: Option<Pass> = None;
945 let mut at: Option<Point3> = None;
946 for m in &path.motions {
947 match &m.kind {
948 MotionKind::Cut { to } => {
949 let start = at.ok_or_else(|| CamError::InvalidInput("relink: a cut has no start position".into()))?;
950 let pass = current.get_or_insert_with(|| Pass::new(vec![start], m.path_id));
951 pass.points.push(*to);
952 at = Some(*to);
953 }
954 MotionKind::ArcCw { .. } | MotionKind::ArcCcw { .. } => {
955 return Err(CamError::InvalidInput("relink: the stream already contains arcs; relink before arc fitting".into()));
956 }
957 other => {
958 if let Some(p) = current.take() {
959 passes.push(p);
960 }
961 if let Some(p) = other.end() {
962 at = Some(p);
963 }
964 }
965 }
966 }
967 if let Some(p) = current.take() {
968 passes.push(p);
969 }
970 Ok(passes)
971}
972
973pub fn relink(path: &Toolpath, ctx: &LinkContext<'_>, runner: &mut Runner<'_>) -> Result<Toolpath, CamError> {
977 let passes = passes_of(path)?;
978 if passes.is_empty() {
979 return Err(CamError::InvalidInput(format!("relink: operation {} has no cut passes", path.operation)));
980 }
981 let linked = connect(&passes, ctx, runner)?;
982 Ok(merge_linked(path, linked))
983}
984
985fn merge_linked(path: &Toolpath, linked: Linked) -> Toolpath {
987 let mut out = path.clone();
988 out.motions = linked.motions;
989 out.report.warnings.retain(|w| w.code != "retract-linking");
991 out.report.warnings.extend(linked.report.warnings);
992 out.report.counts.samples += linked.report.counts.samples;
993 out.report.counts.candidates += linked.report.counts.candidates;
994 out.report.counts.contacts += linked.report.counts.contacts;
995 out.report.counts.motions = out.motions.len() as u64;
996 out.report.finish();
997 out
998}
999
1000#[derive(Clone, Copy, Debug, PartialEq, Eq)]
1002pub enum TrimReason {
1003 Unreachable,
1005 Holder,
1007}
1008
1009impl TrimReason {
1010 pub fn code(self) -> &'static str {
1012 match self {
1013 TrimReason::Unreachable => "unreachable-trimmed",
1014 TrimReason::Holder => "holder-trimmed",
1015 }
1016 }
1017}
1018
1019#[derive(Clone, Debug, PartialEq)]
1021pub struct BlockedSpan {
1022 pub motions: std::ops::Range<usize>,
1024 pub reason: TrimReason,
1025 pub material_top: Option<f64>,
1028 pub reach: f64,
1031}
1032
1033#[derive(Clone, Debug, PartialEq)]
1035pub struct TrimmedSpan {
1036 pub reason: TrimReason,
1037 pub points: Vec<Point3>,
1039 pub bounds: crate::model::Bounds3,
1042}
1043
1044#[derive(Clone, Debug, PartialEq)]
1046pub struct Trimmed {
1047 pub path: Toolpath,
1048 pub spans: Vec<TrimmedSpan>,
1049}
1050
1051pub fn trim_blocked(path: &Toolpath, blocked: &[BlockedSpan], ctx: &LinkContext<'_>, runner: &mut Runner<'_>) -> Result<Trimmed, CamError> {
1059 if blocked.is_empty() {
1060 return Ok(Trimmed { path: path.clone(), spans: Vec::new() });
1061 }
1062 let fallback_top = ctx.options.material_top.or_else(|| ctx.stock.and_then(|e| e.max_top()));
1063 let mut reason_of: Vec<Option<usize>> = vec![None; path.motions.len()];
1064 for (k, span) in blocked.iter().enumerate() {
1065 if span.motions.is_empty() || span.motions.end > path.motions.len() {
1066 return Err(CamError::InvalidInput(format!("trim: span {k} ({:?}) is empty or outside the {} motions", span.motions, path.motions.len())));
1067 }
1068 if !(span.reach.is_finite() && span.reach >= 0.0) || span.material_top.is_some_and(|z| !z.is_finite()) {
1069 return Err(CamError::InvalidInput(format!("trim: span {k} has a non-finite reach or material top")));
1070 }
1071 if span.material_top.or(fallback_top).is_none() {
1072 return Err(CamError::InvalidInput(format!("trim: span {k} has no material top and the operation has no stock")));
1073 }
1074 for i in span.motions.clone() {
1075 if !matches!(path.motions[i].kind, MotionKind::Cut { .. }) {
1076 return Err(CamError::InvalidInput(format!("trim: span {k} covers motion {i}, which is not a straight cut")));
1077 }
1078 reason_of[i].get_or_insert(k);
1079 }
1080 }
1081 let mut passes = Vec::new();
1083 let mut current: Option<Pass> = None;
1084 let mut spans: Vec<(usize, Vec<Point3>)> = Vec::new();
1085 let mut open_span: Option<(usize, Vec<Point3>)> = None;
1086 let mut at: Option<Point3> = None;
1087 for (i, m) in path.motions.iter().enumerate() {
1088 match (&m.kind, reason_of[i]) {
1089 (MotionKind::Cut { to }, Some(k)) => {
1090 let start = at.ok_or_else(|| CamError::InvalidInput("trim: a cut has no start position".into()))?;
1091 if let Some(p) = current.take() {
1092 passes.push(p);
1093 }
1094 match &mut open_span {
1095 Some((open, points)) if *open == k => points.push(*to),
1096 _ => {
1097 if let Some(done) = open_span.take() {
1098 spans.push(done);
1099 }
1100 open_span = Some((k, vec![start, *to]));
1101 }
1102 }
1103 at = Some(*to);
1104 }
1105 (MotionKind::Cut { to }, None) => {
1106 if let Some(done) = open_span.take() {
1107 spans.push(done);
1108 }
1109 let start = at.ok_or_else(|| CamError::InvalidInput("trim: a cut has no start position".into()))?;
1110 current.get_or_insert_with(|| Pass::new(vec![start], m.path_id)).points.push(*to);
1111 at = Some(*to);
1112 }
1113 (MotionKind::ArcCw { .. } | MotionKind::ArcCcw { .. }, _) => {
1114 return Err(CamError::InvalidInput("trim: the stream already contains arcs; trim before arc fitting".into()));
1115 }
1116 (other, _) => {
1117 if let Some(done) = open_span.take() {
1118 spans.push(done);
1119 }
1120 if let Some(p) = current.take() {
1121 passes.push(p);
1122 }
1123 if let Some(p) = other.end() {
1124 at = Some(p);
1125 }
1126 }
1127 }
1128 }
1129 if let Some(done) = open_span.take() {
1130 spans.push(done);
1131 }
1132 if let Some(p) = current.take() {
1133 passes.push(p);
1134 }
1135 if passes.is_empty() {
1136 return Err(CamError::InvalidInput(format!("trim: every cut of operation {} is blocked", path.operation)));
1137 }
1138 let trimmed: Vec<TrimmedSpan> = spans
1139 .iter()
1140 .map(|(k, points)| {
1141 let span = &blocked[*k];
1142 let reach = span.reach.max(ctx.cutter.radius());
1143 let top = span.material_top.or(fallback_top).expect("checked above");
1144 let mut bounds = crate::model::Bounds3::from_points(points.iter().copied()).expanded(reach);
1145 bounds.min.z = points.iter().map(|p| p.z).fold(f64::INFINITY, f64::min);
1146 bounds.max.z = top.max(points.iter().map(|p| p.z).fold(f64::NEG_INFINITY, f64::max));
1147 TrimmedSpan { reason: span.reason, points: points.clone(), bounds }
1148 })
1149 .collect();
1150 let keep: Vec<KeepOut> = spans
1151 .iter()
1152 .map(|(k, points)| {
1153 let span = &blocked[*k];
1154 KeepOut { points: points.clone(), reach: span.reach.max(ctx.cutter.radius()), z: span.material_top.or(fallback_top).expect("checked above") }
1155 })
1156 .collect();
1157 let linked = connect_with(&passes, ctx, &keep, runner)?;
1158 let mut out = merge_linked(path, linked);
1159 for reason in [TrimReason::Unreachable, TrimReason::Holder] {
1160 let of: Vec<&TrimmedSpan> = trimmed.iter().filter(|t| t.reason == reason).collect();
1161 let Some(first) = of.first() else { continue };
1162 let shown: Vec<String> = of.iter().take(20).map(|t| { let p = t.points[0]; format!("({:.4}, {:.4}, {:.4})", p.x, p.y, p.z) }).collect();
1163 let more = if of.len() > shown.len() { format!(" and {} more", of.len() - shown.len()) } else { String::new() };
1164 let why = match reason {
1165 TrimReason::Unreachable => "the cutter cannot reach them",
1166 TrimReason::Holder => "the shank or holder meets material there",
1167 };
1168 let spans: Vec<String> = of
1169 .iter()
1170 .take(20)
1171 .map(|t| {
1172 let (a, b) = (t.points[0], t.points[t.points.len() - 1]);
1173 format!("({:.3}, {:.3}, {:.3})-({:.3}, {:.3}, {:.3}) material to {:.3}", a.x, a.y, a.z, b.x, b.y, b.z, t.bounds.max.z)
1174 })
1175 .collect();
1176 out.report.warn(
1177 Diagnostic::new(reason.code(), format!("{} cut spans are left out because {why}: the path breaks, retracts and re-enters; material is left at {}{more}", of.len(), shown.join(", ")))
1178 .at(first.points[0])
1179 .detail(format!("spans (from-to): {}{more}", spans.join(", "))),
1180 );
1181 for _ in &of {
1182 out.report.empty_regions.push(crate::model::EmptyRegion { reason: reason.code().into(), target: None });
1183 }
1184 }
1185 out.report.finish();
1186 Ok(Trimmed { path: out, spans: trimmed })
1187}
1188
1189pub fn finish(path: &Toolpath, ctx: &LinkContext<'_>, filter_tolerance: f64, arcs: Option<&crate::arcfit::ArcFitOptions>, runner: &mut Runner<'_>) -> Result<Toolpath, CamError> {
1193 let linked = relink(path, ctx, runner)?;
1194 let filtered = if filter_tolerance > 0.0 { crate::filter::filter_toolpath(&linked, filter_tolerance)? } else { linked };
1195 match arcs {
1196 Some(opts) => crate::arcfit::fit_toolpath(&filtered, opts),
1197 None => Ok(filtered),
1198 }
1199}
1200