use std::collections::BTreeSet;
use super::{Clip, Picked, outward, run};
use crate::{
Error, Result, check,
geom::{Curve, Line, NurbsCurve, NurbsSurface, Surface},
math::{Point3, Vec2, Vec3},
tol,
topo::{Brep, CoedgeSpec, EdgeId, FaceId, FaceName, PCurve, VertexId},
};
pub(super) const MARK: &str = "#corner";
pub(super) struct Swallowed {
pub arc: Option<usize>,
pub ends: [(EdgeId, VertexId, Clip); 2],
pub plate: FaceId,
pub tool: Brep,
}
fn unsupported<T>(why: &str) -> Result<T> {
Err(Error::Unsupported(why.into()))
}
fn near(s: &Surface, uv: Vec2, to: Option<Vec2>) -> Vec2 {
let Some(to) = to else { return uv };
let shift = |x: f64, t: f64, per: Option<f64>| per.map_or(x, |p| x + ((t - x) / p).round() * p);
Vec2::new(
shift(uv.x, to.x, s.u_period()),
shift(uv.y, to.y, s.v_period()),
)
}
fn arc_middle(centre: Point3, a: Point3, b: Point3) -> Option<(Point3, f64)> {
let (u, v) = (a - centre, b - centre);
let half = 0.5 * u.cross(v).norm().atan2(u.dot(v));
let w = half.cos();
let mid = (u + v).unit()?;
(w > 1e-6).then(|| (centre + mid * (u.norm() / w), w))
}
#[derive(Clone, Copy)]
enum Wall {
Line { p: Vec2, n: Vec2 },
Circle { centre: Vec2, radius: f64, out: f64 },
}
impl Wall {
fn offset(self, d: f64) -> Wall {
match self {
Wall::Line { p, n } => Wall::Line { p: p - n * d, n },
Wall::Circle {
centre,
radius,
out,
} => Wall::Circle {
centre,
radius: radius - out * d,
out,
},
}
}
}
fn crossing(a: Wall, b: Wall, near: Vec2) -> Option<Vec2> {
let line_circle = |p: Vec2, n: Vec2, centre: Vec2, radius: f64| -> Vec<Vec2> {
let along = Vec2::new(-n.y, n.x);
let foot = p + along * (centre - p).dot(along);
let h2 = radius * radius - (centre - foot).dot(centre - foot);
if h2 < 0.0 {
return Vec::new();
}
let h = h2.sqrt();
vec![foot + along * h, foot - along * h]
};
let found: Vec<Vec2> = match (a, b) {
(Wall::Line { p: p1, n: n1 }, Wall::Line { p: p2, n: n2 }) => {
let det = n1.x * n2.y - n1.y * n2.x;
if det.abs() <= 1e-12 {
return None;
}
let (c1, c2) = (n1.dot(p1), n2.dot(p2));
vec![Vec2::new(
(c1 * n2.y - c2 * n1.y) / det,
(n1.x * c2 - n2.x * c1) / det,
)]
}
(Wall::Line { p, n }, Wall::Circle { centre, radius, .. })
| (Wall::Circle { centre, radius, .. }, Wall::Line { p, n }) => {
line_circle(p, n, centre, radius)
}
(
Wall::Circle {
centre: c1,
radius: r1,
..
},
Wall::Circle {
centre: c2,
radius: r2,
..
},
) => {
let d = (c2 - c1).norm();
if d <= 1e-12 {
return None;
}
let x = (d * d + r1 * r1 - r2 * r2) / (2.0 * d);
let h2 = r1 * r1 - x * x;
if h2 < 0.0 {
return None;
}
let e = (c2 - c1) / d;
let f = Vec2::new(-e.y, e.x);
let m = c1 + e * x;
vec![m + f * h2.sqrt(), m - f * h2.sqrt()]
}
};
found
.into_iter()
.min_by(|x, y| (*x - near).norm().total_cmp(&(*y - near).norm()))
}
struct BaseArc {
centre: Point3,
from: Point3,
to: Point3,
face: FaceId,
}
pub(super) fn sharp(body: &Brep, picked: &[Picked], radius: f64) -> Result<Vec<Swallowed>> {
let r = radius;
let mut out = Vec::new();
let flat_normal = |f: FaceId, p: Point3| match &body.face(f).surface {
Surface::Plane(_) => Some(outward(body, f, p)),
_ => None,
};
for v in body.vertex_ids() {
let at: Vec<EdgeId> = body
.edge_ids()
.filter(|&e| {
let x = body.edge(e);
!x.is_degenerate() && (x.start == v || x.end == v)
})
.collect();
if at.len() != 3 {
continue;
}
let here: Vec<usize> = (0..picked.len())
.filter(|&i| at.contains(&picked[i].edge))
.collect();
let [i, j] = here[..] else { continue };
let Some(&c) = at
.iter()
.find(|&&e| e != picked[i].edge && e != picked[j].edge)
else {
continue;
};
let Some(&plate) = picked[i].faces.iter().find(|f| picked[j].faces.contains(f)) else {
continue;
};
let wall = |k: usize| picked[k].faces.iter().copied().find(|&f| f != plate);
let (Some(w1), Some(w2)) = (wall(i), wall(j)) else {
continue;
};
let p = body.vertex(v).point;
let (Some(n), Some(o1), Some(o2)) = (
flat_normal(plate, p),
flat_normal(w1, p),
flat_normal(w2, p),
) else {
continue;
};
let Some(Curve::Line(line)) = &body.edge(c).curve else {
continue;
};
if line.dir.cross(n).norm() > tol::ANGULAR {
continue;
}
let Some(cc) = body.coedge_ids().find(|&k| body.coedge(k).edge == c) else {
continue;
};
if run::convex(body, cc) != Some(false)
|| run::convex(body, picked[i].coedges[0]) != Some(true)
|| run::convex(body, picked[j].coedges[0]) != Some(true)
{
continue;
}
let (m1, m2) = (-o1, -o2);
let clip = |k: usize| {
let x = body.edge(picked[k].edge);
let far = body
.vertex(if x.start == v { x.end } else { x.start })
.point;
let along = n.cross(if k == i { m1 } else { m2 });
let keep = if (far - p).dot(along) >= 0.0 {
along
} else {
-along
};
Clip::Plane { origin: p, keep }
};
out.push(Swallowed {
arc: None,
ends: [(picked[i].edge, v, clip(i)), (picked[j].edge, v, clip(j))],
plate,
tool: swing_tool(p, n, m1, m2, r)?,
});
}
Ok(out)
}
fn swing_tool(p: Point3, n: Vec3, m1: Vec3, m2: Vec3, r: f64) -> Result<Brep> {
use crate::profile::{NamedSegment, Profile, ProfileLoop, Segment};
let o = p - n * r;
let seg = |segment, name: &str| NamedSegment::new(segment, name);
let meridian = Profile {
loops: vec![ProfileLoop {
segments: vec![
seg(
Segment::Arc {
center: Vec2::new(r, 0.0),
radius: r,
start: std::f64::consts::PI,
sweep: -std::f64::consts::FRAC_PI_2,
},
"patch",
),
seg(
Segment::Line {
a: Vec2::new(r, r),
b: Vec2::new(0.0, r),
},
"plate",
),
seg(
Segment::Line {
a: Vec2::new(0.0, r),
b: Vec2::new(0.0, 0.0),
},
"axis",
),
],
}],
};
let sketch = crate::math::Frame::new(o, m1.cross(n), m1)?;
let turn = m1.dot(m2).clamp(-1.0, 1.0).acos();
let axis = if m1.cross(m2).dot(n) > 0.0 { n } else { -n };
crate::sweep::revolve(&meridian, &sketch, o, axis, turn, MARK)
}
pub(super) fn swallowed(body: &Brep, picked: &[Picked], radius: f64) -> Result<Vec<Swallowed>> {
let mut out = Vec::new();
for (ai, pk) in picked.iter().enumerate() {
let e = body.edge(pk.edge);
let Some(Curve::Circle(circle)) = &e.curve else {
continue;
};
if circle.radius >= radius - tol::LINEAR || e.start == e.end {
continue;
}
let axis = circle.frame.z;
let (mut plate, mut round) = (None, None);
for &f in &pk.faces {
match &body.face(f).surface {
Surface::Plane(p) if p.frame.z.cross(axis).norm() <= tol::ANGULAR => {
plate = Some(f)
}
Surface::Cylinder(c)
if c.frame.z.cross(axis).norm() <= tol::ANGULAR
&& (c.radius - circle.radius).abs() <= tol::LINEAR =>
{
round = Some(f)
}
_ => {}
}
}
let (Some(plate), Some(round)) = (plate, round) else {
continue;
};
let Some(convex) = run::convex(body, pk.coedges[0]) else {
continue;
};
let at = body.vertex(e.start).point;
let radial_at =
|p: Point3| (p - circle.frame.origin) - axis * (p - circle.frame.origin).dot(axis);
if (outward(body, round, at).dot(radial_at(at)) > 0.0) != convex {
continue;
}
let side = if convex { 1.0 } else { -1.0 };
let n_plate = outward(body, plate, at);
let frame = crate::math::Frame::from_axis(at, n_plate)?;
let flat = |p: Point3| {
let l = frame.to_local(p);
Vec2::new(l.x, l.y)
};
let flat_dir = |d: Vec3| Vec2::new(d.dot(frame.x), d.dot(frame.y));
let mut ends = Vec::with_capacity(2);
let mut walls = Vec::with_capacity(2);
for v in [e.start, e.end] {
let beside: Vec<&Picked> = picked
.iter()
.filter(|q| q.edge != pk.edge)
.filter(|q| {
let x = body.edge(q.edge);
x.start == v || x.end == v
})
.collect();
let [next] = beside[..] else {
return unsupported(
"a fillet runs round a round smaller than itself where it does not meet one \
edge on the plate at each end",
);
};
let Some(&wall) = next.faces.iter().find(|&&f| f != plate) else {
return unsupported("a fillet's edge lies on the plate twice");
};
if !next.faces.contains(&plate) {
return unsupported(
"a fillet runs round a round smaller than itself onto an edge off the plate",
);
}
let p = body.vertex(v).point;
let n = outward(body, wall, p);
let shape = match (&body.edge(next.edge).curve, &body.face(wall).surface) {
(Some(Curve::Line(_)), Surface::Plane(_)) => Wall::Line {
p: flat(p),
n: flat_dir(n),
},
(Some(Curve::Circle(_)), Surface::Cylinder(c))
if c.frame.z.cross(n_plate).norm() <= tol::ANGULAR =>
{
let centre = flat(c.frame.origin);
let radial = flat(p) - centre;
Wall::Circle {
centre,
radius: c.radius,
out: if flat_dir(n).dot(radial) > 0.0 {
1.0
} else {
-1.0
},
}
}
_ => {
return unsupported(
"a fillet runs round a round smaller than itself next to a wall that is \
neither flat nor a cylinder square to the plate",
);
}
};
let radial = radial_at(p) * side;
if n.cross(radial).norm() > tol::ANGULAR * radial.norm().max(1.0)
|| n.dot(radial) <= 0.0
{
return unsupported(
"a fillet runs round a round smaller than itself whose walls are not tangent to \
it",
);
}
ends.push((next.edge, v, wall));
walls.push(shape);
}
let lift = n_plate * (side * radius);
let round_centre = flat(circle.frame.origin);
let Some(c2) = crossing(
walls[0].offset(side * radius),
walls[1].offset(side * radius),
round_centre,
) else {
return unsupported("a fillet's corner whose walls never come within its radius");
};
let c = frame.to_world(Vec3::new(c2.x, c2.y, 0.0)) - lift;
let t = c + lift;
let low = |p: Point3| p - lift;
let (a1, a2) = (
low(body.vertex(e.start).point),
low(body.vertex(e.end).point),
);
let on_axis = |o: Point3, d: Vec3, p: Point3| o + d * (p - o).dot(d);
let round_cyl = match &body.face(round).surface {
Surface::Cylinder(cyl) => cyl.clone(),
_ => unreachable!("checked above"),
};
let mut arcs = Vec::new();
let mut base_ends = [a1, a2];
for k in 0..2 {
let (_, _, wall) = ends[k];
let Surface::Cylinder(cyl) = &body.face(wall).surface else {
continue;
};
let o = on_axis(cyl.frame.origin, cyl.frame.z, c);
let Some(dir) = (c - o).unit() else {
return unsupported("a fillet's corner centred on its wall's axis");
};
let foot = o + dir * cyl.radius;
let a = [a1, a2][k];
base_ends[k] = foot;
let arc = if k == 0 {
BaseArc {
centre: o,
from: foot,
to: a,
face: wall,
}
} else {
BaseArc {
centre: o,
from: a,
to: foot,
face: wall,
}
};
arcs.push((k, arc));
}
let mut base = Vec::new();
if let Some(pos) = arcs.iter().position(|(k, _)| *k == 0) {
base.push(arcs.remove(pos).1);
}
base.push(BaseArc {
centre: on_axis(round_cyl.frame.origin, round_cyl.frame.z, a1),
from: a1,
to: a2,
face: round,
});
if let Some(pos) = arcs.iter().position(|(k, _)| *k == 1) {
base.push(arcs.remove(pos).1);
}
let mut clips = Vec::with_capacity(2);
for (k, (edge, v, _)) in ends.iter().enumerate() {
let Some(m) = n_plate.cross(base_ends[k] - c).unit() else {
return unsupported("a fillet's corner whose base touches its axis");
};
let x = body.edge(*edge);
let mid = x.curve.as_ref().map_or(body.vertex(x.end).point, |cv| {
cv.eval(0.5 * (x.range.0 + x.range.1))
});
let far = if (mid - body.vertex(*v).point).norm() > tol::LINEAR {
mid
} else {
body.vertex(if x.start == *v { x.end } else { x.start })
.point
};
let keep = if (far - c).dot(m) >= 0.0 { m } else { -m };
clips.push(Clip::Plane { origin: c, keep });
}
let tool = corner_tool(body, plate, &base, c, t, lift, side)?;
out.push(Swallowed {
arc: Some(ai),
ends: [
(ends[0].0, ends[0].1, clips[0]),
(ends[1].0, ends[1].1, clips[1]),
],
plate,
tool,
});
}
Ok(out)
}
pub(super) struct ToolFace {
pub surface: Surface,
pub out: Vec3,
pub at: Point3,
pub names: BTreeSet<FaceName>,
pub walk: Vec<(EdgeId, bool, Option<PCurve>)>,
}
fn mark(name: &str) -> BTreeSet<FaceName> {
BTreeSet::from([FaceName::new(MARK, name)])
}
fn corner_tool(
body: &Brep,
plate: FaceId,
base: &[BaseArc],
c: Point3,
t: Point3,
lift: Vec3,
side: f64,
) -> Result<Brep> {
let n_plate = outward(body, plate, t);
let n = base.len();
let mut row: Vec<(Point3, f64)> = vec![(base[0].from, 1.0)];
for arc in base {
let Some((mid, w)) = arc_middle(arc.centre, arc.from, arc.to) else {
return unsupported("a fillet's corner base that turns by a half turn");
};
row.push((mid, w));
row.push((arc.to, 1.0));
}
let mut knots_u = vec![0.0; 3];
for k in 1..n {
knots_u.extend([k as f64, k as f64]);
}
knots_u.extend([n as f64; 3]);
let knots = vec![0.0, 0.0, 0.0, 1.0, 1.0, 1.0];
let s = std::f64::consts::FRAC_1_SQRT_2;
let mut ctrl = Vec::with_capacity(3 * row.len());
let mut weights = Vec::with_capacity(3 * row.len());
for &(q, wu) in &row {
for (j, p) in [q, q + lift, t].into_iter().enumerate() {
ctrl.push(p);
weights.push(wu * if j == 1 { s } else { 1.0 });
}
}
let patch = NurbsSurface::new(
(2, 2),
(knots_u, knots.clone()),
(row.len(), 3),
ctrl,
Some(weights),
)?;
let quad = |pts: [Point3; 3], wm: f64| {
NurbsCurve::new(2, knots.clone(), pts.to_vec(), Some(vec![1.0, wm, 1.0]))
.map(|c| Curve::Nurbs(Box::new(c)))
};
let mut tool = Brep::new();
let base_pts: Vec<Point3> = std::iter::once(base[0].from)
.chain(base.iter().map(|a| a.to))
.collect();
let vb: Vec<VertexId> = base_pts.iter().map(|&p| tool.add_vertex(p)).collect();
let vtop: Vec<VertexId> = base_pts
.iter()
.map(|&p| tool.add_vertex(p + lift))
.collect();
let vt = tool.add_vertex(t);
let mut lows = Vec::with_capacity(n);
let mut tops = Vec::with_capacity(n);
for (k, arc) in base.iter().enumerate() {
let (mid, w) = arc_middle(arc.centre, arc.from, arc.to).expect("checked above");
lows.push(tool.add_edge(
quad([arc.from, mid, arc.to], w)?,
(0.0, 1.0),
vb[k],
vb[k + 1],
));
tops.push(tool.add_edge(
quad([arc.from + lift, mid + lift, arc.to + lift], w)?,
(0.0, 1.0),
vtop[k],
vtop[k + 1],
));
}
let (b0, bn) = (base_pts[0], base_pts[n]);
let side1 = tool.add_edge(quad([b0, b0 + lift, t], s)?, (0.0, 1.0), vb[0], vt);
let side2 = tool.add_edge(quad([bn, bn + lift, t], s)?, (0.0, 1.0), vb[n], vt);
let pole = tool.add_degenerate_edge(vt, (0.0, 1.0));
let line =
|tool: &mut Brep, p: Point3, q: Point3, vp: VertexId, vq: VertexId| -> Result<EdgeId> {
let Some((l, len)) = Line::through(p, q) else {
return unsupported("a fillet's corner with no room between its contacts");
};
Ok(tool.add_edge(Curve::Line(l), (0.0, len), vp, vq))
};
let mut ups = Vec::with_capacity(n + 1);
for k in 0..=n {
ups.push(line(
&mut tool,
base_pts[k],
base_pts[k] + lift,
vb[k],
vtop[k],
)?);
}
let flat1 = line(&mut tool, t, b0 + lift, vt, vtop[0])?;
let flat2 = line(&mut tool, t, bn + lift, vt, vtop[n])?;
let affine = |o: (f64, f64), d: (f64, f64)| {
Some(PCurve::Affine {
origin: Vec2::new(o.0, o.1),
dir: Vec2::new(d.0, d.1),
})
};
let plane_through = |p: Point3, nrm: Vec3| -> Result<Surface> {
let frame = crate::math::Frame::from_axis(p, nrm)?;
Ok(Surface::Plane(crate::geom::Plane { frame }))
};
let ends = [b0, bn];
let trim_out = |k: usize| -> Result<Vec3> {
let m = n_plate.cross(ends[k] - c).unit_or_zero();
Ok(if (ends[1 - k] - c).dot(m) <= 0.0 {
m
} else {
-m
})
};
let toward_c = |p: Point3| (c - p).unit_or_zero();
let mid_patch = patch.eval(0.5 * n as f64, 0.5);
let index = |f: FaceId| {
body.face_ids()
.position(|g| g == f)
.expect("a face of the body")
};
let mut walk = Vec::with_capacity(n + 3);
for (k, &e) in lows.iter().enumerate() {
walk.push((e, true, affine((k as f64, 0.0), (1.0, 0.0))));
}
walk.push((side2, true, affine((n as f64, 0.0), (0.0, 1.0))));
walk.push((pole, true, affine((n as f64, 1.0), (-(n as f64), 0.0))));
walk.push((side1, false, affine((0.0, 0.0), (0.0, 1.0))));
let mut faces = vec![ToolFace {
surface: Surface::Nurbs(Box::new(patch)),
out: toward_c(mid_patch),
at: mid_patch,
names: mark("patch"),
walk,
}];
for (k, arc) in base.iter().enumerate() {
let (mid, _) = arc_middle(arc.centre, arc.from, arc.to).expect("checked above");
let mid_arc =
arc.centre + (mid - arc.centre).unit_or_zero() * (arc.from - arc.centre).norm();
faces.push(ToolFace {
surface: body.face(arc.face).surface.clone(),
out: outward(body, arc.face, mid_arc) * side,
at: mid_arc + lift * 0.5,
names: mark(&format!("face_{}", index(arc.face))),
walk: vec![
(lows[k], true, None),
(ups[k + 1], true, None),
(tops[k], false, None),
(ups[k], false, None),
],
});
}
let mut top_walk: Vec<(EdgeId, bool, Option<PCurve>)> =
tops.iter().map(|&e| (e, true, None)).collect();
top_walk.push((flat2, false, None));
top_walk.push((flat1, true, None));
let centroid = base_pts
.iter()
.fold(t, |acc, &p| acc + (p + lift - t) / (n as f64 + 2.0));
faces.push(ToolFace {
surface: body.face(plate).surface.clone(),
out: n_plate * side,
at: centroid,
names: mark("plate"),
walk: top_walk,
});
faces.push(ToolFace {
surface: plane_through(c, trim_out(0)?)?,
out: trim_out(0)?,
at: (b0 + (b0 + lift) + t) / 3.0,
names: BTreeSet::new(),
walk: vec![
(side1, false, None),
(ups[0], true, None),
(flat1, false, None),
],
});
faces.push(ToolFace {
surface: plane_through(c, trim_out(1)?)?,
out: trim_out(1)?,
at: (bn + (bn + lift) + t) / 3.0,
names: BTreeSet::new(),
walk: vec![
(side2, true, None),
(flat2, true, None),
(ups[n], false, None),
],
});
assemble(tool, faces, "a fillet's corner tool")
}
pub(super) fn assemble(mut tool: Brep, faces: Vec<ToolFace>, what: &str) -> Result<Brep> {
let mut ids = Vec::with_capacity(faces.len());
for f in faces {
let uv = f.surface.project(f.at);
let natural = f.surface.normal(uv.x, uv.y);
let reversed = natural.dot(f.out) < 0.0;
let mut walk = f.walk;
if newell(&tool, &walk).dot(f.out) < 0.0 {
walk.reverse();
for w in &mut walk {
w.1 = !w.1;
}
}
let id = tool.add_face(f.surface.clone(), reversed, f.names);
let mut specs = Vec::with_capacity(walk.len());
let mut last: Option<Vec2> = None;
for (e, along, pcurve) in walk {
let x = tool.edge(e);
let pcurve = match pcurve {
Some(p) => p,
None => {
let (t0, t1) = if along {
x.range
} else {
(x.range.1, x.range.0)
};
let at = |tt: f64| {
x.curve
.as_ref()
.map_or(tool.vertex(x.start).point, |cv| cv.eval(tt))
};
let seed = near(&f.surface, f.surface.project(at(t0)), last);
let mut uv = seed;
for k in 1..=16 {
let p = at(t0 + (t1 - t0) * k as f64 / 16.0);
uv = near(&f.surface, f.surface.project(p), Some(uv));
}
last = Some(uv);
PCurve::Projected { seed }
}
};
specs.push(CoedgeSpec {
edge: e,
reversed: !along,
pcurve,
});
}
tool.add_loop(id, specs);
ids.push(id);
}
let shell = tool.add_shell(ids);
tool.add_solid(vec![shell]);
let problems = check::check(&tool);
if let Some(p) = problems.first() {
return Err(Error::Numerical(format!("{what} does not check: {p}")));
}
Ok(tool)
}
fn newell(tool: &Brep, walk: &[(EdgeId, bool, Option<PCurve>)]) -> Vec3 {
let mut pts = Vec::new();
for &(e, along, _) in walk {
let x = tool.edge(e);
let Some(c) = &x.curve else {
pts.push(tool.vertex(x.start).point);
continue;
};
for i in 0..8 {
let s = i as f64 / 8.0;
let s = if along { s } else { 1.0 - s };
pts.push(c.eval(x.range.0 + (x.range.1 - x.range.0) * s));
}
}
let mut n = Vec3::ZERO;
for i in 0..pts.len() {
let (p, q) = (pts[i], pts[(i + 1) % pts.len()]);
n += p.cross(q);
}
n
}