use crate::arcfit::{arc_sweep, from_plane_coords, plane_coords};
use crate::cutter::ToolProfile;
use crate::cycle_time::estimate_cycle_time;
use crate::model::{Diagnostic, Machine, Motion, MotionKind, OperationId, Point3};
use crate::multiaxis::sim::tridexel::{TriDexel, TriDexelOptions};
use crate::program::Program;
use crate::stock_sim::mesh::{stock_mesh, StockMesh};
use crate::stock_sim::removal::{simulate, simulate_on_machine, RemovalOptions};
use crate::stock_sim::DexelGrid;
use crate::{CamError, Cutter, Runner, Tolerance};
use serde::{Deserialize, Serialize};
#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
pub struct TurningStock {
pub section: crate::planar::Region,
pub target: crate::planar::Region,
pub view: DexelGrid,
}
impl TurningStock {
pub fn volume(&self)->f64 {
self.section.loops.iter().flat_map(|l|&l.segments).map(|s| {
let (a,b)=(s.start,s.end);(a.x+b.x)*(a.x*b.y-b.x*a.y)*std::f64::consts::PI/3.
}).sum()
}
pub fn refresh_view(&mut self) {
for iy in 0..self.view.ny { for ix in 0..self.view.nx {
let (x,y)=self.view.center(ix,iy);let radius=x.hypot(y);let mut hits=Vec::new();
for edge in self.section.loops.iter().flat_map(|l|&l.segments) {
let (a,b)=(edge.start,edge.end);
if (a.x<=radius && b.x>radius)||(b.x<=radius && a.x>radius) {
hits.push(a.y+(b.y-a.y)*(radius-a.x)/(b.x-a.x));
}
}
hits.sort_by(f64::total_cmp);
self.view.columns[(iy*self.view.nx+ix) as usize].intervals=hits.chunks_exact(2).filter(|p|p[1]>p[0]).map(|p|[p[0],p[1]]).collect();
}}
}
}
#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
pub struct TurningRange {
pub tool: crate::ToolId,
pub envelope: crate::turning::ToolEnvelope,
pub holders: Vec<crate::turning::ToolEnvelope>,
pub protected: crate::planar::Region,
pub chuck_z: f64,
}
#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(rename_all = "camelCase")]
pub struct OperationRange {
pub operation: OperationId,
pub first: usize,
pub count: usize,
}
#[derive(Clone, Debug, Default, PartialEq, Serialize, Deserialize)]
#[serde(rename_all = "camelCase")]
pub struct Timeline {
pub motions: Vec<Motion>,
pub operations: Vec<OperationRange>,
pub end_times: Vec<f64>,
pub total_seconds: f64,
}
#[derive(Clone, Copy, Debug, Default, PartialEq, Serialize, Deserialize)]
#[serde(rename_all = "camelCase")]
pub struct Position {
pub motion: usize,
pub fraction: f64,
}
impl Timeline {
pub fn new(motions: Vec<Motion>, operations: Vec<OperationRange>, machine: &Machine) -> Timeline {
let times = estimate_cycle_time(&motions, machine);
let mut end_times = Vec::with_capacity(motions.len());
let mut acc = 0.0;
for t in ×.per_motion {
acc += t.seconds.max(0.0);
end_times.push(acc);
}
Timeline { motions, operations, end_times, total_seconds: acc }
}
pub fn from_program(program: &Program, machine: &Machine) -> Timeline {
let motions: Vec<Motion> = program.motions.iter().map(|m| m.motion.clone()).collect();
let operations = program
.sections
.iter()
.map(|s| OperationRange { operation: s.operation.clone(), first: s.first_motion, count: s.motion_count })
.collect();
Timeline::new(motions, operations, machine)
}
pub fn len(&self) -> usize {
self.motions.len()
}
pub fn is_empty(&self) -> bool {
self.motions.is_empty()
}
pub fn start_time(&self, motion: usize) -> f64 {
if motion == 0 || self.end_times.is_empty() {
0.0
} else {
self.end_times[(motion - 1).min(self.end_times.len() - 1)]
}
}
pub fn time_at(&self, p: Position) -> f64 {
if self.motions.is_empty() {
return 0.0;
}
let m = p.motion.min(self.motions.len() - 1);
let start = self.start_time(m);
start + (self.end_times[m] - start) * p.fraction.clamp(0.0, 1.0)
}
pub fn position_at(&self, seconds: f64) -> Position {
if self.motions.is_empty() {
return Position::default();
}
let t = seconds.clamp(0.0, self.total_seconds);
if t >= self.total_seconds {
return Position { motion: self.motions.len() - 1, fraction: 1.0 };
}
if t <= 0.0 {
return Position::default();
}
let m = self.end_times.partition_point(|e| *e <= t).min(self.motions.len() - 1);
let start = self.start_time(m);
let span = self.end_times[m] - start;
let fraction = if span > 0.0 { ((t - start) / span).clamp(0.0, 1.0) } else { 0.0 };
Position { motion: m, fraction }
}
pub fn operation_of(&self, motion: usize) -> Option<usize> {
self.operations.iter().position(|r| motion >= r.first && motion < r.first + r.count)
}
pub fn pose_at(&self, p: Position) -> Option<(Point3, Point3)> {
if self.motions.is_empty() {
return None;
}
let m = p.motion.min(self.motions.len() - 1);
let f = p.fraction.clamp(0.0, 1.0);
let end = self.motions[m].end();
let axis_end = self.motions[m].tool_axis.unwrap_or(Point3::Z);
let mut prev: Option<(Point3, Point3)> = None;
for k in (0..m).rev() {
if let Some(e) = self.motions[k].end() {
prev = Some((e, self.motions[k].tool_axis.unwrap_or(Point3::Z)));
break;
}
}
match (end, prev) {
(Some(to), Some((from, a0))) => {
let axis = crate::multiaxis::slerp(a0.normalized().unwrap_or(Point3::Z), axis_end.normalized().unwrap_or(Point3::Z), f);
let tip = match &self.motions[m].kind {
MotionKind::ArcCw { center, plane, .. } | MotionKind::ArcCcw { center, plane, .. } => {
let cw = matches!(self.motions[m].kind, MotionKind::ArcCw { .. });
match arc_sweep(from, to, *center, *plane, cw) {
Some((sweep, _)) => {
let (cu, cv, _) = plane_coords(*center, *plane);
let (u0, v0, w0) = plane_coords(from, *plane);
let (_, _, w1) = plane_coords(to, *plane);
let r = (u0 - cu).hypot(v0 - cv);
let a0 = (v0 - cv).atan2(u0 - cu);
let a = a0 + if cw { -sweep * f } else { sweep * f };
from_plane_coords(cu + r * a.cos(), cv + r * a.sin(), w0 + (w1 - w0) * f, *plane)
}
None => from.lerp(to, f),
}
}
_ => from.lerp(to, f),
};
Some((tip, axis))
}
(Some(to), None) => Some((to, axis_end)),
(None, prev) => prev,
}
}
}
#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Serialize, Deserialize)]
#[serde(rename_all = "kebab-case")]
pub enum PlayState {
#[default]
Paused,
Playing,
}
#[derive(Clone, Debug, Default, PartialEq, Serialize, Deserialize)]
#[serde(rename_all = "camelCase")]
pub struct Player {
pub timeline: Timeline,
pub position: Position,
pub state: PlayState,
pub speed: f64,
pub stop_at_collision: bool,
}
impl Player {
pub fn new(timeline: Timeline) -> Player {
Player { timeline, position: Position::default(), state: PlayState::Paused, speed: 1.0, stop_at_collision: true }
}
pub fn play(&mut self) {
if !self.is_at_end() {
self.state = PlayState::Playing;
}
}
pub fn pause(&mut self) {
self.state = PlayState::Paused;
}
pub fn toggle(&mut self) {
match self.state {
PlayState::Playing => self.pause(),
PlayState::Paused => self.play(),
}
}
pub fn set_speed(&mut self, speed: f64) -> Result<(), CamError> {
if !speed.is_finite() || speed <= 0.0 {
return Err(CamError::InvalidInput("player speed must be finite and positive".into()));
}
self.speed = speed;
Ok(())
}
pub fn is_at_end(&self) -> bool {
self.timeline.is_empty() || (self.position.motion + 1 >= self.timeline.len() && self.position.fraction >= 1.0)
}
pub fn time(&self) -> f64 {
self.timeline.time_at(self.position)
}
pub fn progress(&self) -> f64 {
if self.timeline.total_seconds > 0.0 {
self.time() / self.timeline.total_seconds
} else {
0.0
}
}
pub fn seek_motion(&mut self, motion: usize) {
if self.timeline.is_empty() {
return;
}
self.position = Position { motion: motion.min(self.timeline.len() - 1), fraction: 0.0 };
}
pub fn scrub_time(&mut self, seconds: f64) {
self.position = self.timeline.position_at(seconds);
}
pub fn scrub(&mut self, fraction: f64) {
self.scrub_time(fraction.clamp(0.0, 1.0) * self.timeline.total_seconds);
}
pub fn step_forward(&mut self) {
if self.timeline.is_empty() {
return;
}
let last = self.timeline.len() - 1;
if self.position.fraction >= 1.0 && self.position.motion < last {
self.position = Position { motion: self.position.motion + 1, fraction: 1.0 };
} else {
self.position.fraction = 1.0;
}
if self.is_at_end() {
self.state = PlayState::Paused;
}
}
pub fn step_back(&mut self) {
if self.position.fraction > 0.0 {
self.position.fraction = 0.0;
} else if self.position.motion > 0 {
self.position = Position { motion: self.position.motion - 1, fraction: 0.0 };
}
}
pub fn advance(&mut self, dt: f64) -> bool {
if self.state != PlayState::Playing || !dt.is_finite() || dt <= 0.0 || self.timeline.is_empty() {
return false;
}
let before = self.position;
let t = self.time() + dt * self.speed;
self.position = self.timeline.position_at(t);
if t >= self.timeline.total_seconds {
self.position = Position { motion: self.timeline.len() - 1, fraction: 1.0 };
self.state = PlayState::Paused;
}
self.position != before
}
pub fn completed(&self) -> usize {
if self.timeline.is_empty() {
0
} else if self.position.fraction >= 1.0 {
(self.position.motion + 1).min(self.timeline.len())
} else {
self.position.motion
}
}
pub fn pose(&self) -> Option<(Point3, Point3)> {
self.timeline.pose_at(self.position)
}
}
#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(tag = "kind", rename_all = "camelCase")]
pub enum StockModel {
Dexel(DexelGrid),
TriDexel(TriDexel),
Turning(TurningStock),
}
impl StockModel {
pub fn mesh(&self) -> StockMesh {
match self {
StockModel::Dexel(g) => stock_mesh(g),
StockModel::TriDexel(t) => stock_mesh(&t.to_dexel_grid()),
StockModel::Turning(t) => stock_mesh(&t.view),
}
}
pub fn volume(&self) -> f64 {
match self {
StockModel::Dexel(g) => g.volume(),
StockModel::TriDexel(t) => t.volume(),
StockModel::Turning(t) => t.volume(),
}
}
}
#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(rename_all = "camelCase")]
#[serde(default)]
pub struct DriverOptions {
pub checkpoint_every: usize,
pub max_checkpoints: usize,
pub removal: RemovalOptions,
pub tridexel: TriDexelOptions,
}
impl Default for DriverOptions {
fn default() -> Self {
Self { checkpoint_every: 50, max_checkpoints: 16, removal: RemovalOptions::default(), tridexel: TriDexelOptions::default() }
}
}
#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(rename_all = "camelCase")]
pub struct RangeCutter {
pub first: usize,
pub count: usize,
pub cutter: Option<Cutter>,
pub profile: Option<ToolProfile>,
#[serde(default, skip_serializing_if = "Option::is_none")]
pub turning: Option<TurningRange>,
}
#[derive(Clone, Copy, Debug, PartialEq, Eq, Serialize, Deserialize)]
#[serde(rename_all = "kebab-case")]
pub enum AdvanceOutcome {
Reached,
Budget,
Stopped,
}
#[derive(Clone, Debug, PartialEq)]
pub struct SimDriver {
initial: StockModel,
pub stock: StockModel,
pub applied: usize,
checkpoints: Vec<(usize, StockModel)>,
pub options: DriverOptions,
pub removed_volume: f64,
pub unsupported: Vec<usize>,
pub warnings: Vec<(usize, Diagnostic)>,
pub version: u64,
pub chain: Option<crate::kinematics::Chain>,
}
pub fn sim_chain(machine: Option<&Machine>) -> Option<crate::kinematics::Chain> {
machine.and_then(|m| crate::kinematics::Chain::from_machine(m, Point3::ZERO).ok())
}
pub fn uses_rotary(motions: &[Motion]) -> bool {
motions.iter().any(|m| {
matches!(
m.kind,
MotionKind::RotaryPosition { .. }
| MotionKind::LinearRotary { .. }
| MotionKind::PolarInterpolation { .. }
| MotionKind::CylindricalInterpolation { .. }
)
})
}
fn machine_state_before(motions: &[Motion], from: usize) -> Vec<Motion> {
let mut words: Vec<crate::model::RotaryWord> = Vec::new();
let mut polar = false;
let mut cylindrical: Option<f64> = None;
for m in &motions[..from] {
match &m.kind {
MotionKind::RotaryPosition { axes } | MotionKind::LinearRotary { axes, .. } => {
for w in axes {
match words.iter_mut().find(|v| v.name.eq_ignore_ascii_case(&w.name)) {
Some(v) => v.angle_deg = w.angle_deg,
None => words.push(w.clone()),
}
}
}
MotionKind::PolarInterpolation { enabled } => polar = *enabled,
MotionKind::CylindricalInterpolation { radius } => cylindrical = *radius,
_ => {}
}
}
let mut out = Vec::new();
if words.iter().any(|w| w.angle_deg != 0.0) {
out.push(Motion::new(MotionKind::RotaryPosition { axes: words }, crate::model::Feed::RAPID));
}
if polar {
out.push(Motion::new(MotionKind::PolarInterpolation { enabled: true }, crate::model::Feed::RAPID));
}
if cylindrical.is_some() {
out.push(Motion::new(MotionKind::CylindricalInterpolation { radius: cylindrical }, crate::model::Feed::RAPID));
}
out
}
impl SimDriver {
pub fn with_chain(mut self, chain: Option<crate::kinematics::Chain>) -> SimDriver {
self.chain = chain;
self
}
pub fn new(stock: StockModel, options: DriverOptions) -> Result<SimDriver, CamError> {
if options.checkpoint_every == 0 || options.max_checkpoints == 0 {
return Err(CamError::InvalidInput("driver: checkpoint interval and count must be positive".into()));
}
options.tridexel.validate()?;
Ok(SimDriver {
initial: stock.clone(),
stock,
applied: 0,
checkpoints: Vec::new(),
options,
removed_volume: 0.0,
unsupported: Vec::new(),
warnings: Vec::new(),
version: 0,
chain: None,
})
}
pub fn rewind(&mut self, target: usize) {
if target < self.applied {
self.rewind_to(target);
}
}
fn rewind_to(&mut self, target: usize) {
while let Some((at, _)) = self.checkpoints.last() {
if *at <= target {
break;
}
self.checkpoints.pop();
}
match self.checkpoints.last() {
Some((at, s)) => {
self.stock = s.clone();
self.applied = *at;
}
None => {
self.stock = self.initial.clone();
self.applied = 0;
}
}
self.removed_volume = self.initial.volume() - self.stock.volume();
let applied = self.applied;
self.unsupported.retain(|i| *i < applied);
self.warnings.retain(|(at, _)| *at <= applied);
self.version += 1;
}
pub fn advance_to(
&mut self,
timeline: &Timeline,
target: usize,
cutter: &Cutter,
profile: &ToolProfile,
tolerance: &Tolerance,
runner: &mut Runner<'_>,
) -> Result<(), CamError> {
let target = target.min(timeline.len());
if target < self.applied {
self.rewind_to(target);
}
while self.applied < target {
let next_checkpoint = ((self.applied / self.options.checkpoint_every) + 1) * self.options.checkpoint_every;
let stop = target.min(next_checkpoint);
self.apply_slice(timeline, self.applied, stop, cutter, profile, tolerance, runner)?;
if stop == next_checkpoint && stop < timeline.len() {
self.checkpoints.push((stop, self.stock.clone()));
if self.checkpoints.len() > self.options.max_checkpoints {
self.checkpoints.remove(0);
}
}
}
Ok(())
}
fn apply_slice(
&mut self,
timeline: &Timeline,
from: usize,
to: usize,
cutter: &Cutter,
profile: &ToolProfile,
tolerance: &Tolerance,
runner: &mut Runner<'_>,
) -> Result<(), CamError> {
let mut slice: Vec<Motion> = machine_state_before(&timeline.motions, from);
for k in (0..from).rev() {
if let Some(e) = timeline.motions[k].end() {
let mut m = Motion::rapid(e);
m.tool_axis = timeline.motions[k].tool_axis;
slice.push(m);
break;
}
}
let offset = slice.len();
slice.extend(timeline.motions[from..to].iter().cloned());
if let StockModel::Turning(turned) = &self.stock {
self.stock = StockModel::Dexel(turned.view.clone());
}
let before = self.stock.volume();
match &mut self.stock {
StockModel::Dexel(grid) => {
let report = match &self.chain {
Some(chain) => simulate_on_machine(grid, cutter, &slice, tolerance, &self.options.removal, chain, runner)?,
None => simulate(grid, cutter, &slice, tolerance, &self.options.removal, runner)?,
};
for i in report.unsupported {
if i >= offset {
self.unsupported.push(from + i - offset);
}
}
self.warnings.extend(report.warnings.into_iter().map(|w| (to, w)));
}
StockModel::TriDexel(td) => {
let report = td.simulate(profile, &slice, &self.options.tridexel, tolerance, runner)?;
self.warnings.extend(report.warnings.into_iter().map(|w| (to, w)));
}
StockModel::Turning(_) => return Err(CamError::InvalidInput("turning stock requires a turning range".into())),
}
self.removed_volume += before - self.stock.volume();
self.applied = to;
self.version += 1;
Ok(())
}
pub fn advance_ranges(
&mut self,
timeline: &Timeline,
ranges: &[RangeCutter],
target: usize,
budget: usize,
tolerance: &Tolerance,
keep_going: &mut dyn FnMut(usize) -> bool,
) -> Result<AdvanceOutcome, CamError> {
let target = target.min(timeline.len());
self.rewind(target);
if ranges.is_empty() && self.applied < target {
return Err(CamError::InvalidInput("driver: no cutter for the timeline's motions".into()));
}
let mut done = 0usize;
while self.applied < target {
if done >= budget {
return Ok(AdvanceOutcome::Budget);
}
if done > 0 && !keep_going(self.applied) {
return Ok(AdvanceOutcome::Stopped);
}
let at = self.applied;
let range = ranges.iter().find(|r| at >= r.first && at < r.first + r.count).unwrap_or(&ranges[0]);
let mut sink = crate::NullSink;
if let Some(turning) = &range.turning {
let StockModel::Turning(stock)=&mut self.stock else {return Err(CamError::InvalidInput("turning range requires axisymmetric stock".into()));};
if range.cutter.is_some() || range.profile.is_some() {return Err(CamError::InvalidInput("turning range cannot also carry a mill cutter".into()));}
let start=timeline.motions[..at].iter().rev().find_map(|m|m.end()).or_else(||timeline.motions[at..].iter().find_map(|m|m.end())).ok_or_else(||CamError::InvalidInput("turning timeline has no position".into()))?;
let state=crate::turning::sim::StockState::new(stock.section.clone(),*tolerance)?;
let mut path=crate::Toolpath::new("turning-replay",turning.tool.clone());path.motions.push(timeline.motions[at].clone());
let next=state.simulate(&path,start,&turning.envelope,&turning.holders,&turning.protected,turning.chuck_z,*tolerance,&mut Runner::detached(&mut sink))?;
let before=stock.volume();stock.section=next.section().clone();stock.refresh_view();
self.removed_volume+=before-stock.volume();self.applied=at+1;self.version+=1;
if self.applied%self.options.checkpoint_every==0 {
self.checkpoints.push((self.applied,self.stock.clone()));
if self.checkpoints.len()>self.options.max_checkpoints {self.checkpoints.remove(0);}
}
} else {
let cutter=range.cutter.as_ref().ok_or_else(||CamError::InvalidInput("mill range has no cutter".into()))?;
let profile=range.profile.as_ref().ok_or_else(||CamError::InvalidInput("mill range has no profile".into()))?;
self.advance_to(timeline, at + 1, cutter, profile, tolerance, &mut Runner::detached(&mut sink))?;
}
done += 1;
}
Ok(AdvanceOutcome::Reached)
}
pub fn checkpoint_count(&self) -> usize {
self.checkpoints.len()
}
}