Skip to main content

cadkit_core/
geom_ops.rs

1//! Geometry helpers: vector math, affine [`Transform`], circular/elliptic arcs, polyline
2//! bulges and NURBS evaluation.
3//!
4//! All angles are radians. Nothing here panics; degenerate input yields `None` or an
5//! identity-like result.
6
7use std::f64::consts::{PI, TAU};
8
9use crate::geom::{BBox, Point3, Vec3};
10
11// ---------------------------------------------------------------------------------------------
12// Vector helpers
13// ---------------------------------------------------------------------------------------------
14
15impl Vec3 {
16    /// Dot product.
17    pub fn dot(self, o: Vec3) -> f64 {
18        self.x * o.x + self.y * o.y + self.z * o.z
19    }
20
21    /// Cross product `self x o`.
22    pub fn cross(self, o: Vec3) -> Vec3 {
23        Vec3::new(
24            self.y * o.z - self.z * o.y,
25            self.z * o.x - self.x * o.z,
26            self.x * o.y - self.y * o.x,
27        )
28    }
29
30    /// Euclidean length.
31    pub fn length(self) -> f64 {
32        self.dot(self).sqrt()
33    }
34
35    /// Unit vector, or `None` when the length is zero or not finite.
36    pub fn normalized(self) -> Option<Vec3> {
37        let l = self.length();
38        if l.is_finite() && l > 1e-300 {
39            Some(self.scaled(1.0 / l))
40        } else {
41            None
42        }
43    }
44
45    /// Multiplies every component by `k`.
46    pub fn scaled(self, k: f64) -> Vec3 {
47        Vec3::new(self.x * k, self.y * k, self.z * k)
48    }
49
50    /// Component-wise sum.
51    pub fn plus(self, o: Vec3) -> Vec3 {
52        Vec3::new(self.x + o.x, self.y + o.y, self.z + o.z)
53    }
54
55    /// Component-wise difference.
56    pub fn minus(self, o: Vec3) -> Vec3 {
57        Vec3::new(self.x - o.x, self.y - o.y, self.z - o.z)
58    }
59}
60
61impl Point3 {
62    /// Vector from `self` to `other`.
63    pub fn vector_to(self, other: Point3) -> Vec3 {
64        Vec3::new(other.x - self.x, other.y - self.y, other.z - self.z)
65    }
66
67    /// The point moved by `v`.
68    pub fn translated(self, v: Vec3) -> Point3 {
69        Point3::new(self.x + v.x, self.y + v.y, self.z + v.z)
70    }
71
72    /// True when all coordinates are finite.
73    pub fn is_finite(self) -> bool {
74        self.x.is_finite() && self.y.is_finite() && self.z.is_finite()
75    }
76}
77
78impl BBox {
79    /// A degenerate box containing exactly `p`.
80    pub fn from_point(p: Point3) -> Self {
81        Self { min: p, max: p }
82    }
83
84    /// Grows the box to contain `p`. Non-finite points are ignored.
85    pub fn extend(&mut self, p: Point3) {
86        if !p.is_finite() {
87            return;
88        }
89        self.min = Point3::new(
90            self.min.x.min(p.x),
91            self.min.y.min(p.y),
92            self.min.z.min(p.z),
93        );
94        self.max = Point3::new(
95            self.max.x.max(p.x),
96            self.max.y.max(p.y),
97            self.max.z.max(p.z),
98        );
99    }
100
101    /// Grows the box to contain `other`.
102    pub fn union(&mut self, other: &BBox) {
103        self.extend(other.min);
104        self.extend(other.max);
105    }
106
107    /// Extent along X.
108    pub fn width(&self) -> f64 {
109        self.max.x - self.min.x
110    }
111
112    /// Extent along Y.
113    pub fn height(&self) -> f64 {
114        self.max.y - self.min.y
115    }
116}
117
118/// Adds `p` to an optional box, creating it on first use. Non-finite points are ignored.
119pub fn extend_bbox(bb: &mut Option<BBox>, p: Point3) {
120    if !p.is_finite() {
121        return;
122    }
123    match bb {
124        Some(b) => b.extend(p),
125        None => *bb = Some(BBox::from_point(p)),
126    }
127}
128
129// ---------------------------------------------------------------------------------------------
130// Transform
131// ---------------------------------------------------------------------------------------------
132
133/// DXF arbitrary-axis algorithm: the plane axes `(Ax, Ay, Az)` for an entity `normal`
134/// (Ax is the zero-angle direction of the plane, Az the normal).
135///
136/// If both |Nx| and |Ny| are below 1/64 the world Y axis is crossed with N, otherwise the
137/// world Z axis; `Ay = N x Ax`. A zero or non-finite normal yields the world axes.
138pub fn arbitrary_axes(normal: Vec3) -> (Vec3, Vec3, Vec3) {
139    let Some(n) = normal.normalized() else {
140        return (Vec3::new(1.0, 0.0, 0.0), Vec3::new(0.0, 1.0, 0.0), Vec3::Z);
141    };
142    let wz = Vec3::Z;
143    let wy = Vec3::new(0.0, 1.0, 0.0);
144    let ax = if n.x.abs() < 1.0 / 64.0 && n.y.abs() < 1.0 / 64.0 {
145        wy.cross(n)
146    } else {
147        wz.cross(n)
148    };
149    let ax = ax.normalized().unwrap_or(Vec3::new(1.0, 0.0, 0.0));
150    let ay = n.cross(ax).normalized().unwrap_or(Vec3::new(0.0, 1.0, 0.0));
151    (ax, ay, n)
152}
153
154/// A 3x4 affine transform: `p' = x*p.x + y*p.y + z*p.z + t`.
155///
156/// `x`, `y`, `z` are the images of the unit axes (the matrix columns) and `t` is the
157/// translation.
158#[derive(Debug, Clone, Copy, PartialEq)]
159pub struct Transform {
160    /// Image of the X axis.
161    pub x: Vec3,
162    /// Image of the Y axis.
163    pub y: Vec3,
164    /// Image of the Z axis.
165    pub z: Vec3,
166    /// Translation.
167    pub t: Vec3,
168}
169
170impl Default for Transform {
171    fn default() -> Self {
172        Self::identity()
173    }
174}
175
176impl Transform {
177    /// The identity transform.
178    pub fn identity() -> Self {
179        Self {
180            x: Vec3::new(1.0, 0.0, 0.0),
181            y: Vec3::new(0.0, 1.0, 0.0),
182            z: Vec3::new(0.0, 0.0, 1.0),
183            t: Vec3::new(0.0, 0.0, 0.0),
184        }
185    }
186
187    /// Pure translation.
188    pub fn translation(v: Vec3) -> Self {
189        Self {
190            t: v,
191            ..Self::identity()
192        }
193    }
194
195    /// Axis-aligned scaling about the origin.
196    pub fn scaling(sx: f64, sy: f64, sz: f64) -> Self {
197        Self {
198            x: Vec3::new(sx, 0.0, 0.0),
199            y: Vec3::new(0.0, sy, 0.0),
200            z: Vec3::new(0.0, 0.0, sz),
201            t: Vec3::new(0.0, 0.0, 0.0),
202        }
203    }
204
205    /// Rotation about the Z axis, counter-clockwise by `angle` radians.
206    pub fn rotation_z(angle: f64) -> Self {
207        let (s, c) = angle.sin_cos();
208        Self {
209            x: Vec3::new(c, s, 0.0),
210            y: Vec3::new(-s, c, 0.0),
211            z: Vec3::new(0.0, 0.0, 1.0),
212            t: Vec3::new(0.0, 0.0, 0.0),
213        }
214    }
215
216    /// Transform whose columns are the given axes, with `origin` as translation.
217    pub fn from_axes(x: Vec3, y: Vec3, z: Vec3, origin: Vec3) -> Self {
218        Self { x, y, z, t: origin }
219    }
220
221    /// Maps plane-local coordinates to world coordinates: local `(x, y)` lie in the plane
222    /// through `origin` spanned by [`arbitrary_axes`]`(normal)`, local `z` runs along the normal.
223    pub fn plane(origin: Point3, normal: Vec3) -> Self {
224        let (ax, ay, az) = arbitrary_axes(normal);
225        Self::from_axes(ax, ay, az, Vec3::new(origin.x, origin.y, origin.z))
226    }
227
228    /// Applies the linear part only (directions, extents).
229    pub fn apply_vector(&self, v: Vec3) -> Vec3 {
230        self.x
231            .scaled(v.x)
232            .plus(self.y.scaled(v.y))
233            .plus(self.z.scaled(v.z))
234    }
235
236    /// Applies the full transform to a point.
237    pub fn apply_point(&self, p: Point3) -> Point3 {
238        let v = self.apply_vector(Vec3::new(p.x, p.y, p.z));
239        Point3::new(v.x + self.t.x, v.y + self.t.y, v.z + self.t.z)
240    }
241
242    /// Composition: the result applies `inner` first, then `self`
243    /// (`self.compose(inner).apply_point(p) == self.apply_point(inner.apply_point(p))`).
244    pub fn compose(&self, inner: &Transform) -> Transform {
245        let t = self.apply_point(Point3::new(inner.t.x, inner.t.y, inner.t.z));
246        Transform {
247            x: self.apply_vector(inner.x),
248            y: self.apply_vector(inner.y),
249            z: self.apply_vector(inner.z),
250            t: Vec3::new(t.x, t.y, t.z),
251        }
252    }
253
254    /// Determinant of the linear part; negative when the transform mirrors.
255    pub fn determinant(&self) -> f64 {
256        self.x.dot(self.y.cross(self.z))
257    }
258
259    /// Transform of a block reference: block coordinates to the coordinates of the space
260    /// holding the insert (WCS).
261    ///
262    /// `p_world = position + Plane(normal) * Rz(rotation) * (offset + scale * (p - base_point))`,
263    /// where `Plane(normal)` is the linear map of [`Transform::plane`]. `position` is a world
264    /// point; `offset` is the array offset of one cell in the rotated, unscaled insert frame.
265    pub fn from_insert_offset(
266        position: Point3,
267        scale: Vec3,
268        rotation: f64,
269        normal: Vec3,
270        base_point: Point3,
271        offset: Vec3,
272    ) -> Transform {
273        let minus_base =
274            Transform::translation(Vec3::new(-base_point.x, -base_point.y, -base_point.z));
275        let s = Transform::scaling(scale.x, scale.y, scale.z);
276        let off = Transform::translation(offset);
277        let r = Transform::rotation_z(rotation);
278        Transform::plane(position, normal)
279            .compose(&r)
280            .compose(&off)
281            .compose(&s)
282            .compose(&minus_base)
283    }
284
285    /// [`Transform::from_insert_offset`] without an array offset.
286    pub fn from_insert(
287        position: Point3,
288        scale: Vec3,
289        rotation: f64,
290        normal: Vec3,
291        base_point: Point3,
292    ) -> Transform {
293        Self::from_insert_offset(
294            position,
295            scale,
296            rotation,
297            normal,
298            base_point,
299            Vec3::new(0.0, 0.0, 0.0),
300        )
301    }
302}
303
304// ---------------------------------------------------------------------------------------------
305// Arcs and ellipses
306// ---------------------------------------------------------------------------------------------
307
308/// A (possibly partial) ellipse in parametric form
309/// `P(t) = center + u*cos(t) + v*sin(t)`, travelled from `t0` to `t1`.
310///
311/// `u` and `v` are conjugate semi-diameters (perpendicular for a true ellipse, but any pair
312/// is valid, which keeps the form closed under affine transforms). `t1 < t0` means the
313/// curve is travelled clockwise.
314#[derive(Debug, Clone, Copy, PartialEq)]
315pub struct EllipseArc {
316    /// Center.
317    pub center: Point3,
318    /// First semi-diameter (point at `t = 0` is `center + u`).
319    pub u: Vec3,
320    /// Second semi-diameter (point at `t = pi/2` is `center + v`).
321    pub v: Vec3,
322    /// Start parameter in radians.
323    pub t0: f64,
324    /// End parameter in radians.
325    pub t1: f64,
326}
327
328/// Counter-clockwise sweep from `start` to `end`, in (0, 2*pi]. Equal angles mean a full turn.
329pub fn ccw_sweep(start: f64, end: f64) -> f64 {
330    let d = (end - start).rem_euclid(TAU);
331    if d < 1e-12 { TAU } else { d }
332}
333
334impl EllipseArc {
335    /// Circular arc in the XY plane of the local frame: center `c`, radius `r`, CCW from
336    /// `start` to `end` (equal angles give a full circle).
337    pub fn circle(c: Point3, r: f64, start: f64, end: f64) -> Self {
338        Self {
339            center: c,
340            u: Vec3::new(r, 0.0, 0.0),
341            v: Vec3::new(0.0, r, 0.0),
342            t0: start,
343            t1: start + ccw_sweep(start, end),
344        }
345    }
346
347    /// Point at parameter `t`.
348    pub fn point(&self, t: f64) -> Point3 {
349        let (s, c) = t.sin_cos();
350        Point3::new(
351            self.center.x + self.u.x * c + self.v.x * s,
352            self.center.y + self.u.y * c + self.v.y * s,
353            self.center.z + self.u.z * c + self.v.z * s,
354        )
355    }
356
357    /// Signed parameter sweep `t1 - t0`.
358    pub fn sweep(&self) -> f64 {
359        self.t1 - self.t0
360    }
361
362    /// The arc after applying an affine transform (parameters are preserved).
363    pub fn transformed(&self, tf: &Transform) -> Self {
364        Self {
365            center: tf.apply_point(self.center),
366            u: tf.apply_vector(self.u),
367            v: tf.apply_vector(self.v),
368            t0: self.t0,
369            t1: self.t1,
370        }
371    }
372
373    /// Exact bounding box of the traveled curve, merged into `bb`.
374    pub fn extend_bbox(&self, bb: &mut Option<BBox>) {
375        extend_bbox(bb, self.point(self.t0));
376        extend_bbox(bb, self.point(self.t1));
377        let (lo, hi) = if self.t0 <= self.t1 {
378            (self.t0, self.t1)
379        } else {
380            (self.t1, self.t0)
381        };
382        let hi = hi.min(lo + TAU);
383        let axes = [
384            (self.u.x, self.v.x),
385            (self.u.y, self.v.y),
386            (self.u.z, self.v.z),
387        ];
388        for (a, b) in axes {
389            if !(a.is_finite() && b.is_finite()) {
390                continue;
391            }
392            // d/dt (a cos t + b sin t) = 0 at t = atan2(b, a) + k*pi.
393            let te = b.atan2(a);
394            let mut k = ((lo - te) / PI).ceil();
395            let mut guard = 0;
396            while guard < 4 {
397                let t = te + k * PI;
398                if t > hi {
399                    break;
400                }
401                extend_bbox(bb, self.point(t));
402                k += 1.0;
403                guard += 1;
404            }
405        }
406    }
407}
408
409/// Principal axes of the ellipse spanned by conjugate semi-diameters `u`, `v` (2D).
410/// Returns `(semi_major, semi_minor, rotation_of_major_axis)`.
411pub fn conjugate_to_axes(u: (f64, f64), v: (f64, f64)) -> (f64, f64, f64) {
412    let uu = u.0 * u.0 + u.1 * u.1;
413    let vv = v.0 * v.0 + v.1 * v.1;
414    let uv = u.0 * v.0 + u.1 * v.1;
415    // |P(t)|^2 is extremal where tan(2t) = 2 u.v / (u.u - v.v).
416    let t0 = 0.5 * (2.0 * uv).atan2(uu - vv);
417    let (s, c) = t0.sin_cos();
418    let a = (u.0 * c + v.0 * s, u.1 * c + v.1 * s);
419    let b = (-u.0 * s + v.0 * c, -u.1 * s + v.1 * c);
420    let ra = a.0.hypot(a.1);
421    let rb = b.0.hypot(b.1);
422    if ra >= rb {
423        (ra, rb, a.1.atan2(a.0))
424    } else {
425        (rb, ra, b.1.atan2(b.0))
426    }
427}
428
429/// Ellipse (or elliptical arc) from DXF-style data: `major_axis` is the vector from the
430/// center to the end of the major axis, `ratio` = minor / major, parameters in radians.
431/// The minor axis direction is `normal x major_axis`.
432pub fn ellipse_arc(
433    center: Point3,
434    major_axis: Vec3,
435    ratio: f64,
436    normal: Vec3,
437    start: f64,
438    end: f64,
439) -> EllipseArc {
440    let n = normal.normalized().unwrap_or(Vec3::Z);
441    let minor_dir = n
442        .cross(major_axis)
443        .scaled(if ratio.is_finite() { ratio } else { 1.0 });
444    EllipseArc {
445        center,
446        u: major_axis,
447        v: minor_dir,
448        t0: start,
449        t1: start + ccw_sweep(start, end),
450    }
451}
452
453/// Point of an ellipse at parameter `t` (see [`ellipse_arc`] for the conventions).
454pub fn ellipse_point(center: Point3, major_axis: Vec3, ratio: f64, normal: Vec3, t: f64) -> Point3 {
455    ellipse_arc(center, major_axis, ratio, normal, 0.0, TAU).point(t)
456}
457
458/// A circular arc recovered from a polyline segment with a bulge.
459#[derive(Debug, Clone, Copy, PartialEq)]
460pub struct BulgeArc {
461    /// Center in the plane of the segment.
462    pub center: (f64, f64),
463    /// Radius.
464    pub radius: f64,
465    /// Angle of the segment start as seen from the center.
466    pub start_angle: f64,
467    /// Signed sweep: positive counter-clockwise, `4 * atan(bulge)`.
468    pub sweep: f64,
469}
470
471impl BulgeArc {
472    /// Angle of the segment end (`start_angle + sweep`).
473    pub fn end_angle(&self) -> f64 {
474        self.start_angle + self.sweep
475    }
476
477    /// True when the arc runs counter-clockwise.
478    pub fn is_ccw(&self) -> bool {
479        self.sweep > 0.0
480    }
481}
482
483/// Converts the segment `p0 -> p1` with the given `bulge` (tan of a quarter of the included
484/// angle, positive = counter-clockwise) into a circular arc. `None` for a straight segment
485/// (|bulge| ~ 0), coincident end points or non-finite input.
486pub fn bulge_to_arc(p0: (f64, f64), p1: (f64, f64), bulge: f64) -> Option<BulgeArc> {
487    if !bulge.is_finite() || bulge.abs() < 1e-12 {
488        return None;
489    }
490    let (dx, dy) = (p1.0 - p0.0, p1.1 - p0.1);
491    let d = dx.hypot(dy);
492    if !d.is_finite() || d < 1e-300 {
493        return None;
494    }
495    let radius = d * (1.0 + bulge * bulge) / (4.0 * bulge.abs());
496    // Distance from the chord midpoint to the center, signed to the left of p0 -> p1:
497    // (d/2) / tan(theta/2) with tan(2*atan(b)) = 2b / (1 - b^2).
498    let off = (d / 2.0) * (1.0 - bulge * bulge) / (2.0 * bulge);
499    let mid = ((p0.0 + p1.0) / 2.0, (p0.1 + p1.1) / 2.0);
500    let center = (mid.0 - dy / d * off, mid.1 + dx / d * off);
501    let start_angle = (p0.1 - center.1).atan2(p0.0 - center.0);
502    Some(BulgeArc {
503        center,
504        radius,
505        start_angle,
506        sweep: 4.0 * bulge.atan(),
507    })
508}
509
510// ---------------------------------------------------------------------------------------------
511// NURBS
512// ---------------------------------------------------------------------------------------------
513
514/// Clamped uniform knot vector for `n` control points of the given degree
515/// (`n + degree + 1` knots, domain `[0, n - degree]`).
516pub fn clamped_uniform_knots(n: usize, degree: usize) -> Vec<f64> {
517    let spans = n.saturating_sub(degree);
518    let mut k = Vec::with_capacity(n + degree + 1);
519    for i in 0..(n + degree + 1) {
520        let v = if i <= degree {
521            0.0
522        } else if i >= n {
523            spans as f64
524        } else {
525            (i - degree) as f64
526        };
527        k.push(v);
528    }
529    k
530}
531
532/// Highest B-spline degree that is evaluated; anything above is drawn as its control polygon.
533pub const MAX_NURBS_DEGREE: usize = 25;
534
535/// Upper bound on de Boor work (samples x degree^2) per flattened spline.
536const MAX_NURBS_FLOP: usize = 5_000_000;
537
538#[derive(Clone, Copy)]
539struct H {
540    x: f64,
541    y: f64,
542    z: f64,
543    w: f64,
544}
545
546impl H {
547    const ZERO: H = H {
548        x: 0.0,
549        y: 0.0,
550        z: 0.0,
551        w: 0.0,
552    };
553
554    fn lerp(a: H, b: H, alpha: f64) -> H {
555        let m = 1.0 - alpha;
556        H {
557            x: a.x * m + b.x * alpha,
558            y: a.y * m + b.y * alpha,
559            z: a.z * m + b.z * alpha,
560            w: a.w * m + b.w * alpha,
561        }
562    }
563}
564
565/// Index of the knot span containing `t`: the largest `k` in `[degree, n - 1]` with
566/// `knots[k] <= t`.
567fn find_span(knots: &[f64], n: usize, degree: usize, t: f64) -> Option<usize> {
568    let s = knots.get(degree..n)?;
569    let count = s.partition_point(|&x| x <= t);
570    Some(degree + count.saturating_sub(1))
571}
572
573fn nurbs_eval_h(degree: usize, knots: &[f64], pts: &[H], t: f64) -> Option<Point3> {
574    let n = pts.len();
575    if degree == 0 || degree > MAX_NURBS_DEGREE || degree >= n || knots.len() != n + degree + 1 {
576        return None;
577    }
578    let k = find_span(knots, n, degree, t)?;
579    // Fixed scratch space: evaluation allocates nothing.
580    let mut d = [H::ZERO; MAX_NURBS_DEGREE + 1];
581    for (slot, p) in d.iter_mut().zip(pts.get(k.checked_sub(degree)?..=k)?) {
582        *slot = *p;
583    }
584    for r in 1..=degree {
585        for j in (r..=degree).rev() {
586            let i = j + k - degree;
587            let lo = *knots.get(i)?;
588            let hi = *knots.get(i + degree + 1 - r)?;
589            let den = hi - lo;
590            let alpha = if den.abs() < 1e-300 {
591                0.0
592            } else {
593                (t - lo) / den
594            };
595            let a = *d.get(j - 1)?;
596            let b = *d.get(j)?;
597            *d.get_mut(j)? = H::lerp(a, b, alpha);
598        }
599    }
600    let r = *d.get(degree)?;
601    if r.w.abs() < 1e-300 {
602        return None;
603    }
604    Some(Point3::new(r.x / r.w, r.y / r.w, r.z / r.w))
605}
606
607fn homogeneous(control: &[Point3], weights: &[f64]) -> Vec<H> {
608    let rational =
609        weights.len() == control.len() && weights.iter().all(|w| w.is_finite() && *w > 0.0);
610    control
611        .iter()
612        .enumerate()
613        .map(|(i, p)| {
614            let w = if rational {
615                weights.get(i).copied().unwrap_or(1.0)
616            } else {
617                1.0
618            };
619            H {
620                x: p.x * w,
621                y: p.y * w,
622                z: p.z * w,
623                w,
624            }
625        })
626        .collect()
627}
628
629/// Evaluates a (rational) B-spline at parameter `t` with de Boor's algorithm.
630///
631/// `weights` may be empty (non-rational). `t` is clamped to the curve domain
632/// `[knots[degree], knots[n]]`. Returns `None` when the arguments are inconsistent
633/// (`knots.len() != control.len() + degree + 1`, degree 0, too few points).
634pub fn nurbs_eval(
635    degree: usize,
636    knots: &[f64],
637    control: &[Point3],
638    weights: &[f64],
639    t: f64,
640) -> Option<Point3> {
641    let n = control.len();
642    let lo = *knots.get(degree)?;
643    let hi = *knots.get(n)?;
644    let t = if hi >= lo { t.clamp(lo, hi) } else { t };
645    nurbs_eval_h(degree, knots, &homogeneous(control, weights), t)
646}
647
648/// Samples a NURBS curve into a polyline.
649///
650/// Robust against messy input: a missing or inconsistent knot vector is replaced by a
651/// clamped uniform one, invalid weights are ignored, and a degree that is too high for the
652/// number of control points is lowered. With fewer than 2 control points the control points
653/// are returned unchanged, and when the domain is empty the control polygon is returned.
654/// `samples_per_span` points are generated per non-empty knot span (total capped at 100 000).
655pub fn nurbs_flatten(
656    degree: u32,
657    knots: &[f64],
658    control: &[Point3],
659    weights: &[f64],
660    samples_per_span: usize,
661) -> Vec<Point3> {
662    let n = control.len();
663    if n < 2 {
664        return control.to_vec();
665    }
666    let p = (degree as usize).clamp(1, n - 1);
667    if p > MAX_NURBS_DEGREE {
668        return control.to_vec();
669    }
670    let knots_ok = knots.len() == n + p + 1
671        && knots.iter().all(|k| k.is_finite())
672        && knots.windows(2).all(|w| w.first() <= w.get(1));
673    let knots: Vec<f64> = if knots_ok {
674        knots.to_vec()
675    } else {
676        clamped_uniform_knots(n, p)
677    };
678    let (Some(&lo), Some(&hi)) = (knots.get(p), knots.get(n)) else {
679        return control.to_vec();
680    };
681    if hi <= lo {
682        return control.to_vec();
683    }
684    let pts = homogeneous(control, weights);
685    let spans: Vec<(f64, f64)> = knots
686        .get(p..=n)
687        .map(|s| {
688            s.windows(2)
689                .filter_map(|w| Some((*w.first()?, *w.get(1)?)))
690                .filter(|(a, b)| b > a)
691                .collect()
692        })
693        .unwrap_or_default();
694    let max_samples = (MAX_NURBS_FLOP / (p * p)).clamp(2, 100_000);
695    let per = samples_per_span.clamp(1, (max_samples / spans.len().max(1)).max(1));
696    let mut out = Vec::with_capacity(spans.len() * per + 1);
697    for (a, b) in &spans {
698        for i in 0..per {
699            let t = a + (b - a) * (i as f64) / (per as f64);
700            if let Some(pt) = nurbs_eval_h(p, &knots, &pts, t) {
701                out.push(pt);
702            }
703        }
704    }
705    if let Some(pt) = nurbs_eval_h(p, &knots, &pts, hi) {
706        out.push(pt);
707    }
708    out
709}
710
711#[cfg(test)]
712mod tests {
713    use super::*;
714
715    fn close(a: f64, b: f64) -> bool {
716        (a - b).abs() < 1e-9
717    }
718    fn pclose(a: Point3, b: Point3) -> bool {
719        close(a.x, b.x) && close(a.y, b.y) && close(a.z, b.z)
720    }
721
722    #[test]
723    fn vec_ops() {
724        let a = Vec3::new(1.0, 0.0, 0.0);
725        let b = Vec3::new(0.0, 1.0, 0.0);
726        assert_eq!(a.cross(b), Vec3::new(0.0, 0.0, 1.0));
727        assert_eq!(a.dot(b), 0.0);
728        assert!(Vec3::new(0.0, 0.0, 0.0).normalized().is_none());
729        assert!(close(Vec3::new(3.0, 4.0, 0.0).length(), 5.0));
730    }
731
732    #[test]
733    fn bbox_helpers() {
734        let mut bb = None;
735        extend_bbox(&mut bb, Point3::new(f64::NAN, 0.0, 0.0));
736        assert!(bb.is_none());
737        extend_bbox(&mut bb, Point3::xy(1.0, 2.0));
738        extend_bbox(&mut bb, Point3::xy(-1.0, 5.0));
739        let b = bb.unwrap();
740        assert!(close(b.width(), 2.0) && close(b.height(), 3.0));
741    }
742
743    #[test]
744    fn arbitrary_axis_default_normal() {
745        let (ax, ay, az) = arbitrary_axes(Vec3::Z);
746        assert_eq!(ax, Vec3::new(1.0, 0.0, 0.0));
747        assert_eq!(ay, Vec3::new(0.0, 1.0, 0.0));
748        assert_eq!(az, Vec3::Z);
749    }
750
751    #[test]
752    fn arbitrary_axis_flipped_normal() {
753        // N = -Z: Ax = Wy x N = (-1, 0, 0); Ay = N x Ax = (0, 1, 0).
754        let (ax, ay, az) = arbitrary_axes(Vec3::new(0.0, 0.0, -1.0));
755        assert_eq!(ax, Vec3::new(-1.0, 0.0, 0.0));
756        assert_eq!(ay, Vec3::new(0.0, 1.0, 0.0));
757        assert_eq!(az, Vec3::new(0.0, 0.0, -1.0));
758    }
759
760    #[test]
761    fn arbitrary_axis_oblique_normal_is_orthonormal() {
762        let n = Vec3::new(1.0, 2.0, 3.0).normalized().unwrap();
763        let (ax, ay, az) = arbitrary_axes(n);
764        assert!(close(ax.length(), 1.0) && close(ay.length(), 1.0));
765        assert!(close(ax.dot(ay), 0.0) && close(ax.dot(az), 0.0) && close(ay.dot(az), 0.0));
766        // Ax = Wz x N is horizontal.
767        assert!(close(ax.z, 0.0));
768        // Right-handed.
769        assert!(close(ax.cross(ay).dot(az), 1.0));
770    }
771
772    #[test]
773    fn arbitrary_axis_near_z_uses_wy() {
774        let n = Vec3::new(0.001, 0.001, 1.0).normalized().unwrap();
775        let (ax, _, _) = arbitrary_axes(n);
776        let expect = Vec3::new(0.0, 1.0, 0.0).cross(n).normalized().unwrap();
777        assert!(close(ax.x, expect.x) && close(ax.y, expect.y) && close(ax.z, expect.z));
778    }
779
780    #[test]
781    fn arbitrary_axis_zero_normal_is_identity() {
782        let (ax, ay, az) = arbitrary_axes(Vec3::new(0.0, 0.0, 0.0));
783        assert_eq!(
784            (ax, ay, az),
785            (Vec3::new(1.0, 0.0, 0.0), Vec3::new(0.0, 1.0, 0.0), Vec3::Z)
786        );
787    }
788
789    #[test]
790    fn transform_compose_order() {
791        let t = Transform::translation(Vec3::new(10.0, 0.0, 0.0));
792        let r = Transform::rotation_z(PI / 2.0);
793        // rotate first, then translate
794        let c = t.compose(&r);
795        let p = c.apply_point(Point3::new(1.0, 0.0, 0.0));
796        assert!(pclose(p, Point3::new(10.0, 1.0, 0.0)));
797        // translate first, then rotate
798        let c = r.compose(&t);
799        let p = c.apply_point(Point3::new(1.0, 0.0, 0.0));
800        assert!(pclose(p, Point3::new(0.0, 11.0, 0.0)));
801    }
802
803    #[test]
804    fn transform_vector_ignores_translation() {
805        let t = Transform::translation(Vec3::new(5.0, 5.0, 5.0));
806        assert_eq!(
807            t.apply_vector(Vec3::new(1.0, 2.0, 3.0)),
808            Vec3::new(1.0, 2.0, 3.0)
809        );
810        assert_eq!(Transform::identity().determinant(), 1.0);
811        assert!(Transform::scaling(-1.0, 1.0, 1.0).determinant() < 0.0);
812    }
813
814    #[test]
815    fn insert_transform() {
816        // base (1,1), scale 2, rotate 90, position (10,20).
817        let tf = Transform::from_insert(
818            Point3::new(10.0, 20.0, 0.0),
819            Vec3::new(2.0, 2.0, 2.0),
820            PI / 2.0,
821            Vec3::Z,
822            Point3::new(1.0, 1.0, 0.0),
823        );
824        // block point (2,1): minus base -> (1,0); scale -> (2,0); rotate -> (0,2); + pos.
825        assert!(pclose(
826            tf.apply_point(Point3::new(2.0, 1.0, 0.0)),
827            Point3::new(10.0, 22.0, 0.0)
828        ));
829    }
830
831    #[test]
832    fn insert_transform_with_offset_and_flipped_normal() {
833        let tf = Transform::from_insert_offset(
834            Point3::new(0.0, 0.0, 0.0),
835            Vec3::new(1.0, 1.0, 1.0),
836            0.0,
837            Vec3::new(0.0, 0.0, -1.0),
838            Point3::default(),
839            Vec3::new(5.0, 0.0, 0.0),
840        );
841        // The plane x axis is -X in the world; the position is a plain world point.
842        assert!(pclose(
843            tf.apply_point(Point3::new(1.0, 2.0, 0.0)),
844            Point3::new(-6.0, 2.0, 0.0)
845        ));
846    }
847
848    #[test]
849    fn bulge_semicircle() {
850        let a = bulge_to_arc((0.0, 0.0), (2.0, 0.0), 1.0).unwrap();
851        assert!(close(a.radius, 1.0));
852        assert!(close(a.center.0, 1.0) && close(a.center.1, 0.0));
853        assert!(close(a.sweep, PI));
854        assert!(a.is_ccw());
855        assert!(close(a.start_angle.abs(), PI));
856        // CCW from (0,0) to (2,0) passes below the chord: point at start + sweep/2 is (1,-1).
857        let mid = a.start_angle + a.sweep / 2.0;
858        assert!(close(a.center.0 + a.radius * mid.cos(), 1.0));
859        assert!(close(a.center.1 + a.radius * mid.sin(), -1.0));
860    }
861
862    #[test]
863    fn bulge_negative_and_quarter() {
864        let a = bulge_to_arc((0.0, 0.0), (2.0, 0.0), -1.0).unwrap();
865        assert!(!a.is_ccw());
866        assert!(close(a.sweep, -PI));
867        // Quarter circle: bulge = tan(pi/8), chord from (1,0) to (0,1) about the origin, CCW.
868        let b = (PI / 8.0).tan();
869        let a = bulge_to_arc((1.0, 0.0), (0.0, 1.0), b).unwrap();
870        assert!(close(a.radius, 1.0));
871        assert!(close(a.center.0, 0.0) && close(a.center.1, 0.0));
872        assert!(close(a.start_angle, 0.0));
873        assert!(close(a.end_angle(), PI / 2.0));
874    }
875
876    #[test]
877    fn bulge_large_arc_center_other_side() {
878        // bulge 2 > 1: more than a semicircle; the center lies on the bulge side of the chord.
879        let a = bulge_to_arc((0.0, 0.0), (2.0, 0.0), 2.0).unwrap();
880        assert!(a.sweep > PI);
881        assert!(a.center.1 < 0.0);
882        let end = a.end_angle();
883        assert!(close(a.center.0 + a.radius * end.cos(), 2.0));
884        assert!(close(a.center.1 + a.radius * end.sin(), 0.0));
885    }
886
887    #[test]
888    fn bulge_degenerate() {
889        assert!(bulge_to_arc((0.0, 0.0), (1.0, 0.0), 0.0).is_none());
890        assert!(bulge_to_arc((1.0, 1.0), (1.0, 1.0), 1.0).is_none());
891        assert!(bulge_to_arc((0.0, 0.0), (1.0, 0.0), f64::NAN).is_none());
892    }
893
894    #[test]
895    fn ellipse_points() {
896        let c = Point3::new(1.0, 2.0, 0.0);
897        let major = Vec3::new(4.0, 0.0, 0.0);
898        let p0 = ellipse_point(c, major, 0.5, Vec3::Z, 0.0);
899        assert!(pclose(p0, Point3::new(5.0, 2.0, 0.0)));
900        let p1 = ellipse_point(c, major, 0.5, Vec3::Z, PI / 2.0);
901        assert!(pclose(p1, Point3::new(1.0, 4.0, 0.0)));
902        // Rotated major axis.
903        let p = ellipse_point(
904            Point3::default(),
905            Vec3::new(0.0, 2.0, 0.0),
906            0.5,
907            Vec3::Z,
908            PI / 2.0,
909        );
910        assert!(pclose(p, Point3::new(-1.0, 0.0, 0.0)));
911    }
912
913    #[test]
914    fn conjugate_axes_recovers_ellipse() {
915        let (a, b, rot) = conjugate_to_axes((4.0, 0.0), (0.0, 2.0));
916        assert!(close(a, 4.0) && close(b, 2.0) && close(rot, 0.0));
917        // Unit circle sheared by x' = x + y: semi-axes are phi and 1/phi.
918        let (a, b, _) = conjugate_to_axes((1.0, 0.0), (1.0, 1.0));
919        let phi = (1.0 + 5f64.sqrt()) / 2.0;
920        assert!(close(a, phi) && close(b, 1.0 / phi));
921    }
922
923    #[test]
924    fn ccw_sweep_wraps() {
925        assert!(close(ccw_sweep(0.0, PI / 2.0), PI / 2.0));
926        assert!(close(ccw_sweep(3.0 * PI / 2.0, PI / 2.0), PI));
927        assert!(close(ccw_sweep(1.0, 1.0), TAU));
928    }
929
930    #[test]
931    fn arc_bbox_includes_extrema() {
932        let arc = EllipseArc::circle(Point3::default(), 1.0, -0.1, 0.1);
933        let mut bb = None;
934        arc.extend_bbox(&mut bb);
935        let bb = bb.unwrap();
936        assert!(close(bb.max.x, 1.0));
937        assert!(bb.min.x > 0.99);
938        let full = EllipseArc::circle(Point3::new(1.0, 1.0, 0.0), 2.0, 0.0, 0.0);
939        let mut bb = None;
940        full.extend_bbox(&mut bb);
941        let bb = bb.unwrap();
942        assert!(
943            close(bb.min.x, -1.0)
944                && close(bb.max.x, 3.0)
945                && close(bb.min.y, -1.0)
946                && close(bb.max.y, 3.0)
947        );
948    }
949
950    #[test]
951    fn nurbs_bezier() {
952        let c = [
953            Point3::xy(0.0, 0.0),
954            Point3::xy(1.0, 2.0),
955            Point3::xy(2.0, 0.0),
956        ];
957        let knots = [0.0, 0.0, 0.0, 1.0, 1.0, 1.0];
958        let p = nurbs_eval(2, &knots, &c, &[], 0.5).unwrap();
959        // Quadratic Bezier at 0.5: 0.25 P0 + 0.5 P1 + 0.25 P2.
960        assert!(pclose(p, Point3::xy(1.0, 1.0)));
961        assert!(pclose(nurbs_eval(2, &knots, &c, &[], 0.0).unwrap(), c[0]));
962        assert!(pclose(nurbs_eval(2, &knots, &c, &[], 1.0).unwrap(), c[2]));
963        // Out-of-domain parameter is clamped.
964        assert!(pclose(nurbs_eval(2, &knots, &c, &[], 5.0).unwrap(), c[2]));
965    }
966
967    #[test]
968    fn nurbs_rational_quarter_circle() {
969        let w = 2f64.sqrt() / 2.0;
970        let c = [
971            Point3::xy(1.0, 0.0),
972            Point3::xy(1.0, 1.0),
973            Point3::xy(0.0, 1.0),
974        ];
975        let knots = [0.0, 0.0, 0.0, 1.0, 1.0, 1.0];
976        for i in 0..=10 {
977            let t = f64::from(i) / 10.0;
978            let p = nurbs_eval(2, &knots, &c, &[1.0, w, 1.0], t).unwrap();
979            assert!(close(p.x.hypot(p.y), 1.0), "t={t} r={}", p.x.hypot(p.y));
980        }
981        // Midpoint is at 45 degrees.
982        let p = nurbs_eval(2, &knots, &c, &[1.0, w, 1.0], 0.5).unwrap();
983        assert!(close(p.x, w) && close(p.y, w));
984    }
985
986    #[test]
987    fn nurbs_cubic_linear_precision() {
988        // Control points at the Greville abscissae reproduce x(t) = t exactly.
989        let knots = clamped_uniform_knots(5, 3);
990        assert_eq!(knots, vec![0.0, 0.0, 0.0, 0.0, 1.0, 2.0, 2.0, 2.0, 2.0]);
991        let g = [0.0, 1.0 / 3.0, 1.0, 5.0 / 3.0, 2.0];
992        let c: Vec<Point3> = g.iter().map(|&x| Point3::xy(x, x * x)).collect();
993        for t in [0.0, 0.25, 0.5, 1.0, 1.7, 2.0] {
994            let p = nurbs_eval(3, &knots, &c, &[], t).unwrap();
995            assert!(close(p.x, t), "t={t} x={}", p.x);
996        }
997    }
998
999    #[test]
1000    fn nurbs_degree_and_work_are_capped() {
1001        let c: Vec<Point3> = (0..200).map(|i| Point3::xy(f64::from(i), 0.0)).collect();
1002        // Degree above the cap: control polygon.
1003        assert_eq!(nurbs_flatten(60, &[], &c, &[], 16).len(), 200);
1004        assert!(nurbs_eval(60, &clamped_uniform_knots(200, 60), &c, &[], 1.0).is_none());
1005        // Degree at the cap: bounded number of samples.
1006        let pts = nurbs_flatten(25, &[], &c, &[], 100_000);
1007        assert!(pts.len() <= 5_000_000 / (25 * 25) + 1, "{}", pts.len());
1008        assert!(pts.len() > 10);
1009    }
1010
1011    #[test]
1012    fn nurbs_rejects_inconsistent_input() {
1013        let c = [Point3::xy(0.0, 0.0), Point3::xy(1.0, 1.0)];
1014        assert!(nurbs_eval(2, &[0.0, 1.0], &c, &[], 0.5).is_none());
1015        assert!(nurbs_eval(0, &[0.0, 1.0, 2.0], &c, &[], 0.5).is_none());
1016    }
1017
1018    #[test]
1019    fn flatten_endpoints_and_fallbacks() {
1020        let c = [
1021            Point3::xy(0.0, 0.0),
1022            Point3::xy(1.0, 2.0),
1023            Point3::xy(2.0, 0.0),
1024        ];
1025        let pts = nurbs_flatten(2, &[0.0, 0.0, 0.0, 1.0, 1.0, 1.0], &c, &[], 8);
1026        assert_eq!(pts.len(), 9);
1027        assert!(pclose(*pts.first().unwrap(), c[0]));
1028        assert!(pclose(*pts.last().unwrap(), c[2]));
1029        // Missing knots and bogus degree still work.
1030        let pts = nurbs_flatten(9, &[], &c, &[], 4);
1031        assert!(pts.len() >= 3);
1032        assert!(pclose(*pts.last().unwrap(), c[2]));
1033        // Single point.
1034        assert_eq!(nurbs_flatten(3, &[], &c[..1], &[], 4).len(), 1);
1035        // Bad weights are ignored.
1036        let pts = nurbs_flatten(2, &[], &c, &[1.0, -1.0, 1.0], 4);
1037        assert!(pts.iter().all(|p| p.is_finite()));
1038    }
1039}