geop_core_geometry/nurb_curve/
evaluate.rs

1use geop_core_math::{
2    geop_error::{GeopError, GeopResult, WithContext},
3    scalars::Scalar,
4    vector::{Vector, Vector2, Vector3},
5};
6
7use super::NurbCurve;
8
9impl<S: Scalar, const D: usize> NurbCurve<S, D> {
10    /// Returns span index k where u[k] <= t < u[k+1].
11    pub(super) fn find_knot_span(&self, t: S) -> GeopResult<usize> {
12        let n = self.control_points.len() - 1;
13        let p = self.degree;
14        let u = &self.knot_vector;
15
16        if t.definitely_less(u[p]) || t.definitely_greater(u[n + 1]) {
17            return Err(GeopError::new(format!(
18                "parameter t={} out of domain [{}, {}]",
19                t,
20                u[p],
21                u[n + 1]
22            )));
23        }
24
25        if !t.definitely_less(u[n + 1]) {
26            for k in (p..=n).rev() {
27                if u[k].definitely_less(u[n + 1]) {
28                    return Ok(k);
29                }
30            }
31            return Ok(p);
32        }
33
34        for k in p..=n {
35            if !t.definitely_less(u[k]) && t.definitely_less(u[k + 1]) {
36                return Ok(k);
37            }
38        }
39
40        Err(GeopError::new("could not find knot span"))
41    }
42
43    /// De Boor triangular recursion; returns the homogeneous result `Vector<S, D>`.
44    pub(super) fn de_boor(&self, t: S, span: usize) -> Vector<S, D> {
45        let p = self.degree;
46        let u = &self.knot_vector;
47        let mut d: Vec<Vector<S, D>> = (0..=p).map(|j| self.control_points[span - p + j]).collect();
48
49        for r in 1..=p {
50            for j in (r..=p).rev() {
51                let i = span - p + j;
52                let denom = u[i + p - r + 1].sub(u[i]);
53                let alpha = if denom.could_be_equal(S::ZERO) {
54                    S::ZERO
55                } else {
56                    t.sub(u[i]).div(denom).unwrap_or(S::ZERO)
57                };
58                d[j] = Vector::interpolate(&d[j - 1], &d[j], alpha);
59            }
60        }
61        d[p]
62    }
63}
64
65// ── 3-D curve: evaluate returns Vector3 ─────────────────────────────────────
66
67impl<S: Scalar> NurbCurve<S, 4> {
68    /// Evaluate the 3-D NURBS curve at `t`, returning a Cartesian `Vector3`.
69    pub fn evaluate(&self, t: S) -> GeopResult<Vector3<S>> {
70        let span = self.find_knot_span(t)?;
71        let hw = self.de_boor(t, span);
72        let w = hw[3];
73        if w.could_be_equal(S::ZERO) {
74            return Err(GeopError::new(format!(
75                "weight is zero at evaluation point (t={t:?}, span={span}, homogeneous de_boor result w={w:?}, degree={}, knot_vector={:?}, control_points={:?})",
76                self.degree, self.knot_vector, self.control_points
77            )));
78        }
79        let inv_w = S::ONE.div(w).with_context(&|e: GeopError| {
80            e.with_context(format!(
81                "NurbCurve::evaluate(t={t}): degree={}, knot_vector={:?}, control_points={:?}",
82                self.degree, self.knot_vector, self.control_points
83            ))
84        })?;
85        let mut result = Vector3::zero();
86        for c in 0..3 {
87            result[c] = hw[c].mul(inv_w);
88        }
89        Ok(result)
90    }
91}
92
93impl<S: Scalar> geop_core_math::primitives::scene::RasterizableCurve<S> for NurbCurve<S, 4> {
94    fn eval_at(&self, t: S) -> GeopResult<Vector3<S>> {
95        self.evaluate(t)
96    }
97}
98
99// ── 2-D curve (pcurve): evaluate returns Vector2 ─────────────────────────────
100
101impl<S: Scalar> NurbCurve<S, 3> {
102    /// Evaluate the 2-D pcurve at `t`, returning a Cartesian `Vector2`.
103    pub fn evaluate(&self, t: S) -> GeopResult<Vector2<S>> {
104        let span = self.find_knot_span(t)?;
105        let hw = self.de_boor(t, span);
106        let w = hw[2];
107        if w.could_be_equal(S::ZERO) {
108            return Err(GeopError::new(format!(
109                "weight is zero at evaluation point (t={t:?}, span={span}, homogeneous de_boor result w={w:?}, degree={}, knot_vector={:?}, control_points={:?})",
110                self.degree, self.knot_vector, self.control_points
111            )));
112        }
113        let inv_w = S::ONE.div(w).with_context(&|e: GeopError| {
114            e.with_context(format!(
115                "NurbCurve::evaluate(t={t}): degree={}, knot_vector={:?}, control_points={:?}",
116                self.degree, self.knot_vector, self.control_points
117            ))
118        })?;
119        let mut result = Vector2::zero();
120        for c in 0..2 {
121            result[c] = hw[c].mul(inv_w);
122        }
123        Ok(result)
124    }
125}
126
127#[cfg(test)]
128mod tests {
129    use crate::nurb_curve::NurbCurve;
130    use geop_core_math::for_all_scalars;
131    use geop_core_math::{scalars::Scalar, vector::Vector4};
132
133    fn pt<S: Scalar>(x: f64, y: f64, z: f64, w: f64) -> Vector4<S> {
134        Vector4::from_array([
135            S::from_f64(x),
136            S::from_f64(y),
137            S::from_f64(z),
138            S::from_f64(w),
139        ])
140    }
141
142    fn line_curve<S: Scalar>() -> NurbCurve<S, 4> {
143        NurbCurve::try_new(
144            1,
145            vec![pt(0.0, 0.0, 0.0, 1.0), pt(1.0, 0.0, 0.0, 1.0)],
146            vec![
147                S::from_f64(0.0),
148                S::from_f64(0.0),
149                S::from_f64(1.0),
150                S::from_f64(1.0),
151            ],
152        )
153        .unwrap()
154    }
155
156    fn check_line_at_start<S: Scalar>() {
157        let c = line_curve::<S>();
158        let p = c.evaluate(S::ZERO).unwrap();
159        assert!(p[0].could_be_equal(S::ZERO));
160        assert!(p[1].could_be_equal(S::ZERO));
161    }
162    #[test]
163    fn line_at_start() {
164        for_all_scalars!(check_line_at_start);
165    }
166
167    fn check_line_at_end<S: Scalar>() {
168        let c = line_curve::<S>();
169        let p = c.evaluate(S::ONE).unwrap();
170        assert!(p[0].could_be_equal(S::ONE));
171        assert!(p[1].could_be_equal(S::ZERO));
172    }
173    #[test]
174    fn line_at_end() {
175        for_all_scalars!(check_line_at_end);
176    }
177
178    fn check_line_at_midpoint<S: Scalar>() {
179        let c = line_curve::<S>();
180        let p = c.evaluate(S::from_f64(0.5)).unwrap();
181        assert!(p[0].could_be_equal(S::from_f64(0.5)));
182    }
183    #[test]
184    fn line_at_midpoint() {
185        for_all_scalars!(check_line_at_midpoint);
186    }
187
188    fn check_out_of_domain_returns_err<S: Scalar>() {
189        let c = line_curve::<S>();
190        assert!(c.evaluate(S::from_f64(-0.1)).is_err());
191        assert!(c.evaluate(S::from_f64(1.1)).is_err());
192    }
193    #[test]
194    fn out_of_domain_returns_err() {
195        for_all_scalars!(check_out_of_domain_returns_err);
196    }
197
198    fn check_quadratic_midpoint<S: Scalar>() {
199        let curve = NurbCurve::try_new(
200            2,
201            vec![
202                pt(0.0, 0.0, 0.0, 1.0),
203                pt(0.5, 0.0, 0.0, 1.0),
204                pt(1.0, 0.0, 0.0, 1.0),
205            ],
206            vec![
207                S::from_f64(0.0),
208                S::from_f64(0.0),
209                S::from_f64(0.0),
210                S::from_f64(1.0),
211                S::from_f64(1.0),
212                S::from_f64(1.0),
213            ],
214        )
215        .unwrap();
216        let p = curve.evaluate(S::from_f64(0.5)).unwrap();
217        assert!(p[0].could_be_equal(S::from_f64(0.5)));
218        assert!(p[1].could_be_equal(S::ZERO));
219    }
220    #[test]
221    fn quadratic_midpoint() {
222        for_all_scalars!(check_quadratic_midpoint);
223    }
224
225    /// Regression check for a `weight is zero at evaluation point` failure
226    /// observed from `remesh` on a degree-1, weight-1-constant, [0,0,1,1]
227    /// curve — data that on paper cannot produce a near-zero interpolated
228    /// weight (both endpoint weights are exactly 1). Reproduces the exact
229    /// control points/knots/`t` from that failure's error context to check
230    /// whether `evaluate` itself is at fault, independent of `remesh`.
231    fn check_weight_one_constant_line_does_not_report_zero_weight<S: Scalar>() {
232        let curve = NurbCurve::try_new(
233            1,
234            vec![pt(0.5, 0.5, 0.5, 1.0), pt(0.5, 0.5, -0.5, 1.0)],
235            vec![
236                S::from_f64(0.0),
237                S::from_f64(0.0),
238                S::from_f64(1.0),
239                S::from_f64(1.0),
240            ],
241        )
242        .unwrap();
243        let p = curve.evaluate(S::from_f64(0.972)).unwrap();
244        assert!(
245            p[2].could_be_equal(S::from_f64(0.5 - 1.0 * 0.972)),
246            "p={p:?}"
247        );
248    }
249    #[test]
250    fn weight_one_constant_line_does_not_report_zero_weight() {
251        for_all_scalars!(check_weight_one_constant_line_does_not_report_zero_weight);
252    }
253
254    fn check_everything_matches_any_point<S: Scalar>() {
255        let c = NurbCurve::<S, 4>::everything();
256        let p = c.evaluate(S::from_f64(7.0)).unwrap();
257        assert!(p[0].could_be_equal(S::from_f64(123.456)));
258        assert!(p[1].could_be_equal(S::from_f64(-9.0)));
259        assert!(p[2].could_be_equal(S::ZERO));
260    }
261    #[test]
262    fn everything_matches_any_point() {
263        for_all_scalars!(check_everything_matches_any_point);
264    }
265}
geop_core_geometry/nurb_curve/evaluate.rs

geop_core_geometry/nurb_curve/
evaluate.rs
