1 commits, squashed; the originals stay reachable from tag archive/merge-main-develop.
- f06581ca5 Brennen Herbruck: STEP geometry as plain data: read and write ISO 10303-42 curves and surfaces without truck
Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01DtL5M41G8Nua7HvuFPZkmJ
194 lines
8.4 KiB
Rust
194 lines
8.4 KiB
Rust
//! Points on STEP curves and surfaces, in STEP's own parameterisation (ISO 10303-42), so tests
|
||
//! and the topology reader can check that vertices lie on their curves. `None` for a parameter
|
||
//! the geometry is not defined at, or for geometry too inconsistent to evaluate.
|
||
|
||
use crate::geom::{BSplineCurve, BSplineSurface, Curve, P3, Surface, add, cross, dot, norm, scale, sub};
|
||
|
||
/// Every knot repeated by its multiplicity.
|
||
fn expanded(knots: &[f64], mults: &[usize]) -> Option<Vec<f64>> {
|
||
if knots.len() != mults.len() {
|
||
return None;
|
||
}
|
||
let total = mults.iter().try_fold(0usize, |a, m| a.checked_add(*m))?;
|
||
let mut v = Vec::with_capacity(total);
|
||
for (k, m) in knots.iter().zip(mults) {
|
||
v.extend(std::iter::repeat_n(*k, *m));
|
||
}
|
||
Some(v)
|
||
}
|
||
|
||
/// Index `i` of the knot span `[U[i], U[i+1])` holding `t`, with `degree <= i < n` for `n`
|
||
/// control points (the last span takes the domain's upper end).
|
||
fn span(u: &[f64], degree: usize, n: usize, t: f64) -> Option<usize> {
|
||
let lo = *u.get(degree)?;
|
||
let hi = *u.get(n)?;
|
||
if !(t >= lo - 1e-12 * (1.0 + lo.abs()) && t <= hi + 1e-12 * (1.0 + hi.abs())) {
|
||
return None;
|
||
}
|
||
let mut i = degree;
|
||
while i + 1 < n && *u.get(i + 1)? <= t {
|
||
i += 1;
|
||
}
|
||
Some(i)
|
||
}
|
||
|
||
/// De Boor on homogeneous points `[x·w, y·w, z·w, w]`.
|
||
fn de_boor(u: &[f64], degree: usize, ctrl: &[[f64; 4]], t: f64) -> Option<[f64; 4]> {
|
||
let k = span(u, degree, ctrl.len(), t)?;
|
||
let mut d: Vec<[f64; 4]> = (0..=degree).map(|j| ctrl.get(j + k - degree).copied()).collect::<Option<_>>()?;
|
||
for r in 1..=degree {
|
||
for j in (r..=degree).rev() {
|
||
let i = j + k - degree;
|
||
let a = *u.get(i)?;
|
||
let b = *u.get(i + 1 + degree - r)?;
|
||
let alpha = if b - a > 0.0 { (t - a) / (b - a) } else { 0.0 };
|
||
let p = *d.get(j - 1)?;
|
||
let q = *d.get(j)?;
|
||
*d.get_mut(j)? = [0, 1, 2, 3].map(|c| (1.0 - alpha) * p[c] + alpha * q[c]);
|
||
}
|
||
}
|
||
d.get(degree).copied()
|
||
}
|
||
|
||
fn from_homogeneous(h: [f64; 4]) -> Option<P3> {
|
||
(h[3].abs() > 1e-300).then(|| [h[0] / h[3], h[1] / h[3], h[2] / h[3]]).filter(|p| p.iter().all(|x| x.is_finite()))
|
||
}
|
||
|
||
fn homogeneous(p: P3, w: f64) -> [f64; 4] {
|
||
[p[0] * w, p[1] * w, p[2] * w, w]
|
||
}
|
||
|
||
impl BSplineCurve {
|
||
/// Parameter range: from knot `degree` to knot `n` of the expanded knot vector.
|
||
pub fn domain(&self) -> Option<(f64, f64)> {
|
||
let u = expanded(&self.knots, &self.multiplicities)?;
|
||
Some((*u.get(self.degree)?, *u.get(self.points.len())?))
|
||
}
|
||
pub fn point(&self, t: f64) -> Option<P3> {
|
||
let u = expanded(&self.knots, &self.multiplicities)?;
|
||
if u.len() != self.points.len() + self.degree + 1 {
|
||
return None;
|
||
}
|
||
let ctrl: Vec<[f64; 4]> = match &self.weights {
|
||
Some(w) if w.len() == self.points.len() => self.points.iter().zip(w).map(|(p, w)| homogeneous(*p, *w)).collect(),
|
||
Some(_) => return None,
|
||
None => self.points.iter().map(|p| homogeneous(*p, 1.0)).collect(),
|
||
};
|
||
from_homogeneous(de_boor(&u, self.degree, &ctrl, t)?)
|
||
}
|
||
}
|
||
|
||
impl BSplineSurface {
|
||
pub fn point(&self, s: f64, t: f64) -> Option<P3> {
|
||
let uk = expanded(&self.u_knots, &self.u_multiplicities)?;
|
||
let vk = expanded(&self.v_knots, &self.v_multiplicities)?;
|
||
let nu = self.points.len();
|
||
let nv = self.points.first()?.len();
|
||
if uk.len() != nu + self.u_degree + 1 || vk.len() != nv + self.v_degree + 1 {
|
||
return None;
|
||
}
|
||
// Each u row evaluated along v, then the column of results along u.
|
||
let mut column = Vec::with_capacity(nu);
|
||
for (i, row) in self.points.iter().enumerate() {
|
||
if row.len() != nv {
|
||
return None;
|
||
}
|
||
let ctrl: Vec<[f64; 4]> = match &self.weights {
|
||
Some(w) => {
|
||
let w = w.get(i).filter(|w| w.len() == nv)?;
|
||
row.iter().zip(w).map(|(p, w)| homogeneous(*p, *w)).collect()
|
||
}
|
||
None => row.iter().map(|p| homogeneous(*p, 1.0)).collect(),
|
||
};
|
||
column.push(de_boor(&vk, self.v_degree, &ctrl, t)?);
|
||
}
|
||
from_homogeneous(de_boor(&uk, self.u_degree, &column, s)?)
|
||
}
|
||
}
|
||
|
||
impl Curve {
|
||
/// The point at parameter `t`.
|
||
pub fn point(&self, t: f64) -> Option<P3> {
|
||
self.point_at(t, 0)
|
||
}
|
||
|
||
fn point_at(&self, t: f64, depth: usize) -> Option<P3> {
|
||
if depth > 16 || !t.is_finite() {
|
||
return None;
|
||
}
|
||
let p = match self {
|
||
Curve::Line { origin, direction, magnitude } => add(*origin, scale(*direction, t * magnitude)),
|
||
Curve::Circle { position, radius } => position.at(radius * t.cos(), radius * t.sin(), 0.0),
|
||
Curve::Ellipse { position, semi_axis_1, semi_axis_2 } => position.at(semi_axis_1 * t.cos(), semi_axis_2 * t.sin(), 0.0),
|
||
Curve::Hyperbola { position, semi_axis, semi_imag_axis } => position.at(semi_axis * t.cosh(), semi_imag_axis * t.sinh(), 0.0),
|
||
Curve::Parabola { position, focal_dist } => position.at(focal_dist * t * t, 2.0 * focal_dist * t, 0.0),
|
||
Curve::BSpline(b) => b.point(t)?,
|
||
Curve::Polyline(pts) => {
|
||
if pts.len() < 2 || t < 0.0 || t > (pts.len() - 1) as f64 {
|
||
return None;
|
||
}
|
||
let i = (t.floor() as usize).min(pts.len() - 2);
|
||
let (a, b) = (*pts.get(i)?, *pts.get(i + 1)?);
|
||
add(a, scale(sub(b, a), t - i as f64))
|
||
}
|
||
Curve::Trimmed { basis, .. } => basis.point_at(t, depth + 1)?,
|
||
Curve::SurfaceCurve { curve_3d, .. } => curve_3d.point_at(t, depth + 1)?,
|
||
// Needs the basis curve's derivative; not evaluated yet.
|
||
Curve::OffsetCurve3d { .. } => return None,
|
||
};
|
||
p.iter().all(|x| x.is_finite()).then_some(p)
|
||
}
|
||
}
|
||
|
||
impl Surface {
|
||
/// The point at parameters `(u, v)`.
|
||
pub fn point(&self, u: f64, v: f64) -> Option<P3> {
|
||
self.point_at(u, v, 0)
|
||
}
|
||
|
||
/// The unit normal `∂S/∂u × ∂S/∂v` at `(u, v)`, by central differences.
|
||
pub fn normal(&self, u: f64, v: f64) -> Option<P3> {
|
||
self.normal_at(u, v, 0)
|
||
}
|
||
|
||
fn normal_at(&self, u: f64, v: f64, depth: usize) -> Option<P3> {
|
||
let h = 1e-6;
|
||
let du = sub(self.point_at(u + h, v, depth)?, self.point_at(u - h, v, depth)?);
|
||
let dv = sub(self.point_at(u, v + h, depth)?, self.point_at(u, v - h, depth)?);
|
||
let n = cross(du, dv);
|
||
let m = norm(n);
|
||
(m > 1e-30 && m.is_finite()).then(|| scale(n, 1.0 / m))
|
||
}
|
||
|
||
fn point_at(&self, u: f64, v: f64, depth: usize) -> Option<P3> {
|
||
if depth > 16 || !u.is_finite() || !v.is_finite() {
|
||
return None;
|
||
}
|
||
let p = match self {
|
||
Surface::Plane { position } => position.at(u, v, 0.0),
|
||
Surface::Cylinder { position, radius } => position.at(radius * u.cos(), radius * u.sin(), v),
|
||
Surface::Cone { position, radius, semi_angle } => {
|
||
let r = radius + v * semi_angle.tan();
|
||
position.at(r * u.cos(), r * u.sin(), v)
|
||
}
|
||
Surface::Sphere { position, radius } => position.at(radius * v.cos() * u.cos(), radius * v.cos() * u.sin(), radius * v.sin()),
|
||
Surface::Torus { position, major_radius, minor_radius } | Surface::DegenerateTorus { position, major_radius, minor_radius, .. } => {
|
||
let r = major_radius + minor_radius * v.cos();
|
||
position.at(r * u.cos(), r * u.sin(), minor_radius * v.sin())
|
||
}
|
||
Surface::BSpline(b) => b.point(u, v)?,
|
||
Surface::Revolution { profile, axis } => {
|
||
// Rodrigues: the profile point turned by u about the axis.
|
||
let q = sub(profile.point_at(v, depth + 1)?, axis.origin);
|
||
let k = axis.direction;
|
||
let (s, c) = u.sin_cos();
|
||
add(axis.origin, add(add(scale(q, c), scale(cross(k, q), s)), scale(k, dot(k, q) * (1.0 - c))))
|
||
}
|
||
Surface::Extrusion { profile, direction, magnitude } => add(profile.point_at(u, depth + 1)?, scale(*direction, v * magnitude)),
|
||
Surface::Offset { basis, distance, .. } => add(basis.point_at(u, v, depth + 1)?, scale(basis.normal_at(u, v, depth + 1)?, *distance)),
|
||
Surface::RectangularTrimmed { basis, .. } => basis.point_at(u, v, depth + 1)?,
|
||
};
|
||
p.iter().all(|x| x.is_finite()).then_some(p)
|
||
}
|
||
}
|