axiolid_curve/
pair_section.rs

1//! Where two parametric surfaces meet, carried by nodes on both (ADR 0077).
2//!
3//! Two B-spline surfaces have no implicit equation to read one in the
4//! other's parameters, so their section is carried directly: a chain of
5//! [`PairNode`]s, each a point on both surfaces with its parameters on each,
6//! found to the last bits. Between two nodes the curve is defined, not
7//! interpolated: at local parameter `s` it is the point where both surfaces
8//! meet on the plane across the chord at `P0 + s (P1 - P0)`, the unique
9//! solution of four equations in the four parameters near the chord --
10//! `S1(a) = S2(b)` and the plane -- found by Newton from the nodes'
11//! parameters. Derivatives follow from the same system.
12
13use axiolid_core::{Point2, Point3, Scalar, Vec3};
14
15use crate::implicit::{Carrier, SurfaceJet};
16
17/// A point on both surfaces, with its parameters on each.
18#[derive(Debug, Clone, Copy, PartialEq)]
19pub struct PairNode {
20    /// The point.
21    pub point: Point3,
22    /// Its parameters on the first surface.
23    pub first: Point2,
24    /// Its parameters on the second surface.
25    pub second: Point2,
26}
27
28/// A stretch of the curve where two surfaces meet. The parameter runs over
29/// `[0, nodes.len() - 1]`, one unit per chord.
30#[derive(Debug, Clone, PartialEq)]
31pub struct PairSection3 {
32    /// The first surface.
33    pub first: Carrier,
34    /// The second surface.
35    pub second: Carrier,
36    /// Nodes on both, in order along the curve.
37    pub nodes: Vec<PairNode>,
38}
39
40/// Solve the 4x4 system `m x = r` by Gaussian elimination with partial
41/// pivoting; `None` when it is singular.
42#[must_use]
43#[allow(clippy::needless_range_loop)]
44pub fn solve4(mut m: [[Scalar; 4]; 4], mut r: [Scalar; 4]) -> Option<[Scalar; 4]> {
45    for col in 0..4 {
46        let pivot = (col..4).max_by(|&a, &b| m[a][col].abs().total_cmp(&m[b][col].abs()))?;
47        if m[pivot][col] == 0.0 || !m[pivot][col].is_finite() {
48            return None;
49        }
50        m.swap(col, pivot);
51        r.swap(col, pivot);
52        for row in col + 1..4 {
53            let f = m[row][col] / m[col][col];
54            for k in col..4 {
55                m[row][k] -= f * m[col][k];
56            }
57            r[row] -= f * r[col];
58        }
59    }
60    let mut x = [0.0; 4];
61    for row in (0..4).rev() {
62        let mut acc = r[row];
63        for k in row + 1..4 {
64            acc -= m[row][k] * x[k];
65        }
66        x[row] = acc / m[row][row];
67    }
68    x.iter().all(|v| v.is_finite()).then_some(x)
69}
70
71/// The system's matrix at `(a, b)` for chord direction `d`.
72fn matrix(j1: &SurfaceJet, j2: &SurfaceJet, d: Vec3) -> [[Scalar; 4]; 4] {
73    [
74        [j1.u.x, j1.v.x, -j2.u.x, -j2.v.x],
75        [j1.u.y, j1.v.y, -j2.u.y, -j2.v.y],
76        [j1.u.z, j1.v.z, -j2.u.z, -j2.v.z],
77        [d.dot(j1.u), d.dot(j1.v), 0.0, 0.0],
78    ]
79}
80
81impl PairSection3 {
82    /// The parameter range's end, `nodes.len() - 1`.
83    #[must_use]
84    pub fn end(&self) -> Scalar {
85        self.nodes.len().saturating_sub(1) as Scalar
86    }
87
88    fn locate(&self, t: Scalar) -> Option<(usize, Scalar)> {
89        let n = self.nodes.len();
90        if n < 2 || !t.is_finite() {
91            return None;
92        }
93        let slack = 1e-12 * (1.0 + self.end());
94        if t < -slack || t > self.end() + slack {
95            return None;
96        }
97        let i = (t.floor().max(0.0) as usize).min(n - 2);
98        Some((i, (t - i as Scalar).clamp(0.0, 1.0)))
99    }
100
101    /// The parameters on both surfaces at `t`, and the point.
102    #[must_use]
103    pub fn solve(&self, t: Scalar) -> Option<(Point2, Point2, Point3)> {
104        let (i, s) = self.locate(t)?;
105        let (n0, n1) = (self.nodes[i], self.nodes[i + 1]);
106        if s == 0.0 {
107            return Some((n0.first, n0.second, n0.point));
108        }
109        if s == 1.0 {
110            return Some((n1.first, n1.second, n1.point));
111        }
112        let d = n1.point - n0.point;
113        let target = n0.point + d * s;
114        let mut a = n0.first + (n1.first - n0.first) * s;
115        let mut b = n0.second + (n1.second - n0.second) * s;
116        let scale = 1.0 + n0.point.length() + d.length();
117        for _ in 0..50 {
118            let j1 = self.first.jet(a.x, a.y);
119            let j2 = self.second.jet(b.x, b.y);
120            let gap = j1.point - j2.point;
121            let plane = d.dot(j1.point - target);
122            let x = solve4(matrix(&j1, &j2, d), [-gap.x, -gap.y, -gap.z, -plane])?;
123            a += Point2::new(x[0], x[1]);
124            b += Point2::new(x[2], x[3]);
125            let step = x.iter().map(|v| v.abs()).fold(0.0, Scalar::max);
126            if step <= 4.0 * Scalar::EPSILON * (1.0 + a.length() + b.length()) {
127                break;
128            }
129        }
130        let (j1, j2) = (self.first.jet(a.x, a.y), self.second.jet(b.x, b.y));
131        ((j1.point - j2.point).length() <= 1e-9 * scale).then_some((a, b, j1.point))
132    }
133
134    /// The point at `t`.
135    #[must_use]
136    pub fn point(&self, t: Scalar) -> Option<Point3> {
137        self.solve(t).map(|(_, _, p)| p)
138    }
139
140    /// The rates of both surfaces' parameters and of the point at `t`:
141    /// from `S1_a a' - S2_b b' = 0` and `d . S1_a a' = |d|^2`.
142    #[must_use]
143    pub fn rates(&self, t: Scalar) -> Option<(Point2, Point2, Vec3)> {
144        let (i, _) = self.locate(t)?;
145        let d = self.nodes[i + 1].point - self.nodes[i].point;
146        let (a, b, _) = self.solve(t)?;
147        let (j1, j2) = (self.first.jet(a.x, a.y), self.second.jet(b.x, b.y));
148        let x = solve4(matrix(&j1, &j2, d), [0.0, 0.0, 0.0, d.dot(d)])?;
149        let (da, db) = (Point2::new(x[0], x[1]), Point2::new(x[2], x[3]));
150        Some((da, db, j1.u * da.x + j1.v * da.y))
151    }
152
153    /// `dP/dt`.
154    #[must_use]
155    pub fn tangent(&self, t: Scalar) -> Option<Vec3> {
156        self.rates(t).map(|(_, _, d)| d)
157    }
158
159    /// The second rates of both surfaces' parameters and of the point at
160    /// `t`, by central differences of [`Self::rates`] inside the chord.
161    #[must_use]
162    pub fn second_rates(&self, t: Scalar) -> Option<(Point2, Point2, Vec3)> {
163        let (i, s) = self.locate(t)?;
164        let h = 1e-5;
165        let (lo, hi) = ((s - h).max(0.0), (s + h).min(1.0));
166        let base = i as Scalar;
167        let (p, q) = (self.rates(base + lo)?, self.rates(base + hi)?);
168        let w = hi - lo;
169        Some(((q.0 - p.0) / w, (q.1 - p.1) / w, (q.2 - p.2) / w))
170    }
171
172    /// `d2P/dt2`.
173    #[must_use]
174    pub fn bend(&self, t: Scalar) -> Option<Vec3> {
175        self.second_rates(t).map(|(_, _, d)| d)
176    }
177
178    /// Which of the two surfaces `carrier` is: `Some(true)` for the first,
179    /// `Some(false)` for the second.
180    #[must_use]
181    pub fn side(&self, carrier: &Carrier) -> Option<bool> {
182        if *carrier == self.first {
183            Some(true)
184        } else if *carrier == self.second {
185            Some(false)
186        } else {
187            None
188        }
189    }
190
191    /// The stretch from `t0` to `t1` (`t0 < t1`) as a curve of its own over
192    /// `[0, nodes]`: the nodes between, and new end nodes solved at `t0` and
193    /// `t1`.
194    #[must_use]
195    pub fn sub(&self, t0: Scalar, t1: Scalar) -> Option<Self> {
196        let (a0, b0, p0) = self.solve(t0)?;
197        let (a1, b1, p1) = self.solve(t1)?;
198        let mut nodes = vec![PairNode {
199            point: p0,
200            first: a0,
201            second: b0,
202        }];
203        let first = t0.floor() as usize + 1;
204        let last = t1.ceil() as usize;
205        for i in first..last.min(self.nodes.len()) {
206            let t = i as Scalar;
207            if t > t0 + 1e-9 && t < t1 - 1e-9 {
208                nodes.push(self.nodes[i]);
209            }
210        }
211        nodes.push(PairNode {
212            point: p1,
213            first: a1,
214            second: b1,
215        });
216        Some(Self {
217            first: self.first.clone(),
218            second: self.second.clone(),
219            nodes,
220        })
221    }
222
223    /// The same curve run backwards.
224    #[must_use]
225    pub fn reversed(&self) -> Self {
226        let mut nodes = self.nodes.clone();
227        nodes.reverse();
228        Self {
229            first: self.first.clone(),
230            second: self.second.clone(),
231            nodes,
232        }
233    }
234
235    /// The parameter of a point on the curve: the chord nearest it, the
236    /// plane across that chord through the point, then Newton along the
237    /// curve's tangent within the chord.
238    #[must_use]
239    pub fn parameter_of(&self, p: Point3) -> Option<Scalar> {
240        let mut best: Option<(Scalar, usize, Scalar)> = None;
241        for (i, w) in self.nodes.windows(2).enumerate() {
242            let d = w[1].point - w[0].point;
243            let l2 = d.length_squared();
244            if l2 == 0.0 {
245                continue;
246            }
247            let s = ((p - w[0].point).dot(d) / l2).clamp(0.0, 1.0);
248            let miss = (w[0].point + d * s - p).length();
249            if best.is_none_or(|(m, _, _)| miss < m) {
250                best = Some((miss, i, s));
251            }
252        }
253        let (_, i, mut s) = best?;
254        let base = i as Scalar;
255        for _ in 0..30 {
256            let t = base + s;
257            let (q, d) = (self.point(t)?, self.tangent(t)?);
258            let l2 = d.length_squared();
259            if l2 == 0.0 {
260                break;
261            }
262            let next = (s - (q - p).dot(d) / l2).clamp(0.0, 1.0);
263            let step = (next - s).abs();
264            s = next;
265            if step <= 4.0 * Scalar::EPSILON * (1.0 + base) {
266                break;
267            }
268        }
269        Some(base + s)
270    }
271
272    /// Whether every number is finite.
273    #[must_use]
274    pub fn is_finite(&self) -> bool {
275        self.first.is_finite()
276            && self.second.is_finite()
277            && self
278                .nodes
279                .iter()
280                .all(|n| n.point.is_finite() && n.first.is_finite() && n.second.is_finite())
281    }
282}
axiolid_curve/pair_section.rs

axiolid_curve/
pair_section.rs
