diff --git a/crates/bone-kernel/src/curve3.rs b/crates/bone-kernel/src/curve3.rs index d9c0f08..eccea69 100644 --- a/crates/bone-kernel/src/curve3.rs +++ b/crates/bone-kernel/src/curve3.rs @@ -1,10 +1,13 @@ -use bone_types::{Aabb3, ChordHeightTolerance, Parameter, Point3, Tolerance, Vec3}; +use bone_types::{Aabb3, Angle, ChordHeightTolerance, Parameter, Point3, Tolerance, Vec3, radian}; +use core::f64::consts::TAU; use crate::arc3::Arc3; use crate::circle3::Circle3; use crate::closest::ClosestPoint3; use crate::line3::Line3; +const SUB_CURVE_TOLERANCE: Tolerance = Tolerance::new(1.0e-9); + pub trait Curve3 { fn evaluate(&self, t: Parameter) -> Point3; fn derivative(&self, t: Parameter) -> Vec3; @@ -20,6 +23,40 @@ pub enum Curve3Kind { Circle(Circle3), } +impl Curve3Kind { + #[must_use] + pub fn sub_curve(self, from: Parameter, to: Parameter) -> Option { + match self { + Self::Line(line) => { + Line3::new(line.evaluate(from), line.evaluate(to), SUB_CURVE_TOLERANCE) + .ok() + .map(Line3::as_kind) + } + Self::Arc(arc) => { + let sweep = arc.sweep_rad(); + Arc3::new( + arc.plane(), + arc.radius(), + Angle::new::(sweep.mul_add(from.value(), arc.start_rad())), + Angle::new::(sweep * (to.value() - from.value())), + SUB_CURVE_TOLERANCE, + ) + .ok() + .map(Arc3::as_kind) + } + Self::Circle(circle) => Arc3::new( + circle.plane(), + circle.radius(), + Angle::new::(TAU * from.value()), + Angle::new::(TAU * (to.value() - from.value())), + SUB_CURVE_TOLERANCE, + ) + .ok() + .map(Arc3::as_kind), + } + } +} + impl Curve3 for Curve3Kind { fn evaluate(&self, t: Parameter) -> Point3 { match self { diff --git a/crates/bone-kernel/src/sweep_surface.rs b/crates/bone-kernel/src/sweep_surface.rs index 97d410c..87a3b5d 100644 --- a/crates/bone-kernel/src/sweep_surface.rs +++ b/crates/bone-kernel/src/sweep_surface.rs @@ -1,11 +1,19 @@ -use bone_types::{Length, Parameter, Plane3, Point2, Point3, Tolerance, UnitVec3, Vec3}; +use bone_types::{ + Aabb3, Angle, ChordHeightTolerance, Length, Parameter, Plane3, Point2, Point3, Tolerance, + UnitVec3, Vec3, radian, +}; use uom::si::length::millimeter; +use crate::circle2::Circle2; +use crate::closest::ClosestPoint3; use crate::curve2::Curve2Kind; -use crate::curve3::Curve3; +use crate::curve3::{Curve3, Curve3Kind}; +use crate::ellipse2::Ellipse2; use crate::nurbs::{basis_functions, count_to_f64, find_span}; const TRANSPORT_TOLERANCE: Tolerance = Tolerance::new(1.0e-12); +const CHAIN_TOLERANCE_MM: f64 = 1.0e-6; +const SMOOTH_TANGENT_TOLERANCE: f64 = 1.0e-6; #[derive(Copy, Clone, Debug, PartialEq, Eq)] pub struct SampleCount(u32); @@ -87,6 +95,31 @@ impl ArcLengthTable { Self { nodes, total_mm } } + fn fraction_of(&self, native: Parameter) -> ArcFraction { + if self.total_mm <= f64::EPSILON { + return ArcFraction::START; + } + let target = native.value(); + let last = self.nodes.len() - 1; + let length = match self.nodes.binary_search_by(|(t, _)| t.total_cmp(&target)) { + Ok(index) => self.nodes[index].1, + Err(0) => self.nodes[0].1, + Err(index) if index > last => self.nodes[last].1, + Err(index) => { + let (t0, l0) = self.nodes[index - 1]; + let (t1, l1) = self.nodes[index]; + let span = t1 - t0; + let weight = if span.abs() <= f64::EPSILON { + 0.0 + } else { + (target - t0) / span + }; + (l1 - l0).mul_add(weight, l0) + } + }; + ArcFraction::new(length / self.total_mm) + } + fn native_at(&self, fraction: ArcFraction) -> Parameter { let target = fraction.get() * self.total_mm; let last = self.nodes.len() - 1; @@ -137,6 +170,16 @@ impl<'c, C: Curve3> SweepPath3<'c, C> { self.curve.evaluate(self.table.native_at(fraction)) } + #[must_use] + pub fn native_at(&self, fraction: ArcFraction) -> Parameter { + self.table.native_at(fraction) + } + + #[must_use] + pub fn fraction_at(&self, native: Parameter) -> ArcFraction { + self.table.fraction_of(native) + } + #[must_use] pub fn tangent_at(&self, fraction: ArcFraction) -> UnitVec3 { let native = self.table.native_at(fraction); @@ -145,6 +188,26 @@ impl<'c, C: Curve3> SweepPath3<'c, C> { .try_normalize(TRANSPORT_TOLERANCE) .unwrap_or_else(|_| UnitVec3::z_axis()) } + + #[must_use] + pub fn min_curvature_radius_mm(&self, stations: StationCount) -> f64 { + let n = stations.get(); + if n < 1 || self.table.total_mm <= TRANSPORT_TOLERANCE.value() { + return f64::INFINITY; + } + let step_mm = self.table.total_mm / f64::from(n); + let tangents: Vec = (0..=n) + .map(|i| dir(self.tangent_at(ArcFraction::new(f64::from(i) / f64::from(n))))) + .collect(); + tangents.windows(2).fold(f64::INFINITY, |tightest, pair| { + let turn = pair[0].dot_mm2(pair[1]).clamp(-1.0, 1.0).acos(); + if turn <= f64::EPSILON { + tightest + } else { + tightest.min(step_mm / turn) + } + }) + } } fn dir(unit: UnitVec3) -> Vec3 { @@ -235,6 +298,33 @@ impl Frame { Self::complete(origin, tangent, reference) } + #[must_use] + pub fn reorient(self, tangent: UnitVec3) -> Self { + let (t0x, t0y, t0z) = self.tangent.components(); + let (t1x, t1y, t1z) = tangent.components(); + let (ax, ay, az) = ( + t0y.mul_add(t1z, -(t0z * t1y)), + t0z.mul_add(t1x, -(t0x * t1z)), + t0x.mul_add(t1y, -(t0y * t1x)), + ); + let sin = (ax * ax + ay * ay + az * az).sqrt(); + if sin <= TRANSPORT_TOLERANCE.value() { + return Self::complete(self.origin, tangent, dir(self.normal)); + } + let cos = t0x.mul_add(t1x, t0y.mul_add(t1y, t0z * t1z)); + let (kx, ky, kz) = (ax / sin, ay / sin, az / sin); + let (nx, ny, nz) = self.normal.components(); + let dot = kx.mul_add(nx, ky.mul_add(ny, kz * nz)) * (1.0 - cos); + let rotate = + |value: f64, cross: f64, axis: f64| value.mul_add(cos, cross.mul_add(sin, axis * dot)); + let reference = Vec3::from_mm( + rotate(nx, ky.mul_add(nz, -(kz * ny)), kx), + rotate(ny, kz.mul_add(nx, -(kx * nz)), ky), + rotate(nz, kx.mul_add(ny, -(ky * nx)), kz), + ); + Self::complete(self.origin, tangent, reference) + } + #[must_use] pub fn section_plane(self) -> Plane3 { Plane3::new_unchecked(self.origin, self.normal, self.binormal) @@ -246,6 +336,62 @@ impl Frame { let (au, av) = anchor.coords_mm(); self.section_plane().point_at_local(u - au, v - av, 0.0) } + + #[must_use] + pub fn twist(self, angle_rad: f64) -> Self { + let (sin, cos) = angle_rad.sin_cos(); + let normal = dir(self.normal) * cos + dir(self.binormal) * sin; + Self::complete(self.origin, self.tangent, normal) + } + + #[must_use] + pub fn locked(origin: Point3, tangent: UnitVec3, up: UnitVec3) -> Self { + let along = dir(tangent); + let raw = dir(up); + let reference = raw - along * raw.dot_mm2(along); + Self::complete(origin, tangent, reference) + } + + #[must_use] + pub fn twist_from(self, other: Self) -> f64 { + let cos = dir(self.normal).dot_mm2(dir(other.normal)); + let sin = dir(self.normal) + .cross(dir(other.normal)) + .dot_mm2(dir(self.tangent)); + sin.atan2(cos) + } + + #[must_use] + pub fn translated(self, origin: Point3) -> Self { + Self { origin, ..self } + } + + #[must_use] + pub fn blend_tangent(self, target: UnitVec3, weight: f64) -> Self { + if weight <= TRANSPORT_TOLERANCE.value() { + return self; + } + self.reorient(slerp_unit(self.tangent, target, weight)) + } +} + +fn slerp_unit(from: UnitVec3, to: UnitVec3, weight: f64) -> UnitVec3 { + let a = dir(from); + let b = dir(to); + let theta = a.dot_mm2(b).clamp(-1.0, 1.0).acos(); + if theta <= TRANSPORT_TOLERANCE.value() { + return to; + } + let sin = theta.sin(); + let scaled = a * (((1.0 - weight) * theta).sin() / sin) + b * ((weight * theta).sin() / sin); + scaled.try_normalize(TRANSPORT_TOLERANCE).unwrap_or(to) +} + +#[derive(Copy, Clone, Debug, PartialEq)] +pub enum FrameGuide { + MinimumTwist, + KeepNormal, + DirectionLocked(UnitVec3), } #[derive(Clone, Debug, PartialEq)] @@ -282,7 +428,10 @@ pub fn chord_params(points: &[Point3]) -> Option> { .map(|pair| (pair[1] - pair[0]).norm_mm()) .collect(); let total: f64 = chords.iter().sum(); - if total <= TRANSPORT_TOLERANCE.value() { + if chords + .iter() + .any(|&chord| chord <= TRANSPORT_TOLERANCE.value()) + { return None; } let params = chords.iter().scan(0.0, |running, chord| { @@ -375,184 +524,859 @@ impl PathInterpolator { } } +pub struct ClampedInterpolator { + knots: Vec, + lu: nalgebra::LU, +} + +impl ClampedInterpolator { + #[must_use] + pub fn new(params: &[f64]) -> Option { + let n = params.len().checked_sub(1)?; + if n < 2 { + return None; + } + let interior = (1..n).map(|j| (params[j - 1] + params[j] + params[j + 1]) / 3.0); + let knots: Vec = core::iter::repeat_n(0.0, 4) + .chain(interior) + .chain(core::iter::repeat_n(1.0, 4)) + .collect(); + let count = n + 3; + if knots[4] <= 0.0 || knots[count - 1] >= 1.0 { + return None; + } + let start_scale = 3.0 / knots[4]; + let end_scale = 3.0 / (1.0 - knots[count - 1]); + let basis_row = |row: usize, col: usize| { + let u = params[row - 1]; + let span = find_span(count - 1, 3, u, &knots); + let first = span - 3; + if col >= first && col <= span { + basis_functions(span, u, 3, &knots)[col - first] + } else { + 0.0 + } + }; + let matrix = nalgebra::DMatrix::from_fn(count, count, |row, col| { + if row == 0 { + f64::from(u8::from(col == 0)) + } else if row == 1 { + match col { + 0 => -start_scale, + 1 => start_scale, + _ => 0.0, + } + } else if row == count - 2 { + if col == count - 2 { + -end_scale + } else if col == count - 1 { + end_scale + } else { + 0.0 + } + } else if row == count - 1 { + f64::from(u8::from(col == count - 1)) + } else { + basis_row(row, col) + } + }); + Some(Self { + knots, + lu: matrix.lu(), + }) + } + + #[must_use] + pub fn knots(&self) -> &[f64] { + &self.knots + } + + #[must_use] + pub fn solve(&self, points: &[Point3], start: Vec3, end: Vec3) -> Option { + let count = self.knots.len() - 4; + if points.len() + 2 != count { + return None; + } + let coords = |point: Point3| { + let (x, y, z) = point.coords_mm(); + [x, y, z] + }; + let derivative = |vector: Vec3| { + let (x, y, z) = vector.coords_mm(); + [x, y, z] + }; + let rhs = nalgebra::DMatrix::from_fn(count, 3, |row, col| { + let values = if row == 0 { + coords(points[0]) + } else if row == 1 { + derivative(start) + } else if row == count - 2 { + derivative(end) + } else if row == count - 1 { + coords(points[points.len() - 1]) + } else { + coords(points[row - 1]) + }; + values[col] + }); + let solution = self.lu.solve(&rhs)?; + let control = (0..count) + .map(|row| Point3::from_mm(solution[(row, 0)], solution[(row, 1)], solution[(row, 2)])) + .collect(); + Some(BSpline3 { + degree: 3, + knots: self.knots.clone(), + control, + }) + } +} + #[must_use] pub fn transport( path: &SweepPath3, profile_plane: Plane3, stations: StationCount, ) -> Vec { - let n = stations.get(); - (0..=n) - .map(|i| ArcFraction::new(f64::from(i) / f64::from(n))) - .fold( - Vec::with_capacity(usize::try_from(n).unwrap_or(0) + 1), - |mut frames, fraction| { - let origin = path.point_at(fraction); - let tangent = path.tangent_at(fraction); - let frame = match frames.last() { - None => Frame::seed(origin, tangent, profile_plane), - Some(previous) => previous.propagate(origin, tangent), - }; - frames.push(frame); - frames - }, - ) + transport_with(path, profile_plane, stations, FrameGuide::MinimumTwist) } -#[derive(Clone, Debug, PartialEq)] -pub struct SectionSpline { - pub degree: usize, - pub knots: Vec, - pub control: Vec, - pub weights: Vec, +#[must_use] +pub fn transport_with( + path: &SweepPath3, + profile_plane: Plane3, + stations: StationCount, + guide: FrameGuide, +) -> Vec { + let (fractions, seed_index) = station_fractions(ArcFraction::START, stations); + transport_stations(path, profile_plane, &fractions, seed_index, guide) } -fn segment_count(sweep_abs: f64) -> usize { - let quarter = core::f64::consts::FRAC_PI_2; - let eps = 1.0e-9; - if sweep_abs <= quarter + eps { - 1 - } else if sweep_abs <= core::f64::consts::PI + eps { - 2 - } else if sweep_abs <= 3.0 * quarter + eps { - 3 +#[must_use] +pub fn station_fractions(origin: ArcFraction, stations: StationCount) -> (Vec, usize) { + let s0 = origin.get(); + let interior = s0 > f64::EPSILON && 1.0 - s0 > f64::EPSILON; + let n = if interior { + stations.get().max(2) } else { - 4 - } + stations.get() + }; + let backward = if s0 <= f64::EPSILON { + 0 + } else if 1.0 - s0 <= f64::EPSILON { + n + } else { + (1..n) + .min_by(|a, b| { + let miss = |i: u32| (f64::from(i) / f64::from(n) - s0).abs(); + miss(*a).total_cmp(&miss(*b)) + }) + .unwrap_or(0) + }; + let forward = n - backward; + let back = (0..backward).map(|i| s0 * f64::from(i) / f64::from(backward.max(1))); + let fore = + (0..=forward).map(|i| (1.0 - s0).mul_add(f64::from(i) / f64::from(forward.max(1)), s0)); + let fractions: Vec = if forward == 0 { + back.chain(core::iter::once(s0)).collect() + } else { + back.chain(fore).collect() + }; + (fractions, usize::try_from(backward).unwrap_or(0)) } -fn arc_spline(center: Point2, radius: f64, start: f64, sweep: f64) -> SectionSpline { - let segments = segment_count(sweep.abs()); - let step = sweep / count_to_f64(segments); - let mid_weight = (step / 2.0).cos().abs(); - let (cx, cy) = center.coords_mm(); - let ring = |angle: f64| { - Point2::from_mm( - radius.mul_add(angle.cos(), cx), - radius.mul_add(angle.sin(), cy), - ) +#[must_use] +pub fn transport_stations( + path: &SweepPath3, + profile_plane: Plane3, + fractions: &[f64], + seed_index: usize, + guide: FrameGuide, +) -> Vec { + if fractions.is_empty() || seed_index >= fractions.len() { + return Vec::new(); + } + let frame_at = |fraction: f64| { + let at = ArcFraction::new(fraction); + (path.point_at(at), path.tangent_at(at)) }; - let apex = |angle: f64| { - let reach = radius / (step / 2.0).cos().abs(); - Point2::from_mm( - reach.mul_add(angle.cos(), cx), - reach.mul_add(angle.sin(), cy), - ) + let (seed_origin, seed_tangent) = frame_at(fractions[seed_index]); + let seed = Frame::seed(seed_origin, seed_tangent, profile_plane); + let lock_offset = match guide { + FrameGuide::DirectionLocked(axis) => { + Frame::locked(seed_origin, seed_tangent, axis).twist_from(seed) + } + FrameGuide::MinimumTwist | FrameGuide::KeepNormal => 0.0, }; - let arcs = (0..segments).flat_map(|index| { - let a1 = start + count_to_f64(index) * step; - [(apex(a1 + step / 2.0), mid_weight), (ring(a1 + step), 1.0)] - }); - let weighted: Vec<(Point2, f64)> = core::iter::once((ring(start), 1.0)).chain(arcs).collect(); - let internal = (1..segments).flat_map(|index| { - let knot = count_to_f64(index) / count_to_f64(segments); - [knot, knot] - }); - let knots: Vec = [0.0, 0.0, 0.0] + let step = |previous: Frame, fraction: f64| { + let (origin, tangent) = frame_at(fraction); + match guide { + FrameGuide::MinimumTwist => previous.propagate(origin, tangent), + FrameGuide::KeepNormal => seed.translated(origin), + FrameGuide::DirectionLocked(axis) => { + Frame::locked(origin, tangent, axis).twist(lock_offset) + } + } + }; + let forward = fractions[seed_index + 1..] + .iter() + .scan(seed, |carried, &fraction| { + *carried = step(*carried, fraction); + Some(*carried) + }); + let backward = fractions[..seed_index] + .iter() + .rev() + .scan(seed, |carried, &fraction| { + *carried = step(*carried, fraction); + Some(*carried) + }); + let mut leading: Vec = backward.collect(); + leading.reverse(); + leading .into_iter() - .chain(internal) - .chain([1.0, 1.0, 1.0]) - .collect(); - SectionSpline { - degree: 2, - knots, - control: weighted.iter().map(|(point, _)| *point).collect(), - weights: weighted.iter().map(|(_, weight)| *weight).collect(), - } + .chain(core::iter::once(seed)) + .chain(forward) + .collect() } -fn line_spline(start: Point2, end: Point2) -> SectionSpline { - SectionSpline { - degree: 1, - knots: vec![0.0, 0.0, 1.0, 1.0], - control: vec![start, end], - weights: vec![1.0, 1.0], - } +#[derive(Copy, Clone, Debug, PartialEq, Eq)] +pub enum SweepDefect { + EmptyPath, + DisconnectedPath, + BranchingPath, + PathTooTight, + CornerTooTight, + PathMissesProfile, + EmptySpan, + GuideEmpty, + GuideMissesProfile, + GuideCollapse, + GuideSpanDegenerate, + GuideBidirectional, + GuideTwist, + GuideKeepNormal, + OrientationNeedsGuides, + TangencyBidirectional, + AlignmentOrientation, + PathNotPlanar, + AlignmentAlongPath, + SolidToolBoss, + SolidToolCorner, + SolidToolOffBody, } -fn bezier_spline(control: [Point2; 4]) -> SectionSpline { - SectionSpline { - degree: 3, - knots: vec![0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0], - control: control.to_vec(), - weights: vec![1.0; 4], +impl core::fmt::Display for SweepDefect { + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { + f.write_str(match self { + Self::EmptyPath => "an empty sweep path", + Self::GuideEmpty => "a sweep guide with no curves", + Self::DisconnectedPath => "a sweep path whose segments do not meet end to end", + Self::BranchingPath => "a sweep path where more than two segments meet at a point", + Self::PathTooTight => "a sweep path that curves tighter than the profile can follow", + Self::CornerTooTight => "a path corner too tight for the profile to miter", + Self::PathMissesProfile => "a sweep path that never crosses the profile plane", + Self::EmptySpan => "a sweep direction with no path beyond the profile", + Self::GuideMissesProfile => "a guide curve that does not touch the profile", + Self::GuideCollapse => "a guide that pinches the section to a point", + Self::GuideSpanDegenerate => "guide contact points that cannot span the section", + Self::GuideBidirectional => "guide curves on a bidirectional sweep", + Self::GuideTwist => "a twisted sweep with guide curves", + Self::GuideKeepNormal => "a keep-normal sweep with guide curves", + Self::OrientationNeedsGuides => "an orientation with too few guide curves", + Self::TangencyBidirectional => "start or end tangency on a bidirectional sweep", + Self::AlignmentOrientation => "a path alignment without follow-path orientation", + Self::PathNotPlanar => "a planar-alignment sweep along a non-planar path", + Self::AlignmentAlongPath => "a direction vector parallel to the path tangent", + Self::SolidToolBoss => "a solid tool profile on a boss sweep", + Self::SolidToolCorner => "a solid tool path with a corner", + Self::SolidToolOffBody => "a solid tool path that does not start on the tool body", + }) } } -fn ellipse_spline( - center: Point2, - semi_major: f64, - semi_minor: f64, - rotation: f64, -) -> SectionSpline { - let unit = arc_spline(Point2::origin(), 1.0, 0.0, core::f64::consts::TAU); - let (cx, cy) = center.coords_mm(); - let (major_x, major_y) = (rotation.cos(), rotation.sin()); - let (minor_x, minor_y) = (-rotation.sin(), rotation.cos()); - let control = unit - .control - .iter() - .map(|point| { - let (px, py) = point.coords_mm(); - let x = (semi_major * px).mul_add(major_x, (semi_minor * py) * minor_x) + cx; - let y = (semi_major * px).mul_add(major_y, (semi_minor * py) * minor_y) + cy; - Point2::from_mm(x, y) - }) - .collect(); - SectionSpline { - degree: unit.degree, - knots: unit.knots, - control, - weights: unit.weights, - } +#[derive(Copy, Clone, Debug, PartialEq)] +struct OrientedSegment { + curve: Curve3Kind, + reversed: bool, } -#[must_use] -pub fn section_spline(curve: Curve2Kind) -> SectionSpline { - match curve { - Curve2Kind::Line(line) => line_spline(line.start(), line.end()), - Curve2Kind::Arc(arc) => arc_spline( - arc.center(), - arc.radius_mm(), - arc.start_rad(), - arc.sweep_rad(), - ), - Curve2Kind::Circle(circle) => arc_spline( - circle.center(), - circle.radius_mm(), - 0.0, - core::f64::consts::TAU, - ), - Curve2Kind::Ellipse(ellipse) => ellipse_spline( - ellipse.center(), - ellipse.semi_major_mm(), - ellipse.semi_minor_mm(), - ellipse.rotation_rad(), - ), - Curve2Kind::Bezier(bezier) => bezier_spline(bezier.control()), +impl OrientedSegment { + fn param(self, t: Parameter) -> Parameter { + if self.reversed { + Parameter::new(1.0 - t.value()) + } else { + t + } + } + + fn head(self) -> Point3 { + self.evaluate(Parameter::new(0.0)) + } + + fn tail(self) -> Point3 { + self.evaluate(Parameter::new(1.0)) } } -#[cfg(test)] -mod tests { - use super::{ - ArcFraction, BSpline3, PathInterpolator, SampleCount, SectionSpline, StationCount, - SweepPath3, chord_params, section_spline, transport, - }; - use crate::circle3::Circle3; - use crate::curve2::Curve2Kind; - use crate::curve3::Curve3; - use crate::line3::Line3; - use crate::nurbs::{Degree, KnotVector, RationalControlPoint}; - use crate::nurbs_curve::NurbsCurve3; - use bone_types::{ - Aabb3, ChordHeightTolerance, Length, Parameter, Plane3, Point2, Point3, Tolerance, - UnitVec3, Vec3, - }; - use core::f64::consts::TAU; - use uom::si::length::millimeter; +impl Curve3 for OrientedSegment { + fn evaluate(&self, t: Parameter) -> Point3 { + self.curve.evaluate(self.param(t)) + } - const TOL: Tolerance = Tolerance::new(1.0e-9); + fn derivative(&self, t: Parameter) -> Vec3 { + let raw = self.curve.derivative(self.param(t)); + if self.reversed { -raw } else { raw } + } + + fn bounding_box(&self) -> Aabb3 { + self.curve.bounding_box() + } + + fn closest_point(&self, p: Point3, tolerance: Tolerance) -> ClosestPoint3 { + let inner = self.curve.closest_point(p, tolerance); + ClosestPoint3::new( + self.param(inner.parameter()), + inner.point(), + inner.distance(), + ) + } + + fn tessellate(&self, tolerance: ChordHeightTolerance) -> Vec { + let mut points = self.curve.tessellate(tolerance); + if self.reversed { + points.reverse(); + } + points + } +} + +#[derive(Clone, Debug, PartialEq)] +pub struct CompositePath3 { + segments: Vec, + closed: bool, +} + +impl CompositePath3 { + pub fn chain(segments: &[Curve3Kind]) -> Result { + match segments { + [] => Err(SweepDefect::EmptyPath), + [only] => Ok(Self { + segments: vec![OrientedSegment { + curve: *only, + reversed: false, + }], + closed: curve_is_closed(*only), + }), + many => Self::order(many), + } + } + + fn order(segments: &[Curve3Kind]) -> Result { + if segments.iter().any(|curve| curve_is_closed(*curve)) { + return Err(SweepDefect::BranchingPath); + } + let endpoints: Vec<(Point3, Point3)> = segments + .iter() + .map(|curve| { + ( + curve.evaluate(Parameter::new(0.0)), + curve.evaluate(Parameter::new(1.0)), + ) + }) + .collect(); + if branches(&endpoints) { + return Err(SweepDefect::BranchingPath); + } + let start = start_index(&endpoints); + let (start_head, start_tail) = endpoints[start]; + let reversed = + incidence(&endpoints, start_tail) == 1 && incidence(&endpoints, start_head) != 1; + let first = OrientedSegment { + curve: segments[start], + reversed, + }; + let ordered = (1..segments.len()).try_fold( + ( + vec![first], + first.tail(), + single_mask(segments.len(), start), + ), + |(mut chain, tail, used), _| { + let (index, oriented) = next_segment(segments, &endpoints, &used, tail) + .ok_or(SweepDefect::DisconnectedPath)?; + let next_tail = oriented.tail(); + chain.push(oriented); + Ok((chain, next_tail, set_mask(used, index))) + }, + ); + let (chain, tail, _) = ordered?; + let closed = near(tail, chain[0].head()); + Ok(Self { + segments: chain, + closed, + }) + } + + #[must_use] + pub fn is_closed(&self) -> bool { + self.closed + } + + #[must_use] + pub fn has_corner(&self) -> bool { + let interior = self.segments.windows(2).any(|pair| { + let outgoing = pair[0].derivative(Parameter::new(1.0)); + let incoming = pair[1].derivative(Parameter::new(0.0)); + !tangent_continuous(outgoing, incoming) + }); + let seam = self.closed + && self.segments.len() >= 2 + && !tangent_continuous( + self.segments[self.segments.len() - 1].derivative(Parameter::new(1.0)), + self.segments[0].derivative(Parameter::new(0.0)), + ); + interior || seam + } + + #[must_use] + pub fn segment_count(&self) -> usize { + self.segments.len() + } + + #[must_use] + pub fn is_polyline(&self) -> bool { + self.segments + .iter() + .all(|segment| matches!(segment.curve, Curve3Kind::Line(_))) + } + + #[must_use] + pub fn runs(&self) -> Vec { + let breaks = (1..self.segments.len()).filter(|&index| { + !tangent_continuous( + self.segments[index - 1].derivative(Parameter::new(1.0)), + self.segments[index].derivative(Parameter::new(0.0)), + ) + }); + let bounds: Vec = core::iter::once(0) + .chain(breaks) + .chain(core::iter::once(self.segments.len())) + .collect(); + bounds + .windows(2) + .map(|window| Self { + segments: self.segments[window[0]..window[1]].to_vec(), + closed: false, + }) + .collect() + } + + #[must_use] + pub fn vertices(&self) -> Vec { + let heads = self.segments.first().map(|segment| segment.head()); + heads + .into_iter() + .chain(self.segments.iter().map(|segment| segment.tail())) + .collect() + } + + #[must_use] + pub fn trimmed(&self, from: Parameter, to: Parameter) -> Option { + if from.value() >= to.value() { + return None; + } + let (first, local_from) = self.locate(from.value()); + let (last, local_to) = self.locate(to.value()); + let sub = |index: usize, a: f64, b: f64| { + let segment = self.segments[index]; + segment.curve.sub_curve( + segment.param(Parameter::new(a)), + segment.param(Parameter::new(b)), + ) + }; + let segments: Vec = if first == last { + sub(first, local_from, local_to) + .map(|curve| OrientedSegment { + curve, + reversed: false, + }) + .into_iter() + .collect() + } else { + let head = sub(first, local_from, 1.0); + let tail = sub(last, 0.0, local_to); + let middle = ((first + 1)..last).map(|index| sub(index, 0.0, 1.0)); + head.into_iter() + .chain(middle.flatten()) + .chain(tail) + .map(|curve| OrientedSegment { + curve, + reversed: false, + }) + .collect() + }; + if segments.is_empty() { + return None; + } + Some(Self { + segments, + closed: false, + }) + } + + #[must_use] + pub fn segment_at(&self, t: Parameter) -> usize { + self.locate(t.value()).0 + } + + #[must_use] + pub fn reversed(&self) -> Self { + Self { + segments: self + .segments + .iter() + .rev() + .map(|segment| OrientedSegment { + curve: segment.curve, + reversed: !segment.reversed, + }) + .collect(), + closed: self.closed, + } + } +} + +impl Curve3 for CompositePath3 { + fn evaluate(&self, t: Parameter) -> Point3 { + let (index, local) = self.locate(t.value()); + self.segments[index].evaluate(Parameter::new(local)) + } + + fn derivative(&self, t: Parameter) -> Vec3 { + let (index, local) = self.locate(t.value()); + self.segments[index].derivative(Parameter::new(local)) + } + + fn bounding_box(&self) -> Aabb3 { + self.segments + .iter() + .map(Curve3::bounding_box) + .reduce(Aabb3::union) + .unwrap_or_else(|| Aabb3::from_corners(Point3::origin(), Point3::origin())) + } + + fn closest_point(&self, p: Point3, tolerance: Tolerance) -> ClosestPoint3 { + let n = self.segments.len(); + self.segments + .iter() + .enumerate() + .map(|(index, segment)| { + let local = segment.closest_point(p, tolerance); + let global = (count_to_f64(index) + local.parameter().value()) / count_to_f64(n); + ClosestPoint3::new(Parameter::new(global), local.point(), local.distance()) + }) + .reduce(|best, candidate| { + if candidate.distance() < best.distance() { + candidate + } else { + best + } + }) + .unwrap_or_else(|| { + ClosestPoint3::new(Parameter::new(0.0), p, Length::new::(0.0)) + }) + } + + fn tessellate(&self, tolerance: ChordHeightTolerance) -> Vec { + self.segments + .iter() + .enumerate() + .flat_map(|(index, segment)| { + let points = segment.tessellate(tolerance); + let skip = usize::from(index > 0); + points.into_iter().skip(skip).collect::>() + }) + .collect() + } +} + +impl CompositePath3 { + fn locate(&self, t: f64) -> (usize, f64) { + let n = self.segments.len(); + let scaled = t.clamp(0.0, 1.0) * count_to_f64(n); + let index = (0..n) + .find(|&i| scaled < count_to_f64(i + 1)) + .unwrap_or(n - 1); + (index, scaled - count_to_f64(index)) + } +} + +fn curve_is_closed(curve: Curve3Kind) -> bool { + near( + curve.evaluate(Parameter::new(0.0)), + curve.evaluate(Parameter::new(1.0)), + ) +} + +fn near(a: Point3, b: Point3) -> bool { + (a - b).norm_mm() <= CHAIN_TOLERANCE_MM +} + +fn tangent_continuous(outgoing: Vec3, incoming: Vec3) -> bool { + match ( + outgoing.try_normalize(TRANSPORT_TOLERANCE), + incoming.try_normalize(TRANSPORT_TOLERANCE), + ) { + (Ok(a), Ok(b)) => 1.0 - a.dot(b) <= SMOOTH_TANGENT_TOLERANCE, + _ => false, + } +} + +fn incidence(endpoints: &[(Point3, Point3)], point: Point3) -> usize { + endpoints + .iter() + .flat_map(|(head, tail)| [*head, *tail]) + .filter(|end| near(*end, point)) + .count() +} + +fn branches(endpoints: &[(Point3, Point3)]) -> bool { + endpoints + .iter() + .flat_map(|(head, tail)| [*head, *tail]) + .any(|point| incidence(endpoints, point) > 2) +} + +fn start_index(endpoints: &[(Point3, Point3)]) -> usize { + endpoints + .iter() + .position(|(head, _)| incidence(endpoints, *head) == 1) + .or_else(|| { + endpoints + .iter() + .position(|(_, tail)| incidence(endpoints, *tail) == 1) + }) + .unwrap_or(0) +} + +fn single_mask(count: usize, set: usize) -> Vec { + (0..count).map(|index| index == set).collect() +} + +fn set_mask(mut mask: Vec, index: usize) -> Vec { + mask[index] = true; + mask +} + +fn next_segment( + segments: &[Curve3Kind], + endpoints: &[(Point3, Point3)], + used: &[bool], + tail: Point3, +) -> Option<(usize, OrientedSegment)> { + (0..segments.len()) + .filter(|&index| !used[index]) + .find_map(|index| { + let (head, back) = endpoints[index]; + if near(head, tail) { + Some(( + index, + OrientedSegment { + curve: segments[index], + reversed: false, + }, + )) + } else if near(back, tail) { + Some(( + index, + OrientedSegment { + curve: segments[index], + reversed: true, + }, + )) + } else { + None + } + }) +} + +#[derive(Clone, Debug, PartialEq)] +pub struct SectionSpline { + pub degree: usize, + pub knots: Vec, + pub control: Vec, + pub weights: Vec, +} + +fn segment_count(sweep_abs: f64) -> usize { + let quarter = core::f64::consts::FRAC_PI_2; + let eps = 1.0e-9; + if sweep_abs <= quarter + eps { + 1 + } else if sweep_abs <= core::f64::consts::PI + eps { + 2 + } else if sweep_abs <= 3.0 * quarter + eps { + 3 + } else { + 4 + } +} + +fn arc_spline(center: Point2, radius: f64, start: f64, sweep: f64) -> SectionSpline { + let segments = segment_count(sweep.abs()); + let step = sweep / count_to_f64(segments); + let mid_weight = (step / 2.0).cos().abs(); + let (cx, cy) = center.coords_mm(); + let ring = |angle: f64| { + Point2::from_mm( + radius.mul_add(angle.cos(), cx), + radius.mul_add(angle.sin(), cy), + ) + }; + let apex = |angle: f64| { + let reach = radius / (step / 2.0).cos().abs(); + Point2::from_mm( + reach.mul_add(angle.cos(), cx), + reach.mul_add(angle.sin(), cy), + ) + }; + let arcs = (0..segments).flat_map(|index| { + let a1 = start + count_to_f64(index) * step; + [(apex(a1 + step / 2.0), mid_weight), (ring(a1 + step), 1.0)] + }); + let weighted: Vec<(Point2, f64)> = core::iter::once((ring(start), 1.0)).chain(arcs).collect(); + let internal = (1..segments).flat_map(|index| { + let knot = count_to_f64(index) / count_to_f64(segments); + [knot, knot] + }); + let knots: Vec = [0.0, 0.0, 0.0] + .into_iter() + .chain(internal) + .chain([1.0, 1.0, 1.0]) + .collect(); + SectionSpline { + degree: 2, + knots, + control: weighted.iter().map(|(point, _)| *point).collect(), + weights: weighted.iter().map(|(_, weight)| *weight).collect(), + } +} + +fn line_spline(start: Point2, end: Point2) -> SectionSpline { + SectionSpline { + degree: 1, + knots: vec![0.0, 0.0, 1.0, 1.0], + control: vec![start, end], + weights: vec![1.0, 1.0], + } +} + +fn bezier_spline(control: [Point2; 4]) -> SectionSpline { + SectionSpline { + degree: 3, + knots: vec![0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0], + control: control.to_vec(), + weights: vec![1.0; 4], + } +} + +#[must_use] +pub fn circle_arc_spline(circle: Circle2, start: Angle, sweep: Angle) -> SectionSpline { + arc_spline( + circle.center(), + circle.radius_mm(), + start.get::(), + sweep.get::(), + ) +} + +#[must_use] +pub fn ellipse_arc_spline(ellipse: Ellipse2, start: Angle, sweep: Angle) -> SectionSpline { + let unit = arc_spline( + Point2::origin(), + 1.0, + start.get::(), + sweep.get::(), + ); + let (cx, cy) = ellipse.center().coords_mm(); + let rotation = ellipse.rotation_rad(); + let (semi_major, semi_minor) = (ellipse.semi_major_mm(), ellipse.semi_minor_mm()); + let (major_x, major_y) = (rotation.cos(), rotation.sin()); + let (minor_x, minor_y) = (-rotation.sin(), rotation.cos()); + let control = unit + .control + .iter() + .map(|point| { + let (px, py) = point.coords_mm(); + let x = (semi_major * px).mul_add(major_x, (semi_minor * py) * minor_x) + cx; + let y = (semi_major * px).mul_add(major_y, (semi_minor * py) * minor_y) + cy; + Point2::from_mm(x, y) + }) + .collect(); + SectionSpline { + degree: unit.degree, + knots: unit.knots, + control, + weights: unit.weights, + } +} + +#[must_use] +pub fn section_spline(curve: Curve2Kind) -> SectionSpline { + let full_turn = Angle::new::(core::f64::consts::TAU); + match curve { + Curve2Kind::Line(line) => line_spline(line.start(), line.end()), + Curve2Kind::Arc(arc) => arc_spline( + arc.center(), + arc.radius_mm(), + arc.start_rad(), + arc.sweep_rad(), + ), + Curve2Kind::Circle(circle) => { + circle_arc_spline(circle, Angle::new::(0.0), full_turn) + } + Curve2Kind::Ellipse(ellipse) => { + ellipse_arc_spline(ellipse, Angle::new::(0.0), full_turn) + } + Curve2Kind::Bezier(bezier) => bezier_spline(bezier.control()), + } +} + +#[must_use] +pub fn reverse_section(spline: &SectionSpline) -> SectionSpline { + let first = spline.knots.first().copied().unwrap_or(0.0); + let last = spline.knots.last().copied().unwrap_or(1.0); + let span = first + last; + SectionSpline { + degree: spline.degree, + knots: spline.knots.iter().rev().map(|knot| span - knot).collect(), + control: spline.control.iter().rev().copied().collect(), + weights: spline.weights.iter().rev().copied().collect(), + } +} + +#[cfg(test)] +mod tests { + use super::{ + ArcFraction, BSpline3, CompositePath3, Frame, FrameGuide, PathInterpolator, SampleCount, + SectionSpline, StationCount, SweepDefect, SweepPath3, chord_params, section_spline, + transport, transport_with, + }; + use crate::arc3::Arc3; + use crate::circle3::Circle3; + use crate::curve2::Curve2Kind; + use crate::curve3::{Curve3, Curve3Kind}; + use crate::line3::Line3; + use crate::nurbs::{Degree, KnotVector, RationalControlPoint}; + use crate::nurbs_curve::NurbsCurve3; + use bone_types::{ + Aabb3, Angle, ChordHeightTolerance, Length, Parameter, Plane3, Point2, Point3, Tolerance, + UnitVec3, Vec3, radian, + }; + use core::f64::consts::{FRAC_PI_2, TAU}; + use uom::si::length::millimeter; + + const TOL: Tolerance = Tolerance::new(1.0e-9); fn interpolate(points: &[Point3], degree: usize) -> Option { let effective = degree.min(points.len().saturating_sub(1)); @@ -562,7 +1386,7 @@ mod tests { fn plane(origin: Point3, x: UnitVec3, y: UnitVec3) -> Plane3 { let Ok(plane) = Plane3::new(origin, x, y, TOL) else { - panic!("axes orthonormal"); + panic!("axes orthonormal") }; plane } @@ -571,142 +1395,113 @@ mod tests { plane(Point3::origin(), UnitVec3::x_axis(), UnitVec3::y_axis()) } + fn xz_plane() -> Plane3 { + plane(Point3::origin(), UnitVec3::x_axis(), UnitVec3::z_axis()) + } + + fn circle(radius_mm: f64) -> Circle3 { + let Ok(circle) = Circle3::new(xy_plane(), Length::new::(radius_mm), TOL) else { + panic!("positive radius") + }; + circle + } + + fn z_line(len_mm: f64) -> Line3 { + let Ok(line) = Line3::new(Point3::origin(), Point3::from_mm(0.0, 0.0, len_mm), TOL) else { + panic!("distinct endpoints") + }; + line + } + fn unit(x: f64, y: f64, z: f64) -> UnitVec3 { - let Ok(unit) = Vec3::from_mm(x, y, z).try_normalize(TOL) else { - panic!("nonzero direction"); + let Ok(direction) = Vec3::from_mm(x, y, z).try_normalize(TOL) else { + panic!("nonzero direction") }; - unit + direction } - fn eval_bspline(spline: &BSpline3, u: f64) -> Point3 { - let Ok(degree) = Degree::new(spline.degree()) else { - panic!("positive degree"); + fn off(a: UnitVec3, target: Vec3) -> f64 { + (super::dir(a) - target).norm_mm() + } + + fn nurbs(degree: usize, knots: Vec, control: Vec) -> NurbsCurve3 { + let Ok(degree) = Degree::new(degree) else { + panic!("positive degree") + }; + let Ok(knots) = KnotVector::new(knots) else { + panic!("valid knot vector") }; - let Ok(knots) = KnotVector::new(spline.knots().to_vec()) else { - panic!("valid knot vector"); + let Ok(spline) = NurbsCurve3::new(degree, knots, control) else { + panic!("valid spline") }; - let control: Vec = spline + spline + } + + fn eval_bspline(spline: &BSpline3, u: f64) -> Point3 { + let control = spline .control() .iter() .map(|&point| { - let Ok(rational) = RationalControlPoint::new(point, 1.0, TOL) else { - panic!("unit weight"); + let Ok(control_point) = RationalControlPoint::new(point, 1.0, TOL) else { + panic!("unit weight") }; - rational + control_point }) .collect(); - let Ok(curve) = NurbsCurve3::new(degree, knots, control) else { - panic!("valid spline"); - }; - curve.evaluate(Parameter::new(u)) + nurbs(spline.degree(), spline.knots().to_vec(), control).evaluate(Parameter::new(u)) } - #[test] - fn interpolation_passes_through_its_data_points() { - let points: Vec = (0..=10) - .map(|i| { - let t = f64::from(i) / 10.0; - Point3::from_mm(t * 5.0, (t * TAU).sin() * 2.0, t * t * 3.0) - }) - .collect(); - let Some(spline) = interpolate(&points, 3) else { - panic!("interpolates"); - }; - let Some(params) = chord_params(&points) else { - panic!("chord params"); + fn eval_bspline_derivative(spline: &BSpline3, u: f64) -> Vec3 { + let step = 1.0e-6; + let (lo, hi) = if u < 0.5 { + (u, u + step) + } else { + (u - step, u) }; - points.iter().zip(params).for_each(|(expected, u)| { - let got = eval_bspline(&spline, u); - assert!( - (got - *expected).norm_mm() < 1.0e-7, - "interpolant misses {expected:?}, got {got:?}" - ); - }); + (eval_bspline(spline, hi) - eval_bspline(spline, lo)) * (1.0 / step) } fn eval_section(spline: &SectionSpline, u: f64) -> Point2 { - let Ok(degree) = Degree::new(spline.degree) else { - panic!("positive degree"); - }; - let Ok(knots) = KnotVector::new(spline.knots.clone()) else { - panic!("valid knot vector"); - }; - let control: Vec = spline + let control = spline .control .iter() .zip(&spline.weights) .map(|(point, &weight)| { let (x, y) = point.coords_mm(); - let Ok(rational) = + let Ok(control_point) = RationalControlPoint::new(Point3::from_mm(x, y, 0.0), weight, TOL) else { - panic!("positive weight"); + panic!("positive weight") }; - rational + control_point }) .collect(); - let Ok(curve) = NurbsCurve3::new(degree, knots, control) else { - panic!("valid spline"); - }; - let point = curve.evaluate(Parameter::new(u)); - let (x, y, _) = point.coords_mm(); + let (x, y, _) = nurbs(spline.degree, spline.knots.clone(), control) + .evaluate(Parameter::new(u)) + .coords_mm(); Point2::from_mm(x, y) } - #[test] - fn arc_section_spline_lies_on_its_circle() { - let Ok(circle) = crate::circle2::Circle2::new( - Point2::from_mm(1.0, 2.0), - Length::new::(3.0), - TOL, - ) else { - panic!("positive radius"); + fn line_kind(a: Point3, b: Point3) -> Curve3Kind { + let Ok(line) = Line3::new(a, b, TOL) else { + panic!("distinct endpoints") }; - let spline = section_spline(Curve2Kind::Circle(circle)); - (0..=40).for_each(|i| { - let point = eval_section(&spline, f64::from(i) / 40.0); - let (x, y) = point.coords_mm(); - let radius = (x - 1.0).hypot(y - 2.0); - assert!( - (radius - 3.0).abs() < 1.0e-6, - "sample sits on the r=3 circle: drift {}", - (radius - 3.0).abs() - ); - }); + line.as_kind() } - #[test] - fn arc_section_spline_hits_its_endpoints() { - let Ok(arc) = crate::arc2::Arc2::new( - Point2::origin(), - Length::new::(2.0), - bone_types::Angle::new::(0.0), - bone_types::Angle::new::(core::f64::consts::FRAC_PI_2), + fn tangent_bend() -> Curve3Kind { + let center = Point3::from_mm(10.0, 10.0, 0.0); + let arc_plane = plane(center, UnitVec3::x_axis(), UnitVec3::y_axis()); + let Ok(arc) = Arc3::new( + arc_plane, + Length::new::(10.0), + Angle::new::(-FRAC_PI_2), + Angle::new::(FRAC_PI_2), TOL, ) else { - panic!("valid arc"); - }; - let spline = section_spline(Curve2Kind::Arc(arc)); - let start = eval_section(&spline, 0.0); - let end = eval_section(&spline, 1.0); - assert!((start.coords_mm().0 - 2.0).abs() < 1.0e-9 && start.coords_mm().1.abs() < 1.0e-9); - assert!(end.coords_mm().0.abs() < 1.0e-9 && (end.coords_mm().1 - 2.0).abs() < 1.0e-9); - } - - #[test] - fn interpolation_of_collinear_points_stays_straight() { - let points: Vec = (0..=6) - .map(|i| Point3::from_mm(f64::from(i), 2.0 * f64::from(i), -f64::from(i))) - .collect(); - let Some(spline) = interpolate(&points, 3) else { - panic!("interpolates"); + panic!("valid quarter arc") }; - let mid = eval_bspline(&spline, 0.5); - let (x, y, z) = mid.coords_mm(); - assert!( - (y - 2.0 * x).abs() < 1.0e-7 && (z + x).abs() < 1.0e-7, - "interpolant stays on the line: {mid:?}" - ); + arc.as_kind() } struct Helix { @@ -753,73 +1548,264 @@ mod tests { } #[test] - fn arc_length_table_matches_a_line_length() { - let Ok(segment) = Line3::new(Point3::origin(), Point3::from_mm(0.0, 0.0, 10.0), TOL) else { - panic!("distinct endpoints"); + fn interpolation_hits_points_lines_and_clamped_end_tangents() + -> Result<(), Box> { + let wobble: Vec = (0..=10) + .map(|i| { + let t = f64::from(i) / 10.0; + Point3::from_mm(t * 5.0, (t * TAU).sin() * 2.0, t * t * 3.0) + }) + .collect(); + let spline = interpolate(&wobble, 3).ok_or("interpolates")?; + let params = chord_params(&wobble).ok_or("chord params")?; + wobble.iter().zip(params).for_each(|(expected, u)| { + let got = eval_bspline(&spline, u); + assert!( + (got - *expected).norm_mm() < 1.0e-7, + "interpolant misses {expected:?}, got {got:?}" + ); + }); + + let collinear: Vec = (0..=6) + .map(|i| Point3::from_mm(f64::from(i), 2.0 * f64::from(i), -f64::from(i))) + .collect(); + let straight = interpolate(&collinear, 3).ok_or("interpolates")?; + let mid = eval_bspline(&straight, 0.5); + let (x, y, z) = mid.coords_mm(); + assert!( + (y - 2.0 * x).abs() < 1.0e-7 && (z + x).abs() < 1.0e-7, + "interpolant stays on the line: {mid:?}" + ); + + let arc: Vec = (0..=8) + .map(|i| { + let theta = FRAC_PI_2 * f64::from(i) / 8.0; + Point3::from_mm(5.0 * theta.cos(), 5.0 * theta.sin(), 0.0) + }) + .collect(); + let arc_params = chord_params(&arc).ok_or("chord params")?; + let clamped = super::ClampedInterpolator::new(&arc_params).ok_or("clamped interpolator")?; + let start = Vec3::from_mm(0.0, 7.85, 0.0); + let end = Vec3::from_mm(-7.85, 0.0, 0.0); + let clamped_spline = clamped.solve(&arc, start, end).ok_or("clamped solve")?; + arc.iter().zip(&arc_params).for_each(|(expected, &u)| { + let got = eval_bspline(&clamped_spline, u); + assert!( + (got - *expected).norm_mm() < 1.0e-6, + "clamped interpolant misses {expected:?}: got {got:?}" + ); + }); + let aligned = |vector: Vec3, wanted: Vec3| { + vector.dot_mm2(wanted) / (vector.norm_mm() * wanted.norm_mm()) }; - let path = SweepPath3::new(&segment, SampleCount::new(32)); - assert!((path.length().get::() - 10.0).abs() < 1.0e-9); - let mid = path.point_at(ArcFraction::new(0.5)); + let start_d = eval_bspline_derivative(&clamped_spline, 0.0); + let end_d = eval_bspline_derivative(&clamped_spline, 1.0); assert!( - (mid.coords_mm().2 - 5.0).abs() < 1.0e-9, - "half arc is the midpoint" + aligned(start_d, start) > 0.9999, + "the start derivative honors the clamp: {start_d:?}" + ); + assert!( + aligned(end_d, end) > 0.9999, + "the end derivative honors the clamp: {end_d:?}" + ); + assert!( + (start_d.norm_mm() - start.norm_mm()).abs() < 0.05 * start.norm_mm(), + "the clamped derivative magnitude matches the request: {}", + start_d.norm_mm() ); + Ok(()) } #[test] - fn arc_length_table_is_constant_speed_on_a_circle() { - let Ok(circle) = Circle3::new(xy_plane(), Length::new::(4.0), TOL) else { - panic!("positive radius"); - }; - let path = SweepPath3::new(&circle, SampleCount::new(512)); - let expected = TAU * 4.0; + fn section_spline_lies_on_its_curve_hits_endpoints_and_reverses() + -> Result<(), Box> { + let circle2 = crate::circle2::Circle2::new( + Point2::from_mm(1.0, 2.0), + Length::new::(3.0), + TOL, + )?; + let on_circle = section_spline(Curve2Kind::Circle(circle2)); + (0..=40).for_each(|i| { + let (x, y) = eval_section(&on_circle, f64::from(i) / 40.0).coords_mm(); + let radius = (x - 1.0).hypot(y - 2.0); + assert!( + (radius - 3.0).abs() < 1.0e-6, + "sample sits on the r=3 circle: drift {}", + (radius - 3.0).abs() + ); + }); + + let quarter = crate::arc2::Arc2::new( + Point2::origin(), + Length::new::(2.0), + Angle::new::(0.0), + Angle::new::(FRAC_PI_2), + TOL, + )?; + let ends = section_spline(Curve2Kind::Arc(quarter)); + let start = eval_section(&ends, 0.0); + let stop = eval_section(&ends, 1.0); + assert!((start.coords_mm().0 - 2.0).abs() < 1.0e-9 && start.coords_mm().1.abs() < 1.0e-9); + assert!(stop.coords_mm().0.abs() < 1.0e-9 && (stop.coords_mm().1 - 2.0).abs() < 1.0e-9); + + let tilted = crate::arc2::Arc2::new( + Point2::from_mm(1.0, -1.0), + Length::new::(2.0), + Angle::new::(0.3), + Angle::new::(1.1), + TOL, + )?; + let forward = section_spline(Curve2Kind::Arc(tilted)); + let reversed = super::reverse_section(&forward); + (0..=40).for_each(|i| { + let u = f64::from(i) / 40.0; + let (fx, fy) = eval_section(&forward, u).coords_mm(); + let (bx, by) = eval_section(&reversed, 1.0 - u).coords_mm(); + assert!( + (fx - bx).hypot(fy - by) < 1.0e-9, + "reverse(u') matches forward(1-u'): drift {}", + (fx - bx).hypot(fy - by) + ); + }); + Ok(()) + } + + #[test] + fn frame_reorient_and_twist_swing_the_section_axes() -> Result<(), Box> { + let turned = Frame::seed(Point3::origin(), UnitVec3::x_axis(), xy_plane()) + .reorient(UnitVec3::y_axis()); assert!( - (path.length().get::() - expected).abs() < 1.0e-2, + off(turned.tangent(), super::dir(UnitVec3::y_axis())) < 1.0e-9, + "tangent tracks the target" + ); + assert!( + turned.normal().dot(turned.tangent()).abs() < 1.0e-9, + "normal stays perpendicular to the reoriented tangent" + ); + assert!( + off(turned.normal(), super::dir(unit(-1.0, 0.0, 0.0))) < 1.0e-9, + "a +X to +Y turn rotates the +Y normal onto -X" + ); + assert!( + off(turned.binormal(), super::dir(unit(0.0, 0.0, 1.0))) < 1.0e-9, + "binormal is preserved" + ); + + let arc = circle(4.0); + let path = SweepPath3::new(&arc, SampleCount::new(64)); + let frame = transport(&path, xz_plane(), StationCount::new(4)) + .first() + .copied() + .ok_or("at least one frame")?; + let twisted = frame.twist(FRAC_PI_2); + let onto_binormal = off(twisted.normal(), super::dir(frame.binormal())); + let onto_neg_normal = + (super::dir(twisted.binormal()) + super::dir(frame.normal())).norm_mm(); + assert!( + onto_binormal < 1.0e-9 && onto_neg_normal < 1.0e-9, + "a quarter twist rotates the normal onto the binormal" + ); + assert!( + super::dir(twisted.tangent()).dot_mm2(super::dir(frame.tangent())) > 1.0 - 1.0e-12, + "twist leaves the tangent unchanged" + ); + Ok(()) + } + + #[test] + fn sweep_path_measures_length_and_curvature_on_lines_and_circles() { + let straight = z_line(10.0); + let line_path = SweepPath3::new(&straight, SampleCount::new(32)); + assert!((line_path.length().get::() - 10.0).abs() < 1.0e-9); + assert!( + (line_path.point_at(ArcFraction::new(0.5)).coords_mm().2 - 5.0).abs() < 1.0e-9, + "half arc is the midpoint" + ); + assert!( + SweepPath3::new(&z_line(10.0), SampleCount::new(8)) + .min_curvature_radius_mm(StationCount::new(8)) + .is_infinite(), + "a straight path never curves" + ); + + let ring = circle(4.0); + let circle_path = SweepPath3::new(&ring, SampleCount::new(512)); + assert!( + (circle_path.length().get::() - TAU * 4.0).abs() < 1.0e-2, "circle arc length is the circumference" ); - let quarter = path.point_at(ArcFraction::new(0.25)); + let quarter = circle_path.point_at(ArcFraction::new(0.25)); let (x, y, _) = quarter.coords_mm(); assert!( - (x - 0.0).abs() < 1.0e-2 && (y - 4.0).abs() < 1.0e-2, + x.abs() < 1.0e-2 && (y - 4.0).abs() < 1.0e-2, "a quarter of the arc length lands a quarter turn around: {quarter:?}" ); + let radius = SweepPath3::new(&circle(4.0), SampleCount::new(2048)) + .min_curvature_radius_mm(StationCount::new(720)); + assert!( + (radius - 4.0).abs() < 0.02, + "a circle's tightest curvature radius is its radius: got {radius}" + ); } #[test] - fn rmf_reference_tracks_the_radial_direction_on_a_planar_circle() { - let Ok(circle) = Circle3::new(xy_plane(), Length::new::(5.0), TOL) else { - panic!("positive radius"); - }; - let path = SweepPath3::new(&circle, SampleCount::new(2048)); - let profile = plane(Point3::origin(), UnitVec3::x_axis(), UnitVec3::z_axis()); - let frames = transport(&path, profile, StationCount::new(720)); - frames.iter().enumerate().for_each(|(i, frame)| { - let angle = TAU * f64::from(u32::try_from(i).unwrap_or(0)) / 720.0; - let radial = Vec3::from_mm(angle.cos(), angle.sin(), 0.0); - let drift = (super::dir(frame.normal()) - radial).norm_mm(); + fn rmf_on_a_circle_tracks_radial_and_can_hold_the_seed_normal() + -> Result<(), Box> { + let ring = circle(5.0); + let path = SweepPath3::new(&ring, SampleCount::new(2048)); + let profile = xz_plane(); + + transport(&path, profile, StationCount::new(720)) + .iter() + .enumerate() + .for_each(|(i, frame)| { + let angle = TAU * f64::from(u32::try_from(i).unwrap_or(0)) / 720.0; + let radial = Vec3::from_mm(angle.cos(), angle.sin(), 0.0); + assert!( + off(frame.normal(), radial) < 5.0e-3, + "station {i}: reference {:?} should track radial {radial:?}", + frame.normal() + ); + }); + + let stations = StationCount::new(360); + let held = transport_with(&path, profile, stations, FrameGuide::KeepNormal); + let seed = held.first().copied().ok_or("at least one frame")?; + held.iter().for_each(|frame| { assert!( - drift < 5.0e-3, - "station {i}: reference {:?} should track radial {radial:?}", + off(frame.normal(), super::dir(seed.normal())) < 1.0e-9 + && off(frame.binormal(), super::dir(seed.binormal())) < 1.0e-9, + "keep-normal sections stay parallel to the seed: normal {:?}", frame.normal() ); }); + let follow_drift = transport_with(&path, profile, stations, FrameGuide::MinimumTwist) + .iter() + .map(|frame| off(frame.normal(), super::dir(seed.normal()))) + .fold(0.0_f64, f64::max); + assert!( + follow_drift > 0.5, + "the same curved path swings the follow-path normal well away from the seed, so \ + holding it constant is a real choice, not what transport does anyway: drift {follow_drift}" + ); + Ok(()) } #[test] - fn rmf_has_zero_twist_on_a_helix() { + fn rmf_on_a_helix_has_zero_twist_and_matches_the_closed_form() { let helix = Helix { radius: 3.0, climb: 1.5, turns: 1.0, }; + let path = SweepPath3::new(&helix, SampleCount::new(4096)); let profile = plane(Point3::origin(), unit(0.0, 1.0, 0.0), unit(0.0, 0.0, 1.0)); let frames = transport(&path, profile, StationCount::new(1024)); let max_twist = frames.windows(2).fold(0.0_f64, |worst, pair| { let delta = super::dir(pair[1].normal()) - super::dir(pair[0].normal()); let mean_binormal = super::dir(pair[0].binormal()) + super::dir(pair[1].binormal()); - let twist = delta.dot_mm2(mean_binormal).abs(); - worst.max(twist) + worst.max(delta.dot_mm2(mean_binormal).abs()) }); assert!(max_twist < 1.0e-4, "helix frame twists by {max_twist}"); frames.iter().for_each(|frame| { @@ -829,21 +1815,13 @@ mod tests { ); assert!(frame.binormal().dot(frame.tangent()).abs() < 1.0e-6); }); - } - #[test] - fn rmf_matches_the_helix_closed_form() { - let helix = Helix { - radius: 3.0, - climb: 1.5, - turns: 1.0, - }; - let path = SweepPath3::new(&helix, SampleCount::new(8192)); - let profile = plane(Point3::origin(), unit(-1.0, 0.0, 0.0), unit(0.0, 1.0, 0.0)); - let frames = transport(&path, profile, StationCount::new(2048)); + let fine = SweepPath3::new(&helix, SampleCount::new(8192)); + let closed_profile = plane(Point3::origin(), unit(-1.0, 0.0, 0.0), unit(0.0, 1.0, 0.0)); + let closed = transport(&fine, closed_profile, StationCount::new(2048)); let speed = (helix.radius * helix.radius + helix.climb * helix.climb).sqrt(); - let stations = frames.len() - 1; - frames.iter().enumerate().for_each(|(i, frame)| { + let stations = closed.len() - 1; + closed.iter().enumerate().for_each(|(i, frame)| { let t = f64::from(u32::try_from(i).unwrap_or(0)) / f64::from(u32::try_from(stations).unwrap_or(1)); let angle = TAU * t; @@ -855,17 +1833,15 @@ mod tests { ); let lag = helix.climb / speed * angle; let expected = frenet_n * lag.cos() - frenet_b * lag.sin(); - let drift = (super::dir(frame.normal()) - expected).norm_mm(); + let drift = off(frame.normal(), expected); assert!(drift < 5.0e-3, "station {i}: closed-form drift {drift}"); }); } #[test] - fn section_placement_reproduces_the_profile_at_the_seed() { - let Ok(segment) = Line3::new(Point3::origin(), Point3::from_mm(0.0, 0.0, 8.0), TOL) else { - panic!("distinct endpoints"); - }; - let path = SweepPath3::new(&segment, SampleCount::new(16)); + fn section_placement_reproduces_the_seed_and_rides_the_path() { + let axis = z_line(8.0); + let path = SweepPath3::new(&axis, SampleCount::new(16)); let frames = transport(&path, xy_plane(), StationCount::new(4)); let begin = frames[0] .place_point(Point2::from_mm(1.0, 2.0), Point2::origin()) @@ -876,15 +1852,6 @@ mod tests { && begin.2.abs() < 1.0e-9, "the seed section sits on the profile plane: {begin:?}" ); - } - - #[test] - fn section_placement_rides_the_frame_down_the_path() { - let Ok(segment) = Line3::new(Point3::origin(), Point3::from_mm(0.0, 0.0, 8.0), TOL) else { - panic!("distinct endpoints"); - }; - let path = SweepPath3::new(&segment, SampleCount::new(16)); - let frames = transport(&path, xy_plane(), StationCount::new(2)); let end = frames[frames.len() - 1] .place_point(Point2::from_mm(1.0, 0.0), Point2::origin()) .coords_mm(); @@ -893,4 +1860,221 @@ mod tests { "the end section has travelled the full path length: {end:?}" ); } + + #[test] + fn composite_path_orders_open_chains_detects_shape_and_rejects_broken() + -> Result<(), Box> { + type Chain = (Vec, usize, f64, [Point3; 2], bool); + let origin = Point3::from_mm(0.0, 0.0, 0.0); + let east = Point3::from_mm(10.0, 0.0, 0.0); + let northeast = Point3::from_mm(10.0, 10.0, 0.0); + let north = Point3::from_mm(0.0, 10.0, 0.0); + let far_east = Point3::from_mm(20.0, 0.0, 0.0); + #[rustfmt::skip] + let chains: [Chain; 2] = [ + (vec![line_kind(east, northeast), line_kind(north, northeast), line_kind(origin, east)], 3, 30.0, [origin, north], true), + (vec![line_kind(east, origin), line_kind(east, far_east)], 2, 20.0, [origin, far_east], false), + ]; + chains.into_iter().try_for_each( + |(segments, count, length_mm, termini, corner)| -> Result<(), Box> { + let path = CompositePath3::chain(&segments).map_err(|_| "one connected open chain")?; + assert!(!path.is_closed(), "an open chain does not return"); + assert_eq!(path.segment_count(), count); + let spine = SweepPath3::new(&path, SampleCount::new(300)); + assert!( + (spine.length().get::() - length_mm).abs() < 1.0e-6, + "arc length is the sum of the edges: {}", + spine.length().get::() + ); + let start = path.evaluate(Parameter::new(0.0)); + let end = path.evaluate(Parameter::new(1.0)); + assert!( + termini.iter().any(|t| (start - *t).norm_mm() < 1.0e-9), + "the path opens at a terminus: {start:?}" + ); + assert!( + termini.iter().any(|t| (end - *t).norm_mm() < 1.0e-9), + "the path closes at the other terminus: {end:?}" + ); + assert_eq!( + path.has_corner(), + corner, + "corner detection matches the polyline shape" + ); + Ok(()) + }, + )?; + + #[rustfmt::skip] + let l1 = line_kind(Point3::from_mm(0.0, 0.0, 0.0), Point3::from_mm(10.0, 0.0, 0.0)); + #[rustfmt::skip] + let l2 = line_kind(Point3::from_mm(20.0, 10.0, 0.0), Point3::from_mm(20.0, 20.0, 0.0)); + let smooth = + CompositePath3::chain(&[l1, tangent_bend(), l2]).map_err(|_| "tangent chain")?; + assert_eq!(smooth.segment_count(), 3); + assert!(!smooth.is_closed()); + assert!( + !smooth.has_corner(), + "a line into a tangent arc into a line has no tangent break" + ); + + #[rustfmt::skip] + let square = [line_kind(origin, east), line_kind(east, northeast), line_kind(northeast, north), line_kind(north, origin)]; + let closed = CompositePath3::chain(&square).map_err(|_| "the square closes")?; + assert!( + closed.is_closed(), + "a chain that returns to its start is closed" + ); + + #[rustfmt::skip] + let gap = [ + line_kind(Point3::from_mm(0.0, 0.0, 0.0), Point3::from_mm(10.0, 0.0, 0.0)), + line_kind(Point3::from_mm(11.0, 0.0, 0.0), Point3::from_mm(20.0, 0.0, 0.0)), + ]; + let hub = Point3::from_mm(5.0, 0.0, 0.0); + let branch = [ + line_kind(Point3::from_mm(0.0, 0.0, 0.0), hub), + line_kind(hub, Point3::from_mm(10.0, 0.0, 0.0)), + line_kind(hub, Point3::from_mm(5.0, 5.0, 0.0)), + ]; + [ + (&gap[..], SweepDefect::DisconnectedPath), + (&branch[..], SweepDefect::BranchingPath), + ] + .into_iter() + .for_each(|(segments, defect)| { + assert_eq!(CompositePath3::chain(segments), Err(defect)); + }); + Ok(()) + } + + #[test] + fn composite_path_trims_reverses_and_finds_closest_points() + -> Result<(), Box> { + let a = Point3::from_mm(0.0, 0.0, 0.0); + let b = Point3::from_mm(10.0, 0.0, 0.0); + let bent_c = Point3::from_mm(10.0, 10.0, 0.0); + let bent = CompositePath3::chain(&[line_kind(a, b), line_kind(b, bent_c)]) + .map_err(|_| "two joined edges chain")?; + + let trimmed = bent + .trimmed(Parameter::new(0.25), Parameter::new(1.0)) + .ok_or("a quarter-in trim keeps three quarters of the chain")?; + let trim_start = trimmed.evaluate(Parameter::new(0.0)); + assert!( + (trim_start - Point3::from_mm(5.0, 0.0, 0.0)).norm_mm() < 1.0e-9, + "the trim opens a quarter of the way along the first edge: {trim_start:?}" + ); + let trim_end = trimmed.evaluate(Parameter::new(1.0)); + assert!( + (trim_end - bent_c).norm_mm() < 1.0e-9, + "the trim keeps the original terminus: {trim_end:?}" + ); + let spine = SweepPath3::new(&trimmed, SampleCount::new(200)); + assert!( + (spine.length().get::() - 15.0).abs() < 1.0e-6, + "the trimmed chain keeps 15 of the 20 mm" + ); + + let reversed = bent.reversed(); + assert!( + (reversed.evaluate(Parameter::new(0.0)) - bent.evaluate(Parameter::new(1.0))).norm_mm() + < 1.0e-9 + && (reversed.evaluate(Parameter::new(1.0)) - bent.evaluate(Parameter::new(0.0))) + .norm_mm() + < 1.0e-9, + "reversal walks the same chain backward" + ); + + let one_edge = CompositePath3::chain(&[line_kind(a, b)]).map_err(|_| "one edge chains")?; + assert!( + one_edge + .trimmed(Parameter::new(0.5), Parameter::new(0.5)) + .is_none(), + "a zero-width window has no curve to keep" + ); + + let straight_c = Point3::from_mm(20.0, 0.0, 0.0); + let straight = CompositePath3::chain(&[line_kind(b, a), line_kind(b, straight_c)]) + .map_err(|_| "a-b-c is one connected open path")?; + [ + (Point3::from_mm(3.0, 4.0, 0.0), Point3::from_mm(3.0, 0.0, 0.0)), + (Point3::from_mm(17.0, 4.0, 0.0), Point3::from_mm(17.0, 0.0, 0.0)), + ] + .into_iter() + .for_each(|(query, foot)| { + let hit = straight.closest_point(query, TOL); + let echoed = straight.evaluate(hit.parameter()); + assert!( + (echoed - hit.point()).norm_mm() < 1.0e-9, + "feeding the returned parameter back into evaluate reproduces the returned point: {echoed:?} vs {:?}", + hit.point() + ); + assert!( + (hit.point() - foot).norm_mm() < 1.0e-9, + "the closest point is the perpendicular foot: {:?} vs {foot:?}", + hit.point() + ); + assert!( + (hit.distance().get::() - 4.0).abs() < 1.0e-9, + "the distance is the perpendicular offset: {}", + hit.distance().get::() + ); + }); + Ok(()) + } + + #[test] + fn station_fractions_pin_the_seed_and_seeded_transport_matches_on_a_line() { + let (mid, mid_seed) = + super::station_fractions(ArcFraction::new(0.4), StationCount::new(10)); + assert_eq!(mid.len(), 11, "n stations produce n + 1 fractions"); + assert!( + (mid[mid_seed] - 0.4).abs() < 1.0e-12, + "the seed station sits exactly on the origin fraction: {}", + mid[mid_seed] + ); + assert!( + mid.windows(2).all(|pair| pair[1] > pair[0]), + "fractions ascend" + ); + assert!( + mid[0].abs() < 1.0e-12 && (mid[10] - 1.0).abs() < 1.0e-12, + "the stations still span the whole path" + ); + + let (uniform, start_seed) = + super::station_fractions(ArcFraction::START, StationCount::new(8)); + assert_eq!(start_seed, 0); + uniform.iter().enumerate().for_each(|(i, fraction)| { + let expected = f64::from(u32::try_from(i).unwrap_or(0)) / 8.0; + assert!( + (fraction - expected).abs() < 1.0e-12, + "station {i} sits at {expected}: got {fraction}" + ); + }); + + let axis = z_line(8.0); + let path = SweepPath3::new(&axis, SampleCount::new(64)); + let (fractions, seed) = + super::station_fractions(ArcFraction::new(0.5), StationCount::new(8)); + let seeded = super::transport_stations( + &path, + xy_plane(), + &fractions, + seed, + FrameGuide::MinimumTwist, + ); + let start_seeded = transport(&path, xy_plane(), StationCount::new(8)); + seeded + .iter() + .zip(&start_seeded) + .for_each(|(mid_frame, start_frame)| { + let drift = off(mid_frame.normal(), super::dir(start_frame.normal())); + assert!( + drift < 1.0e-9, + "on a straight path the seed position cannot twist the frame: drift {drift}" + ); + }); + } }