M0: workspace, geometry foundation, sketch constraint solver, kernel boundary
Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
bherbruckandClaude Opus 5.5 committed
commit
dafce1f4e2
47 files changed
+5624
No files matched your search
@@ -0,0 +1,21 @@
|
||||
//! Sketches: points and curves on a plane, geometric constraints and driving dimensions, a
|
||||
//! Levenberg–Marquardt constraint solver with degree-of-freedom analysis, and closed-profile
|
||||
//! (region) detection for features.
|
||||
//!
|
||||
//! The solver is our own clean-room implementation (residual functions per constraint, local
|
||||
//! finite-difference Jacobians, damped Gauss–Newton per connected component, rank analysis by
|
||||
//! Gaussian elimination). See `plan/architecture.md` §sketch.
|
||||
#![deny(clippy::unwrap_used, clippy::expect_used, clippy::panic, clippy::unimplemented, clippy::todo, clippy::unreachable)]
|
||||
#![forbid(unsafe_code)]
|
||||
|
||||
mod linalg;
|
||||
mod model;
|
||||
mod profiles;
|
||||
mod solver;
|
||||
|
||||
pub use model::{Constraint, ConstraintKind, Curve, CurveKind, SPoint, Sketch, SketchError};
|
||||
pub use profiles::{Profile, find_profiles};
|
||||
pub use solver::{SolveReport, SolveStatus, solve};
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests;
|
||||
@@ -0,0 +1,182 @@
|
||||
//! Small dense linear algebra for the solver: row-major matrices, Gaussian elimination with
|
||||
//! partial pivoting, and reduced row echelon form for rank and null-space analysis.
|
||||
|
||||
/// Row-major dense matrix.
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct Mat {
|
||||
pub rows: usize,
|
||||
pub cols: usize,
|
||||
pub data: Vec<f64>,
|
||||
}
|
||||
|
||||
impl Mat {
|
||||
pub fn zeros(rows: usize, cols: usize) -> Mat {
|
||||
Mat { rows, cols, data: vec![0.0; rows.saturating_mul(cols)] }
|
||||
}
|
||||
pub fn get(&self, r: usize, c: usize) -> f64 {
|
||||
self.data.get(r * self.cols + c).copied().unwrap_or(0.0)
|
||||
}
|
||||
pub fn set(&mut self, r: usize, c: usize, v: f64) {
|
||||
if let Some(x) = self.data.get_mut(r * self.cols + c) {
|
||||
*x = v;
|
||||
}
|
||||
}
|
||||
pub fn add(&mut self, r: usize, c: usize, v: f64) {
|
||||
if let Some(x) = self.data.get_mut(r * self.cols + c) {
|
||||
*x += v;
|
||||
}
|
||||
}
|
||||
fn swap_rows(&mut self, a: usize, b: usize) {
|
||||
if a == b {
|
||||
return;
|
||||
}
|
||||
for c in 0..self.cols {
|
||||
self.data.swap(a * self.cols + c, b * self.cols + c);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Solve `A x = b` (A square). Returns `None` when singular.
|
||||
pub fn solve(mut a: Mat, mut b: Vec<f64>) -> Option<Vec<f64>> {
|
||||
let n = a.rows;
|
||||
if a.cols != n || b.len() != n {
|
||||
return None;
|
||||
}
|
||||
for k in 0..n {
|
||||
let mut piv = k;
|
||||
let mut best = a.get(k, k).abs();
|
||||
for r in k + 1..n {
|
||||
let v = a.get(r, k).abs();
|
||||
if v > best {
|
||||
best = v;
|
||||
piv = r;
|
||||
}
|
||||
}
|
||||
if !(best > 1e-300) || !best.is_finite() {
|
||||
return None;
|
||||
}
|
||||
a.swap_rows(k, piv);
|
||||
b.swap(k, piv);
|
||||
let d = a.get(k, k);
|
||||
for r in k + 1..n {
|
||||
let f = a.get(r, k) / d;
|
||||
if f == 0.0 {
|
||||
continue;
|
||||
}
|
||||
for c in k..n {
|
||||
let v = a.get(k, c);
|
||||
a.add(r, c, -f * v);
|
||||
}
|
||||
let bk = *b.get(k)?;
|
||||
*b.get_mut(r)? -= f * bk;
|
||||
}
|
||||
}
|
||||
let mut x = vec![0.0; n];
|
||||
for k in (0..n).rev() {
|
||||
let mut s = *b.get(k)?;
|
||||
for c in k + 1..n {
|
||||
s -= a.get(k, c) * x.get(c)?;
|
||||
}
|
||||
*x.get_mut(k)? = s / a.get(k, k);
|
||||
}
|
||||
x.iter().all(|v| v.is_finite()).then_some(x)
|
||||
}
|
||||
|
||||
/// Reduce to row echelon form in place; returns the pivot column of each pivot row.
|
||||
pub fn rref(a: &mut Mat, tol: f64) -> Vec<usize> {
|
||||
let mut pivots = Vec::new();
|
||||
let mut row = 0;
|
||||
for col in 0..a.cols {
|
||||
if row >= a.rows {
|
||||
break;
|
||||
}
|
||||
let mut piv = row;
|
||||
let mut best = a.get(row, col).abs();
|
||||
for r in row + 1..a.rows {
|
||||
let v = a.get(r, col).abs();
|
||||
if v > best {
|
||||
best = v;
|
||||
piv = r;
|
||||
}
|
||||
}
|
||||
if !(best > tol) {
|
||||
continue;
|
||||
}
|
||||
a.swap_rows(row, piv);
|
||||
let d = a.get(row, col);
|
||||
for c in 0..a.cols {
|
||||
let v = a.get(row, c) / d;
|
||||
a.set(row, c, v);
|
||||
}
|
||||
for r in 0..a.rows {
|
||||
if r == row {
|
||||
continue;
|
||||
}
|
||||
let f = a.get(r, col);
|
||||
if f == 0.0 {
|
||||
continue;
|
||||
}
|
||||
for c in 0..a.cols {
|
||||
let v = a.get(row, c);
|
||||
a.add(r, c, -f * v);
|
||||
}
|
||||
}
|
||||
pivots.push(col);
|
||||
row += 1;
|
||||
}
|
||||
pivots
|
||||
}
|
||||
|
||||
/// For each column of `j`: is that variable fixed by the constraints (zero in every null-space
|
||||
/// vector)? Also returns the rank.
|
||||
pub fn determined_vars(j: &Mat, tol: f64) -> (Vec<bool>, usize) {
|
||||
let mut a = j.clone();
|
||||
let pivots = rref(&mut a, tol);
|
||||
let rank = pivots.len();
|
||||
let free: Vec<usize> = (0..a.cols).filter(|c| !pivots.contains(c)).collect();
|
||||
let mut det = vec![true; a.cols];
|
||||
for f in &free {
|
||||
if let Some(d) = det.get_mut(*f) {
|
||||
*d = false;
|
||||
}
|
||||
}
|
||||
// Null vector for free column f: x_f = 1, x_pivot(r) = -a[r][f].
|
||||
for (r, pc) in pivots.iter().enumerate() {
|
||||
if free.iter().any(|f| a.get(r, *f).abs() > tol)
|
||||
&& let Some(d) = det.get_mut(*pc)
|
||||
{
|
||||
*d = false;
|
||||
}
|
||||
}
|
||||
(det, rank)
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn solves_and_detects_singular() {
|
||||
let mut a = Mat::zeros(2, 2);
|
||||
a.set(0, 0, 2.0);
|
||||
a.set(0, 1, 1.0);
|
||||
a.set(1, 0, 1.0);
|
||||
a.set(1, 1, 3.0);
|
||||
let x = solve(a, vec![3.0, 5.0]).unwrap();
|
||||
assert!((x[0] - 0.8).abs() < 1e-12 && (x[1] - 1.4).abs() < 1e-12);
|
||||
let s = Mat::zeros(2, 2);
|
||||
assert!(solve(s, vec![1.0, 1.0]).is_none());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn rank_and_determined() {
|
||||
// x0 + x1 = .., x2 = .. ; x0 and x1 not individually determined.
|
||||
let mut j = Mat::zeros(2, 3);
|
||||
j.set(0, 0, 1.0);
|
||||
j.set(0, 1, 1.0);
|
||||
j.set(1, 2, 1.0);
|
||||
let (det, rank) = determined_vars(&j, 1e-9);
|
||||
assert_eq!(rank, 2);
|
||||
assert_eq!(det, vec![false, false, true]);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,674 @@
|
||||
use serde::{Deserialize, Serialize};
|
||||
use solvecraft_geom::{Seg2, Vec2};
|
||||
|
||||
/// Hard caps that keep hostile input from exhausting memory or time.
|
||||
pub const MAX_POINTS: usize = 20_000;
|
||||
pub const MAX_CONSTRAINTS: usize = 40_000;
|
||||
|
||||
#[derive(Debug, Clone, PartialEq, thiserror::Error)]
|
||||
pub enum SketchError {
|
||||
#[error("unknown sketch entity `{0}`")]
|
||||
Unknown(String),
|
||||
#[error("`{0}` is the wrong kind of entity for this: {1}")]
|
||||
WrongKind(String, String),
|
||||
#[error("duplicate id `{0}`")]
|
||||
Duplicate(String),
|
||||
#[error("invalid value: {0}")]
|
||||
Invalid(String),
|
||||
#[error("sketch is too large")]
|
||||
TooLarge,
|
||||
}
|
||||
|
||||
type Result<T> = std::result::Result<T, SketchError>;
|
||||
|
||||
/// A sketch point. `fixed` points are constants for the solver.
|
||||
#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
|
||||
pub struct SPoint {
|
||||
pub id: String,
|
||||
pub pos: Vec2,
|
||||
#[serde(default)]
|
||||
pub fixed: bool,
|
||||
}
|
||||
|
||||
#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
|
||||
#[serde(tag = "type", rename_all = "snake_case")]
|
||||
pub enum CurveKind {
|
||||
/// Segment between two points (indices into `Sketch::points`).
|
||||
Line { a: usize, b: usize },
|
||||
/// Full circle: centre point and radius.
|
||||
Circle { c: usize, r: f64 },
|
||||
/// Counter-clockwise arc around `c` from `a` to `b` (radius = |a − c|; the solver keeps
|
||||
/// |b − c| equal to it).
|
||||
Arc { c: usize, a: usize, b: usize },
|
||||
}
|
||||
|
||||
#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
|
||||
pub struct Curve {
|
||||
pub id: String,
|
||||
#[serde(flatten)]
|
||||
pub kind: CurveKind,
|
||||
#[serde(default)]
|
||||
pub construction: bool,
|
||||
}
|
||||
|
||||
/// Geometric constraints and dimensions. Indices refer to `Sketch::points` (`p`, `q`) or
|
||||
/// `Sketch::curves` (`l`, `c`, `a`, `b`). Dimension values are millimetres or radians.
|
||||
#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
|
||||
#[serde(tag = "type", rename_all = "snake_case")]
|
||||
pub enum ConstraintKind {
|
||||
Coincident {
|
||||
p: usize,
|
||||
q: usize,
|
||||
},
|
||||
PointOnCurve {
|
||||
p: usize,
|
||||
c: usize,
|
||||
},
|
||||
Horizontal {
|
||||
l: usize,
|
||||
},
|
||||
Vertical {
|
||||
l: usize,
|
||||
},
|
||||
HorizontalPoints {
|
||||
p: usize,
|
||||
q: usize,
|
||||
},
|
||||
VerticalPoints {
|
||||
p: usize,
|
||||
q: usize,
|
||||
},
|
||||
Parallel {
|
||||
a: usize,
|
||||
b: usize,
|
||||
},
|
||||
Perpendicular {
|
||||
a: usize,
|
||||
b: usize,
|
||||
},
|
||||
Collinear {
|
||||
a: usize,
|
||||
b: usize,
|
||||
},
|
||||
Tangent {
|
||||
a: usize,
|
||||
b: usize,
|
||||
},
|
||||
Equal {
|
||||
a: usize,
|
||||
b: usize,
|
||||
},
|
||||
Concentric {
|
||||
a: usize,
|
||||
b: usize,
|
||||
},
|
||||
Midpoint {
|
||||
p: usize,
|
||||
l: usize,
|
||||
},
|
||||
Symmetric {
|
||||
p: usize,
|
||||
q: usize,
|
||||
l: usize,
|
||||
},
|
||||
Fix {
|
||||
p: usize,
|
||||
},
|
||||
// Dimensions (driving).
|
||||
Distance {
|
||||
p: usize,
|
||||
q: usize,
|
||||
value: f64,
|
||||
},
|
||||
DistanceX {
|
||||
p: usize,
|
||||
q: usize,
|
||||
value: f64,
|
||||
},
|
||||
DistanceY {
|
||||
p: usize,
|
||||
q: usize,
|
||||
value: f64,
|
||||
},
|
||||
PointLineDistance {
|
||||
p: usize,
|
||||
l: usize,
|
||||
value: f64,
|
||||
},
|
||||
Length {
|
||||
l: usize,
|
||||
value: f64,
|
||||
},
|
||||
Radius {
|
||||
c: usize,
|
||||
value: f64,
|
||||
},
|
||||
Diameter {
|
||||
c: usize,
|
||||
value: f64,
|
||||
},
|
||||
/// Angle from line `a` to line `b`, radians, counter-clockwise.
|
||||
Angle {
|
||||
a: usize,
|
||||
b: usize,
|
||||
value: f64,
|
||||
},
|
||||
}
|
||||
|
||||
impl ConstraintKind {
|
||||
pub fn is_dimension(&self) -> bool {
|
||||
self.value().is_some()
|
||||
}
|
||||
pub fn value(&self) -> Option<f64> {
|
||||
use ConstraintKind::*;
|
||||
match *self {
|
||||
Distance { value, .. }
|
||||
| DistanceX { value, .. }
|
||||
| DistanceY { value, .. }
|
||||
| PointLineDistance { value, .. }
|
||||
| Length { value, .. }
|
||||
| Radius { value, .. }
|
||||
| Diameter { value, .. }
|
||||
| Angle { value, .. } => Some(value),
|
||||
_ => None,
|
||||
}
|
||||
}
|
||||
pub fn set_value(&mut self, v: f64) {
|
||||
use ConstraintKind::*;
|
||||
match self {
|
||||
Distance { value, .. }
|
||||
| DistanceX { value, .. }
|
||||
| DistanceY { value, .. }
|
||||
| PointLineDistance { value, .. }
|
||||
| Length { value, .. }
|
||||
| Radius { value, .. }
|
||||
| Diameter { value, .. }
|
||||
| Angle { value, .. } => *value = v,
|
||||
_ => {}
|
||||
}
|
||||
}
|
||||
pub fn is_angle(&self) -> bool {
|
||||
matches!(self, ConstraintKind::Angle { .. })
|
||||
}
|
||||
/// Short type name (Fusion-like) for listings.
|
||||
pub fn name(&self) -> &'static str {
|
||||
use ConstraintKind::*;
|
||||
match self {
|
||||
Coincident { .. } | PointOnCurve { .. } => "Coincident",
|
||||
Horizontal { .. } | HorizontalPoints { .. } => "Horizontal",
|
||||
Vertical { .. } | VerticalPoints { .. } => "Vertical",
|
||||
Parallel { .. } => "Parallel",
|
||||
Perpendicular { .. } => "Perpendicular",
|
||||
Collinear { .. } => "Collinear",
|
||||
Tangent { .. } => "Tangent",
|
||||
Equal { .. } => "Equal",
|
||||
Concentric { .. } => "Concentric",
|
||||
Midpoint { .. } => "MidPoint",
|
||||
Symmetric { .. } => "Symmetry",
|
||||
Fix { .. } => "Fix",
|
||||
Distance { .. } | DistanceX { .. } | DistanceY { .. } | PointLineDistance { .. } | Length { .. } => "LinearDimension",
|
||||
Radius { .. } => "RadialDimension",
|
||||
Diameter { .. } => "DiameterDimension",
|
||||
Angle { .. } => "AngularDimension",
|
||||
}
|
||||
}
|
||||
fn indices(&self) -> (Vec<usize>, Vec<usize>) {
|
||||
use ConstraintKind::*;
|
||||
match *self {
|
||||
Coincident { p, q } | HorizontalPoints { p, q } | VerticalPoints { p, q } => (vec![p, q], vec![]),
|
||||
Distance { p, q, .. } | DistanceX { p, q, .. } | DistanceY { p, q, .. } => (vec![p, q], vec![]),
|
||||
PointOnCurve { p, c } => (vec![p], vec![c]),
|
||||
Horizontal { l } | Vertical { l } | Length { l, .. } => (vec![], vec![l]),
|
||||
Radius { c, .. } | Diameter { c, .. } => (vec![], vec![c]),
|
||||
Parallel { a, b } | Perpendicular { a, b } | Collinear { a, b } | Tangent { a, b } | Equal { a, b } | Concentric { a, b } => {
|
||||
(vec![], vec![a, b])
|
||||
}
|
||||
Angle { a, b, .. } => (vec![], vec![a, b]),
|
||||
Midpoint { p, l } | PointLineDistance { p, l, .. } => (vec![p], vec![l]),
|
||||
Symmetric { p, q, l } => (vec![p, q], vec![l]),
|
||||
Fix { p } => (vec![p], vec![]),
|
||||
}
|
||||
}
|
||||
fn remap(&mut self, pmap: &dyn Fn(usize) -> Option<usize>, cmap: &dyn Fn(usize) -> Option<usize>) -> bool {
|
||||
use ConstraintKind::*;
|
||||
let mut ok = true;
|
||||
let mut fp = |i: &mut usize| match pmap(*i) {
|
||||
Some(n) => *i = n,
|
||||
None => ok = false,
|
||||
};
|
||||
let mut ok2 = true;
|
||||
let mut fc = |i: &mut usize| match cmap(*i) {
|
||||
Some(n) => *i = n,
|
||||
None => ok2 = false,
|
||||
};
|
||||
match self {
|
||||
Coincident { p, q } | HorizontalPoints { p, q } | VerticalPoints { p, q } => {
|
||||
fp(p);
|
||||
fp(q);
|
||||
}
|
||||
Distance { p, q, .. } | DistanceX { p, q, .. } | DistanceY { p, q, .. } => {
|
||||
fp(p);
|
||||
fp(q);
|
||||
}
|
||||
PointOnCurve { p, c } => {
|
||||
fp(p);
|
||||
fc(c);
|
||||
}
|
||||
Horizontal { l } | Vertical { l } | Length { l, .. } => fc(l),
|
||||
Radius { c, .. } | Diameter { c, .. } => fc(c),
|
||||
Parallel { a, b } | Perpendicular { a, b } | Collinear { a, b } | Tangent { a, b } | Equal { a, b } | Concentric { a, b } => {
|
||||
fc(a);
|
||||
fc(b);
|
||||
}
|
||||
Angle { a, b, .. } => {
|
||||
fc(a);
|
||||
fc(b);
|
||||
}
|
||||
Midpoint { p, l } | PointLineDistance { p, l, .. } => {
|
||||
fp(p);
|
||||
fc(l);
|
||||
}
|
||||
Symmetric { p, q, l } => {
|
||||
fp(p);
|
||||
fp(q);
|
||||
fc(l);
|
||||
}
|
||||
Fix { p } => fp(p),
|
||||
}
|
||||
ok && ok2
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
|
||||
pub struct Constraint {
|
||||
pub id: String,
|
||||
#[serde(flatten)]
|
||||
pub kind: ConstraintKind,
|
||||
/// For dimensions: the model parameter that drives the value (e.g. `d1`).
|
||||
#[serde(default, skip_serializing_if = "Option::is_none")]
|
||||
pub param: Option<String>,
|
||||
}
|
||||
|
||||
/// A 2D sketch. Point 0 is always the fixed sketch origin (id `origin`).
|
||||
#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
|
||||
pub struct Sketch {
|
||||
pub points: Vec<SPoint>,
|
||||
pub curves: Vec<Curve>,
|
||||
pub constraints: Vec<Constraint>,
|
||||
#[serde(default)]
|
||||
next_id: u64,
|
||||
}
|
||||
|
||||
impl Default for Sketch {
|
||||
fn default() -> Self {
|
||||
Sketch::new()
|
||||
}
|
||||
}
|
||||
|
||||
impl Sketch {
|
||||
pub fn new() -> Self {
|
||||
Sketch { points: vec![SPoint { id: "origin".into(), pos: Vec2::ZERO, fixed: true }], curves: Vec::new(), constraints: Vec::new(), next_id: 1 }
|
||||
}
|
||||
|
||||
fn fresh(&mut self, prefix: &str) -> String {
|
||||
loop {
|
||||
let id = format!("{prefix}{}", self.next_id);
|
||||
self.next_id += 1;
|
||||
if !self.id_taken(&id) {
|
||||
return id;
|
||||
}
|
||||
}
|
||||
}
|
||||
fn id_taken(&self, id: &str) -> bool {
|
||||
self.points.iter().any(|p| p.id == id) || self.curves.iter().any(|c| c.id == id) || self.constraints.iter().any(|c| c.id == id)
|
||||
}
|
||||
|
||||
pub fn point_index(&self, id: &str) -> Option<usize> {
|
||||
self.points.iter().position(|p| p.id == id)
|
||||
}
|
||||
pub fn curve_index(&self, id: &str) -> Option<usize> {
|
||||
self.curves.iter().position(|c| c.id == id)
|
||||
}
|
||||
pub fn point(&self, i: usize) -> Option<Vec2> {
|
||||
self.points.get(i).map(|p| p.pos)
|
||||
}
|
||||
|
||||
fn check_pos(p: Vec2) -> Result<Vec2> {
|
||||
if p.is_finite() && p.x.abs() < 1e9 && p.y.abs() < 1e9 { Ok(p) } else { Err(SketchError::Invalid(format!("point {:?}", [p.x, p.y]))) }
|
||||
}
|
||||
|
||||
/// Add a free point (or reuse an existing one within `merge_tol` of `p` when `merge_tol > 0`).
|
||||
pub fn add_point(&mut self, p: Vec2, id: Option<&str>) -> Result<usize> {
|
||||
let p = Self::check_pos(p)?;
|
||||
if self.points.len() >= MAX_POINTS {
|
||||
return Err(SketchError::TooLarge);
|
||||
}
|
||||
let id = match id {
|
||||
Some(i) if self.id_taken(i) => return Err(SketchError::Duplicate(i.into())),
|
||||
Some("") => return Err(SketchError::Invalid("empty id".into())),
|
||||
Some(i) => i.to_string(),
|
||||
None => self.fresh("p"),
|
||||
};
|
||||
self.points.push(SPoint { id, pos: p, fixed: false });
|
||||
Ok(self.points.len() - 1)
|
||||
}
|
||||
|
||||
fn curve_id(&mut self, id: Option<&str>, prefix: &str) -> Result<String> {
|
||||
match id {
|
||||
Some(i) if self.id_taken(i) => Err(SketchError::Duplicate(i.into())),
|
||||
Some("") => Err(SketchError::Invalid("empty id".into())),
|
||||
Some(i) => Ok(i.into()),
|
||||
None => Ok(self.fresh(prefix)),
|
||||
}
|
||||
}
|
||||
|
||||
/// Endpoint ids of a curve's own points: `<curve>.start`, `.end`, `.center`.
|
||||
fn own_point(&mut self, curve: &str, role: &str, p: Vec2) -> Result<usize> {
|
||||
let id = format!("{curve}.{role}");
|
||||
if self.id_taken(&id) {
|
||||
return self.add_point(p, None);
|
||||
}
|
||||
self.add_point(p, Some(&id))
|
||||
}
|
||||
|
||||
/// Line between existing points `a` and `b`.
|
||||
pub fn add_line_pts(&mut self, a: usize, b: usize, id: Option<&str>) -> Result<usize> {
|
||||
if a >= self.points.len() || b >= self.points.len() || a == b {
|
||||
return Err(SketchError::Invalid("line needs two distinct points".into()));
|
||||
}
|
||||
let id = self.curve_id(id, "l")?;
|
||||
self.curves.push(Curve { id, kind: CurveKind::Line { a, b }, construction: false });
|
||||
Ok(self.curves.len() - 1)
|
||||
}
|
||||
|
||||
/// Line from `p0` to `p1`, reusing given point indices when provided.
|
||||
pub fn add_line(&mut self, p0: Vec2, p1: Vec2, a: Option<usize>, b: Option<usize>, id: Option<&str>) -> Result<usize> {
|
||||
Self::check_pos(p0)?;
|
||||
Self::check_pos(p1)?;
|
||||
let id = self.curve_id(id, "l")?;
|
||||
let a = match a {
|
||||
Some(i) if i < self.points.len() => i,
|
||||
_ => self.own_point(&id, "start", p0)?,
|
||||
};
|
||||
let b = match b {
|
||||
Some(i) if i < self.points.len() && i != a => i,
|
||||
_ => self.own_point(&id, "end", p1)?,
|
||||
};
|
||||
self.add_line_pts(a, b, Some(&id))
|
||||
}
|
||||
|
||||
pub fn add_circle(&mut self, center: Vec2, r: f64, c: Option<usize>, id: Option<&str>) -> Result<usize> {
|
||||
if !(r.is_finite() && r > 1e-9 && r < 1e9) {
|
||||
return Err(SketchError::Invalid(format!("radius {r}")));
|
||||
}
|
||||
let id = self.curve_id(id, "c")?;
|
||||
let c = match c {
|
||||
Some(i) if i < self.points.len() => i,
|
||||
_ => self.own_point(&id, "center", center)?,
|
||||
};
|
||||
self.curves.push(Curve { id, kind: CurveKind::Circle { c, r }, construction: false });
|
||||
Ok(self.curves.len() - 1)
|
||||
}
|
||||
|
||||
/// Counter-clockwise arc around `center` from `p0` to `p1` (the end point is projected onto
|
||||
/// the circle through `p0`).
|
||||
pub fn add_arc(&mut self, center: Vec2, p0: Vec2, p1: Vec2, pts: [Option<usize>; 3], id: Option<&str>) -> Result<usize> {
|
||||
let r = center.dist(p0);
|
||||
if !(r.is_finite() && r > 1e-9 && r < 1e9) {
|
||||
return Err(SketchError::Invalid("degenerate arc".into()));
|
||||
}
|
||||
let p1 = center + (p1 - center).normalized().ok_or_else(|| SketchError::Invalid("degenerate arc".into()))? * r;
|
||||
let id = self.curve_id(id, "a")?;
|
||||
let n = self.points.len();
|
||||
let c = match pts[0] {
|
||||
Some(i) if i < n => i,
|
||||
_ => self.own_point(&id, "center", center)?,
|
||||
};
|
||||
let a = match pts[1] {
|
||||
Some(i) if i < n => i,
|
||||
_ => self.own_point(&id, "start", p0)?,
|
||||
};
|
||||
let b = match pts[2] {
|
||||
Some(i) if i < n && i != a => i,
|
||||
_ => self.own_point(&id, "end", p1)?,
|
||||
};
|
||||
self.curves.push(Curve { id, kind: CurveKind::Arc { c, a, b }, construction: false });
|
||||
Ok(self.curves.len() - 1)
|
||||
}
|
||||
|
||||
pub fn add_constraint(&mut self, kind: ConstraintKind, param: Option<String>) -> Result<String> {
|
||||
if self.constraints.len() >= MAX_CONSTRAINTS {
|
||||
return Err(SketchError::TooLarge);
|
||||
}
|
||||
self.validate(&kind)?;
|
||||
let id = self.fresh("k");
|
||||
self.constraints.push(Constraint { id: id.clone(), kind, param });
|
||||
Ok(id)
|
||||
}
|
||||
|
||||
/// Check indices and entity kinds of a constraint.
|
||||
pub fn validate(&self, k: &ConstraintKind) -> Result<()> {
|
||||
use ConstraintKind::*;
|
||||
let (ps, cs) = k.indices();
|
||||
for p in &ps {
|
||||
if *p >= self.points.len() {
|
||||
return Err(SketchError::Unknown(format!("point #{p}")));
|
||||
}
|
||||
}
|
||||
for c in &cs {
|
||||
if *c >= self.curves.len() {
|
||||
return Err(SketchError::Unknown(format!("curve #{c}")));
|
||||
}
|
||||
}
|
||||
let is_line = |i: usize| matches!(self.curves.get(i).map(|c| &c.kind), Some(CurveKind::Line { .. }));
|
||||
let is_round = |i: usize| matches!(self.curves.get(i).map(|c| &c.kind), Some(CurveKind::Circle { .. } | CurveKind::Arc { .. }));
|
||||
let name = |i: usize| self.curves.get(i).map(|c| c.id.clone()).unwrap_or_default();
|
||||
let need_line = |i: usize| if is_line(i) { Ok(()) } else { Err(SketchError::WrongKind(name(i), "needs a line".into())) };
|
||||
let need_round = |i: usize| if is_round(i) { Ok(()) } else { Err(SketchError::WrongKind(name(i), "needs a circle or arc".into())) };
|
||||
match *k {
|
||||
Horizontal { l } | Vertical { l } | Length { l, .. } | Midpoint { l, .. } | Symmetric { l, .. } | PointLineDistance { l, .. } => {
|
||||
need_line(l)?
|
||||
}
|
||||
Parallel { a, b } | Perpendicular { a, b } | Collinear { a, b } | Angle { a, b, .. } => {
|
||||
need_line(a)?;
|
||||
need_line(b)?;
|
||||
}
|
||||
Radius { c, .. } | Diameter { c, .. } => need_round(c)?,
|
||||
Concentric { a, b } => {
|
||||
need_round(a)?;
|
||||
need_round(b)?;
|
||||
}
|
||||
Tangent { a, b } => {
|
||||
if !(is_round(a) || is_round(b)) {
|
||||
return Err(SketchError::WrongKind(name(a), "tangent needs a circle or arc".into()));
|
||||
}
|
||||
}
|
||||
Equal { a, b } => {
|
||||
if is_line(a) != is_line(b) {
|
||||
return Err(SketchError::WrongKind(name(b), "equal needs two lines or two circles/arcs".into()));
|
||||
}
|
||||
}
|
||||
Coincident { p, q } if p == q => return Err(SketchError::Invalid("a point is always coincident with itself".into())),
|
||||
_ => {}
|
||||
}
|
||||
if let Some(v) = k.value()
|
||||
&& (!v.is_finite() || v.abs() > 1e9 || (v < 0.0 && !k.is_angle()))
|
||||
{
|
||||
return Err(SketchError::Invalid(format!("dimension value {v}")));
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// Radius of a circle or arc.
|
||||
pub fn radius(&self, ci: usize) -> Option<f64> {
|
||||
match self.curves.get(ci)?.kind {
|
||||
CurveKind::Circle { r, .. } => Some(r),
|
||||
CurveKind::Arc { c, a, .. } => Some(self.point(c)?.dist(self.point(a)?)),
|
||||
CurveKind::Line { .. } => None,
|
||||
}
|
||||
}
|
||||
pub fn center(&self, ci: usize) -> Option<Vec2> {
|
||||
match self.curves.get(ci)?.kind {
|
||||
CurveKind::Circle { c, .. } | CurveKind::Arc { c, .. } => self.point(c),
|
||||
CurveKind::Line { .. } => None,
|
||||
}
|
||||
}
|
||||
|
||||
/// Boundary segments of a curve (a circle gives two half arcs).
|
||||
pub fn segs(&self, ci: usize) -> Vec<Seg2> {
|
||||
let Some(cu) = self.curves.get(ci) else { return Vec::new() };
|
||||
match cu.kind {
|
||||
CurveKind::Line { a, b } => match (self.point(a), self.point(b)) {
|
||||
(Some(a), Some(b)) => vec![Seg2::Line { a, b }],
|
||||
_ => Vec::new(),
|
||||
},
|
||||
CurveKind::Circle { c, r } => match self.point(c) {
|
||||
Some(c) => solvecraft_geom::Loop2::circle(c, r).segs,
|
||||
None => Vec::new(),
|
||||
},
|
||||
CurveKind::Arc { c, a, b } => match (self.point(c), self.point(a), self.point(b)) {
|
||||
(Some(c), Some(pa), Some(pb)) => {
|
||||
let start = (pa - c).angle();
|
||||
let mut sweep = (pb - c).angle() - start;
|
||||
while sweep <= 1e-12 {
|
||||
sweep += std::f64::consts::TAU;
|
||||
}
|
||||
vec![Seg2::Arc { center: c, radius: c.dist(pa), start, sweep }]
|
||||
}
|
||||
_ => Vec::new(),
|
||||
},
|
||||
}
|
||||
}
|
||||
|
||||
/// Remove curves (by index) and then any points no longer used by a curve (except the
|
||||
/// origin and free points), and constraints that referred to removed entities.
|
||||
pub fn remove_curves(&mut self, which: &[usize]) {
|
||||
let mut owned: Vec<usize> = Vec::new();
|
||||
for (i, c) in self.curves.iter().enumerate() {
|
||||
if which.contains(&i) {
|
||||
match c.kind {
|
||||
CurveKind::Line { a, b } => owned.extend([a, b]),
|
||||
CurveKind::Circle { c, .. } => owned.push(c),
|
||||
CurveKind::Arc { c, a, b } => owned.extend([c, a, b]),
|
||||
}
|
||||
}
|
||||
}
|
||||
self.drop_curves(which);
|
||||
let mut orphans: Vec<usize> = owned.into_iter().filter(|p| *p != 0 && !self.point_used_by_curve(*p)).collect();
|
||||
orphans.sort_unstable();
|
||||
orphans.dedup();
|
||||
self.remove_points(&orphans);
|
||||
}
|
||||
|
||||
fn point_used_by_curve(&self, p: usize) -> bool {
|
||||
self.curves.iter().any(|c| match c.kind {
|
||||
CurveKind::Line { a, b } => a == p || b == p,
|
||||
CurveKind::Circle { c, .. } => c == p,
|
||||
CurveKind::Arc { c, a, b } => c == p || a == p || b == p,
|
||||
})
|
||||
}
|
||||
|
||||
/// Remove curves only (points stay), remapping constraints.
|
||||
fn drop_curves(&mut self, which: &[usize]) {
|
||||
let mut cmap = vec![None; self.curves.len()];
|
||||
let mut n = 0;
|
||||
for (i, slot) in cmap.iter_mut().enumerate() {
|
||||
if !which.contains(&i) {
|
||||
*slot = Some(n);
|
||||
n += 1;
|
||||
}
|
||||
}
|
||||
let mut idx = 0;
|
||||
self.curves.retain(|_| {
|
||||
let k = !which.contains(&idx);
|
||||
idx += 1;
|
||||
k
|
||||
});
|
||||
let cm = |i: usize| cmap.get(i).copied().flatten();
|
||||
let pm = |i: usize| Some(i);
|
||||
self.constraints.retain_mut(|c| c.kind.remap(&pm, &cm));
|
||||
}
|
||||
|
||||
/// Remove points (never the origin) and every curve/constraint using them.
|
||||
pub fn remove_points(&mut self, which: &[usize]) {
|
||||
if which.is_empty() {
|
||||
return;
|
||||
}
|
||||
let gone = |p: usize| p != 0 && which.contains(&p);
|
||||
let dead_curves: Vec<usize> = self
|
||||
.curves
|
||||
.iter()
|
||||
.enumerate()
|
||||
.filter(|(_, c)| match c.kind {
|
||||
CurveKind::Line { a, b } => gone(a) || gone(b),
|
||||
CurveKind::Circle { c, .. } => gone(c),
|
||||
CurveKind::Arc { c, a, b } => gone(c) || gone(a) || gone(b),
|
||||
})
|
||||
.map(|(i, _)| i)
|
||||
.collect();
|
||||
self.drop_curves(&dead_curves);
|
||||
let mut pmap = vec![None; self.points.len()];
|
||||
let mut n = 0;
|
||||
for (i, slot) in pmap.iter_mut().enumerate() {
|
||||
if !gone(i) {
|
||||
*slot = Some(n);
|
||||
n += 1;
|
||||
}
|
||||
}
|
||||
let pm = |i: usize| pmap.get(i).copied().flatten();
|
||||
for c in &mut self.curves {
|
||||
match &mut c.kind {
|
||||
CurveKind::Line { a, b } => {
|
||||
*a = pm(*a).unwrap_or(0);
|
||||
*b = pm(*b).unwrap_or(0);
|
||||
}
|
||||
CurveKind::Circle { c, .. } => *c = pm(*c).unwrap_or(0),
|
||||
CurveKind::Arc { c, a, b } => {
|
||||
*c = pm(*c).unwrap_or(0);
|
||||
*a = pm(*a).unwrap_or(0);
|
||||
*b = pm(*b).unwrap_or(0);
|
||||
}
|
||||
}
|
||||
}
|
||||
let cm = |i: usize| Some(i);
|
||||
self.constraints.retain_mut(|c| c.kind.remap(&pm, &cm));
|
||||
let mut idx = 0;
|
||||
self.points.retain(|_| {
|
||||
let k = !gone(idx);
|
||||
idx += 1;
|
||||
k
|
||||
});
|
||||
}
|
||||
|
||||
/// Resolve a point reference: a point id (`origin`, `l1.end`, `p3`) or `<curve>.start|end|center`.
|
||||
pub fn resolve_point(&self, r: &str) -> Option<usize> {
|
||||
if let Some(i) = self.point_index(r) {
|
||||
return Some(i);
|
||||
}
|
||||
let (cid, role) = r.rsplit_once('.')?;
|
||||
let c = self.curves.get(self.curve_index(cid)?)?;
|
||||
match (&c.kind, role) {
|
||||
(CurveKind::Line { a, .. }, "start") | (CurveKind::Arc { a, .. }, "start") => Some(*a),
|
||||
(CurveKind::Line { b, .. }, "end") | (CurveKind::Arc { b, .. }, "end") => Some(*b),
|
||||
(CurveKind::Circle { c, .. }, "center") | (CurveKind::Arc { c, .. }, "center") => Some(*c),
|
||||
_ => None,
|
||||
}
|
||||
}
|
||||
|
||||
/// Axis-aligned bounds of all geometry (for views).
|
||||
pub fn bounds(&self) -> Option<(Vec2, Vec2)> {
|
||||
let mut lo = Vec2::new(f64::INFINITY, f64::INFINITY);
|
||||
let mut hi = Vec2::new(f64::NEG_INFINITY, f64::NEG_INFINITY);
|
||||
let mut any = false;
|
||||
for i in 0..self.curves.len() {
|
||||
for s in self.segs(i) {
|
||||
for p in s.polyline(0.5) {
|
||||
lo = Vec2::new(lo.x.min(p.x), lo.y.min(p.y));
|
||||
hi = Vec2::new(hi.x.max(p.x), hi.y.max(p.y));
|
||||
any = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
any.then_some((lo, hi))
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,273 @@
|
||||
//! Closed-profile detection.
|
||||
//!
|
||||
//! Non-construction lines and arcs form a planar graph (end points within a small tolerance are
|
||||
//! merged). Dangling edges are pruned, then faces are traced with the "first clockwise turn"
|
||||
//! rule, which yields every bounded region counter-clockwise. Circles are loops of their own.
|
||||
//! A loop that lies inside another loop of a different connected component becomes a hole of
|
||||
//! the innermost loop containing it, so a circle inside a rectangle gives two profiles: the
|
||||
//! rectangle with a hole, and the disc.
|
||||
|
||||
use std::f64::consts::TAU;
|
||||
|
||||
use serde::{Deserialize, Serialize};
|
||||
use solvecraft_geom::{Loop2, Region2, Seg2, Vec2};
|
||||
|
||||
use crate::model::{CurveKind, Sketch};
|
||||
|
||||
const MERGE_TOL: f64 = 1e-6;
|
||||
const MAX_EDGES: usize = 50_000;
|
||||
|
||||
/// A closed region usable by features, with the ids of the curves bounding it.
|
||||
#[derive(Clone, Debug, Serialize, Deserialize)]
|
||||
pub struct Profile {
|
||||
pub region: Region2,
|
||||
/// Curve ids of the outer loop.
|
||||
pub outer_curves: Vec<String>,
|
||||
/// Curve ids of each hole loop.
|
||||
pub hole_curves: Vec<Vec<String>>,
|
||||
pub area: f64,
|
||||
pub centroid: Vec2,
|
||||
}
|
||||
|
||||
impl Profile {
|
||||
/// All curve ids bounding the profile.
|
||||
pub fn curve_ids(&self) -> Vec<String> {
|
||||
let mut v = self.outer_curves.clone();
|
||||
for h in &self.hole_curves {
|
||||
v.extend(h.iter().cloned());
|
||||
}
|
||||
v
|
||||
}
|
||||
}
|
||||
|
||||
struct Edge {
|
||||
curve: usize,
|
||||
seg: Seg2,
|
||||
u: usize,
|
||||
v: usize,
|
||||
}
|
||||
|
||||
fn find(p: &mut [usize], mut i: usize) -> usize {
|
||||
while let Some(&q) = p.get(i) {
|
||||
if q == i {
|
||||
break;
|
||||
}
|
||||
i = q;
|
||||
}
|
||||
i
|
||||
}
|
||||
|
||||
/// Find all closed profiles of a sketch.
|
||||
pub fn find_profiles(sk: &Sketch) -> Vec<Profile> {
|
||||
// Node ids: point indices merged by position.
|
||||
let n = sk.points.len();
|
||||
let mut parent: Vec<usize> = (0..n).collect();
|
||||
let mut order: Vec<usize> = (0..n).collect();
|
||||
order.sort_by(|a, b| {
|
||||
let (pa, pb) = (sk.points.get(*a).map(|p| p.pos.x).unwrap_or(0.0), sk.points.get(*b).map(|p| p.pos.x).unwrap_or(0.0));
|
||||
pa.total_cmp(&pb)
|
||||
});
|
||||
for (k, &i) in order.iter().enumerate() {
|
||||
let Some(pi) = sk.points.get(i).map(|p| p.pos) else { continue };
|
||||
for &j in order.iter().skip(k + 1) {
|
||||
let Some(pj) = sk.points.get(j).map(|p| p.pos) else { continue };
|
||||
if pj.x - pi.x > MERGE_TOL {
|
||||
break;
|
||||
}
|
||||
if pi.dist(pj) <= MERGE_TOL {
|
||||
let (ri, rj) = (find(&mut parent, i), find(&mut parent, j));
|
||||
if ri != rj
|
||||
&& let Some(s) = parent.get_mut(rj)
|
||||
{
|
||||
*s = ri;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
let mut loops: Vec<(Loop2, Vec<String>, usize)> = Vec::new(); // (ccw loop, curve ids, component)
|
||||
let mut edges: Vec<Edge> = Vec::new();
|
||||
for (ci, c) in sk.curves.iter().enumerate() {
|
||||
if c.construction {
|
||||
continue;
|
||||
}
|
||||
match c.kind {
|
||||
CurveKind::Circle { .. } => {
|
||||
let segs = sk.segs(ci);
|
||||
if !segs.is_empty() {
|
||||
loops.push((Loop2 { segs }, vec![c.id.clone()], usize::MAX - ci));
|
||||
}
|
||||
}
|
||||
CurveKind::Line { a, b } | CurveKind::Arc { a, b, .. } => {
|
||||
let (u, v) = (find(&mut parent, a), find(&mut parent, b));
|
||||
if u == v {
|
||||
continue;
|
||||
}
|
||||
if let Some(seg) = sk.segs(ci).into_iter().next()
|
||||
&& seg.length() > MERGE_TOL
|
||||
{
|
||||
edges.push(Edge { curve: ci, seg, u, v });
|
||||
}
|
||||
}
|
||||
}
|
||||
if edges.len() > MAX_EDGES {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Prune dangling edges.
|
||||
let mut alive = vec![true; edges.len()];
|
||||
loop {
|
||||
let mut deg = vec![0usize; n];
|
||||
for (e, a) in edges.iter().zip(&alive) {
|
||||
if *a {
|
||||
if let Some(d) = deg.get_mut(e.u) {
|
||||
*d += 1;
|
||||
}
|
||||
if let Some(d) = deg.get_mut(e.v) {
|
||||
*d += 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
let mut changed = false;
|
||||
for (e, a) in edges.iter().zip(alive.iter_mut()) {
|
||||
if *a && (deg.get(e.u).copied().unwrap_or(0) < 2 || deg.get(e.v).copied().unwrap_or(0) < 2) {
|
||||
*a = false;
|
||||
changed = true;
|
||||
}
|
||||
}
|
||||
if !changed {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Components over nodes for nesting.
|
||||
let mut comp: Vec<usize> = (0..n).collect();
|
||||
for (e, a) in edges.iter().zip(&alive) {
|
||||
if *a {
|
||||
let (ru, rv) = (find(&mut comp, e.u), find(&mut comp, e.v));
|
||||
if ru != rv
|
||||
&& let Some(s) = comp.get_mut(rv)
|
||||
{
|
||||
*s = ru;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Half-edges: 2e = forward (u→v), 2e+1 = backward.
|
||||
let half = |h: usize| -> Option<(Seg2, usize, usize)> {
|
||||
let e = edges.get(h / 2)?;
|
||||
Some(if h.is_multiple_of(2) { (e.seg, e.u, e.v) } else { (e.seg.reversed(), e.v, e.u) })
|
||||
};
|
||||
let mut outgoing: Vec<Vec<usize>> = vec![Vec::new(); n];
|
||||
for (ei, a) in alive.iter().enumerate() {
|
||||
if !*a {
|
||||
continue;
|
||||
}
|
||||
for h in [2 * ei, 2 * ei + 1] {
|
||||
if let Some((_, from, _)) = half(h)
|
||||
&& let Some(o) = outgoing.get_mut(from)
|
||||
{
|
||||
o.push(h);
|
||||
}
|
||||
}
|
||||
}
|
||||
let mut visited = vec![false; edges.len() * 2];
|
||||
for start in 0..edges.len() * 2 {
|
||||
if !alive.get(start / 2).copied().unwrap_or(false) || visited.get(start).copied().unwrap_or(true) {
|
||||
continue;
|
||||
}
|
||||
let mut segs = Vec::new();
|
||||
let mut ids = Vec::new();
|
||||
let mut h = start;
|
||||
let mut ok = true;
|
||||
for _ in 0..=edges.len() * 2 {
|
||||
if let Some(v) = visited.get_mut(h) {
|
||||
*v = true;
|
||||
}
|
||||
let Some((seg, _, to)) = half(h) else {
|
||||
ok = false;
|
||||
break;
|
||||
};
|
||||
segs.push(seg);
|
||||
if let Some(e) = edges.get(h / 2)
|
||||
&& let Some(c) = sk.curves.get(e.curve)
|
||||
{
|
||||
ids.push(c.id.clone());
|
||||
}
|
||||
let back = (-seg.end_tangent()).angle();
|
||||
let twin = h ^ 1;
|
||||
let mut best: Option<(f64, usize)> = None;
|
||||
for &o in outgoing.get(to).map(Vec::as_slice).unwrap_or(&[]) {
|
||||
if o == twin {
|
||||
continue;
|
||||
}
|
||||
let Some((os, _, _)) = half(o) else { continue };
|
||||
let mut d = (back - os.start_tangent().angle()) % TAU;
|
||||
if d <= 1e-12 {
|
||||
d += TAU;
|
||||
}
|
||||
if best.is_none_or(|(bd, _)| d < bd) {
|
||||
best = Some((d, o));
|
||||
}
|
||||
}
|
||||
let Some((_, next)) = best else {
|
||||
ok = false;
|
||||
break;
|
||||
};
|
||||
h = next;
|
||||
if h == start {
|
||||
break;
|
||||
}
|
||||
if visited.get(h).copied().unwrap_or(true) {
|
||||
ok = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if !ok || h != start {
|
||||
continue;
|
||||
}
|
||||
let lp = Loop2 { segs };
|
||||
if lp.signed_area() > 1e-9 {
|
||||
let c = edges.get(start / 2).map(|e| find(&mut comp, e.u)).unwrap_or(0);
|
||||
loops.push((lp, ids, c));
|
||||
}
|
||||
}
|
||||
|
||||
// Nesting: parent = smallest containing loop from another component.
|
||||
let areas: Vec<f64> = loops.iter().map(|(l, _, _)| l.signed_area().abs()).collect();
|
||||
let mut parent_of: Vec<Option<usize>> = vec![None; loops.len()];
|
||||
for (i, (li, _, ci)) in loops.iter().enumerate() {
|
||||
let probe = li.segs.first().map(Seg2::mid).unwrap_or_default();
|
||||
let ai = areas.get(i).copied().unwrap_or(0.0);
|
||||
let mut best: Option<(f64, usize)> = None;
|
||||
for (j, (lj, _, cj)) in loops.iter().enumerate() {
|
||||
let aj = areas.get(j).copied().unwrap_or(0.0);
|
||||
if i == j || ci == cj || aj <= ai {
|
||||
continue;
|
||||
}
|
||||
if lj.contains(probe) && best.is_none_or(|(ba, _)| aj < ba) {
|
||||
best = Some((aj, j));
|
||||
}
|
||||
}
|
||||
if let Some(p) = parent_of.get_mut(i) {
|
||||
*p = best.map(|(_, j)| j);
|
||||
}
|
||||
}
|
||||
let mut out: Vec<Profile> = loops
|
||||
.iter()
|
||||
.enumerate()
|
||||
.map(|(i, (l, ids, _))| {
|
||||
let kids: Vec<usize> = (0..loops.len()).filter(|k| parent_of.get(*k).copied().flatten() == Some(i)).collect();
|
||||
let holes: Vec<Loop2> = kids.iter().filter_map(|k| loops.get(*k).map(|(h, _, _)| h.ccw().reversed())).collect();
|
||||
let hole_curves: Vec<Vec<String>> = kids.iter().filter_map(|k| loops.get(*k).map(|(_, ids, _)| ids.clone())).collect();
|
||||
let region = Region2 { outer: l.ccw(), holes };
|
||||
let area = region.area();
|
||||
let centroid = region.centroid();
|
||||
Profile { region, outer_curves: ids.clone(), hole_curves, area, centroid }
|
||||
})
|
||||
.collect();
|
||||
let first_curve = |p: &Profile| p.outer_curves.iter().filter_map(|id| sk.curve_index(id)).min().unwrap_or(usize::MAX);
|
||||
out.sort_by(|a, b| first_curve(a).cmp(&first_curve(b)).then(b.area.total_cmp(&a.area)));
|
||||
out
|
||||
}
|
||||
@@ -0,0 +1,636 @@
|
||||
//! The constraint solver.
|
||||
//!
|
||||
//! Unknowns are the coordinates of every non-fixed point and the radius of every circle. Each
|
||||
//! constraint contributes residuals (zero when satisfied, in millimetres so that all rows have a
|
||||
//! comparable scale). The system is split into connected components and each is solved with a
|
||||
//! damped Gauss–Newton (Levenberg–Marquardt) iteration on a dense Jacobian built from local
|
||||
//! central differences. Small steps from the current positions are preferred, so
|
||||
//! under-constrained sketches move as little as possible. Afterwards the rank of the Jacobian
|
||||
//! gives the remaining degrees of freedom and which points are fully determined.
|
||||
|
||||
use serde::{Deserialize, Serialize};
|
||||
use solvecraft_geom::Vec2;
|
||||
|
||||
use crate::linalg::{Mat, determined_vars, solve as lin_solve};
|
||||
use crate::model::{ConstraintKind, CurveKind, Sketch};
|
||||
|
||||
const MAX_ITERS: usize = 200;
|
||||
/// Converged when every residual is below this (mm).
|
||||
const TOL: f64 = 1e-9;
|
||||
/// A solution is accepted when every residual is below this (mm).
|
||||
const ACCEPT: f64 = 1e-6;
|
||||
|
||||
#[derive(Clone, Copy, Debug, PartialEq, Eq, Serialize, Deserialize)]
|
||||
#[serde(rename_all = "snake_case")]
|
||||
pub enum SolveStatus {
|
||||
/// All constraints satisfied.
|
||||
Solved,
|
||||
/// The constraints conflict (or the solver could not satisfy them); geometry was left as it
|
||||
/// was before solving.
|
||||
Failed,
|
||||
}
|
||||
|
||||
#[derive(Clone, Debug, Serialize, Deserialize)]
|
||||
pub struct SolveReport {
|
||||
pub status: SolveStatus,
|
||||
/// Remaining degrees of freedom (0 = fully constrained).
|
||||
pub dof: usize,
|
||||
pub iterations: usize,
|
||||
pub max_residual: f64,
|
||||
/// Constraints still violated after a failed solve.
|
||||
pub failing: Vec<String>,
|
||||
/// Per point: fully determined by constraints (fixed points count as determined).
|
||||
pub point_determined: Vec<bool>,
|
||||
/// Per curve: all of its defining values determined.
|
||||
pub curve_determined: Vec<bool>,
|
||||
}
|
||||
|
||||
impl SolveReport {
|
||||
pub fn ok(&self) -> bool {
|
||||
self.status == SolveStatus::Solved
|
||||
}
|
||||
pub fn fully_constrained(&self) -> bool {
|
||||
self.dof == 0
|
||||
}
|
||||
}
|
||||
|
||||
/// Variable layout: point → (x var, y var), circle → radius var.
|
||||
struct Layout {
|
||||
pvar: Vec<Option<usize>>,
|
||||
rvar: Vec<Option<usize>>,
|
||||
n: usize,
|
||||
}
|
||||
|
||||
fn layout(sk: &Sketch) -> Layout {
|
||||
let mut fixed: Vec<bool> = sk.points.iter().map(|p| p.fixed).collect();
|
||||
for c in &sk.constraints {
|
||||
if let ConstraintKind::Fix { p } = c.kind
|
||||
&& let Some(f) = fixed.get_mut(p)
|
||||
{
|
||||
*f = true;
|
||||
}
|
||||
}
|
||||
let mut n = 0;
|
||||
let pvar = fixed
|
||||
.iter()
|
||||
.map(|f| {
|
||||
if *f {
|
||||
None
|
||||
} else {
|
||||
n += 2;
|
||||
Some(n - 2)
|
||||
}
|
||||
})
|
||||
.collect();
|
||||
let rvar = sk
|
||||
.curves
|
||||
.iter()
|
||||
.map(|c| match c.kind {
|
||||
CurveKind::Circle { .. } => {
|
||||
n += 1;
|
||||
Some(n - 1)
|
||||
}
|
||||
_ => None,
|
||||
})
|
||||
.collect();
|
||||
Layout { pvar, rvar, n }
|
||||
}
|
||||
|
||||
/// Read-only view of the sketch with variable values substituted.
|
||||
struct State<'a> {
|
||||
sk: &'a Sketch,
|
||||
lay: &'a Layout,
|
||||
x: &'a [f64],
|
||||
}
|
||||
|
||||
impl State<'_> {
|
||||
fn p(&self, i: usize) -> Vec2 {
|
||||
match self.lay.pvar.get(i).copied().flatten() {
|
||||
Some(v) => Vec2::new(self.x.get(v).copied().unwrap_or(0.0), self.x.get(v + 1).copied().unwrap_or(0.0)),
|
||||
None => self.sk.points.get(i).map(|p| p.pos).unwrap_or_default(),
|
||||
}
|
||||
}
|
||||
fn line(&self, l: usize) -> (Vec2, Vec2) {
|
||||
match self.sk.curves.get(l).map(|c| &c.kind) {
|
||||
Some(CurveKind::Line { a, b }) => (self.p(*a), self.p(*b)),
|
||||
_ => (Vec2::ZERO, Vec2::X),
|
||||
}
|
||||
}
|
||||
/// Centre and radius of a circle or arc.
|
||||
fn round(&self, c: usize) -> (Vec2, f64) {
|
||||
match self.sk.curves.get(c).map(|c| &c.kind) {
|
||||
Some(CurveKind::Circle { c: ci, r }) => {
|
||||
let r = match self.lay.rvar.get(c).copied().flatten() {
|
||||
Some(v) => self.x.get(v).copied().unwrap_or(*r),
|
||||
None => *r,
|
||||
};
|
||||
(self.p(*ci), r)
|
||||
}
|
||||
Some(CurveKind::Arc { c: ci, a, .. }) => {
|
||||
let cc = self.p(*ci);
|
||||
(cc, cc.dist(self.p(*a)))
|
||||
}
|
||||
_ => (Vec2::ZERO, 0.0),
|
||||
}
|
||||
}
|
||||
fn is_line(&self, c: usize) -> bool {
|
||||
matches!(self.sk.curves.get(c).map(|c| &c.kind), Some(CurveKind::Line { .. }))
|
||||
}
|
||||
}
|
||||
|
||||
fn wrap_angle(a: f64) -> f64 {
|
||||
let t = std::f64::consts::TAU;
|
||||
let mut a = a % t;
|
||||
if a > std::f64::consts::PI {
|
||||
a -= t;
|
||||
} else if a <= -std::f64::consts::PI {
|
||||
a += t;
|
||||
}
|
||||
a
|
||||
}
|
||||
|
||||
/// Distance from `p` to the infinite line through `a`, `b` (signed by side).
|
||||
fn line_dist(p: Vec2, a: Vec2, b: Vec2) -> f64 {
|
||||
let d = b - a;
|
||||
let l = d.len().max(1e-12);
|
||||
d.cross(p - a) / l
|
||||
}
|
||||
|
||||
/// Residual rows of one constraint.
|
||||
fn residuals(s: &State, k: &ConstraintKind, out: &mut Vec<f64>) {
|
||||
use ConstraintKind::*;
|
||||
match *k {
|
||||
Coincident { p, q } => {
|
||||
let d = s.p(p) - s.p(q);
|
||||
out.extend([d.x, d.y]);
|
||||
}
|
||||
PointOnCurve { p, c } => {
|
||||
let pp = s.p(p);
|
||||
if s.is_line(c) {
|
||||
let (a, b) = s.line(c);
|
||||
out.push(line_dist(pp, a, b));
|
||||
} else {
|
||||
let (cc, r) = s.round(c);
|
||||
out.push(pp.dist(cc) - r);
|
||||
}
|
||||
}
|
||||
Horizontal { l } => {
|
||||
let (a, b) = s.line(l);
|
||||
out.push(b.y - a.y);
|
||||
}
|
||||
Vertical { l } => {
|
||||
let (a, b) = s.line(l);
|
||||
out.push(b.x - a.x);
|
||||
}
|
||||
HorizontalPoints { p, q } => out.push(s.p(q).y - s.p(p).y),
|
||||
VerticalPoints { p, q } => out.push(s.p(q).x - s.p(p).x),
|
||||
Parallel { a, b } | Perpendicular { a, b } => {
|
||||
let (a0, a1) = s.line(a);
|
||||
let (b0, b1) = s.line(b);
|
||||
let (da, db) = (a1 - a0, b1 - b0);
|
||||
let scale = (da.len() + db.len()).max(1e-12);
|
||||
let v = if matches!(k, Parallel { .. }) { da.cross(db) } else { da.dot(db) };
|
||||
out.push(v / scale);
|
||||
}
|
||||
Collinear { a, b } => {
|
||||
let (a0, a1) = s.line(a);
|
||||
let (b0, b1) = s.line(b);
|
||||
out.push(line_dist(b0, a0, a1));
|
||||
out.push(line_dist(b1, a0, a1));
|
||||
}
|
||||
Tangent { a, b } => {
|
||||
let (la, lb) = (s.is_line(a), s.is_line(b));
|
||||
if la || lb {
|
||||
let (l, c) = if la { (a, b) } else { (b, a) };
|
||||
let (p0, p1) = s.line(l);
|
||||
let (cc, r) = s.round(c);
|
||||
out.push(line_dist(cc, p0, p1).abs() - r);
|
||||
} else {
|
||||
let (c1, r1) = s.round(a);
|
||||
let (c2, r2) = s.round(b);
|
||||
let d = c1.dist(c2);
|
||||
let ext = d - (r1 + r2);
|
||||
let int = d - (r1 - r2).abs();
|
||||
out.push(if ext.abs() <= int.abs() { ext } else { int });
|
||||
}
|
||||
}
|
||||
Equal { a, b } => {
|
||||
if s.is_line(a) {
|
||||
let (a0, a1) = s.line(a);
|
||||
let (b0, b1) = s.line(b);
|
||||
out.push(a0.dist(a1) - b0.dist(b1));
|
||||
} else {
|
||||
out.push(s.round(a).1 - s.round(b).1);
|
||||
}
|
||||
}
|
||||
Concentric { a, b } => {
|
||||
let d = s.round(a).0 - s.round(b).0;
|
||||
out.extend([d.x, d.y]);
|
||||
}
|
||||
Midpoint { p, l } => {
|
||||
let (a, b) = s.line(l);
|
||||
let d = s.p(p) - (a + b) * 0.5;
|
||||
out.extend([d.x, d.y]);
|
||||
}
|
||||
Symmetric { p, q, l } => {
|
||||
let (a, b) = s.line(l);
|
||||
let (pp, qq) = (s.p(p), s.p(q));
|
||||
let dir = b - a;
|
||||
let len = dir.len().max(1e-12);
|
||||
out.push(line_dist((pp + qq) * 0.5, a, b));
|
||||
out.push((qq - pp).dot(dir) / len);
|
||||
}
|
||||
Fix { .. } => {}
|
||||
Distance { p, q, value } => out.push(s.p(p).dist(s.p(q)) - value),
|
||||
DistanceX { p, q, value } => out.push((s.p(q).x - s.p(p).x).abs() - value),
|
||||
DistanceY { p, q, value } => out.push((s.p(q).y - s.p(p).y).abs() - value),
|
||||
PointLineDistance { p, l, value } => {
|
||||
let (a, b) = s.line(l);
|
||||
out.push(line_dist(s.p(p), a, b).abs() - value);
|
||||
}
|
||||
Length { l, value } => {
|
||||
let (a, b) = s.line(l);
|
||||
out.push(a.dist(b) - value);
|
||||
}
|
||||
Radius { c, value } => out.push(s.round(c).1 - value),
|
||||
Diameter { c, value } => out.push(2.0 * s.round(c).1 - value),
|
||||
Angle { a, b, value } => {
|
||||
let (a0, a1) = s.line(a);
|
||||
let (b0, b1) = s.line(b);
|
||||
let (da, db) = (a1 - a0, b1 - b0);
|
||||
let ang = da.cross(db).atan2(da.dot(db));
|
||||
let scale = 0.5 * (da.len() + db.len());
|
||||
out.push(wrap_angle(ang - value) * scale.max(1e-6));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// One residual block: implicit arc radius equality or a user constraint.
|
||||
enum Block<'a> {
|
||||
Arc { c: usize, a: usize, b: usize },
|
||||
User(usize, &'a ConstraintKind),
|
||||
}
|
||||
|
||||
fn block_residuals(s: &State, b: &Block, out: &mut Vec<f64>) {
|
||||
match b {
|
||||
Block::Arc { c, a, b } => {
|
||||
let cc = s.p(*c);
|
||||
out.push(cc.dist(s.p(*b)) - cc.dist(s.p(*a)));
|
||||
}
|
||||
Block::User(_, k) => residuals(s, k, out),
|
||||
}
|
||||
}
|
||||
|
||||
/// Variables a block depends on.
|
||||
fn block_vars(sk: &Sketch, lay: &Layout, b: &Block) -> Vec<usize> {
|
||||
let mut v = Vec::new();
|
||||
let pt = |v: &mut Vec<usize>, p: usize| {
|
||||
if let Some(x) = lay.pvar.get(p).copied().flatten() {
|
||||
v.extend([x, x + 1]);
|
||||
}
|
||||
};
|
||||
let curve = |v: &mut Vec<usize>, c: usize| match sk.curves.get(c).map(|c| &c.kind) {
|
||||
Some(CurveKind::Line { a, b }) => {
|
||||
pt(v, *a);
|
||||
pt(v, *b);
|
||||
}
|
||||
Some(CurveKind::Circle { c: ci, .. }) => {
|
||||
pt(v, *ci);
|
||||
if let Some(r) = lay.rvar.get(c).copied().flatten() {
|
||||
v.push(r);
|
||||
}
|
||||
}
|
||||
Some(CurveKind::Arc { c, a, b }) => {
|
||||
pt(v, *c);
|
||||
pt(v, *a);
|
||||
pt(v, *b);
|
||||
}
|
||||
None => {}
|
||||
};
|
||||
use ConstraintKind::*;
|
||||
match b {
|
||||
Block::Arc { c, a, b } => {
|
||||
pt(&mut v, *c);
|
||||
pt(&mut v, *a);
|
||||
pt(&mut v, *b);
|
||||
}
|
||||
Block::User(_, k) => match **k {
|
||||
Coincident { p, q } | HorizontalPoints { p, q } | VerticalPoints { p, q } => {
|
||||
pt(&mut v, p);
|
||||
pt(&mut v, q);
|
||||
}
|
||||
Distance { p, q, .. } | DistanceX { p, q, .. } | DistanceY { p, q, .. } => {
|
||||
pt(&mut v, p);
|
||||
pt(&mut v, q);
|
||||
}
|
||||
PointOnCurve { p, c } | Midpoint { p, l: c } | PointLineDistance { p, l: c, .. } => {
|
||||
pt(&mut v, p);
|
||||
curve(&mut v, c);
|
||||
}
|
||||
Horizontal { l } | Vertical { l } | Length { l, .. } | Radius { c: l, .. } | Diameter { c: l, .. } => curve(&mut v, l),
|
||||
Parallel { a, b }
|
||||
| Perpendicular { a, b }
|
||||
| Collinear { a, b }
|
||||
| Tangent { a, b }
|
||||
| Equal { a, b }
|
||||
| Concentric { a, b }
|
||||
| Angle { a, b, .. } => {
|
||||
curve(&mut v, a);
|
||||
curve(&mut v, b);
|
||||
}
|
||||
Symmetric { p, q, l } => {
|
||||
pt(&mut v, p);
|
||||
pt(&mut v, q);
|
||||
curve(&mut v, l);
|
||||
}
|
||||
Fix { .. } => {}
|
||||
},
|
||||
}
|
||||
v.sort_unstable();
|
||||
v.dedup();
|
||||
v
|
||||
}
|
||||
|
||||
fn find(parent: &mut [usize], mut i: usize) -> usize {
|
||||
while let Some(&p) = parent.get(i) {
|
||||
if p == i {
|
||||
break;
|
||||
}
|
||||
let gp = parent.get(p).copied().unwrap_or(p);
|
||||
if let Some(slot) = parent.get_mut(i) {
|
||||
*slot = gp;
|
||||
}
|
||||
i = p;
|
||||
}
|
||||
i
|
||||
}
|
||||
|
||||
/// Residual vector and Jacobian (rows × all n vars) for the given blocks.
|
||||
fn system(sk: &Sketch, lay: &Layout, blocks: &[(Block, Vec<usize>)], x: &mut [f64], with_j: bool) -> (Vec<f64>, Option<Mat>, Vec<usize>) {
|
||||
let mut r = Vec::new();
|
||||
let mut owner = Vec::new();
|
||||
for (bi, (b, _)) in blocks.iter().enumerate() {
|
||||
let before = r.len();
|
||||
block_residuals(&State { sk, lay, x }, b, &mut r);
|
||||
owner.extend(std::iter::repeat_n(bi, r.len() - before));
|
||||
}
|
||||
if !with_j {
|
||||
return (r, None, owner);
|
||||
}
|
||||
let mut j = Mat::zeros(r.len(), lay.n);
|
||||
let mut row = 0;
|
||||
let mut plus = Vec::new();
|
||||
let mut minus = Vec::new();
|
||||
for (b, vars) in blocks {
|
||||
plus.clear();
|
||||
block_residuals(&State { sk, lay, x }, b, &mut plus);
|
||||
let rows = plus.len();
|
||||
for &v in vars {
|
||||
let Some(x0) = x.get(v).copied() else { continue };
|
||||
let h = 1e-7 * x0.abs().max(1.0);
|
||||
if let Some(xv) = x.get_mut(v) {
|
||||
*xv = x0 + h;
|
||||
}
|
||||
plus.clear();
|
||||
block_residuals(&State { sk, lay, x }, b, &mut plus);
|
||||
if let Some(xv) = x.get_mut(v) {
|
||||
*xv = x0 - h;
|
||||
}
|
||||
minus.clear();
|
||||
block_residuals(&State { sk, lay, x }, b, &mut minus);
|
||||
if let Some(xv) = x.get_mut(v) {
|
||||
*xv = x0;
|
||||
}
|
||||
for k in 0..rows {
|
||||
let d = (plus.get(k).copied().unwrap_or(0.0) - minus.get(k).copied().unwrap_or(0.0)) / (2.0 * h);
|
||||
j.set(row + k, v, d);
|
||||
}
|
||||
}
|
||||
row += rows;
|
||||
}
|
||||
(r, Some(j), owner)
|
||||
}
|
||||
|
||||
fn max_abs(v: &[f64]) -> f64 {
|
||||
v.iter().fold(0.0_f64, |m, x| if x.is_finite() { m.max(x.abs()) } else { f64::INFINITY })
|
||||
}
|
||||
fn norm2(v: &[f64]) -> f64 {
|
||||
v.iter().map(|x| x * x).sum()
|
||||
}
|
||||
|
||||
/// Levenberg–Marquardt on one component (variables `vars`, blocks `blocks`). Returns iterations.
|
||||
fn lm(sk: &Sketch, lay: &Layout, blocks: &[(Block, Vec<usize>)], vars: &[usize], x: &mut Vec<f64>) -> usize {
|
||||
let n = vars.len();
|
||||
let mut lambda = 1e-3;
|
||||
let (mut r, _, _) = system(sk, lay, blocks, x, false);
|
||||
let mut f = norm2(&r);
|
||||
for it in 0..MAX_ITERS {
|
||||
if max_abs(&r) < TOL {
|
||||
return it;
|
||||
}
|
||||
let (_, Some(j), _) = system(sk, lay, blocks, x, true) else { return it };
|
||||
// Normal equations restricted to this component's variables.
|
||||
let mut a = Mat::zeros(n, n);
|
||||
let mut g = vec![0.0; n];
|
||||
for row in 0..j.rows {
|
||||
let ri = r.get(row).copied().unwrap_or(0.0);
|
||||
let nz: Vec<(usize, f64)> = vars.iter().enumerate().map(|(li, &v)| (li, j.get(row, v))).filter(|(_, d)| *d != 0.0).collect();
|
||||
for &(li, di) in &nz {
|
||||
if let Some(gi) = g.get_mut(li) {
|
||||
*gi += di * ri;
|
||||
}
|
||||
for &(lk, dk) in &nz {
|
||||
a.add(li, lk, di * dk);
|
||||
}
|
||||
}
|
||||
}
|
||||
let mut improved = false;
|
||||
for _ in 0..30 {
|
||||
let mut m = a.clone();
|
||||
for i in 0..n {
|
||||
let d = a.get(i, i);
|
||||
m.add(i, i, lambda * (d + 1e-9) + 1e-12);
|
||||
}
|
||||
let rhs: Vec<f64> = g.iter().map(|v| -v).collect();
|
||||
let Some(step) = lin_solve(m, rhs) else {
|
||||
lambda *= 10.0;
|
||||
continue;
|
||||
};
|
||||
let mut trial = x.clone();
|
||||
for (li, &v) in vars.iter().enumerate() {
|
||||
if let (Some(t), Some(s)) = (trial.get_mut(v), step.get(li)) {
|
||||
*t += s;
|
||||
}
|
||||
}
|
||||
let (rt, _, _) = system(sk, lay, blocks, &mut trial, false);
|
||||
let ft = norm2(&rt);
|
||||
if ft.is_finite() && ft < f {
|
||||
*x = trial;
|
||||
r = rt;
|
||||
f = ft;
|
||||
lambda = (lambda / 3.0).max(1e-12);
|
||||
improved = true;
|
||||
break;
|
||||
}
|
||||
lambda *= 4.0;
|
||||
if lambda > 1e12 {
|
||||
break;
|
||||
}
|
||||
}
|
||||
if !improved {
|
||||
return it;
|
||||
}
|
||||
}
|
||||
MAX_ITERS
|
||||
}
|
||||
|
||||
/// Solve the sketch in place. On failure the geometry is restored and the report lists the
|
||||
/// violated constraints.
|
||||
pub fn solve(sk: &mut Sketch) -> SolveReport {
|
||||
let lay = layout(sk);
|
||||
let mut x = vec![0.0; lay.n];
|
||||
for (i, p) in sk.points.iter().enumerate() {
|
||||
if let Some(v) = lay.pvar.get(i).copied().flatten() {
|
||||
if let Some(s) = x.get_mut(v) {
|
||||
*s = p.pos.x;
|
||||
}
|
||||
if let Some(s) = x.get_mut(v + 1) {
|
||||
*s = p.pos.y;
|
||||
}
|
||||
}
|
||||
}
|
||||
for (i, c) in sk.curves.iter().enumerate() {
|
||||
if let (CurveKind::Circle { r, .. }, Some(v)) = (&c.kind, lay.rvar.get(i).copied().flatten())
|
||||
&& let Some(s) = x.get_mut(v)
|
||||
{
|
||||
*s = *r;
|
||||
}
|
||||
}
|
||||
let x0 = x.clone();
|
||||
|
||||
let mut blocks: Vec<(Block, Vec<usize>)> = Vec::new();
|
||||
for c in &sk.curves {
|
||||
if let CurveKind::Arc { c, a, b } = c.kind {
|
||||
let bl = Block::Arc { c, a, b };
|
||||
let vars = block_vars(sk, &lay, &bl);
|
||||
blocks.push((bl, vars));
|
||||
}
|
||||
}
|
||||
for (i, c) in sk.constraints.iter().enumerate() {
|
||||
let bl = Block::User(i, &c.kind);
|
||||
let vars = block_vars(sk, &lay, &bl);
|
||||
blocks.push((bl, vars));
|
||||
}
|
||||
|
||||
// Connected components over variables.
|
||||
let mut parent: Vec<usize> = (0..lay.n).collect();
|
||||
for (_, vars) in &blocks {
|
||||
if let Some(&first) = vars.first() {
|
||||
for &v in vars.iter().skip(1) {
|
||||
let (ra, rb) = (find(&mut parent, first), find(&mut parent, v));
|
||||
if let Some(p) = parent.get_mut(ra) {
|
||||
*p = rb;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
let mut comps: Vec<(usize, Vec<usize>, Vec<usize>)> = Vec::new(); // (root, vars, block idx)
|
||||
for v in 0..lay.n {
|
||||
let r = find(&mut parent, v);
|
||||
match comps.iter_mut().find(|c| c.0 == r) {
|
||||
Some(c) => c.1.push(v),
|
||||
None => comps.push((r, vec![v], Vec::new())),
|
||||
}
|
||||
}
|
||||
let mut constant_blocks = Vec::new();
|
||||
for (bi, (_, vars)) in blocks.iter().enumerate() {
|
||||
match vars.first() {
|
||||
Some(&v) => {
|
||||
let r = find(&mut parent, v);
|
||||
if let Some(c) = comps.iter_mut().find(|c| c.0 == r) {
|
||||
c.2.push(bi);
|
||||
}
|
||||
}
|
||||
None => constant_blocks.push(bi),
|
||||
}
|
||||
}
|
||||
|
||||
let mut iterations = 0;
|
||||
// Solve each component; `blocks` is consumed by index so rebuild per component.
|
||||
let all_blocks = blocks;
|
||||
for (_, vars, bis) in &comps {
|
||||
if bis.is_empty() {
|
||||
continue;
|
||||
}
|
||||
let sub: Vec<(Block, Vec<usize>)> = bis
|
||||
.iter()
|
||||
.filter_map(|&bi| {
|
||||
all_blocks.get(bi).map(|(b, v)| {
|
||||
let b2 = match b {
|
||||
Block::Arc { c, a, b } => Block::Arc { c: *c, a: *a, b: *b },
|
||||
Block::User(i, k) => Block::User(*i, k),
|
||||
};
|
||||
(b2, v.clone())
|
||||
})
|
||||
})
|
||||
.collect();
|
||||
iterations += lm(sk, &lay, &sub, vars, &mut x);
|
||||
}
|
||||
|
||||
// Final residuals, Jacobian and analysis over everything.
|
||||
let (r, j, owner) = system(sk, &lay, &all_blocks, &mut x, true);
|
||||
let max_residual = max_abs(&r);
|
||||
let mut failing: Vec<String> = Vec::new();
|
||||
for (row, v) in r.iter().enumerate() {
|
||||
if !(v.abs() < ACCEPT)
|
||||
&& let Some((Block::User(ci, _), _)) = owner.get(row).and_then(|bi| all_blocks.get(*bi))
|
||||
&& let Some(c) = sk.constraints.get(*ci)
|
||||
&& !failing.contains(&c.id)
|
||||
{
|
||||
failing.push(c.id.clone());
|
||||
}
|
||||
}
|
||||
let _ = constant_blocks;
|
||||
let status = if max_residual < ACCEPT { SolveStatus::Solved } else { SolveStatus::Failed };
|
||||
let use_x = if status == SolveStatus::Solved { &x } else { &x0 };
|
||||
let (det, rank) = match &j {
|
||||
Some(j) if j.rows > 0 => determined_vars(j, 1e-7),
|
||||
_ => (vec![false; lay.n], 0),
|
||||
};
|
||||
// Write back.
|
||||
for (i, p) in sk.points.iter_mut().enumerate() {
|
||||
if let Some(v) = lay.pvar.get(i).copied().flatten() {
|
||||
p.pos = Vec2::new(use_x.get(v).copied().unwrap_or(p.pos.x), use_x.get(v + 1).copied().unwrap_or(p.pos.y));
|
||||
}
|
||||
}
|
||||
for (i, c) in sk.curves.iter_mut().enumerate() {
|
||||
if let (CurveKind::Circle { r, .. }, Some(v)) = (&mut c.kind, lay.rvar.get(i).copied().flatten()) {
|
||||
*r = use_x.get(v).copied().unwrap_or(*r).abs().max(1e-9);
|
||||
}
|
||||
}
|
||||
let vdet = |v: Option<usize>| v.is_none_or(|v| det.get(v).copied().unwrap_or(false));
|
||||
let pdet = |p: usize| match lay.pvar.get(p).copied().flatten() {
|
||||
Some(v) => vdet(Some(v)) && vdet(Some(v + 1)),
|
||||
None => true,
|
||||
};
|
||||
let point_determined: Vec<bool> = (0..sk.points.len()).map(pdet).collect();
|
||||
let curve_determined = sk
|
||||
.curves
|
||||
.iter()
|
||||
.enumerate()
|
||||
.map(|(i, c)| match c.kind {
|
||||
CurveKind::Line { a, b } => pdet(a) && pdet(b),
|
||||
CurveKind::Circle { c, .. } => pdet(c) && vdet(lay.rvar.get(i).copied().flatten()),
|
||||
CurveKind::Arc { c, a, b } => pdet(c) && pdet(a) && pdet(b),
|
||||
})
|
||||
.collect();
|
||||
SolveReport {
|
||||
status,
|
||||
dof: lay.n.saturating_sub(rank),
|
||||
iterations,
|
||||
max_residual: if max_residual.is_finite() { max_residual } else { f64::MAX },
|
||||
failing,
|
||||
point_determined,
|
||||
curve_determined,
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,252 @@
|
||||
use std::f64::consts::{FRAC_PI_2, PI};
|
||||
|
||||
use solvecraft_geom::Vec2;
|
||||
|
||||
use crate::*;
|
||||
|
||||
fn v(x: f64, y: f64) -> Vec2 {
|
||||
Vec2::new(x, y)
|
||||
}
|
||||
|
||||
/// Rough rectangle from four lines sharing corner points.
|
||||
fn rect(sk: &mut Sketch, p: [Vec2; 4]) -> [usize; 4] {
|
||||
let a = sk.add_point(p[0], None).unwrap();
|
||||
let b = sk.add_point(p[1], None).unwrap();
|
||||
let c = sk.add_point(p[2], None).unwrap();
|
||||
let d = sk.add_point(p[3], None).unwrap();
|
||||
[
|
||||
sk.add_line_pts(a, b, None).unwrap(),
|
||||
sk.add_line_pts(b, c, None).unwrap(),
|
||||
sk.add_line_pts(c, d, None).unwrap(),
|
||||
sk.add_line_pts(d, a, None).unwrap(),
|
||||
]
|
||||
}
|
||||
|
||||
fn line_pts(sk: &Sketch, l: usize) -> (Vec2, Vec2) {
|
||||
match sk.curves[l].kind {
|
||||
CurveKind::Line { a, b } => (sk.points[a].pos, sk.points[b].pos),
|
||||
_ => panic!(),
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn fully_constrained_rectangle() {
|
||||
let mut sk = Sketch::new();
|
||||
let l = rect(&mut sk, [v(0.3, -0.2), v(38.0, 1.0), v(41.0, 29.0), v(-1.0, 31.0)]);
|
||||
use ConstraintKind::*;
|
||||
sk.add_constraint(Horizontal { l: l[0] }, None).unwrap();
|
||||
sk.add_constraint(Horizontal { l: l[2] }, None).unwrap();
|
||||
sk.add_constraint(Vertical { l: l[1] }, None).unwrap();
|
||||
sk.add_constraint(Vertical { l: l[3] }, None).unwrap();
|
||||
let (a, _) = match sk.curves[l[0]].kind {
|
||||
CurveKind::Line { a, b } => (a, b),
|
||||
_ => panic!(),
|
||||
};
|
||||
sk.add_constraint(Coincident { p: a, q: 0 }, None).unwrap();
|
||||
sk.add_constraint(Length { l: l[0], value: 40.0 }, Some("d1".into())).unwrap();
|
||||
sk.add_constraint(Length { l: l[1], value: 30.0 }, Some("d2".into())).unwrap();
|
||||
let rep = solve(&mut sk);
|
||||
assert!(rep.ok(), "{rep:?}");
|
||||
assert_eq!(rep.dof, 0, "{rep:?}");
|
||||
assert!(rep.point_determined.iter().all(|d| *d));
|
||||
let (p0, p1) = line_pts(&sk, l[0]);
|
||||
assert!(p0.dist(v(0.0, 0.0)) < 1e-7, "{p0:?}");
|
||||
assert!(p1.dist(v(40.0, 0.0)) < 1e-7, "{p1:?}");
|
||||
let (_, p2) = line_pts(&sk, l[1]);
|
||||
assert!(p2.dist(v(40.0, 30.0)) < 1e-7, "{p2:?}");
|
||||
// Change a dimension and re-solve.
|
||||
sk.constraints[5].kind.set_value(55.0);
|
||||
assert!(solve(&mut sk).ok());
|
||||
let (_, p1) = line_pts(&sk, l[0]);
|
||||
assert!(p1.dist(v(55.0, 0.0)) < 1e-7);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn under_constrained_reports_dof() {
|
||||
let mut sk = Sketch::new();
|
||||
let l = sk.add_line(v(0.0, 0.0), v(10.0, 1.0), None, None, None).unwrap();
|
||||
sk.add_constraint(ConstraintKind::Horizontal { l }, None).unwrap();
|
||||
let rep = solve(&mut sk);
|
||||
assert!(rep.ok());
|
||||
assert_eq!(rep.dof, 3);
|
||||
let (a, b) = line_pts(&sk, l);
|
||||
assert!((a.y - b.y).abs() < 1e-9);
|
||||
// Minimal movement: the line moved by about half a unit each.
|
||||
assert!((a.y - 0.5).abs() < 0.05 && (b.y - 0.5).abs() < 0.05, "{a:?} {b:?}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn conflicting_constraints_fail_and_restore() {
|
||||
let mut sk = Sketch::new();
|
||||
let l = sk.add_line(v(0.0, 0.0), v(10.0, 0.0), None, None, None).unwrap();
|
||||
sk.add_constraint(ConstraintKind::Horizontal { l }, None).unwrap();
|
||||
sk.add_constraint(ConstraintKind::Vertical { l }, None).unwrap();
|
||||
sk.add_constraint(ConstraintKind::Length { l, value: 10.0 }, None).unwrap();
|
||||
let before = sk.clone();
|
||||
let rep = solve(&mut sk);
|
||||
assert_eq!(rep.status, SolveStatus::Failed);
|
||||
assert!(!rep.failing.is_empty());
|
||||
assert_eq!(sk.points, before.points);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn circles_tangent_equal_concentric() {
|
||||
let mut sk = Sketch::new();
|
||||
let c1 = sk.add_circle(v(0.0, 0.0), 5.0, Some(0), None).unwrap();
|
||||
let c2 = sk.add_circle(v(12.0, 1.0), 3.0, None, None).unwrap();
|
||||
use ConstraintKind::*;
|
||||
sk.add_constraint(Radius { c: c1, value: 10.0 }, None).unwrap();
|
||||
sk.add_constraint(Equal { a: c1, b: c2 }, None).unwrap();
|
||||
sk.add_constraint(Tangent { a: c1, b: c2 }, None).unwrap();
|
||||
let rep = solve(&mut sk);
|
||||
assert!(rep.ok(), "{rep:?}");
|
||||
assert!((sk.radius(c2).unwrap() - 10.0).abs() < 1e-7);
|
||||
assert!((sk.center(c2).unwrap().len() - 20.0).abs() < 1e-7);
|
||||
let c3 = sk.add_circle(v(3.0, 3.0), 2.0, None, None).unwrap();
|
||||
sk.add_constraint(Concentric { a: c3, b: c2 }, None).unwrap();
|
||||
assert!(solve(&mut sk).ok());
|
||||
assert!(sk.center(c3).unwrap().dist(sk.center(c2).unwrap()) < 1e-7);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn line_tangent_to_circle_and_angles() {
|
||||
let mut sk = Sketch::new();
|
||||
let c = sk.add_circle(v(0.0, 0.0), 10.0, Some(0), None).unwrap();
|
||||
use ConstraintKind::*;
|
||||
sk.add_constraint(Radius { c, value: 10.0 }, None).unwrap();
|
||||
let l = sk.add_line(v(-20.0, 12.0), v(20.0, 11.0), None, None, None).unwrap();
|
||||
sk.add_constraint(Tangent { a: l, b: c }, None).unwrap();
|
||||
sk.add_constraint(Horizontal { l }, None).unwrap();
|
||||
assert!(solve(&mut sk).ok());
|
||||
let (a, _) = line_pts(&sk, l);
|
||||
assert!((a.y.abs() - 10.0).abs() < 1e-7, "{a:?}");
|
||||
let m = sk.add_line(v(0.0, 0.0), v(10.0, 3.0), Some(0), None, None).unwrap();
|
||||
sk.add_constraint(Angle { a: l, b: m, value: PI / 6.0 }, None).unwrap();
|
||||
sk.add_constraint(Length { l: m, value: 8.0 }, None).unwrap();
|
||||
assert!(solve(&mut sk).ok());
|
||||
let (p, q) = line_pts(&sk, m);
|
||||
let (la, lb) = line_pts(&sk, l);
|
||||
assert!(((q - p).angle() - (lb - la).angle() - PI / 6.0).abs() < 1e-7, "{la:?} {lb:?} {p:?} {q:?}");
|
||||
let n2 = sk.add_line(v(1.0, 1.0), v(2.0, 5.0), None, None, None).unwrap();
|
||||
sk.add_constraint(Perpendicular { a: m, b: n2 }, None).unwrap();
|
||||
sk.add_constraint(Parallel { a: n2, b: n2 }, None).unwrap();
|
||||
assert!(solve(&mut sk).ok());
|
||||
let (p2, q2) = line_pts(&sk, n2);
|
||||
assert!(((q2 - p2).angle() - (PI / 6.0 + FRAC_PI_2)).abs() < 1e-6 || ((q2 - p2).angle() - (PI / 6.0 - FRAC_PI_2)).abs() < 1e-6);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn midpoint_symmetry_point_on_curve_arc() {
|
||||
let mut sk = Sketch::new();
|
||||
use ConstraintKind::*;
|
||||
let l = sk.add_line(v(-5.0, 0.0), v(5.0, 0.0), None, None, None).unwrap();
|
||||
let p = sk.add_point(v(1.0, 1.0), None).unwrap();
|
||||
sk.add_constraint(Midpoint { p, l }, None).unwrap();
|
||||
let a = sk.add_arc(v(0.0, 0.0), v(4.0, 0.0), v(0.0, 4.0), [None, None, None], None).unwrap();
|
||||
sk.add_constraint(Radius { c: a, value: 6.0 }, None).unwrap();
|
||||
let q = sk.add_point(v(3.0, 3.0), None).unwrap();
|
||||
sk.add_constraint(PointOnCurve { p: q, c: a }, None).unwrap();
|
||||
let s1 = sk.add_point(v(-2.0, 1.0), None).unwrap();
|
||||
let s2 = sk.add_point(v(3.0, -1.5), None).unwrap();
|
||||
let axis = sk.add_line(v(0.0, -10.0), v(0.0, 10.0), None, None, None).unwrap();
|
||||
sk.add_constraint(Vertical { l: axis }, None).unwrap();
|
||||
sk.add_constraint(Symmetric { p: s1, q: s2, l: axis }, None).unwrap();
|
||||
let rep = solve(&mut sk);
|
||||
assert!(rep.ok(), "{rep:?}");
|
||||
let (la, lb) = line_pts(&sk, l);
|
||||
assert!(sk.points[p].pos.dist((la + lb) * 0.5) < 1e-7);
|
||||
assert!((sk.radius(a).unwrap() - 6.0).abs() < 1e-7);
|
||||
let c = sk.center(a).unwrap();
|
||||
assert!((sk.points[q].pos.dist(c) - 6.0).abs() < 1e-7);
|
||||
let (ax, _) = line_pts(&sk, axis);
|
||||
assert!((sk.points[s1].pos.x + sk.points[s2].pos.x - 2.0 * ax.x).abs() < 1e-7);
|
||||
assert!((sk.points[s1].pos.y - sk.points[s2].pos.y).abs() < 1e-7);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn profiles_rectangle_with_hole_and_split() {
|
||||
let mut sk = Sketch::new();
|
||||
rect(&mut sk, [v(0.0, 0.0), v(40.0, 0.0), v(40.0, 30.0), v(0.0, 30.0)]);
|
||||
sk.add_circle(v(20.0, 15.0), 5.0, None, None).unwrap();
|
||||
let ps = find_profiles(&sk);
|
||||
assert_eq!(ps.len(), 2, "{ps:?}");
|
||||
let big = ps.iter().find(|p| p.region.holes.len() == 1).unwrap();
|
||||
assert!((big.area - (1200.0 - PI * 25.0)).abs() < 1e-6);
|
||||
let disc = ps.iter().find(|p| p.region.holes.is_empty()).unwrap();
|
||||
assert!((disc.area - PI * 25.0).abs() < 1e-6);
|
||||
assert_eq!(big.hole_curves.len(), 1);
|
||||
|
||||
// A line across the rectangle splits it into two regions (with the hole in one).
|
||||
let mut sk2 = Sketch::new();
|
||||
let l = rect(&mut sk2, [v(0.0, 0.0), v(40.0, 0.0), v(40.0, 30.0), v(0.0, 30.0)]);
|
||||
let (b0, _) = line_pts(&sk2, l[0]);
|
||||
let _ = b0;
|
||||
let pa = sk2.add_point(v(10.0, 0.0), None).unwrap();
|
||||
let pb = sk2.add_point(v(10.0, 30.0), None).unwrap();
|
||||
// Re-split bottom and top edges at x = 10 by rebuilding the sketch as 7 lines.
|
||||
let mut s3 = Sketch::new();
|
||||
let pts = [v(0.0, 0.0), v(10.0, 0.0), v(40.0, 0.0), v(40.0, 30.0), v(10.0, 30.0), v(0.0, 30.0)];
|
||||
let ids: Vec<usize> = pts.iter().map(|p| s3.add_point(*p, None).unwrap()).collect();
|
||||
for i in 0..6 {
|
||||
s3.add_line_pts(ids[i], ids[(i + 1) % 6], None).unwrap();
|
||||
}
|
||||
s3.add_line_pts(ids[1], ids[4], None).unwrap();
|
||||
let _ = (pa, pb);
|
||||
let ps = find_profiles(&s3);
|
||||
assert_eq!(ps.len(), 2);
|
||||
let mut areas: Vec<f64> = ps.iter().map(|p| p.area).collect();
|
||||
areas.sort_by(f64::total_cmp);
|
||||
assert!((areas[0] - 300.0).abs() < 1e-9 && (areas[1] - 900.0).abs() < 1e-9, "{areas:?}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn profile_with_arcs_and_construction_ignored() {
|
||||
// Slot: two lines and two half arcs.
|
||||
let mut sk = Sketch::new();
|
||||
let p = [v(0.0, 0.0), v(20.0, 0.0), v(20.0, 10.0), v(0.0, 10.0)];
|
||||
let ids: Vec<usize> = p.iter().map(|q| sk.add_point(*q, None).unwrap()).collect();
|
||||
sk.add_line_pts(ids[0], ids[1], None).unwrap();
|
||||
sk.add_arc(v(20.0, 5.0), p[1], p[2], [None, Some(ids[1]), Some(ids[2])], None).unwrap();
|
||||
sk.add_line_pts(ids[2], ids[3], None).unwrap();
|
||||
sk.add_arc(v(0.0, 5.0), p[3], p[0], [None, Some(ids[3]), Some(ids[0])], None).unwrap();
|
||||
let c = sk.add_line(v(-5.0, -5.0), v(30.0, 20.0), None, None, None).unwrap();
|
||||
sk.curves[c].construction = true;
|
||||
let ps = find_profiles(&sk);
|
||||
assert_eq!(ps.len(), 1, "{ps:?}");
|
||||
assert!((ps[0].area - (200.0 + PI * 25.0)).abs() < 1e-6, "{}", ps[0].area);
|
||||
assert_eq!(ps[0].outer_curves.len(), 4);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn removal_and_references() {
|
||||
let mut sk = Sketch::new();
|
||||
let l1 = sk.add_line(v(0.0, 0.0), v(1.0, 0.0), None, None, Some("l1")).unwrap();
|
||||
let e = sk.resolve_point("l1.end").unwrap();
|
||||
let l2 = sk.add_line(v(1.0, 0.0), v(1.0, 1.0), Some(e), None, Some("l2")).unwrap();
|
||||
sk.add_constraint(ConstraintKind::Horizontal { l: l1 }, None).unwrap();
|
||||
sk.add_constraint(ConstraintKind::Vertical { l: l2 }, None).unwrap();
|
||||
assert!(sk.add_line(v(0.0, 0.0), v(1.0, 0.0), None, None, Some("l1")).is_err());
|
||||
sk.remove_curves(&[l1]);
|
||||
assert_eq!(sk.curves.len(), 1);
|
||||
assert_eq!(sk.constraints.len(), 1);
|
||||
assert!(sk.resolve_point("l2.start").is_some());
|
||||
assert!(sk.resolve_point("l1.start").is_none());
|
||||
assert_eq!(sk.points.len(), 3, "origin + l2's two points");
|
||||
assert!(solve(&mut sk).ok());
|
||||
let json = serde_json::to_string(&sk).unwrap();
|
||||
let back: Sketch = serde_json::from_str(&json).unwrap();
|
||||
assert_eq!(back, sk);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn hostile_values_rejected() {
|
||||
let mut sk = Sketch::new();
|
||||
assert!(sk.add_point(v(f64::NAN, 0.0), None).is_err());
|
||||
assert!(sk.add_circle(v(0.0, 0.0), -1.0, None, None).is_err());
|
||||
assert!(sk.add_circle(v(0.0, 0.0), f64::INFINITY, None, None).is_err());
|
||||
assert!(sk.add_constraint(ConstraintKind::Horizontal { l: 99 }, None).is_err());
|
||||
let c = sk.add_circle(v(0.0, 0.0), 1.0, None, None).unwrap();
|
||||
assert!(sk.add_constraint(ConstraintKind::Horizontal { l: c }, None).is_err());
|
||||
assert!(sk.add_constraint(ConstraintKind::Radius { c, value: f64::NAN }, None).is_err());
|
||||
assert!(sk.add_constraint(ConstraintKind::Radius { c, value: -2.0 }, None).is_err());
|
||||
}
|
||||
Reference in new issue
Block a user