Round gcode output within provided tolerance, fixes #34

lib/Cargo.toml

@@ -12,6 +12,7 @@ serde = ["dep:serde", "dep:serde_repr", "g-code/serde"]
 
 [dependencies]
 g-code.workspace = true
+rust_decimal = { version = "1", default-features = false }
 lyon_geom = "=1.0.6"
 euclid = "0.22"
 log.workspace = true

lib/src/turtle/g_code.rs

@@ -2,6 +2,7 @@ use std::{borrow::Cow, fmt::Debug};
 
 use ::g_code::{command, emit::Token};
 use lyon_geom::{CubicBezierSegment, Point, QuadraticBezierSegment, SvgArc};
+use rust_decimal::{Decimal, prelude::*};
 
 use super::Turtle;
 use crate::{
@@ -19,20 +20,77 @@ pub struct GCodeTurtle<'input> {
 }
 
 impl<'input> GCodeTurtle<'input> {
+    /// Converts [`Self::tolerance`] to a [`Decimal`].
+    fn tolerance_to_decimal(&self) -> Decimal {
+        Decimal::from_f64(self.tolerance)
+            .unwrap_or_default()
+            .normalize()
+    }
+
+    /// Rounds `val` to the number of decimal places in [`Self::tolerance`].
+    ///
+    /// i.e. 3 decimal places for 0.002
+    fn round(&self, val: f64) -> f64 {
+        let places = self.tolerance_to_decimal().scale();
+        Decimal::from_f64(val)
+            .unwrap_or_default()
+            .round_dp(places)
+            .to_f64()
+            .unwrap_or(val)
+    }
+
+    /// Tolerance passed to [`lyon_geom`] calls for line segments.
+    ///
+    /// Reserves some headroom so that [`Self::round`] won't take
+    /// the measurement outside of the overall tolernace bounds.
+    ///
+    /// i.e. e.g. 0.002 & 3 dp returns 0.0015 so we have +/- 0.0005
+    fn flattening_tolerance(&self) -> f64 {
+        let tolerance = self.tolerance_to_decimal();
+        let rounding_epsilon = Decimal::new(5, tolerance.scale() + 1);
+        if rounding_epsilon < tolerance {
+            (tolerance - rounding_epsilon)
+                .to_f64()
+                .unwrap_or(self.tolerance)
+        } else {
+            self.tolerance
+        }
+    }
+
+    /// Tolerance passed to [`lyon_geom`] calls when circular interpolation is used.
+    ///
+    /// For G2/G3 arcs, X, Y, and R are all rounded independently.
+    /// The worst-case combined error is:
+    /// - endpoint 2D: `rounding_epsilon * sqrt(2)`
+    /// - radius: `rounding_epsilon`
+    /// - total: `rounding_epsilon * (sqrt(2) + 1)`
+    fn arc_flattening_tolerance(&self) -> f64 {
+        let tolerance = self.tolerance_to_decimal();
+        let rounding_epsilon = Decimal::new(5, tolerance.scale() + 1)
+            * Decimal::from_f64(std::f64::consts::SQRT_2 + 1.).unwrap();
+        if rounding_epsilon < tolerance {
+            (tolerance - rounding_epsilon)
+                .to_f64()
+                .unwrap_or(self.tolerance)
+        } else {
+            self.tolerance
+        }
+    }
+
     fn circular_interpolation(&self, svg_arc: SvgArc<f64>) -> Vec<Token<'input>> {
         debug_assert!((svg_arc.radii.x.abs() - svg_arc.radii.y.abs()).abs() < f64::EPSILON);
         match (svg_arc.flags.large_arc, svg_arc.flags.sweep) {
             (false, true) => command!(CounterclockwiseCircularInterpolation {
-                X: svg_arc.to.x,
-                Y: svg_arc.to.y,
-                R: svg_arc.radii.x,
+                X: self.round(svg_arc.to.x),
+                Y: self.round(svg_arc.to.y),
+                R: self.round(svg_arc.radii.x),
                 F: self.feedrate,
             })
             .into_token_vec(),
             (false, false) => command!(ClockwiseCircularInterpolation {
-                X: svg_arc.to.x,
-                Y: svg_arc.to.y,
-                R: svg_arc.radii.x,
+                X: self.round(svg_arc.to.x),
+                Y: self.round(svg_arc.to.y),
+                R: self.round(svg_arc.radii.x),
                 F: self.feedrate,
             })
             .into_token_vec(),
@@ -80,16 +138,21 @@ impl<'input> Turtle for GCodeTurtle<'input> {
 
     fn move_to(&mut self, to: Point<f64>) {
         self.tool_off();
-        self.program
-            .append(&mut command!(RapidPositioning { X: to.x, Y: to.y }).into_token_vec());
+        self.program.append(
+            &mut command!(RapidPositioning {
+                X: self.round(to.x),
+                Y: self.round(to.y),
+            })
+            .into_token_vec(),
+        );
     }
 
     fn line_to(&mut self, to: Point<f64>) {
         self.tool_on();
         self.program.append(
             &mut command!(LinearInterpolation {
-                X: to.x,
-                Y: to.y,
+                X: self.round(to.x),
+                Y: self.round(to.y),
                 F: self.feedrate,
             })
             .into_token_vec(),
@@ -109,7 +172,7 @@ impl<'input> Turtle for GCodeTurtle<'input> {
             .supported_functionality()
             .circular_interpolation
         {
-            FlattenWithArcs::flattened(&svg_arc, self.tolerance)
+            FlattenWithArcs::flattened(&svg_arc, self.arc_flattening_tolerance())
                 .into_iter()
                 .for_each(|segment| match segment {
                     ArcOrLineSegment::Arc(arc) => {
@@ -122,7 +185,7 @@ impl<'input> Turtle for GCodeTurtle<'input> {
         } else {
             svg_arc
                 .to_arc()
-                .flattened(self.tolerance)
+                .flattened(self.flattening_tolerance())
                 .for_each(|point| self.line_to(point));
         };
     }
@@ -135,7 +198,7 @@ impl<'input> Turtle for GCodeTurtle<'input> {
             .supported_functionality()
             .circular_interpolation
         {
-            FlattenWithArcs::<f64>::flattened(&cbs, self.tolerance)
+            FlattenWithArcs::<f64>::flattened(&cbs, self.arc_flattening_tolerance())
                 .into_iter()
                 .for_each(|segment| match segment {
                     ArcOrLineSegment::Arc(arc) => {
@@ -144,7 +207,7 @@ impl<'input> Turtle for GCodeTurtle<'input> {
                     ArcOrLineSegment::Line(line) => self.line_to(line.to),
                 });
         } else {
-            cbs.flattened(self.tolerance)
+            cbs.flattened(self.flattening_tolerance())
                 .for_each(|point| self.line_to(point));
         };
     }