use std::f64::consts::TAU;
use crate::{
Error, Result,
boolean::csi,
check,
error::invalid,
geom::{Curve, Cylinder, NurbsCurve, NurbsSurface, Plane, Surface},
loft::rail::{Hom, Rail, hom, point},
math::{Frame, Point3, Transform, Vec2, Vec3},
profile::{Names, Profile},
tol,
topo::{Brep, CoedgeSpec, EdgeId, FaceName, PCurve, VertexId},
};
const SCREW_FIT: f64 = tol::LINEAR * 1e-4;
const DEGREE: usize = 7;
#[derive(Clone, Copy, Debug)]
pub struct Helix {
pub origin: Point3,
pub axis: Vec3,
pub rise: f64,
pub sign: f64,
}
impl Helix {
fn d3(&self, p: Point3, t: f64) -> [Vec3; 4] {
let q = p - self.origin;
let along = self.axis * q.dot(self.axis);
let x = q - along;
let y = self.axis.cross(x);
let (s, c) = (self.sign * t).sin_cos();
let at = self.origin + along + x * c + y * s + self.axis * (self.rise * t);
let d1 = (y * c - x * s) * self.sign + self.axis * self.rise;
let d2 = -(x * c + y * s);
let d3 = (x * s - y * c) * self.sign;
[at, d1, d2, d3]
}
fn at(&self, p: Point3, t: f64) -> Point3 {
self.d3(p, t)[0]
}
pub fn transform(&self, t: f64) -> Transform {
Transform::rotation(self.origin, self.axis, self.sign * t)
.then(&Transform::translation(self.axis * (self.rise * t)))
}
}
fn knots(nodes: &[f64]) -> Vec<f64> {
let mut k = vec![nodes[0]; DEGREE + 1];
for &n in &nodes[1..nodes.len() - 1] {
k.extend(std::iter::repeat_n(n, DEGREE));
}
k.extend(std::iter::repeat_n(nodes[nodes.len() - 1], DEGREE + 1));
k
}
fn hermite(a: [Vec3; 4], b: [Vec3; 4], h: f64) -> [Point3; 7] {
let (k1, k2, k3) = (h / 7.0, h * h / 42.0, h * h * h / 210.0);
let p1 = a[0] + a[1] * k1;
let p2 = p1 * 2.0 - a[0] + a[2] * k2;
let p3 = (p2 - p1) * 3.0 + a[0] + a[3] * k3;
let q1 = b[0] - b[1] * k1;
let q2 = q1 * 2.0 - b[0] + b[2] * k2;
let q3 = (q2 - q1) * 3.0 + b[0] - b[3] * k3;
[p1, p2, p3, q3, q2, q1, b[0]]
}
fn column(screw: &Helix, p: Point3, nodes: &[f64]) -> Vec<Point3> {
let mut out = vec![screw.at(p, nodes[0])];
for w in nodes.windows(2) {
let (a, b) = (screw.d3(p, w[0]), screw.d3(p, w[1]));
out.extend(hermite(a, b, w[1] - w[0]));
}
out
}
fn nodes(angle: f64, radius: f64) -> Vec<f64> {
let h = 2.0 * (40320.0 * SCREW_FIT / radius.max(tol::LINEAR)).powf(1.0 / 8.0);
let n = (angle / h).ceil().max(1.0) as usize;
(0..=n).map(|k| angle * k as f64 / n as f64).collect()
}
fn coedge(edge: EdgeId, reversed: bool, pcurve: PCurve) -> CoedgeSpec {
CoedgeSpec {
edge,
reversed,
pcurve,
}
}
fn affine(origin: Vec2, dir: Vec2) -> PCurve {
PCurve::Affine { origin, dir }
}
fn plane_seed(frame: &Frame, p: Point3) -> PCurve {
let l = frame.to_local(p);
PCurve::Projected {
seed: Vec2::new(l.x, l.y),
}
}
fn split(pw: &[Hom]) -> (Vec<Point3>, Option<Vec<f64>>) {
let w: Vec<f64> = pw.iter().map(|h| h[3]).collect();
let rational = w.iter().any(|&x| (x - 1.0).abs() > 1e-15);
(
pw.iter().map(|&h| point(h)).collect(),
rational.then_some(w),
)
}
pub(crate) struct Seg {
pub rail: Rail,
pub name: Names,
}
fn self_contact(profile: &Profile, frame: &Frame, screw: &Helix, angle: f64) -> Result<()> {
let side = screw.axis.cross(frame.z);
let curves: Vec<(Curve, (f64, f64))> = profile
.loops
.iter()
.flat_map(|lp| &lp.segments)
.map(|s| {
let placed = s.segment.place(frame);
(placed.curve, placed.range)
})
.collect();
let (mut lo, mut hi) = (f64::INFINITY, f64::NEG_INFINITY);
let (mut z_lo, mut z_hi) = (f64::INFINITY, f64::NEG_INFINITY);
for (c, (t0, t1)) in &curves {
let mut ts = vec![*t0, *t1];
ts.extend(c.extremes(side, *t0, *t1));
ts.extend(c.extremes(screw.axis, *t0, *t1));
for t in ts {
let q = c.eval(t) - screw.origin;
let (r, z) = (q.dot(side), q.dot(screw.axis));
(lo, hi) = (lo.min(r), hi.max(r));
(z_lo, z_hi) = (z_lo.min(z), z_hi.max(z));
}
}
let near = lo.abs().min(hi.abs());
if lo <= tol::LINEAR && hi >= -tol::LINEAR {
return invalid("a screw sweep's profile reaches its axis");
}
let turns = ((angle + tol::LINEAR / near) / TAU).floor();
let rise = screw.rise * TAU;
let mut k = 1.0;
while k <= turns && (k * rise).abs() <= z_hi - z_lo + tol::LINEAR {
let shift = Transform::translation(screw.axis * (k * rise));
let meets = curves.iter().any(|(c, r)| {
let moved = c.transformed(&shift);
curves
.iter()
.any(|(d, rd)| !csi::curve_curve(&moved, *r, d, *rd).is_empty())
});
if meets || (k * rise).abs() <= tol::LINEAR {
return invalid("a screw sweep's coils meet: its pitch is too short for its profile");
}
k += 1.0;
}
Ok(())
}
pub fn helix(
profile: &Profile,
frame: &Frame,
screw: &Helix,
angle: f64,
feature: &str,
) -> Result<Brep> {
if !(angle.is_finite() && angle > tol::ANGULAR) {
return invalid("a screw sweep turns through a positive angle");
}
let Some(axis) = screw.axis.unit() else {
return invalid("a screw sweep needs an axis");
};
let screw = Helix { axis, ..*screw };
let square = frame.z.cross(axis).norm() <= tol::ANGULAR;
if !square
&& (frame.z.dot(axis).abs() > tol::ANGULAR
|| (frame.origin - screw.origin).dot(frame.z).abs() > tol::LINEAR)
{
return Err(Error::Unsupported(
"a screw sweep's profile off a plane through its axis or square to it".into(),
));
}
if square && screw.rise.abs() <= tol::LINEAR {
return invalid("a twisted extrusion must rise");
}
let mut profile = profile.normalized()?;
if !square {
self_contact(&profile, frame, &screw, angle)?;
}
let (mut radius, mut far) = (0.0f64, frame.origin);
for lp in &profile.loops {
for p in lp.polygon() {
let q = frame.at(p.x, p.y) - screw.origin;
let r = (q - axis * q.dot(axis)).norm();
if r > radius {
(radius, far) = (r, frame.at(p.x, p.y));
}
}
}
let motion = screw.d3(far, 0.0)[1];
if motion.dot(frame.z).abs() <= tol::ANGULAR * motion.norm() {
return invalid("a screw sweep's profile plane runs along its motion");
}
let sketch = *frame;
let mut frame = *frame;
if motion.dot(frame.z) < 0.0 {
for lp in &mut profile.loops {
lp.reverse();
}
frame = Frame::new(frame.origin, -frame.z, frame.x)?;
}
let loops: Vec<Vec<Seg>> = profile
.loops
.iter()
.map(|lp| {
lp.segments
.iter()
.map(|s| {
let placed = s.segment.place(&sketch);
let (t0, t1) = if placed.forward {
placed.range
} else {
(placed.range.1, placed.range.0)
};
Ok(Seg {
rail: Rail::of_curve(&placed.curve, t0, t1)?,
name: s.names.clone(),
})
})
.collect::<Result<Vec<_>>>()
})
.collect::<Result<_>>()?;
let far = loops
.iter()
.flatten()
.flat_map(|s| s.rail.pw.iter())
.map(|&h| {
let q = point(h) - screw.origin;
(q - axis * q.dot(axis)).norm()
})
.fold(radius, f64::max);
let nodes = nodes(angle, far);
let ku = knots(&nodes);
let end_frame = frame.transformed(&screw.transform(angle));
let motion = Motion {
degree: DEGREE,
knots: ku,
range: (0.0, angle),
column: Box::new(|p| column(&screw, p, &nodes)),
at_end: Box::new(|p| screw.at(p, angle)),
velocity: Box::new(|p| screw.d3(p, 0.0)[1]),
end_frame,
screw: Some(screw),
};
assemble(&loops, frame, &motion, feature)
}
pub(crate) struct Motion<'a> {
pub degree: usize,
pub knots: Vec<f64>,
pub range: (f64, f64),
pub column: Box<dyn Fn(Point3) -> Vec<Point3> + 'a>,
pub at_end: Box<dyn Fn(Point3) -> Point3 + 'a>,
pub velocity: Box<dyn Fn(Point3) -> Vec3 + 'a>,
pub end_frame: Frame,
pub screw: Option<Helix>,
}
pub(crate) fn assemble(
loops: &[Vec<Seg>],
frame: Frame,
m: &Motion,
feature: &str,
) -> Result<Brep> {
let helix = |p: Point3| -> Result<Curve> {
let c = NurbsCurve::new(m.degree, m.knots.clone(), (m.column)(p), None)?;
Ok(Curve::Nurbs(Box::new(c)))
};
let mut cuts = vec![m.range.0];
if m.screw.is_some() {
let mut t = m.range.0 + TAU;
while t < m.range.1 - tol::ANGULAR {
cuts.push(t);
t += TAU;
}
}
cuts.push(m.range.1);
let pieces = cuts.len() - 1;
let at = |p: Point3, k: usize| -> Point3 {
match (&m.screw, k) {
(_, 0) => p,
(_, k) if k == pieces => (m.at_end)(p),
(Some(sc), k) => sc.at(p, cuts[k]),
(None, _) => unreachable!("only a screw is cut between its ends"),
}
};
let line = |a: Point3, b: Point3| -> Result<Curve> {
Ok(Curve::Nurbs(Box::new(NurbsCurve::new(
1,
vec![0.0, 0.0, 1.0, 1.0],
vec![a, b],
None,
)?)))
};
let end_frame = m.end_frame;
let mut b = Brep::new();
let start = b.add_face(
Surface::Plane(Plane { frame }),
true,
[FaceName::new(feature, FaceName::START)],
);
let end = b.add_face(
Surface::Plane(Plane { frame: end_frame }),
false,
[FaceName::new(feature, FaceName::END)],
);
let mut faces = vec![start, end];
for segs in loops {
let k = segs.len();
let corners: Vec<Point3> = segs.iter().map(|s| s.rail.start()).collect();
let verts: Vec<Vec<VertexId>> = corners
.iter()
.map(|&p| (0..=pieces).map(|c| b.add_vertex(at(p, c))).collect())
.collect();
let rings: Vec<Vec<EdgeId>> = (0..k)
.map(|i| {
let curve = helix(corners[i])?;
Ok((0..pieces)
.map(|c| {
b.add_edge(
curve.clone(),
(cuts[c], cuts[c + 1]),
verts[i][c],
verts[i][c + 1],
)
})
.collect())
})
.collect::<Result<_>>()?;
let mut e0 = Vec::with_capacity(k);
let mut e1 = Vec::with_capacity(k);
for (i, s) in segs.iter().enumerate() {
let r = &s.rail;
let (pts, w) = split(&r.pw);
let at_end: Vec<Hom> =
r.pw.iter()
.map(|&h| hom((m.at_end)(point(h)), h[3]))
.collect();
let (end_pts, _) = split(&at_end);
let j = (i + 1) % k;
let c0 = NurbsCurve::new(r.degree, r.knots.clone(), pts.clone(), w.clone())?;
let c1 = NurbsCurve::new(r.degree, r.knots.clone(), end_pts, w.clone())?;
e0.push(b.add_edge(
Curve::Nurbs(Box::new(c0)),
(0.0, 1.0),
verts[i][0],
verts[j][0],
));
e1.push(b.add_edge(
Curve::Nurbs(Box::new(c1)),
(0.0, 1.0),
verts[i][pieces],
verts[j][pieces],
));
let name = FaceName::all(feature, &s.name);
let widest = cuts.windows(2).map(|w| w[1] - w[0]).fold(0.0, f64::max);
let strip = m.screw.as_ref().and_then(|sc| {
along_axis(sc, &pts, &w)
.map(|(h0, dh, radius)| (sc, h0, dh, radius, 0.0))
.or_else(|| {
about_axis(sc, r)
.filter(|&(_, _, sweep)| sweep.abs() + widest <= TAU - 1e-9)
.map(|(h0, radius, sweep)| (sc, h0, 0.0, radius, sweep * sc.sign))
})
});
if let Some((sc, h0, dh, radius, round)) = strip {
let z = sc.axis * sc.sign;
let (sign, rise) = (sc.sign, sc.rise);
let seps: Vec<EdgeId> = (0..=pieces)
.map(|c| match c {
0 => Ok(e0[i]),
c if c == pieces => Ok(e1[i]),
c if round == 0.0 => Ok(b.add_edge(
line(at(corners[i], c), at(corners[j], c))?,
(0.0, 1.0),
verts[i][c],
verts[j][c],
)),
c => {
let turned: Vec<Hom> =
r.pw.iter()
.map(|&h| hom(sc.at(point(h), cuts[c]), h[3]))
.collect();
let (ctrl, wts) = split(&turned);
let arc = NurbsCurve::new(r.degree, r.knots.clone(), ctrl, wts)?;
Ok(b.add_edge(
Curve::Nurbs(Box::new(arc)),
(0.0, 1.0),
verts[i][c],
verts[j][c],
))
}
})
.collect::<Result<_>>()?;
for c in 0..pieces {
let p = at(corners[i], c) - sc.origin;
let x = (p - sc.axis * p.dot(sc.axis)).unit().ok_or_else(|| {
Error::InvalidInput("a screw's crest lies on its axis".into())
})?;
let surface = Surface::Cylinder(Cylinder {
frame: Frame::new(sc.origin, z, x)?,
radius,
});
let (t0, t1) = (cuts[c], cuts[c + 1]);
let v = |t: f64, s: f64| sign * (h0 + rise * t + s * dh);
let across = |t: f64| {
if round == 0.0 {
affine(Vec2::new(t - t0, v(t, 0.0)), Vec2::new(0.0, sign * dh))
} else {
PCurve::Projected {
seed: Vec2::new(t - t0, v(t, 0.0)),
}
}
};
let (_, su, sv) = surface.d1(0.5 * round, v(t0, 0.5));
let (pm, d) = r_mid(r);
let outward = d.cross((m.velocity)(pm));
let face = b.add_face(surface, su.cross(sv).dot(outward) < 0.0, name.clone());
b.add_loop(
face,
vec![
coedge(seps[c], false, across(t0)),
coedge(
rings[j][c],
false,
affine(
Vec2::new(round - t0, v(0.0, 1.0)),
Vec2::new(1.0, sign * rise),
),
),
coedge(seps[c + 1], true, across(t1)),
coedge(
rings[i][c],
true,
affine(Vec2::new(-t0, v(0.0, 0.0)), Vec2::new(1.0, sign * rise)),
),
],
);
faces.push(face);
}
continue;
}
let nv = pts.len();
let mut net = Vec::with_capacity(nv * (m.knots.len() - m.degree - 1));
let cols: Vec<Vec<Point3>> = pts.iter().map(|&p| (m.column)(p)).collect();
for row in 0..cols[0].len() {
net.extend(cols.iter().map(|c| c[row]));
}
let nu = cols[0].len();
let weights = w.map(|w| (0..nu).flat_map(|_| w.iter().copied()).collect());
let surface = Surface::Nurbs(Box::new(NurbsSurface::new(
(m.degree, r.degree),
(m.knots.clone(), r.knots.clone()),
(nu, nv),
net,
weights,
)?));
let (_, su, sv) = surface.d1(m.range.0, 0.5);
let (p, d) = r_mid(r);
let outward = d.cross((m.velocity)(p));
let face = b.add_face(surface, su.cross(sv).dot(outward) < 0.0, name);
let mut lp = vec![coedge(
e0[i],
false,
affine(Vec2::new(m.range.0, 0.0), Vec2::new(0.0, 1.0)),
)];
lp.extend(
rings[j]
.iter()
.map(|&e| coedge(e, false, affine(Vec2::new(0.0, 1.0), Vec2::new(1.0, 0.0)))),
);
lp.push(coedge(
e1[i],
true,
affine(Vec2::new(m.range.1, 0.0), Vec2::new(0.0, 1.0)),
));
lp.extend(
rings[i]
.iter()
.rev()
.map(|&e| coedge(e, true, affine(Vec2::ZERO, Vec2::new(1.0, 0.0)))),
);
b.add_loop(face, lp);
faces.push(face);
}
let lo: Vec<CoedgeSpec> = (0..k)
.rev()
.map(|i| coedge(e0[i], true, plane_seed(&frame, corners[(i + 1) % k])))
.collect();
b.add_loop(start, lo);
let hi: Vec<CoedgeSpec> = (0..k)
.map(|i| coedge(e1[i], false, plane_seed(&end_frame, (m.at_end)(corners[i]))))
.collect();
b.add_loop(end, hi);
}
let shell = b.add_shell(faces);
b.add_solid(vec![shell]);
let issues = check::check(&b);
if issues.is_empty() {
Ok(b)
} else {
Err(Error::InvalidResult(
issues.into_iter().map(|i| i.to_string()).collect(),
))
}
}
fn along_axis(sc: &Helix, pts: &[Point3], w: &Option<Vec<f64>>) -> Option<(f64, f64, f64)> {
if pts.len() != 2
|| w.as_ref()
.is_some_and(|w| w.iter().any(|&x| (x - w[0]).abs() > 1e-12))
{
return None;
}
let (a, b) = (pts[0] - sc.origin, pts[1] - sc.origin);
let d = b - a;
if d.cross(sc.axis).norm() > tol::LINEAR * 1e-3 {
return None;
}
let radius = (a - sc.axis * a.dot(sc.axis)).norm();
(radius > tol::LINEAR).then(|| (a.dot(sc.axis), d.dot(sc.axis), radius))
}
fn about_axis(sc: &Helix, r: &Rail) -> Option<(f64, f64, f64)> {
const N: usize = 16;
let (ctrl, w) = split(&r.pw);
let c = NurbsCurve::new(r.degree, r.knots.clone(), ctrl, w).ok()?;
let at = |k: usize| {
let q = c.eval(k as f64 / N as f64) - sc.origin;
let h = q.dot(sc.axis);
(q - sc.axis * h, h)
};
let (first, h0) = at(0);
let radius = first.norm();
if radius <= tol::LINEAR {
return None;
}
let (mut last, mut sweep) = (first, 0.0);
for k in 1..=N {
let (q, h) = at(k);
let fit = tol::LINEAR * 1e-3;
if (h - h0).abs() > fit || (q.norm() - radius).abs() > fit {
return None;
}
sweep += last.cross(q).dot(sc.axis).atan2(last.dot(q));
last = q;
}
(sweep.abs() > tol::ANGULAR).then_some((h0, radius, sweep))
}
fn r_mid(r: &Rail) -> (Point3, Vec3) {
let c = NurbsCurve::new(r.degree, r.knots.clone(), split(&r.pw).0, split(&r.pw).1)
.expect("a rail is a valid curve");
let (p, d, _) = c.d2(0.5);
(p, d)
}
#[cfg(test)]
mod tests;