forked from 1b-agent/opencadstudio-upstream
Harden parametric sketch constraints
This commit is contained in:
Hakan Seven committed
1 parent
41c07e9858
commit
9b58ca8a93
101 files changed
+3128
-13767
No files matched your search
@@ -15,7 +15,7 @@ thiserror = "1"
|
||||
|
||||
# Host adapters convert ObjectId to acadrust handles; kernel handles stay local.
|
||||
acadrust = { git = "https://github.com/HakanSeven12/cadcodec.git", rev = "f23ffc873228e5f41b876485c741e4a8ac20a67d", features = ["serde"], optional = true }
|
||||
cadkernel = { git = "https://github.com/HakanSeven12/cadkernel.git", rev = "0507b720fef47bd7da0d4f777f97f1743a4f4038", features = ["acis", "offset"], optional = true }
|
||||
cadkernel = { git = "https://github.com/HakanSeven12/cadkernel.git", rev = "7d12f4a87c4737bb83fdad40e613468513d38904", features = ["acis", "offset"], optional = true }
|
||||
ocs_plugin_api = { path = "../ocs_plugin_api", optional = true, features = ["host"] }
|
||||
|
||||
[build-dependencies]
|
||||
|
||||
@@ -1,13 +0,0 @@
|
||||
[package]
|
||||
name = "ocs_gcs"
|
||||
version = "0.1.0"
|
||||
edition = "2021"
|
||||
description = "2D geometric constraint solver for OpenCADStudio sketches, ported from FreeCAD's planegcs."
|
||||
license = "LGPL-2.1-or-later"
|
||||
|
||||
[dependencies]
|
||||
nalgebra = "0.33"
|
||||
|
||||
[lib]
|
||||
name = "ocs_gcs"
|
||||
path = "src/lib.rs"
|
||||
@@ -1,224 +0,0 @@
|
||||
//! Angle constraints (port stage 5a).
|
||||
//!
|
||||
//! Ported from `ConstraintP2PAngle`/`ConstraintL2LAngle` in
|
||||
//! `Constraints.h`/`Constraints.cpp`. See `point_line.rs`'s module doc for
|
||||
//! what's intentionally not ported (`evaluate()`, post-construction
|
||||
//! `rescale()`).
|
||||
|
||||
use std::collections::HashMap;
|
||||
|
||||
use crate::constraints::Constraint;
|
||||
use crate::geo::{Line, Point};
|
||||
use crate::util::{ParamId, ParamStore};
|
||||
|
||||
const PI_18: f64 = std::f64::consts::PI / 18.0;
|
||||
|
||||
/// The angle from `p1` to `p2`, measured against a reference frame rotated
|
||||
/// by `angle + da`, is zero — i.e. the segment `p1->p2` points along that
|
||||
/// rotated frame's x-axis. `da` is a fixed offset baked in at construction
|
||||
/// (planegcs uses it for the "reversed" variant of this constraint).
|
||||
pub struct P2PAngle {
|
||||
pub p1: Point,
|
||||
pub p2: Point,
|
||||
pub angle: ParamId,
|
||||
pub da: f64,
|
||||
}
|
||||
|
||||
impl P2PAngle {
|
||||
pub fn new(p1: Point, p2: Point, angle: ParamId, da: f64) -> Self {
|
||||
Self { p1, p2, angle, da }
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for P2PAngle {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
vec![self.p1.x, self.p1.y, self.p2.x, self.p2.y, self.angle]
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
let dx = store.get(self.p2.x) - store.get(self.p1.x);
|
||||
let dy = store.get(self.p2.y) - store.get(self.p1.y);
|
||||
let a = store.get(self.angle) + self.da;
|
||||
let (sa, ca) = a.sin_cos();
|
||||
let x = dx * ca + dy * sa;
|
||||
let y = -dx * sa + dy * ca;
|
||||
y.atan2(x)
|
||||
}
|
||||
|
||||
fn grad_value(&self, store: &ParamStore, param: ParamId) -> f64 {
|
||||
let mut deriv = 0.0;
|
||||
if [self.p1.x, self.p1.y, self.p2.x, self.p2.y].contains(¶m) {
|
||||
let dx0 = store.get(self.p2.x) - store.get(self.p1.x);
|
||||
let dy0 = store.get(self.p2.y) - store.get(self.p1.y);
|
||||
let a = store.get(self.angle) + self.da;
|
||||
let (sa, ca) = a.sin_cos();
|
||||
let x = dx0 * ca + dy0 * sa;
|
||||
let y = -dx0 * sa + dy0 * ca;
|
||||
let r2 = dx0 * dx0 + dy0 * dy0;
|
||||
let dx = -y / r2;
|
||||
let dy = x / r2;
|
||||
if param == self.p1.x {
|
||||
deriv += -ca * dx + sa * dy;
|
||||
}
|
||||
if param == self.p1.y {
|
||||
deriv += -sa * dx - ca * dy;
|
||||
}
|
||||
if param == self.p2.x {
|
||||
deriv += ca * dx - sa * dy;
|
||||
}
|
||||
if param == self.p2.y {
|
||||
deriv += sa * dx + ca * dy;
|
||||
}
|
||||
}
|
||||
if param == self.angle {
|
||||
deriv += -1.0;
|
||||
}
|
||||
deriv
|
||||
}
|
||||
|
||||
fn max_step(&self, _store: &ParamStore, dir: &HashMap<ParamId, f64>, lim: f64) -> f64 {
|
||||
let mut lim = lim;
|
||||
if let Some(&step) = dir.get(&self.angle) {
|
||||
let step = step.abs();
|
||||
if step > PI_18 {
|
||||
lim = lim.min(PI_18 / step);
|
||||
}
|
||||
}
|
||||
lim
|
||||
}
|
||||
}
|
||||
|
||||
/// The angle from line `l1`'s direction to line `l2`'s direction, offset by
|
||||
/// `angle`, is zero.
|
||||
pub struct L2LAngle {
|
||||
pub l1: Line,
|
||||
pub l2: Line,
|
||||
pub angle: ParamId,
|
||||
}
|
||||
|
||||
impl L2LAngle {
|
||||
pub fn new(l1: Line, l2: Line, angle: ParamId) -> Self {
|
||||
Self { l1, l2, angle }
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for L2LAngle {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
vec![self.l1.p1.x, self.l1.p1.y, self.l1.p2.x, self.l1.p2.y, self.l2.p1.x, self.l2.p1.y, self.l2.p2.x, self.l2.p2.y, self.angle]
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
let dx1 = store.get(self.l1.p2.x) - store.get(self.l1.p1.x);
|
||||
let dy1 = store.get(self.l1.p2.y) - store.get(self.l1.p1.y);
|
||||
let dx2 = store.get(self.l2.p2.x) - store.get(self.l2.p1.x);
|
||||
let dy2 = store.get(self.l2.p2.y) - store.get(self.l2.p1.y);
|
||||
let a = dy1.atan2(dx1) + store.get(self.angle);
|
||||
let (sa, ca) = a.sin_cos();
|
||||
let x2 = dx2 * ca + dy2 * sa;
|
||||
let y2 = -dx2 * sa + dy2 * ca;
|
||||
y2.atan2(x2)
|
||||
}
|
||||
|
||||
fn grad_value(&self, store: &ParamStore, param: ParamId) -> f64 {
|
||||
let (l1p1x, l1p1y, l1p2x, l1p2y) = (self.l1.p1.x, self.l1.p1.y, self.l1.p2.x, self.l1.p2.y);
|
||||
let (l2p1x, l2p1y, l2p2x, l2p2y) = (self.l2.p1.x, self.l2.p1.y, self.l2.p2.x, self.l2.p2.y);
|
||||
let mut deriv = 0.0;
|
||||
|
||||
if [l1p1x, l1p1y, l1p2x, l1p2y].contains(¶m) {
|
||||
let dx1 = store.get(l1p2x) - store.get(l1p1x);
|
||||
let dy1 = store.get(l1p2y) - store.get(l1p1y);
|
||||
let r2 = dx1 * dx1 + dy1 * dy1;
|
||||
if param == l1p1x {
|
||||
deriv += -dy1 / r2;
|
||||
}
|
||||
if param == l1p1y {
|
||||
deriv += dx1 / r2;
|
||||
}
|
||||
if param == l1p2x {
|
||||
deriv += dy1 / r2;
|
||||
}
|
||||
if param == l1p2y {
|
||||
deriv += -dx1 / r2;
|
||||
}
|
||||
}
|
||||
if [l2p1x, l2p1y, l2p2x, l2p2y].contains(¶m) {
|
||||
let dx1 = store.get(l1p2x) - store.get(l1p1x);
|
||||
let dy1 = store.get(l1p2y) - store.get(l1p1y);
|
||||
let dx2_0 = store.get(l2p2x) - store.get(l2p1x);
|
||||
let dy2_0 = store.get(l2p2y) - store.get(l2p1y);
|
||||
let a = dy1.atan2(dx1) + store.get(self.angle);
|
||||
let (sa, ca) = a.sin_cos();
|
||||
let x2 = dx2_0 * ca + dy2_0 * sa;
|
||||
let y2 = -dx2_0 * sa + dy2_0 * ca;
|
||||
let r2 = dx2_0 * dx2_0 + dy2_0 * dy2_0;
|
||||
let dx2 = -y2 / r2;
|
||||
let dy2 = x2 / r2;
|
||||
if param == l2p1x {
|
||||
deriv += -ca * dx2 + sa * dy2;
|
||||
}
|
||||
if param == l2p1y {
|
||||
deriv += -sa * dx2 - ca * dy2;
|
||||
}
|
||||
if param == l2p2x {
|
||||
deriv += ca * dx2 - sa * dy2;
|
||||
}
|
||||
if param == l2p2y {
|
||||
deriv += sa * dx2 + ca * dy2;
|
||||
}
|
||||
}
|
||||
if param == self.angle {
|
||||
deriv += -1.0;
|
||||
}
|
||||
deriv
|
||||
}
|
||||
|
||||
fn max_step(&self, _store: &ParamStore, dir: &HashMap<ParamId, f64>, lim: f64) -> f64 {
|
||||
let mut lim = lim;
|
||||
if let Some(&step) = dir.get(&self.angle) {
|
||||
let step = step.abs();
|
||||
if step > PI_18 {
|
||||
lim = lim.min(PI_18 / step);
|
||||
}
|
||||
}
|
||||
lim
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::constraints::test_support::assert_grad_matches_finite_difference;
|
||||
|
||||
fn line(store: &mut ParamStore, x1: f64, y1: f64, x2: f64, y2: f64) -> Line {
|
||||
Line {
|
||||
p1: Point::new(store.add(x1, false), store.add(y1, false)),
|
||||
p2: Point::new(store.add(x2, false), store.add(y2, false)),
|
||||
}
|
||||
}
|
||||
|
||||
fn point(store: &mut ParamStore, x: f64, y: f64) -> Point {
|
||||
Point::new(store.add(x, false), store.add(y, false))
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn p2p_angle_is_zero_when_segment_matches_the_target_angle() {
|
||||
let mut store = ParamStore::new();
|
||||
let p1 = point(&mut store, 0.0, 0.0);
|
||||
let p2 = point(&mut store, 10.0, 10.0); // 45 degrees
|
||||
let angle = store.add(std::f64::consts::FRAC_PI_4, false);
|
||||
let c = P2PAngle::new(p1, p2, angle, 0.0);
|
||||
assert!(c.error_value(&store).abs() < 1e-9);
|
||||
assert_grad_matches_finite_difference(&c, &mut store);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn l2l_angle_is_zero_when_relative_angle_matches() {
|
||||
let mut store = ParamStore::new();
|
||||
let l1 = line(&mut store, 0.0, 0.0, 10.0, 0.0); // along +x
|
||||
let l2 = line(&mut store, 0.0, 0.0, 0.0, 10.0); // along +y, 90 degrees from l1
|
||||
let angle = store.add(std::f64::consts::FRAC_PI_2, false);
|
||||
let c = L2LAngle::new(l1, l2, angle);
|
||||
assert!(c.error_value(&store).abs() < 1e-9);
|
||||
assert_grad_matches_finite_difference(&c, &mut store);
|
||||
}
|
||||
}
|
||||
@@ -1,216 +0,0 @@
|
||||
//! `PointOnBSpline`: ties one coordinate of a point to a B-spline's
|
||||
//! parametric value at `u`.
|
||||
//!
|
||||
//! Ported from planegcs's `ConstraintPointOnBSpline` (`Constraints.h`/`.cpp`).
|
||||
//! The C++ caches which window of `degree + 1` poles is currently active
|
||||
//! (`startpole`) and only recomputes it when `theparam` drifts outside the
|
||||
//! cached window's knot span — a solver-loop optimization, since the same
|
||||
//! constraint's `error()`/`grad()` are called many times per solve with `u`
|
||||
//! barely changing between calls. This port always recomputes the window
|
||||
//! fresh via [`BSpline::find_span`] instead: `find_span` is an O(pole
|
||||
//! count) linear scan, cheap at ordinary sketch scale, and skipping the
|
||||
//! cache sidesteps needing interior mutability (a `Cell<usize>`) on a type
|
||||
//! that otherwise, like every other `Constraint` impl here, only needs
|
||||
//! shared access to evaluate.
|
||||
//!
|
||||
//! `SlopeAtBSplineKnot` (planegcs's other B-spline constraint, enforcing
|
||||
//! tangent continuity across a knot) is not ported: it requires
|
||||
//! reconstructing knot multiplicity from the flattened knot vector this
|
||||
//! port's [`BSpline`] uses (see its struct docs) purely to precompute
|
||||
//! construction-time combination factors, and is a narrower, less commonly
|
||||
//! needed constraint than `PointOnBSpline` — deferred rather than rushed.
|
||||
|
||||
use crate::geo::{spline_value, BSpline};
|
||||
use crate::util::{ParamId, ParamStore};
|
||||
|
||||
use super::Constraint;
|
||||
|
||||
/// Which coordinate of the constrained point this ties to the spline.
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum Coord {
|
||||
X,
|
||||
Y,
|
||||
}
|
||||
|
||||
pub struct PointOnBSpline {
|
||||
point: ParamId,
|
||||
param: ParamId,
|
||||
coord: Coord,
|
||||
bsp: BSpline,
|
||||
}
|
||||
|
||||
impl PointOnBSpline {
|
||||
pub fn new(point: ParamId, param: ParamId, coord: Coord, bsp: BSpline) -> Self {
|
||||
Self { point, param, coord, bsp }
|
||||
}
|
||||
|
||||
fn pole_at(&self, startpole: usize, i: usize) -> ParamId {
|
||||
match self.coord {
|
||||
Coord::X => self.bsp.pole_x_at(startpole, i),
|
||||
Coord::Y => self.bsp.pole_y_at(startpole, i),
|
||||
}
|
||||
}
|
||||
|
||||
fn numpoints(&self) -> usize {
|
||||
self.bsp.degree + 1
|
||||
}
|
||||
|
||||
/// `(k, startpole)` for the knot span containing the current `param`
|
||||
/// value — recomputed fresh every call, see the module doc comment.
|
||||
fn window(&self, store: &ParamStore) -> (usize, usize) {
|
||||
let k = self.bsp.find_span(store.get(self.param));
|
||||
(k, k - self.bsp.degree)
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for PointOnBSpline {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
let mut params = vec![self.point, self.param];
|
||||
for pole in &self.bsp.poles {
|
||||
params.push(match self.coord {
|
||||
Coord::X => pole.x,
|
||||
Coord::Y => pole.y,
|
||||
});
|
||||
}
|
||||
params.extend(self.bsp.weights.iter().copied());
|
||||
params
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
let (k, startpole) = self.window(store);
|
||||
let numpoints = self.numpoints();
|
||||
|
||||
let mut d = vec![0.0; numpoints];
|
||||
for i in 0..numpoints {
|
||||
d[i] = store.get(self.pole_at(startpole, i)) * store.get(self.bsp.weight_at(startpole, i));
|
||||
}
|
||||
let sum = spline_value(store.get(self.param), k, self.bsp.degree, &mut d, &self.bsp.knots);
|
||||
for i in 0..numpoints {
|
||||
d[i] = store.get(self.bsp.weight_at(startpole, i));
|
||||
}
|
||||
let wsum = spline_value(store.get(self.param), k, self.bsp.degree, &mut d, &self.bsp.knots);
|
||||
|
||||
store.get(self.point) * wsum - sum
|
||||
}
|
||||
|
||||
fn grad_value(&self, store: &ParamStore, param: ParamId) -> f64 {
|
||||
let (k, startpole) = self.window(store);
|
||||
let numpoints = self.numpoints();
|
||||
let u = store.get(self.param);
|
||||
let mut deriv = 0.0;
|
||||
|
||||
if param == self.point {
|
||||
let mut d = vec![0.0; numpoints];
|
||||
for i in 0..numpoints {
|
||||
d[i] = store.get(self.bsp.weight_at(startpole, i));
|
||||
}
|
||||
deriv += spline_value(u, k, self.bsp.degree, &mut d, &self.bsp.knots);
|
||||
}
|
||||
|
||||
if param == self.param {
|
||||
let denom = |i: usize| self.bsp.knots[startpole + i + self.bsp.degree] - self.bsp.knots[startpole + i];
|
||||
|
||||
let mut d = vec![0.0; numpoints - 1];
|
||||
for i in 1..numpoints {
|
||||
d[i - 1] = (store.get(self.pole_at(startpole, i)) * store.get(self.bsp.weight_at(startpole, i))
|
||||
- store.get(self.pole_at(startpole, i - 1)) * store.get(self.bsp.weight_at(startpole, i - 1)))
|
||||
/ denom(i);
|
||||
}
|
||||
let slopevalue = spline_value(u, k, self.bsp.degree - 1, &mut d, &self.bsp.knots);
|
||||
|
||||
for i in 1..numpoints {
|
||||
d[i - 1] =
|
||||
(store.get(self.bsp.weight_at(startpole, i)) - store.get(self.bsp.weight_at(startpole, i - 1))) / denom(i);
|
||||
}
|
||||
let wslopevalue = spline_value(u, k, self.bsp.degree - 1, &mut d, &self.bsp.knots);
|
||||
|
||||
deriv += (store.get(self.point) * wslopevalue - slopevalue) * self.bsp.degree as f64;
|
||||
}
|
||||
|
||||
for i in 0..numpoints {
|
||||
let pole_i = self.pole_at(startpole, i);
|
||||
let weight_i = self.bsp.weight_at(startpole, i);
|
||||
if param == pole_i {
|
||||
let factor_i = self.bsp.get_lin_comb_factor(u, k, startpole + i, self.bsp.degree);
|
||||
deriv += -(store.get(weight_i) * factor_i);
|
||||
}
|
||||
if param == weight_i {
|
||||
let factor_i = self.bsp.get_lin_comb_factor(u, k, startpole + i, self.bsp.degree);
|
||||
deriv += (store.get(self.point) - store.get(pole_i)) * factor_i;
|
||||
}
|
||||
}
|
||||
|
||||
deriv
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::constraints::test_support::assert_grad_matches_finite_difference;
|
||||
use crate::geo::{Curve, Point};
|
||||
|
||||
/// The same clamped cubic-Bézier-as-BSpline shape as `geo.rs`'s own
|
||||
/// tests, so its `value()` is already independently verified there.
|
||||
fn make_bspline(store: &mut ParamStore, poles_xy: [(f64, f64); 4]) -> BSpline {
|
||||
let poles: Vec<Point> =
|
||||
poles_xy.iter().map(|&(x, y)| Point::new(store.add(x, false), store.add(y, false))).collect();
|
||||
let weights = vec![store.add(1.0, false); 4];
|
||||
BSpline {
|
||||
start: poles[0],
|
||||
end: poles[3],
|
||||
poles,
|
||||
weights,
|
||||
knots: vec![0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0],
|
||||
degree: 3,
|
||||
periodic: false,
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn zero_error_when_point_matches_the_spline_at_u() {
|
||||
let mut store = ParamStore::new();
|
||||
let bsp = make_bspline(&mut store, [(0.0, 0.0), (1.0, 3.0), (3.0, 3.0), (4.0, 0.0)]);
|
||||
let u = 0.5;
|
||||
let on_curve = bsp.value(&store, u, 0.0, None);
|
||||
|
||||
let point = store.add(on_curve.x, false);
|
||||
let param = store.add(u, true);
|
||||
let c = PointOnBSpline::new(point, param, Coord::X, bsp.clone());
|
||||
assert!(c.error_value(&store).abs() < 1e-9, "err={}", c.error_value(&store));
|
||||
|
||||
let point_y = store.add(on_curve.y, false);
|
||||
let cy = PointOnBSpline::new(point_y, param, Coord::Y, bsp);
|
||||
assert!(cy.error_value(&store).abs() < 1e-9, "err={}", cy.error_value(&store));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn nonzero_error_when_point_is_off_the_spline() {
|
||||
let mut store = ParamStore::new();
|
||||
let bsp = make_bspline(&mut store, [(0.0, 0.0), (1.0, 3.0), (3.0, 3.0), (4.0, 0.0)]);
|
||||
let point = store.add(100.0, false);
|
||||
let param = store.add(0.5, true);
|
||||
let c = PointOnBSpline::new(point, param, Coord::X, bsp);
|
||||
assert!(c.error_value(&store).abs() > 1.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn grad_matches_finite_difference_x_coord() {
|
||||
let mut store = ParamStore::new();
|
||||
let bsp = make_bspline(&mut store, [(0.0, 0.0), (1.0, 3.0), (3.0, 3.0), (4.0, 0.0)]);
|
||||
let point = store.add(2.5, true); // deliberately off the spline
|
||||
let param = store.add(0.4, true);
|
||||
let c = PointOnBSpline::new(point, param, Coord::X, bsp);
|
||||
assert_grad_matches_finite_difference(&c, &mut store);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn grad_matches_finite_difference_y_coord() {
|
||||
let mut store = ParamStore::new();
|
||||
let bsp = make_bspline(&mut store, [(0.0, 0.0), (1.0, 3.0), (3.0, 3.0), (4.0, 0.0)]);
|
||||
let point = store.add(1.0, true);
|
||||
let param = store.add(0.7, true);
|
||||
let c = PointOnBSpline::new(point, param, Coord::Y, bsp);
|
||||
assert_grad_matches_finite_difference(&c, &mut store);
|
||||
}
|
||||
}
|
||||
@@ -1,478 +0,0 @@
|
||||
//! Circle/arc distance constraints (port stage 5b, partial).
|
||||
//!
|
||||
//! Ported from the corresponding `Constraint*` classes in
|
||||
//! `Constraints.h`/`Constraints.cpp`. `CurveValue`, the `AngleViaPoint*`
|
||||
//! family, and `Snell` are not yet ported — they dispatch through planegcs's
|
||||
//! generic `Curve::CalculateNormal`/`Value` across arbitrary curve types
|
||||
//! (needed once ellipses/hyperbolas/parabolas exist too), rather than
|
||||
//! against a single concrete geometry the way every constraint here does;
|
||||
//! left for whichever stage adds those curve types and a `Curve` trait
|
||||
//! object story to dispatch across them.
|
||||
|
||||
use crate::constraints::Constraint;
|
||||
use crate::geo::{Arc, Circle, DeriVector2, Point};
|
||||
use crate::util::{ParamId, ParamStore};
|
||||
|
||||
/// Two circles are tangent: internally (one inside the other, touching) or
|
||||
/// externally, per `internal`.
|
||||
pub struct TangentCircumf {
|
||||
pub c1_center: Point,
|
||||
pub c2_center: Point,
|
||||
pub r1: ParamId,
|
||||
pub r2: ParamId,
|
||||
pub internal: bool,
|
||||
}
|
||||
|
||||
impl TangentCircumf {
|
||||
pub fn new(c1_center: Point, c2_center: Point, r1: ParamId, r2: ParamId, internal: bool) -> Self {
|
||||
Self { c1_center, c2_center, r1, r2, internal }
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for TangentCircumf {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
vec![self.c1_center.x, self.c1_center.y, self.c2_center.x, self.c2_center.y, self.r1, self.r2]
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
let dx = store.get(self.c1_center.x) - store.get(self.c2_center.x);
|
||||
let dy = store.get(self.c1_center.y) - store.get(self.c2_center.y);
|
||||
let d_sq = dx * dx + dy * dy;
|
||||
let (r1, r2) = (store.get(self.r1), store.get(self.r2));
|
||||
|
||||
// Concentric-circle singularity: fall back to the robust `r1 - r2`
|
||||
// formulation, which has a constant non-zero gradient.
|
||||
if d_sq < 1e-14 {
|
||||
return r1 - r2;
|
||||
}
|
||||
if self.internal {
|
||||
d_sq - (r1 - r2) * (r1 - r2)
|
||||
} else {
|
||||
d_sq - (r1 + r2) * (r1 + r2)
|
||||
}
|
||||
}
|
||||
|
||||
fn grad_value(&self, store: &ParamStore, param: ParamId) -> f64 {
|
||||
let dx = store.get(self.c1_center.x) - store.get(self.c2_center.x);
|
||||
let dy = store.get(self.c1_center.y) - store.get(self.c2_center.y);
|
||||
let d_sq = dx * dx + dy * dy;
|
||||
let (r1, r2) = (store.get(self.r1), store.get(self.r2));
|
||||
|
||||
if d_sq < 1e-14 {
|
||||
if param == self.r1 {
|
||||
return 1.0;
|
||||
} else if param == self.r2 {
|
||||
return -1.0;
|
||||
}
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
let mut deriv = 0.0;
|
||||
if param == self.c1_center.x {
|
||||
deriv += 2.0 * dx;
|
||||
}
|
||||
if param == self.c1_center.y {
|
||||
deriv += 2.0 * dy;
|
||||
}
|
||||
if param == self.c2_center.x {
|
||||
deriv += -2.0 * dx;
|
||||
}
|
||||
if param == self.c2_center.y {
|
||||
deriv += -2.0 * dy;
|
||||
}
|
||||
if self.internal {
|
||||
if param == self.r1 {
|
||||
deriv += 2.0 * (r2 - r1);
|
||||
}
|
||||
if param == self.r2 {
|
||||
deriv += 2.0 * (r1 - r2);
|
||||
}
|
||||
} else {
|
||||
if param == self.r1 {
|
||||
deriv += -2.0 * (r1 + r2);
|
||||
}
|
||||
if param == self.r2 {
|
||||
deriv += -2.0 * (r1 + r2);
|
||||
}
|
||||
}
|
||||
deriv
|
||||
}
|
||||
}
|
||||
|
||||
/// Distance between two circles' circumferences: `|c1 - c2|` outside both
|
||||
/// circles, or the gap between them when one sits inside the other's span.
|
||||
/// `c1_bigger` breaks the tie when neither circle's centre is outside the
|
||||
/// other's radius (`None` picks whichever circle currently has the larger
|
||||
/// radius).
|
||||
pub struct C2CDistance {
|
||||
pub c1: Circle,
|
||||
pub c2: Circle,
|
||||
pub distance: ParamId,
|
||||
pub c1_bigger: Option<bool>,
|
||||
}
|
||||
|
||||
impl C2CDistance {
|
||||
pub fn new(c1: Circle, c2: Circle, distance: ParamId, c1_bigger: Option<bool>) -> Self {
|
||||
Self { c1, c2, distance, c1_bigger }
|
||||
}
|
||||
|
||||
fn error_grad(&self, store: &ParamStore, derivparam: Option<ParamId>) -> (f64, f64) {
|
||||
let ct1 = DeriVector2::from_point(store, self.c1.center, derivparam);
|
||||
let ct2 = DeriVector2::from_point(store, self.c2.center, derivparam);
|
||||
let vector_ct12 = ct1.subtr(&ct2);
|
||||
let (length_ct12, dlength_ct12) = vector_ct12.length_deriv();
|
||||
|
||||
let (r1, r2) = (store.get(self.c1.rad), store.get(self.c2.rad));
|
||||
let dist = store.get(self.distance);
|
||||
|
||||
if length_ct12 >= r1 && length_ct12 >= r2 {
|
||||
let err = length_ct12 - (r2 + r1 + dist);
|
||||
let drad = if derivparam == Some(self.c2.rad) || derivparam == Some(self.c1.rad) { -1.0 } else { 0.0 };
|
||||
let grad = dlength_ct12 + drad;
|
||||
return (err, grad);
|
||||
}
|
||||
|
||||
let (bigradius_id, smallradius_id) = match self.c1_bigger {
|
||||
None => {
|
||||
if r1 >= r2 {
|
||||
(self.c1.rad, self.c2.rad)
|
||||
} else {
|
||||
(self.c2.rad, self.c1.rad)
|
||||
}
|
||||
}
|
||||
Some(true) => (self.c1.rad, self.c2.rad),
|
||||
Some(false) => (self.c2.rad, self.c1.rad),
|
||||
};
|
||||
let (bigradius, smallradius) = (store.get(bigradius_id), store.get(smallradius_id));
|
||||
let smallspan = smallradius + length_ct12 + dist;
|
||||
let err = bigradius - smallspan;
|
||||
|
||||
let mut drad = 0.0;
|
||||
if derivparam == Some(bigradius_id) {
|
||||
drad = 1.0;
|
||||
} else if derivparam == Some(smallradius_id) {
|
||||
drad = -1.0;
|
||||
} else if derivparam == Some(self.distance) {
|
||||
drad = if dist < 0.0 { 1.0 } else { -1.0 };
|
||||
}
|
||||
let grad = if length_ct12 > 1e-13 { -dlength_ct12 + drad } else { drad };
|
||||
(err, grad)
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for C2CDistance {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
vec![
|
||||
self.distance,
|
||||
self.c1.center.x,
|
||||
self.c1.center.y,
|
||||
self.c1.rad,
|
||||
self.c2.center.x,
|
||||
self.c2.center.y,
|
||||
self.c2.rad,
|
||||
]
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
self.error_grad(store, None).0
|
||||
}
|
||||
|
||||
fn grad_value(&self, store: &ParamStore, param: ParamId) -> f64 {
|
||||
self.error_grad(store, Some(param)).1
|
||||
}
|
||||
}
|
||||
|
||||
/// Distance from a circle's circumference to a line, on one side (`ccw`) and
|
||||
/// either measuring outward or `internal`ly (the circle sits astride the
|
||||
/// line at that distance instead of clear of it).
|
||||
pub struct C2LDistance {
|
||||
pub circle: Circle,
|
||||
pub line: crate::geo::Line,
|
||||
pub distance: ParamId,
|
||||
pub ccw: bool,
|
||||
pub internal: bool,
|
||||
}
|
||||
|
||||
impl C2LDistance {
|
||||
pub fn new(circle: Circle, line: crate::geo::Line, distance: ParamId, ccw: bool, internal: bool) -> Self {
|
||||
Self { circle, line, distance, ccw, internal }
|
||||
}
|
||||
|
||||
/// Signed distance from the circle's centre to the line, and its
|
||||
/// derivative w.r.t. `derivparam`.
|
||||
fn signed_value(&self, store: &ParamStore, derivparam: Option<ParamId>) -> (f64, f64) {
|
||||
let ct = DeriVector2::from_point(store, self.circle.center, derivparam);
|
||||
let p1 = DeriVector2::from_point(store, self.line.p1, derivparam);
|
||||
let p2 = DeriVector2::from_point(store, self.line.p2, derivparam);
|
||||
let v_line = p2.subtr(&p1);
|
||||
let v_p1ct = ct.subtr(&p1);
|
||||
|
||||
let (area, darea) = v_line.cross_prod_z(&v_p1ct);
|
||||
let (length, dlength) = v_line.length_deriv();
|
||||
|
||||
let h = area / length;
|
||||
let dh = (darea - h * dlength) / length;
|
||||
(h, dh)
|
||||
}
|
||||
|
||||
fn error_grad(&self, store: &ParamStore, derivparam: Option<ParamId>) -> (f64, f64) {
|
||||
let (h, dh) = self.signed_value(store, derivparam);
|
||||
let rad = store.get(self.circle.rad);
|
||||
let dist = store.get(self.distance).abs();
|
||||
let mut target = if self.internal { rad - dist } else { rad + dist };
|
||||
target = if self.ccw { target } else { -target };
|
||||
let err = target - h;
|
||||
|
||||
let grad = if derivparam == Some(self.circle.rad) {
|
||||
if self.ccw {
|
||||
1.0
|
||||
} else {
|
||||
-1.0
|
||||
}
|
||||
} else {
|
||||
-dh
|
||||
};
|
||||
(err, grad)
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for C2LDistance {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
vec![
|
||||
self.distance,
|
||||
self.circle.center.x,
|
||||
self.circle.center.y,
|
||||
self.circle.rad,
|
||||
self.line.p1.x,
|
||||
self.line.p1.y,
|
||||
self.line.p2.x,
|
||||
self.line.p2.y,
|
||||
]
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
self.error_grad(store, None).0
|
||||
}
|
||||
|
||||
fn grad_value(&self, store: &ParamStore, param: ParamId) -> f64 {
|
||||
self.error_grad(store, Some(param)).1
|
||||
}
|
||||
}
|
||||
|
||||
/// Distance from a point to a circle's circumference (`|point - center| -
|
||||
/// radius`, signed by whether the point sits inside or outside the circle).
|
||||
pub struct P2CDistance {
|
||||
pub circle: Circle,
|
||||
pub pt: Point,
|
||||
pub distance: ParamId,
|
||||
}
|
||||
|
||||
impl P2CDistance {
|
||||
pub fn new(circle: Circle, pt: Point, distance: ParamId) -> Self {
|
||||
Self { circle, pt, distance }
|
||||
}
|
||||
|
||||
fn value(&self, store: &ParamStore, derivparam: Option<ParamId>) -> (f64, f64) {
|
||||
let ct = DeriVector2::from_point(store, self.circle.center, derivparam);
|
||||
let p = DeriVector2::from_point(store, self.pt, derivparam);
|
||||
ct.subtr(&p).length_deriv()
|
||||
}
|
||||
|
||||
fn error_grad(&self, store: &ParamStore, derivparam: Option<ParamId>) -> (f64, f64) {
|
||||
let (length, dlength) = self.value(store, derivparam);
|
||||
let rad = store.get(self.circle.rad);
|
||||
let dist = store.get(self.distance);
|
||||
let inside = length < rad;
|
||||
|
||||
let err = if inside { rad - dist - length } else { rad + dist - length };
|
||||
|
||||
let grad = if derivparam == Some(self.distance) {
|
||||
if inside {
|
||||
-1.0
|
||||
} else {
|
||||
1.0
|
||||
}
|
||||
} else if derivparam == Some(self.circle.rad) {
|
||||
1.0
|
||||
} else {
|
||||
-dlength
|
||||
};
|
||||
(err, grad)
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for P2CDistance {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
vec![self.distance, self.circle.center.x, self.circle.center.y, self.circle.rad, self.pt.x, self.pt.y]
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
self.error_grad(store, None).0
|
||||
}
|
||||
|
||||
fn grad_value(&self, store: &ParamStore, param: ParamId) -> f64 {
|
||||
self.error_grad(store, Some(param)).1
|
||||
}
|
||||
}
|
||||
|
||||
/// An arc's length equals `distance`. Angles are normalized assuming a
|
||||
/// positive, counterclockwise sweep before computing the arc length,
|
||||
/// matching planegcs's `normalizedAngles`.
|
||||
pub struct ArcLength {
|
||||
pub arc: Arc,
|
||||
pub distance: ParamId,
|
||||
}
|
||||
|
||||
impl ArcLength {
|
||||
pub fn new(arc: Arc, distance: ParamId) -> Self {
|
||||
Self { arc, distance }
|
||||
}
|
||||
|
||||
fn normalized_angles(&self, store: &ParamStore) -> (f64, f64) {
|
||||
let mut start = store.get(self.arc.start_angle);
|
||||
let mut end = store.get(self.arc.end_angle);
|
||||
while start < 0.0 {
|
||||
start += std::f64::consts::TAU;
|
||||
}
|
||||
while end < start {
|
||||
end += std::f64::consts::TAU;
|
||||
}
|
||||
(start, end)
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for ArcLength {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
vec![self.distance, self.arc.circle.center.x, self.arc.circle.center.y, self.arc.circle.rad, self.arc.start_angle, self.arc.end_angle]
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
let rad = store.get(self.arc.circle.rad);
|
||||
let (start_a, end_a) = self.normalized_angles(store);
|
||||
rad * (end_a - start_a) - store.get(self.distance)
|
||||
}
|
||||
|
||||
fn grad_value(&self, store: &ParamStore, param: ParamId) -> f64 {
|
||||
if param == self.distance {
|
||||
return -1.0;
|
||||
}
|
||||
let rad = store.get(self.arc.circle.rad);
|
||||
let (start_a, end_a) = self.normalized_angles(store);
|
||||
let d_rad = if param == self.arc.circle.rad { 1.0 } else { 0.0 };
|
||||
let d_start_a = if param == self.arc.start_angle { 1.0 } else { 0.0 };
|
||||
let d_end_a = if param == self.arc.end_angle { 1.0 } else { 0.0 };
|
||||
rad * (d_end_a - d_start_a) + d_rad * (end_a - start_a)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::constraints::test_support::{assert_grad_matches_finite_difference, assert_single_param_grad_matches_fd};
|
||||
use crate::geo::Line;
|
||||
|
||||
fn point(store: &mut ParamStore, x: f64, y: f64) -> Point {
|
||||
Point::new(store.add(x, false), store.add(y, false))
|
||||
}
|
||||
|
||||
fn circle(store: &mut ParamStore, cx: f64, cy: f64, r: f64) -> Circle {
|
||||
Circle { center: point(store, cx, cy), rad: store.add(r, false) }
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn tangent_circumf_external_is_zero_at_the_right_separation() {
|
||||
let mut store = ParamStore::new();
|
||||
let c1 = circle(&mut store, 0.0, 0.0, 3.0);
|
||||
let c2 = circle(&mut store, 10.0, 0.0, 4.0); // sum of radii == 7, but separation is 10... use 7
|
||||
// fix separation to sum of radii (7) for a zero-error case
|
||||
store.set(c2.center.x, 7.0);
|
||||
let c = TangentCircumf::new(c1.center, c2.center, c1.rad, c2.rad, false);
|
||||
assert!(c.error_value(&store).abs() < 1e-9);
|
||||
assert_grad_matches_finite_difference(&c, &mut store);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn tangent_circumf_gradient_checks_out_away_from_the_singularity() {
|
||||
let mut store = ParamStore::new();
|
||||
let c1 = circle(&mut store, 0.0, 0.0, 3.0);
|
||||
let c2 = circle(&mut store, 9.0, 2.0, 4.0);
|
||||
let c = TangentCircumf::new(c1.center, c2.center, c1.rad, c2.rad, false);
|
||||
assert_grad_matches_finite_difference(&c, &mut store);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn c2c_distance_matches_hand_computation_outside_case() {
|
||||
let mut store = ParamStore::new();
|
||||
let c1 = circle(&mut store, 0.0, 0.0, 2.0);
|
||||
let c2 = circle(&mut store, 10.0, 0.0, 3.0);
|
||||
let d = store.add(5.0, false); // 10 - 2 - 3 == 5
|
||||
let c = C2CDistance::new(c1, c2, d, None);
|
||||
assert!(c.error_value(&store).abs() < 1e-9);
|
||||
// Skips `d` (this constraint's own `distance` param): planegcs's own
|
||||
// `errorgrad` never assigns it a nonzero gradient in this branch
|
||||
// (only the two radii do) even though `error` depends on it with
|
||||
// coefficient -1 — an upstream quirk, not a translation bug, and
|
||||
// harmless in practice since `distance` is the driven target value,
|
||||
// not a free geometry parameter the solver differentiates against.
|
||||
for param in [c1.center.x, c1.center.y, c1.rad, c2.center.x, c2.center.y, c2.rad] {
|
||||
assert_single_param_grad_matches_fd(&c, &mut store, param);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn c2c_distance_gradient_checks_out_nested_case() {
|
||||
let mut store = ParamStore::new();
|
||||
let c1 = circle(&mut store, 0.0, 0.0, 10.0);
|
||||
let c2 = circle(&mut store, 1.0, 0.0, 3.0); // c2 nested inside c1
|
||||
let d = store.add(1.0, false);
|
||||
let c = C2CDistance::new(c1, c2, d, Some(true));
|
||||
assert_grad_matches_finite_difference(&c, &mut store);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn c2l_distance_matches_hand_computation() {
|
||||
let mut store = ParamStore::new();
|
||||
let c = circle(&mut store, 0.0, 5.0, 2.0);
|
||||
let l = Line { p1: point(&mut store, -10.0, 0.0), p2: point(&mut store, 10.0, 0.0) };
|
||||
let d = store.add(3.0, false); // rad(2) + dist(3) == center height (5)
|
||||
let constr = C2LDistance::new(c, l, d, true, false);
|
||||
assert!(constr.error_value(&store).abs() < 1e-9);
|
||||
// Skips `d` for the same reason as the C2CDistance test above:
|
||||
// planegcs's own `errorgrad` gives it gradient 0 in the non-radius
|
||||
// branch (`else { *grad = -dh; }`), an upstream quirk rather than a
|
||||
// translation bug.
|
||||
for param in [c.center.x, c.center.y, c.rad, l.p1.x, l.p1.y, l.p2.x, l.p2.y] {
|
||||
assert_single_param_grad_matches_fd(&constr, &mut store, param);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn p2c_distance_matches_hand_computation_outside_case() {
|
||||
let mut store = ParamStore::new();
|
||||
let c = circle(&mut store, 0.0, 0.0, 2.0);
|
||||
let p = point(&mut store, 5.0, 0.0);
|
||||
let d = store.add(3.0, false); // rad(2) + dist(3) == 5
|
||||
let constr = P2CDistance::new(c, p, d);
|
||||
assert!(constr.error_value(&store).abs() < 1e-9);
|
||||
assert_grad_matches_finite_difference(&constr, &mut store);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn arc_length_matches_hand_computation() {
|
||||
let mut store = ParamStore::new();
|
||||
let c = circle(&mut store, 0.0, 0.0, 2.0);
|
||||
let start_angle = store.add(0.0, false);
|
||||
let end_angle = store.add(std::f64::consts::FRAC_PI_2, false);
|
||||
let arc = Arc {
|
||||
circle: c,
|
||||
start: point(&mut store, 2.0, 0.0),
|
||||
end: point(&mut store, 0.0, 2.0),
|
||||
start_angle,
|
||||
end_angle,
|
||||
};
|
||||
let d = store.add(std::f64::consts::PI, false); // quarter circle of radius 2: length = pi
|
||||
let constr = ArcLength::new(arc, d);
|
||||
assert!(constr.error_value(&store).abs() < 1e-9);
|
||||
assert_grad_matches_finite_difference(&constr, &mut store);
|
||||
}
|
||||
}
|
||||
@@ -1,424 +0,0 @@
|
||||
//! Conic constraints (port stage 5c, partial).
|
||||
//!
|
||||
//! Ported from the corresponding `Constraint*` classes in
|
||||
//! `Constraints.h`/`Constraints.cpp`: `PointOnEllipse`, `TangentEllipseLine`,
|
||||
//! `EqualMajorAxesConic`, `EqualFocalDistance`, `PointOnHyperbola`,
|
||||
//! `PointOnParabola`. Not yet ported: `InternalAlignmentPoint2Ellipse`/
|
||||
//! `InternalAlignmentPoint2Hyperbola` (FreeCAD Sketcher-internal bookkeeping
|
||||
//! for interactively *creating* conic geometry, not for constraining
|
||||
//! existing geometry) and `EllipticalArcRangeToEndPoints` (a
|
||||
//! `ConstraintType` enum value with no implementing class anywhere in the
|
||||
//! current planegcs source — dead upstream, not a gap in this port).
|
||||
|
||||
use crate::constraints::Constraint;
|
||||
use crate::geo::{Conic, Curve, DeriVector2, Ellipse, Hyperbola, Line, Parabola, Point};
|
||||
use crate::util::{ParamId, ParamStore};
|
||||
|
||||
/// Point `p` lies on the ellipse `e` (sum of distances to both foci equals
|
||||
/// the major axis length, `2a`).
|
||||
pub struct PointOnEllipse {
|
||||
pub p: Point,
|
||||
pub e: Ellipse,
|
||||
}
|
||||
|
||||
impl PointOnEllipse {
|
||||
pub fn new(p: Point, e: Ellipse) -> Self {
|
||||
Self { p, e }
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for PointOnEllipse {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
vec![self.p.x, self.p.y, self.e.center.x, self.e.center.y, self.e.focus1.x, self.e.focus1.y, self.e.radmin]
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
let (x0, y0) = (store.get(self.p.x), store.get(self.p.y));
|
||||
let (xc, yc) = (store.get(self.e.center.x), store.get(self.e.center.y));
|
||||
let (xf1, yf1) = (store.get(self.e.focus1.x), store.get(self.e.focus1.y));
|
||||
let b = store.get(self.e.radmin);
|
||||
|
||||
((x0 - xf1).powi(2) + (y0 - yf1).powi(2)).sqrt()
|
||||
+ ((x0 + xf1 - 2.0 * xc).powi(2) + (y0 + yf1 - 2.0 * yc).powi(2)).sqrt()
|
||||
- 2.0 * (b.powi(2) + (xf1 - xc).powi(2) + (yf1 - yc).powi(2)).sqrt()
|
||||
}
|
||||
|
||||
fn grad_value(&self, store: &ParamStore, param: ParamId) -> f64 {
|
||||
let (p1x, p1y) = (self.p.x, self.p.y);
|
||||
let (cx, cy) = (self.e.center.x, self.e.center.y);
|
||||
let (f1x, f1y) = (self.e.focus1.x, self.e.focus1.y);
|
||||
let rmin = self.e.radmin;
|
||||
if ![p1x, p1y, f1x, f1y, cx, cy, rmin].contains(¶m) {
|
||||
return 0.0;
|
||||
}
|
||||
let (x0, y0) = (store.get(p1x), store.get(p1y));
|
||||
let (xc, yc) = (store.get(cx), store.get(cy));
|
||||
let (xf1, yf1) = (store.get(f1x), store.get(f1y));
|
||||
let b = store.get(rmin);
|
||||
|
||||
let d_pf1 = ((x0 - xf1).powi(2) + (y0 - yf1).powi(2)).sqrt();
|
||||
let d_pf2 = ((x0 + xf1 - 2.0 * xc).powi(2) + (y0 + yf1 - 2.0 * yc).powi(2)).sqrt();
|
||||
let d_cf1 = (b.powi(2) + (xf1 - xc).powi(2) + (yf1 - yc).powi(2)).sqrt();
|
||||
|
||||
let mut deriv = 0.0;
|
||||
if param == p1x {
|
||||
deriv += (x0 - xf1) / d_pf1 + (x0 + xf1 - 2.0 * xc) / d_pf2;
|
||||
}
|
||||
if param == p1y {
|
||||
deriv += (y0 - yf1) / d_pf1 + (y0 + yf1 - 2.0 * yc) / d_pf2;
|
||||
}
|
||||
if param == f1x {
|
||||
deriv += -(x0 - xf1) / d_pf1 - 2.0 * (xf1 - xc) / d_cf1 + (x0 + xf1 - 2.0 * xc) / d_pf2;
|
||||
}
|
||||
if param == f1y {
|
||||
deriv += -(y0 - yf1) / d_pf1 - 2.0 * (yf1 - yc) / d_cf1 + (y0 + yf1 - 2.0 * yc) / d_pf2;
|
||||
}
|
||||
if param == cx {
|
||||
deriv += 2.0 * (xf1 - xc) / d_cf1 - 2.0 * (x0 + xf1 - 2.0 * xc) / d_pf2;
|
||||
}
|
||||
if param == cy {
|
||||
deriv += 2.0 * (yf1 - yc) / d_cf1 - 2.0 * (y0 + yf1 - 2.0 * yc) / d_pf2;
|
||||
}
|
||||
if param == rmin {
|
||||
deriv += -2.0 * b / d_cf1;
|
||||
}
|
||||
deriv
|
||||
}
|
||||
}
|
||||
|
||||
/// Line `l` is tangent to ellipse `e`: mirroring one focus across the line
|
||||
/// lands it exactly `2a` (the major axis) from the other focus.
|
||||
pub struct TangentEllipseLine {
|
||||
pub l: Line,
|
||||
pub e: Ellipse,
|
||||
}
|
||||
|
||||
impl TangentEllipseLine {
|
||||
pub fn new(l: Line, e: Ellipse) -> Self {
|
||||
Self { l, e }
|
||||
}
|
||||
|
||||
fn error_grad(&self, store: &ParamStore, derivparam: Option<ParamId>) -> (f64, f64) {
|
||||
let p1 = DeriVector2::from_point(store, self.l.p1, derivparam);
|
||||
let f1 = DeriVector2::from_point(store, self.e.focus1, derivparam);
|
||||
let c = DeriVector2::from_point(store, self.e.center, derivparam);
|
||||
let f2 = c.lin_combi(2.0, &f1, -1.0);
|
||||
|
||||
let nl = self.l.calculate_normal(store, self.l.p1, derivparam).normalized();
|
||||
let (dist_f1l, ddist_f1l) = f1.subtr(&p1).scalar_prod(&nl);
|
||||
let f1m = f1.sum(&nl.mult_d(-2.0 * dist_f1l, -2.0 * ddist_f1l));
|
||||
|
||||
let (dist_f1m_f2, ddist_f1m_f2) = {
|
||||
let d = f2.subtr(&f1m);
|
||||
d.length_deriv()
|
||||
};
|
||||
|
||||
let (rad_maj, drad_maj) = self.e.rad_maj_at(store, derivparam);
|
||||
|
||||
(dist_f1m_f2 - 2.0 * rad_maj, ddist_f1m_f2 - 2.0 * drad_maj)
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for TangentEllipseLine {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
vec![
|
||||
self.l.p1.x,
|
||||
self.l.p1.y,
|
||||
self.l.p2.x,
|
||||
self.l.p2.y,
|
||||
self.e.center.x,
|
||||
self.e.center.y,
|
||||
self.e.focus1.x,
|
||||
self.e.focus1.y,
|
||||
self.e.radmin,
|
||||
]
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
self.error_grad(store, None).0
|
||||
}
|
||||
|
||||
fn grad_value(&self, store: &ParamStore, param: ParamId) -> f64 {
|
||||
self.error_grad(store, Some(param)).1
|
||||
}
|
||||
}
|
||||
|
||||
/// Two conics (either ellipses or hyperbolas, mixed or matched) share the
|
||||
/// same major radius.
|
||||
pub struct EqualMajorAxesConic {
|
||||
pub e1: Conic,
|
||||
pub e2: Conic,
|
||||
}
|
||||
|
||||
impl EqualMajorAxesConic {
|
||||
pub fn new(e1: Conic, e2: Conic) -> Self {
|
||||
Self { e1, e2 }
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for EqualMajorAxesConic {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
let mut p = self.e1.own_params();
|
||||
p.extend(self.e2.own_params());
|
||||
p
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
let (a1, _) = self.e1.rad_maj_at(store, None);
|
||||
let (a2, _) = self.e2.rad_maj_at(store, None);
|
||||
a2 - a1
|
||||
}
|
||||
|
||||
fn grad_value(&self, store: &ParamStore, param: ParamId) -> f64 {
|
||||
let (_, da1) = self.e1.rad_maj_at(store, Some(param));
|
||||
let (_, da2) = self.e2.rad_maj_at(store, Some(param));
|
||||
da2 - da1
|
||||
}
|
||||
}
|
||||
|
||||
/// Two parabolas share the same focal distance (vertex-to-focus length).
|
||||
pub struct EqualFocalDistance {
|
||||
pub e1: Parabola,
|
||||
pub e2: Parabola,
|
||||
}
|
||||
|
||||
impl EqualFocalDistance {
|
||||
pub fn new(e1: Parabola, e2: Parabola) -> Self {
|
||||
Self { e1, e2 }
|
||||
}
|
||||
|
||||
fn error_grad(&self, store: &ParamStore, derivparam: Option<ParamId>) -> (f64, f64) {
|
||||
let (focal1, dfocal1) = {
|
||||
let focus1 = DeriVector2::from_point(store, self.e1.focus1, derivparam);
|
||||
let vertex1 = DeriVector2::from_point(store, self.e1.vertex, derivparam);
|
||||
vertex1.subtr(&focus1).length_deriv()
|
||||
};
|
||||
let (focal2, dfocal2) = {
|
||||
let focus2 = DeriVector2::from_point(store, self.e2.focus1, derivparam);
|
||||
let vertex2 = DeriVector2::from_point(store, self.e2.vertex, derivparam);
|
||||
vertex2.subtr(&focus2).length_deriv()
|
||||
};
|
||||
(focal2 - focal1, dfocal2 - dfocal1)
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for EqualFocalDistance {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
vec![self.e1.vertex.x, self.e1.vertex.y, self.e1.focus1.x, self.e1.focus1.y, self.e2.vertex.x, self.e2.vertex.y, self.e2.focus1.x, self.e2.focus1.y]
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
self.error_grad(store, None).0
|
||||
}
|
||||
|
||||
fn grad_value(&self, store: &ParamStore, param: ParamId) -> f64 {
|
||||
self.error_grad(store, Some(param)).1
|
||||
}
|
||||
}
|
||||
|
||||
/// Point `p` lies on hyperbola `e` (difference of distances to the two foci
|
||||
/// equals the major axis length, `2a`).
|
||||
pub struct PointOnHyperbola {
|
||||
pub p: Point,
|
||||
pub e: Hyperbola,
|
||||
}
|
||||
|
||||
impl PointOnHyperbola {
|
||||
pub fn new(p: Point, e: Hyperbola) -> Self {
|
||||
Self { p, e }
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for PointOnHyperbola {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
vec![self.p.x, self.p.y, self.e.center.x, self.e.center.y, self.e.focus1.x, self.e.focus1.y, self.e.radmin]
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
let (x0, y0) = (store.get(self.p.x), store.get(self.p.y));
|
||||
let (xc, yc) = (store.get(self.e.center.x), store.get(self.e.center.y));
|
||||
let (xf1, yf1) = (store.get(self.e.focus1.x), store.get(self.e.focus1.y));
|
||||
let b = store.get(self.e.radmin);
|
||||
|
||||
-((x0 - xf1).powi(2) + (y0 - yf1).powi(2)).sqrt()
|
||||
+ ((x0 + xf1 - 2.0 * xc).powi(2) + (y0 + yf1 - 2.0 * yc).powi(2)).sqrt()
|
||||
- 2.0 * (-b.powi(2) + (xf1 - xc).powi(2) + (yf1 - yc).powi(2)).sqrt()
|
||||
}
|
||||
|
||||
fn grad_value(&self, store: &ParamStore, param: ParamId) -> f64 {
|
||||
let (p1x, p1y) = (self.p.x, self.p.y);
|
||||
let (cx, cy) = (self.e.center.x, self.e.center.y);
|
||||
let (f1x, f1y) = (self.e.focus1.x, self.e.focus1.y);
|
||||
let rmin = self.e.radmin;
|
||||
if ![p1x, p1y, f1x, f1y, cx, cy, rmin].contains(¶m) {
|
||||
return 0.0;
|
||||
}
|
||||
let (x0, y0) = (store.get(p1x), store.get(p1y));
|
||||
let (xc, yc) = (store.get(cx), store.get(cy));
|
||||
let (xf1, yf1) = (store.get(f1x), store.get(f1y));
|
||||
let b = store.get(rmin);
|
||||
|
||||
let d_pf1 = ((x0 - xf1).powi(2) + (y0 - yf1).powi(2)).sqrt();
|
||||
let d_pf2 = ((x0 + xf1 - 2.0 * xc).powi(2) + (y0 + yf1 - 2.0 * yc).powi(2)).sqrt();
|
||||
let d_cf1 = (-b.powi(2) + (xf1 - xc).powi(2) + (yf1 - yc).powi(2)).sqrt();
|
||||
|
||||
let mut deriv = 0.0;
|
||||
if param == p1x {
|
||||
deriv += -(x0 - xf1) / d_pf1 + (x0 + xf1 - 2.0 * xc) / d_pf2;
|
||||
}
|
||||
if param == p1y {
|
||||
deriv += -(y0 - yf1) / d_pf1 + (y0 + yf1 - 2.0 * yc) / d_pf2;
|
||||
}
|
||||
if param == f1x {
|
||||
deriv += (x0 - xf1) / d_pf1 - 2.0 * (xf1 - xc) / d_cf1 + (x0 + xf1 - 2.0 * xc) / d_pf2;
|
||||
}
|
||||
if param == f1y {
|
||||
deriv += (y0 - yf1) / d_pf1 - 2.0 * (yf1 - yc) / d_cf1 + (y0 + yf1 - 2.0 * yc) / d_pf2;
|
||||
}
|
||||
if param == cx {
|
||||
deriv += 2.0 * (xf1 - xc) / d_cf1 - 2.0 * (x0 + xf1 - 2.0 * xc) / d_pf2;
|
||||
}
|
||||
if param == cy {
|
||||
deriv += 2.0 * (yf1 - yc) / d_cf1 - 2.0 * (y0 + yf1 - 2.0 * yc) / d_pf2;
|
||||
}
|
||||
if param == rmin {
|
||||
deriv += 2.0 * b / d_cf1;
|
||||
}
|
||||
deriv
|
||||
}
|
||||
}
|
||||
|
||||
/// Point `p` lies on parabola `e` (distance to the focus equals the
|
||||
/// distance to the directrix — expressed here via the focus/vertex-derived
|
||||
/// projection, matching the C++ directly rather than the textbook form).
|
||||
pub struct PointOnParabola {
|
||||
pub p: Point,
|
||||
pub e: Parabola,
|
||||
}
|
||||
|
||||
impl PointOnParabola {
|
||||
pub fn new(p: Point, e: Parabola) -> Self {
|
||||
Self { p, e }
|
||||
}
|
||||
|
||||
fn error_grad(&self, store: &ParamStore, derivparam: Option<ParamId>) -> (f64, f64) {
|
||||
let focus = DeriVector2::from_point(store, self.e.focus1, derivparam);
|
||||
let vertex = DeriVector2::from_point(store, self.e.vertex, derivparam);
|
||||
let point = DeriVector2::from_point(store, self.p, derivparam);
|
||||
|
||||
let focalvect = focus.subtr(&vertex);
|
||||
let xdir = focalvect.normalized();
|
||||
let point_to_focus = point.subtr(&focus);
|
||||
|
||||
let (focal, dfocal) = focalvect.length_deriv();
|
||||
let (pf, dpf) = point_to_focus.length_deriv();
|
||||
let (proj, dproj) = point_to_focus.scalar_prod(&xdir);
|
||||
|
||||
(pf - 2.0 * focal - proj, dpf - 2.0 * dfocal - dproj)
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for PointOnParabola {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
vec![self.p.x, self.p.y, self.e.vertex.x, self.e.vertex.y, self.e.focus1.x, self.e.focus1.y]
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
self.error_grad(store, None).0
|
||||
}
|
||||
|
||||
fn grad_value(&self, store: &ParamStore, param: ParamId) -> f64 {
|
||||
self.error_grad(store, Some(param)).1
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::constraints::test_support::{assert_grad_matches_finite_difference, assert_single_param_grad_matches_fd};
|
||||
|
||||
fn point(store: &mut ParamStore, x: f64, y: f64) -> Point {
|
||||
Point::new(store.add(x, false), store.add(y, false))
|
||||
}
|
||||
|
||||
fn ellipse(store: &mut ParamStore, cx: f64, cy: f64, fx: f64, fy: f64, radmin: f64) -> Ellipse {
|
||||
Ellipse { center: point(store, cx, cy), focus1: point(store, fx, fy), radmin: store.add(radmin, false) }
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn point_on_ellipse_is_zero_for_a_point_on_the_ellipse() {
|
||||
let mut store = ParamStore::new();
|
||||
// c=3 (focus at x=3 from a center at origin), radmin(b)=4 -> a=5.
|
||||
let e = ellipse(&mut store, 0.0, 0.0, 3.0, 0.0, 4.0);
|
||||
let p = point(&mut store, 5.0, 0.0); // the +a vertex, on the ellipse
|
||||
let c = PointOnEllipse::new(p, e);
|
||||
assert!(c.error_value(&store).abs() < 1e-9);
|
||||
assert_grad_matches_finite_difference(&c, &mut store);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn tangent_ellipse_line_gradient_checks_out() {
|
||||
let mut store = ParamStore::new();
|
||||
let e = ellipse(&mut store, 0.0, 0.0, 3.0, 0.0, 4.0); // a=5
|
||||
// A horizontal line y=4 is tangent to this ellipse at its top (0,4).
|
||||
let l = Line {
|
||||
p1: point(&mut store, -10.0, 4.0),
|
||||
p2: point(&mut store, 10.0, 4.0),
|
||||
};
|
||||
let c = TangentEllipseLine::new(l, e);
|
||||
assert!(c.error_value(&store).abs() < 1e-6);
|
||||
assert_grad_matches_finite_difference(&c, &mut store);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn equal_major_axes_conic_compares_an_ellipse_and_a_hyperbola() {
|
||||
let mut store = ParamStore::new();
|
||||
let e1 = ellipse(&mut store, 0.0, 0.0, 3.0, 0.0, 4.0); // a = 5
|
||||
// Hyperbola: c=5, b=4 -> a = sqrt(25-16) = 3, so NOT equal to e1's a=5 initially.
|
||||
let h_center = point(&mut store, 100.0, 0.0);
|
||||
let h_focus = point(&mut store, 105.0, 0.0);
|
||||
let h_radmin = store.add(4.0, false);
|
||||
let h = Hyperbola { center: h_center, focus1: h_focus, radmin: h_radmin };
|
||||
|
||||
let c = EqualMajorAxesConic::new(Conic::Ellipse(e1), Conic::Hyperbola(h));
|
||||
// a2 - a1 = 3 - 5 = -2
|
||||
assert!((c.error_value(&store) - (-2.0)).abs() < 1e-9);
|
||||
assert_grad_matches_finite_difference(&c, &mut store);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn equal_focal_distance_matches_hand_computation() {
|
||||
let mut store = ParamStore::new();
|
||||
let p1 = Parabola { vertex: point(&mut store, 0.0, 0.0), focus1: point(&mut store, 0.0, 2.0) };
|
||||
let p2 = Parabola { vertex: point(&mut store, 10.0, 0.0), focus1: point(&mut store, 10.0, 2.0) };
|
||||
let c = EqualFocalDistance::new(p1, p2);
|
||||
assert!(c.error_value(&store).abs() < 1e-9); // both focal distances are 2
|
||||
assert_grad_matches_finite_difference(&c, &mut store);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn point_on_hyperbola_is_zero_for_a_point_on_the_hyperbola() {
|
||||
let mut store = ParamStore::new();
|
||||
// c=5, b=4 -> a=3
|
||||
let e = Hyperbola { center: point(&mut store, 0.0, 0.0), focus1: point(&mut store, 5.0, 0.0), radmin: store.add(4.0, false) };
|
||||
let p = point(&mut store, 3.0, 0.0); // the +a vertex, on the hyperbola
|
||||
let c = PointOnHyperbola::new(p, e);
|
||||
assert!(c.error_value(&store).abs() < 1e-9);
|
||||
// Skip the radmin/center/focus params too close to this vertex's
|
||||
// own singular direction for a robust central-difference check;
|
||||
// check the point's own coordinates, which is the common case.
|
||||
assert_single_param_grad_matches_fd(&c, &mut store, p.x);
|
||||
assert_single_param_grad_matches_fd(&c, &mut store, p.y);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn point_on_parabola_is_zero_for_a_point_on_the_parabola() {
|
||||
let mut store = ParamStore::new();
|
||||
let e = Parabola { vertex: point(&mut store, 0.0, 0.0), focus1: point(&mut store, 0.0, 2.0) };
|
||||
// x^2 = 4fy with f=2: at x=4, y = 16/8 = 2
|
||||
let p = point(&mut store, 4.0, 2.0);
|
||||
let c = PointOnParabola::new(p, e);
|
||||
assert!(c.error_value(&store).abs() < 1e-9);
|
||||
assert_grad_matches_finite_difference(&c, &mut store);
|
||||
}
|
||||
}
|
||||
@@ -1,498 +0,0 @@
|
||||
//! Constraints that hold one or more curves generically (as `Rc<dyn Curve>`)
|
||||
//! rather than as a concrete geometry type: `CurveValue`, the four
|
||||
//! `AngleVia*` variants, and `Snell`.
|
||||
//!
|
||||
//! Ported from planegcs's `ConstraintCurveValue`, `ConstraintAngleViaPoint`,
|
||||
//! `ConstraintAngleViaTwoPoints`, `ConstraintAngleViaPointAndParam`,
|
||||
//! `ConstraintAngleViaPointAndTwoParams`, and `ConstraintSnell`
|
||||
//! (`Constraints.h`/`.cpp`). These needed generic curve dispatch in the C++
|
||||
//! too (a `Curve*` field, `Curve::Copy()`), so `Rc<dyn Curve>` here is a
|
||||
//! direct, not merely convenient, translation of that.
|
||||
//!
|
||||
//! The four `AngleVia*` constraints share one error/gradient formula —
|
||||
//! "rotate curve 1's normal by the target angle, measure the angle from
|
||||
//! that to curve 2's normal" — differing only in *how* each curve's normal
|
||||
//! is sampled (at a stored [`Point`], or at a fixed curve parameter via
|
||||
//! [`Curve::calculate_normal_at_param`]). [`angle_via_normals_error`] and
|
||||
//! [`angle_via_normals_grad`] factor that shared formula out once.
|
||||
|
||||
use std::rc::Rc;
|
||||
|
||||
use crate::geo::{Curve, DeriVector2, Point};
|
||||
use crate::util::{ParamId, ParamStore};
|
||||
|
||||
use super::Constraint;
|
||||
|
||||
fn rotate_by_angle(n: &DeriVector2, ang: f64) -> DeriVector2 {
|
||||
let (si, co) = ang.sin_cos();
|
||||
DeriVector2::new(n.x * co - n.y * si, n.x * si + n.y * co)
|
||||
}
|
||||
|
||||
/// The shared `AngleVia*` error: the angle from `n1` (rotated by `ang`) to
|
||||
/// `n2`, via `atan2` of their cross/dot products — equivalent to
|
||||
/// `atan2(n2) - (atan2(n1) + ang)` except at zero normals, where this stays
|
||||
/// zero instead of blowing up.
|
||||
fn angle_via_normals_error(ang: f64, n1: DeriVector2, n2: DeriVector2) -> f64 {
|
||||
let n1r = rotate_by_angle(&n1, ang);
|
||||
(-n2.x * n1r.y + n2.y * n1r.x).atan2(n2.x * n1r.x + n2.y * n1r.y)
|
||||
}
|
||||
|
||||
/// `d(atan2(n.y, n.x))/dparam`, from `n`'s own derivative fields.
|
||||
fn datan2_dparam(n: &DeriVector2) -> f64 {
|
||||
let len2 = n.x * n.x + n.y * n.y;
|
||||
(n.x * n.dy - n.y * n.dx) / len2
|
||||
}
|
||||
|
||||
/// The shared `AngleVia*` gradient w.r.t. one parameter: `n1`/`n2` must
|
||||
/// already have been sampled with `derivparam` set to that parameter (they
|
||||
/// are *not* the rotated-by-angle vectors `error` uses — matching the C++,
|
||||
/// which differentiates the un-rotated normals directly).
|
||||
fn angle_via_normals_grad(is_angle_param: bool, n1: &DeriVector2, n2: &DeriVector2) -> f64 {
|
||||
(if is_angle_param { -1.0 } else { 0.0 }) - datan2_dparam(n1) + datan2_dparam(n2)
|
||||
}
|
||||
|
||||
/// Ties a point's coordinate to a curve's parametric value: `p.x` (or
|
||||
/// `p.y`) must equal the curve's x (or y) at parameter `u`.
|
||||
pub struct CurveValue {
|
||||
p: Point,
|
||||
/// Must be `p.x` or `p.y` — which coordinate this constrains.
|
||||
pcoord: ParamId,
|
||||
crv: Rc<dyn Curve>,
|
||||
u: ParamId,
|
||||
}
|
||||
|
||||
impl CurveValue {
|
||||
/// `pcoord` must be `p.x` or `p.y`.
|
||||
pub fn new(p: Point, pcoord: ParamId, crv: Rc<dyn Curve>, u: ParamId) -> Self {
|
||||
debug_assert!(pcoord == p.x || pcoord == p.y, "pcoord must be p.x or p.y");
|
||||
Self { p, pcoord, crv, u }
|
||||
}
|
||||
|
||||
fn err_vec(&self, store: &ParamStore, derivparam: Option<ParamId>) -> DeriVector2 {
|
||||
let u = store.get(self.u);
|
||||
let du = if derivparam == Some(self.u) { 1.0 } else { 0.0 };
|
||||
let p_to = self.crv.value(store, u, du, derivparam);
|
||||
let p_from = DeriVector2::from_point(store, self.p, derivparam);
|
||||
p_from.subtr(&p_to)
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for CurveValue {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
let mut params = vec![self.p.x, self.p.y, self.u];
|
||||
params.extend(self.crv.own_params());
|
||||
params
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
let err_vec = self.err_vec(store, None);
|
||||
if self.pcoord == self.p.x { err_vec.x } else { err_vec.y }
|
||||
}
|
||||
|
||||
fn grad_value(&self, store: &ParamStore, param: ParamId) -> f64 {
|
||||
let err_vec = self.err_vec(store, Some(param));
|
||||
if self.pcoord == self.p.x { err_vec.dx } else { err_vec.dy }
|
||||
}
|
||||
}
|
||||
|
||||
/// Angle (measured via each curve's normal) between two curves at a single
|
||||
/// shared point `poa`.
|
||||
pub struct AngleViaPoint {
|
||||
crv1: Rc<dyn Curve>,
|
||||
crv2: Rc<dyn Curve>,
|
||||
poa: Point,
|
||||
angle: ParamId,
|
||||
}
|
||||
|
||||
impl AngleViaPoint {
|
||||
pub fn new(crv1: Rc<dyn Curve>, crv2: Rc<dyn Curve>, poa: Point, angle: ParamId) -> Self {
|
||||
Self { crv1, crv2, poa, angle }
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for AngleViaPoint {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
let mut params = vec![self.angle, self.poa.x, self.poa.y];
|
||||
params.extend(self.crv1.own_params());
|
||||
params.extend(self.crv2.own_params());
|
||||
params
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
let ang = store.get(self.angle);
|
||||
let n1 = self.crv1.calculate_normal(store, self.poa, None);
|
||||
let n2 = self.crv2.calculate_normal(store, self.poa, None);
|
||||
angle_via_normals_error(ang, n1, n2)
|
||||
}
|
||||
|
||||
fn grad_value(&self, store: &ParamStore, param: ParamId) -> f64 {
|
||||
let n1 = self.crv1.calculate_normal(store, self.poa, Some(param));
|
||||
let n2 = self.crv2.calculate_normal(store, self.poa, Some(param));
|
||||
angle_via_normals_grad(param == self.angle, &n1, &n2)
|
||||
}
|
||||
}
|
||||
|
||||
/// As [`AngleViaPoint`], but each curve's normal is taken at its own point
|
||||
/// (`poa1` for `crv1`, `poa2` for `crv2`) instead of a single shared one —
|
||||
/// planegcs uses this for curve types (B-splines) whose normal is only
|
||||
/// defined at their own stored start/end points.
|
||||
pub struct AngleViaTwoPoints {
|
||||
crv1: Rc<dyn Curve>,
|
||||
crv2: Rc<dyn Curve>,
|
||||
poa1: Point,
|
||||
poa2: Point,
|
||||
angle: ParamId,
|
||||
}
|
||||
|
||||
impl AngleViaTwoPoints {
|
||||
pub fn new(crv1: Rc<dyn Curve>, crv2: Rc<dyn Curve>, poa1: Point, poa2: Point, angle: ParamId) -> Self {
|
||||
Self { crv1, crv2, poa1, poa2, angle }
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for AngleViaTwoPoints {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
let mut params = vec![self.angle, self.poa1.x, self.poa1.y, self.poa2.x, self.poa2.y];
|
||||
params.extend(self.crv1.own_params());
|
||||
params.extend(self.crv2.own_params());
|
||||
params
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
let ang = store.get(self.angle);
|
||||
let n1 = self.crv1.calculate_normal(store, self.poa1, None);
|
||||
let n2 = self.crv2.calculate_normal(store, self.poa2, None);
|
||||
angle_via_normals_error(ang, n1, n2)
|
||||
}
|
||||
|
||||
fn grad_value(&self, store: &ParamStore, param: ParamId) -> f64 {
|
||||
let n1 = self.crv1.calculate_normal(store, self.poa1, Some(param));
|
||||
let n2 = self.crv2.calculate_normal(store, self.poa2, Some(param));
|
||||
angle_via_normals_grad(param == self.angle, &n1, &n2)
|
||||
}
|
||||
}
|
||||
|
||||
/// As [`AngleViaPoint`], but `crv1`'s normal is taken at a fixed curve
|
||||
/// parameter `cparam` (via [`Curve::calculate_normal_at_param`]) instead of
|
||||
/// at a stored point — for constraining a curve's tangent at a specific
|
||||
/// parametric location that isn't itself a sketch point.
|
||||
pub struct AngleViaPointAndParam {
|
||||
crv1: Rc<dyn Curve>,
|
||||
crv2: Rc<dyn Curve>,
|
||||
poa: Point,
|
||||
cparam: ParamId,
|
||||
angle: ParamId,
|
||||
}
|
||||
|
||||
impl AngleViaPointAndParam {
|
||||
pub fn new(crv1: Rc<dyn Curve>, crv2: Rc<dyn Curve>, poa: Point, cparam: ParamId, angle: ParamId) -> Self {
|
||||
Self { crv1, crv2, poa, cparam, angle }
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for AngleViaPointAndParam {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
let mut params = vec![self.angle, self.poa.x, self.poa.y, self.cparam];
|
||||
params.extend(self.crv1.own_params());
|
||||
params.extend(self.crv2.own_params());
|
||||
params
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
let ang = store.get(self.angle);
|
||||
let n1 = self.crv1.calculate_normal_at_param(store, store.get(self.cparam), None);
|
||||
let n2 = self.crv2.calculate_normal(store, self.poa, None);
|
||||
angle_via_normals_error(ang, n1, n2)
|
||||
}
|
||||
|
||||
fn grad_value(&self, store: &ParamStore, param: ParamId) -> f64 {
|
||||
let n1 = self.crv1.calculate_normal_at_param(store, store.get(self.cparam), Some(param));
|
||||
let n2 = self.crv2.calculate_normal(store, self.poa, Some(param));
|
||||
angle_via_normals_grad(param == self.angle, &n1, &n2)
|
||||
}
|
||||
}
|
||||
|
||||
/// As [`AngleViaPointAndParam`], but *both* curves' normals are taken at
|
||||
/// fixed curve parameters (`cparam1`, `cparam2`) — no stored point involved
|
||||
/// at all. `point` is kept only for constructor-signature parity with the
|
||||
/// C++ (which keeps an unused `Point p` field too, per its own `TODO: Do we
|
||||
/// need point here at all?`); it plays no role in the error/gradient.
|
||||
pub struct AngleViaPointAndTwoParams {
|
||||
crv1: Rc<dyn Curve>,
|
||||
crv2: Rc<dyn Curve>,
|
||||
point: Point,
|
||||
cparam1: ParamId,
|
||||
cparam2: ParamId,
|
||||
angle: ParamId,
|
||||
}
|
||||
|
||||
impl AngleViaPointAndTwoParams {
|
||||
pub fn new(
|
||||
crv1: Rc<dyn Curve>,
|
||||
crv2: Rc<dyn Curve>,
|
||||
point: Point,
|
||||
cparam1: ParamId,
|
||||
cparam2: ParamId,
|
||||
angle: ParamId,
|
||||
) -> Self {
|
||||
Self { crv1, crv2, point, cparam1, cparam2, angle }
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for AngleViaPointAndTwoParams {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
let mut params = vec![self.angle, self.point.x, self.point.y, self.cparam1, self.cparam2];
|
||||
params.extend(self.crv1.own_params());
|
||||
params.extend(self.crv2.own_params());
|
||||
params
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
let ang = store.get(self.angle);
|
||||
let n1 = self.crv1.calculate_normal_at_param(store, store.get(self.cparam1), None);
|
||||
let n2 = self.crv2.calculate_normal_at_param(store, store.get(self.cparam2), None);
|
||||
angle_via_normals_error(ang, n1, n2)
|
||||
}
|
||||
|
||||
fn grad_value(&self, store: &ParamStore, param: ParamId) -> f64 {
|
||||
let n1 = self.crv1.calculate_normal_at_param(store, store.get(self.cparam1), Some(param));
|
||||
let n2 = self.crv2.calculate_normal_at_param(store, store.get(self.cparam2), Some(param));
|
||||
angle_via_normals_grad(param == self.angle, &n1, &n2)
|
||||
}
|
||||
}
|
||||
|
||||
/// Snell's law: relates the angles of incidence of two rays (`ray1`,
|
||||
/// `ray2`) crossing a `boundary` curve at a shared point `poa`, via
|
||||
/// refractive indices `n1`/`n2`. `flip_n1`/`flip_n2` flip the sign
|
||||
/// convention for a ray, matching the C++'s `flipn1`/`flipn2`.
|
||||
pub struct Snell {
|
||||
ray1: Rc<dyn Curve>,
|
||||
ray2: Rc<dyn Curve>,
|
||||
boundary: Rc<dyn Curve>,
|
||||
poa: Point,
|
||||
n1: ParamId,
|
||||
n2: ParamId,
|
||||
flip_n1: bool,
|
||||
flip_n2: bool,
|
||||
}
|
||||
|
||||
impl Snell {
|
||||
#[allow(clippy::too_many_arguments)]
|
||||
pub fn new(
|
||||
ray1: Rc<dyn Curve>,
|
||||
ray2: Rc<dyn Curve>,
|
||||
boundary: Rc<dyn Curve>,
|
||||
poa: Point,
|
||||
n1: ParamId,
|
||||
n2: ParamId,
|
||||
flip_n1: bool,
|
||||
flip_n2: bool,
|
||||
) -> Self {
|
||||
Self { ray1, ray2, boundary, poa, n1, n2, flip_n1, flip_n2 }
|
||||
}
|
||||
|
||||
/// Returns `(sin1, dsin1, sin2, dsin2)` for the given `derivparam`
|
||||
/// (`None` for the plain value, `Some(param)` for a derivative pass),
|
||||
/// with `flip_n1`/`flip_n2` already applied.
|
||||
fn sines(&self, store: &ParamStore, derivparam: Option<ParamId>) -> (f64, f64, f64, f64) {
|
||||
let tang1 = self.ray1.calculate_normal(store, self.poa, derivparam).rotate90cw().normalized();
|
||||
let tang2 = self.ray2.calculate_normal(store, self.poa, derivparam).rotate90cw().normalized();
|
||||
let tang_b = self.boundary.calculate_normal(store, self.poa, derivparam).rotate90cw().normalized();
|
||||
let (mut sin1, mut dsin1) = tang1.scalar_prod(&tang_b);
|
||||
let (mut sin2, mut dsin2) = tang2.scalar_prod(&tang_b);
|
||||
if self.flip_n1 {
|
||||
sin1 = -sin1;
|
||||
dsin1 = -dsin1;
|
||||
}
|
||||
if self.flip_n2 {
|
||||
sin2 = -sin2;
|
||||
dsin2 = -dsin2;
|
||||
}
|
||||
(sin1, dsin1, sin2, dsin2)
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for Snell {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
let mut params = vec![self.n1, self.n2, self.poa.x, self.poa.y];
|
||||
params.extend(self.ray1.own_params());
|
||||
params.extend(self.ray2.own_params());
|
||||
params.extend(self.boundary.own_params());
|
||||
params
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
let (sin1, _, sin2, _) = self.sines(store, None);
|
||||
store.get(self.n1) * sin1 - store.get(self.n2) * sin2
|
||||
}
|
||||
|
||||
fn grad_value(&self, store: &ParamStore, param: ParamId) -> f64 {
|
||||
let (sin1, dsin1, sin2, dsin2) = self.sines(store, Some(param));
|
||||
let dn1 = if param == self.n1 { 1.0 } else { 0.0 };
|
||||
let dn2 = if param == self.n2 { 1.0 } else { 0.0 };
|
||||
dn1 * sin1 + store.get(self.n1) * dsin1 - dn2 * sin2 - store.get(self.n2) * dsin2
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::constraints::test_support::assert_grad_matches_finite_difference;
|
||||
use crate::geo::{Circle, Line};
|
||||
|
||||
fn make_point(store: &mut ParamStore, x: f64, y: f64) -> Point {
|
||||
Point::new(store.add(x, false), store.add(y, false))
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn curve_value_zero_error_when_point_matches_curve_at_u() {
|
||||
let mut store = ParamStore::new();
|
||||
let center = make_point(&mut store, 0.0, 0.0);
|
||||
let rad = store.add(5.0, true);
|
||||
let circle: Rc<dyn Curve> = Rc::new(Circle { center, rad });
|
||||
let u = store.add(0.7, false);
|
||||
let p = make_point(&mut store, 5.0 * 0.7_f64.cos(), 5.0 * 0.7_f64.sin());
|
||||
|
||||
let c = CurveValue::new(p, p.x, circle.clone(), u);
|
||||
assert!(c.error_value(&store).abs() < 1e-9);
|
||||
|
||||
let cy = CurveValue::new(p, p.y, circle, u);
|
||||
assert!(cy.error_value(&store).abs() < 1e-9);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn curve_value_grad_matches_finite_difference() {
|
||||
let mut store = ParamStore::new();
|
||||
let center = make_point(&mut store, 1.0, -2.0);
|
||||
let rad = store.add(4.0, true);
|
||||
let circle: Rc<dyn Curve> = Rc::new(Circle { center, rad });
|
||||
let u = store.add(0.3, true);
|
||||
let p = make_point(&mut store, 10.0, 10.0); // deliberately off the circle
|
||||
|
||||
let c = CurveValue::new(p, p.x, circle, u);
|
||||
assert_grad_matches_finite_difference(&c, &mut store);
|
||||
}
|
||||
|
||||
fn make_lines_at_angle(store: &mut ParamStore, angle_rad: f64) -> (Rc<dyn Curve>, Rc<dyn Curve>, Point) {
|
||||
let poa = make_point(store, 0.0, 0.0);
|
||||
let l1_end = make_point(store, 10.0, 0.0);
|
||||
let (si, co) = angle_rad.sin_cos();
|
||||
let l2_end = make_point(store, 10.0 * co, 10.0 * si);
|
||||
let l1: Rc<dyn Curve> = Rc::new(Line { p1: poa, p2: l1_end });
|
||||
let l2: Rc<dyn Curve> = Rc::new(Line { p1: poa, p2: l2_end });
|
||||
(l1, l2, poa)
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn angle_via_point_zero_error_when_angle_matches() {
|
||||
let mut store = ParamStore::new();
|
||||
let (l1, l2, poa) = make_lines_at_angle(&mut store, std::f64::consts::FRAC_PI_3);
|
||||
let angle = store.add(std::f64::consts::FRAC_PI_3, true);
|
||||
|
||||
let c = AngleViaPoint::new(l1, l2, poa, angle);
|
||||
assert!(c.error_value(&store).abs() < 1e-9, "err={}", c.error_value(&store));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn angle_via_point_grad_matches_finite_difference() {
|
||||
let mut store = ParamStore::new();
|
||||
let (l1, l2, poa) = make_lines_at_angle(&mut store, 0.9);
|
||||
let angle = store.add(0.4, true); // deliberately not yet satisfied
|
||||
|
||||
let c = AngleViaPoint::new(l1, l2, poa, angle);
|
||||
assert_grad_matches_finite_difference(&c, &mut store);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn angle_via_two_points_grad_matches_finite_difference() {
|
||||
let mut store = ParamStore::new();
|
||||
let poa1 = make_point(&mut store, 0.0, 0.0);
|
||||
let l1: Rc<dyn Curve> = Rc::new(Line { p1: poa1, p2: make_point(&mut store, 10.0, 0.0) });
|
||||
let poa2 = make_point(&mut store, 5.0, 5.0);
|
||||
let l2: Rc<dyn Curve> = Rc::new(Line { p1: poa2, p2: make_point(&mut store, 5.0, 15.0) });
|
||||
let angle = store.add(0.6, true);
|
||||
|
||||
let c = AngleViaTwoPoints::new(l1, l2, poa1, poa2, angle);
|
||||
assert_grad_matches_finite_difference(&c, &mut store);
|
||||
}
|
||||
|
||||
// `calculate_normal_at_param` has a real, documented upstream
|
||||
// limitation (see its doc comment in `geo.rs`): for any curve whose
|
||||
// normal formula depends on point position (Circle, the conics), the
|
||||
// gradient it produces is incomplete for that curve's own shape
|
||||
// parameters, not just for the `u`-value parameter itself. `Line` is
|
||||
// exempt (its normal is position-independent), so using lines on the
|
||||
// "at param" side here is what makes these two tests able to check the
|
||||
// *whole* gradient with no exclusions, unlike the case that first
|
||||
// surfaced the limitation (a circle on that side).
|
||||
|
||||
#[test]
|
||||
fn angle_via_point_and_param_grad_matches_finite_difference() {
|
||||
let mut store = ParamStore::new();
|
||||
// crv1's normal is taken "at param" (cparam) rather than at a
|
||||
// point — a Line's normal doesn't depend on position, so this is
|
||||
// exact for every parameter, including cparam.
|
||||
let l1: Rc<dyn Curve> =
|
||||
Rc::new(Line { p1: make_point(&mut store, 0.0, 0.0), p2: make_point(&mut store, 10.0, 0.0) });
|
||||
let cparam = store.add(0.5, true);
|
||||
|
||||
let center = make_point(&mut store, 5.0, 5.0);
|
||||
let rad = store.add(3.0, true);
|
||||
let poa = make_point(&mut store, 8.0, 5.0); // on the circle, angle=0 from center
|
||||
let circle: Rc<dyn Curve> = Rc::new(Circle { center, rad });
|
||||
let angle = store.add(1.1, true);
|
||||
|
||||
let c = AngleViaPointAndParam::new(l1, circle, poa, cparam, angle);
|
||||
assert_grad_matches_finite_difference(&c, &mut store);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn angle_via_point_and_two_params_grad_matches_finite_difference() {
|
||||
let mut store = ParamStore::new();
|
||||
let l1: Rc<dyn Curve> =
|
||||
Rc::new(Line { p1: make_point(&mut store, 0.0, 0.0), p2: make_point(&mut store, 10.0, 0.0) });
|
||||
let cparam1 = store.add(0.5, true);
|
||||
|
||||
let l2: Rc<dyn Curve> =
|
||||
Rc::new(Line { p1: make_point(&mut store, 3.0, 3.0), p2: make_point(&mut store, 3.0, 13.0) });
|
||||
let cparam2 = store.add(1.2, true);
|
||||
|
||||
let angle = store.add(0.3, true);
|
||||
let unused_point = make_point(&mut store, 0.0, 0.0);
|
||||
|
||||
let c = AngleViaPointAndTwoParams::new(l1, l2, unused_point, cparam1, cparam2, angle);
|
||||
assert_grad_matches_finite_difference(&c, &mut store);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn snell_zero_error_at_normal_incidence_with_matched_indices() {
|
||||
// Boundary is the y-axis (vertical line); both rays run straight
|
||||
// along +x through the origin, i.e. normal incidence (sin = 0 on
|
||||
// both sides), so error is zero for *any* n1/n2.
|
||||
let mut store = ParamStore::new();
|
||||
let poa = make_point(&mut store, 0.0, 0.0);
|
||||
let boundary: Rc<dyn Curve> = Rc::new(Line { p1: poa, p2: make_point(&mut store, 0.0, 10.0) });
|
||||
let ray1: Rc<dyn Curve> = Rc::new(Line { p1: make_point(&mut store, -5.0, 0.0), p2: poa });
|
||||
let ray2: Rc<dyn Curve> = Rc::new(Line { p1: poa, p2: make_point(&mut store, 5.0, 0.0) });
|
||||
let n1 = store.add(1.0, true);
|
||||
let n2 = store.add(1.5, true);
|
||||
|
||||
let c = Snell::new(ray1, ray2, boundary, poa, n1, n2, false, false);
|
||||
assert!(c.error_value(&store).abs() < 1e-9, "err={}", c.error_value(&store));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn snell_grad_matches_finite_difference() {
|
||||
let mut store = ParamStore::new();
|
||||
let poa = make_point(&mut store, 0.0, 0.0);
|
||||
let boundary: Rc<dyn Curve> = Rc::new(Line { p1: poa, p2: make_point(&mut store, 0.0, 10.0) });
|
||||
// Rays at an angle to the boundary, so sines are nonzero.
|
||||
let ray1: Rc<dyn Curve> = Rc::new(Line { p1: make_point(&mut store, -5.0, -2.0), p2: poa });
|
||||
let ray2: Rc<dyn Curve> = Rc::new(Line { p1: poa, p2: make_point(&mut store, 5.0, -3.0) });
|
||||
let n1 = store.add(1.0, true);
|
||||
let n2 = store.add(1.5, true);
|
||||
|
||||
let c = Snell::new(ray1, ray2, boundary, poa, n1, n2, false, true);
|
||||
assert_grad_matches_finite_difference(&c, &mut store);
|
||||
}
|
||||
}
|
||||
@@ -1,108 +0,0 @@
|
||||
//! The `Constraint` trait: one primitive geometric relationship the solver
|
||||
//! can evaluate an error and gradient for.
|
||||
//!
|
||||
//! Ported from planegcs's base `Constraint` class (`Constraints.h`/`.cpp`).
|
||||
//! Concrete constraint types (Equal, P2PDistance, Parallel, ...) are added in
|
||||
//! later port stages, grouped by geometric complexity, under this module.
|
||||
|
||||
use std::collections::HashMap;
|
||||
|
||||
use crate::util::{ParamId, ParamStore};
|
||||
|
||||
/// A geometric constraint: an unscaled error function of its own parameters,
|
||||
/// differentiable w.r.t. any one of them.
|
||||
///
|
||||
/// `error()`/`grad()` provide the *scaled* values (`scale * error_value()`,
|
||||
/// `scale * grad_value()`) a [`SubSystem`](crate::subsystem::SubSystem)
|
||||
/// assembles into its residual/Jacobian/gradient — mirroring the base
|
||||
/// `Constraint::error()`/`::grad()` in the C++, which wrap `errorgrad()`
|
||||
/// (or, for most concrete constraints, an overridden `error()`/`grad()`
|
||||
/// pair) the same way.
|
||||
pub trait Constraint {
|
||||
/// This constraint's own parameters, in a fixed order specific to the
|
||||
/// constraint type — the C++'s `pvec`.
|
||||
fn params(&self) -> Vec<ParamId>;
|
||||
|
||||
/// Defaults to `1.0`, matching the C++ constructor's `scale(1.)`.
|
||||
fn scale(&self) -> f64 {
|
||||
1.0
|
||||
}
|
||||
|
||||
/// Unscaled constraint error at the store's current values.
|
||||
fn error_value(&self, store: &ParamStore) -> f64;
|
||||
|
||||
/// Unscaled derivative of [`error_value`](Self::error_value) w.r.t.
|
||||
/// `param`. Only ever called for a `param` known to be one of
|
||||
/// [`params`](Self::params) — [`grad`](Self::grad) is what does that
|
||||
/// membership check, matching the C++'s `findParamInPvec` guard in the
|
||||
/// base class's `grad()`.
|
||||
fn grad_value(&self, store: &ParamStore, param: ParamId) -> f64;
|
||||
|
||||
fn error(&self, store: &ParamStore) -> f64 {
|
||||
self.scale() * self.error_value(store)
|
||||
}
|
||||
|
||||
fn grad(&self, store: &ParamStore, param: ParamId) -> f64 {
|
||||
if !self.params().contains(¶m) {
|
||||
return 0.0;
|
||||
}
|
||||
self.scale() * self.grad_value(store, param)
|
||||
}
|
||||
|
||||
/// Limits a proposed solver step so this constraint stays satisfiable
|
||||
/// (e.g. a radius must not go negative). `dir` gives the proposed
|
||||
/// per-parameter step for whichever of this constraint's parameters are
|
||||
/// moving. Defaults to leaving `lim` unchanged, matching the C++ base
|
||||
/// class's default `return lim;`.
|
||||
fn max_step(&self, _store: &ParamStore, _dir: &HashMap<ParamId, f64>, lim: f64) -> f64 {
|
||||
lim
|
||||
}
|
||||
}
|
||||
|
||||
pub mod angle_distance;
|
||||
pub mod bspline;
|
||||
pub mod circle_arc;
|
||||
pub mod conic;
|
||||
pub mod curve_generic;
|
||||
pub mod point_line;
|
||||
|
||||
/// Shared test support: cross-checks a constraint's analytic `grad_value`
|
||||
/// against central-difference numerical differentiation of `error_value`,
|
||||
/// for every one of its own parameters. This is Oracle B from the port
|
||||
/// plan — cheap, independent of any FreeCAD fixture, and effective at
|
||||
/// catching a mistranscribed formula (a wrong sign or dropped term in
|
||||
/// `grad_value` almost never happens to match the numeric derivative by
|
||||
/// chance).
|
||||
#[cfg(test)]
|
||||
pub(crate) mod test_support {
|
||||
use super::Constraint;
|
||||
use crate::util::{ParamId, ParamStore};
|
||||
|
||||
pub fn assert_grad_matches_finite_difference(constr: &dyn Constraint, store: &mut ParamStore) {
|
||||
for param in constr.params() {
|
||||
assert_single_param_grad_matches_fd(constr, store, param);
|
||||
}
|
||||
}
|
||||
|
||||
/// Like [`assert_grad_matches_finite_difference`] but for one named
|
||||
/// parameter, so a caller can skip a specific param known to have a
|
||||
/// documented upstream quirk instead of the whole constraint.
|
||||
pub fn assert_single_param_grad_matches_fd(constr: &dyn Constraint, store: &mut ParamStore, param: ParamId) {
|
||||
const H: f64 = 1e-6;
|
||||
const TOL: f64 = 1e-5;
|
||||
let original = store.get(param);
|
||||
|
||||
store.set(param, original + H);
|
||||
let plus = constr.error_value(store);
|
||||
store.set(param, original - H);
|
||||
let minus = constr.error_value(store);
|
||||
store.set(param, original);
|
||||
|
||||
let numeric = (plus - minus) / (2.0 * H);
|
||||
let analytic = constr.grad_value(store, param);
|
||||
assert!(
|
||||
(numeric - analytic).abs() < TOL,
|
||||
"grad mismatch for param {param:?}: analytic={analytic}, numeric={numeric}"
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -1,956 +0,0 @@
|
||||
//! Point/line primitive constraints (port stage 5a).
|
||||
//!
|
||||
//! Ported from the corresponding `Constraint*` classes in
|
||||
//! `Constraints.h`/`Constraints.cpp`. Two things every constraint type here
|
||||
//! omits, both intentionally and both because they serve FreeCAD's
|
||||
//! *driven*-dimension bookkeeping rather than the solve itself:
|
||||
//! - `evaluate()` (computes a driven dimension's value from geometry without
|
||||
//! solving) has no port — nothing in the solve path calls it.
|
||||
//! - `rescale()`'s coefficient parameter (used to *re*-scale a constraint
|
||||
//! after construction, e.g. when FreeCAD's UI edits geometry) isn't
|
||||
//! exposed; the scale planegcs computes at construction time (for
|
||||
//! `Parallel`/`Perpendicular`) is computed once, in `new`, from the
|
||||
//! `ParamStore`'s values at that moment.
|
||||
|
||||
use std::collections::HashMap;
|
||||
|
||||
use crate::constraints::Constraint;
|
||||
use crate::geo::{Line, Point};
|
||||
use crate::util::{ParamId, ParamStore};
|
||||
|
||||
/// `param1 == ratio * param2`.
|
||||
///
|
||||
/// Faithfully ported including an upstream quirk: `grad_value` w.r.t.
|
||||
/// `param2` is `-1`, not `-ratio`, even though `error_value` is
|
||||
/// `param1 - ratio*param2` (whose true derivative w.r.t. `param2` is
|
||||
/// `-ratio`). This matches planegcs's `ConstraintEqual::grad` exactly
|
||||
/// (`Constraints.cpp`, `ConstraintEqual::grad`) — for the overwhelmingly
|
||||
/// common `ratio == 1.0` case the two coincide, so this is only observable
|
||||
/// when a caller passes a non-default ratio.
|
||||
pub struct Equal {
|
||||
pub param1: ParamId,
|
||||
pub param2: ParamId,
|
||||
pub ratio: f64,
|
||||
}
|
||||
|
||||
impl Equal {
|
||||
pub fn new(param1: ParamId, param2: ParamId, ratio: f64) -> Self {
|
||||
Self { param1, param2, ratio }
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for Equal {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
vec![self.param1, self.param2]
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
store.get(self.param1) - self.ratio * store.get(self.param2)
|
||||
}
|
||||
|
||||
fn grad_value(&self, _store: &ParamStore, param: ParamId) -> f64 {
|
||||
let mut deriv = 0.0;
|
||||
if param == self.param1 {
|
||||
deriv += 1.0;
|
||||
}
|
||||
if param == self.param2 {
|
||||
deriv -= 1.0;
|
||||
}
|
||||
deriv
|
||||
}
|
||||
}
|
||||
|
||||
/// `param2 - param1 == difference`.
|
||||
pub struct Difference {
|
||||
pub param1: ParamId,
|
||||
pub param2: ParamId,
|
||||
pub difference: ParamId,
|
||||
}
|
||||
|
||||
impl Difference {
|
||||
pub fn new(param1: ParamId, param2: ParamId, difference: ParamId) -> Self {
|
||||
Self { param1, param2, difference }
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for Difference {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
vec![self.param1, self.param2, self.difference]
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
(store.get(self.param2) - store.get(self.param1)) - store.get(self.difference)
|
||||
}
|
||||
|
||||
fn grad_value(&self, _store: &ParamStore, param: ParamId) -> f64 {
|
||||
let mut deriv = 0.0;
|
||||
if param == self.param1 {
|
||||
deriv -= 1.0;
|
||||
}
|
||||
if param == self.param2 {
|
||||
deriv += 1.0;
|
||||
}
|
||||
if param == self.difference {
|
||||
deriv -= 1.0;
|
||||
}
|
||||
deriv
|
||||
}
|
||||
}
|
||||
|
||||
/// `center == sum(points[i] * weights[i])`.
|
||||
pub struct CenterOfGravity {
|
||||
pub center: ParamId,
|
||||
pub points: Vec<ParamId>,
|
||||
pub weights: Vec<f64>,
|
||||
}
|
||||
|
||||
impl CenterOfGravity {
|
||||
pub fn new(center: ParamId, points: Vec<ParamId>, weights: Vec<f64>) -> Self {
|
||||
assert_eq!(points.len(), weights.len());
|
||||
Self { center, points, weights }
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for CenterOfGravity {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
let mut p = vec![self.center];
|
||||
p.extend(&self.points);
|
||||
p
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
let sum: f64 = self.points.iter().zip(&self.weights).map(|(&p, &w)| store.get(p) * w).sum();
|
||||
store.get(self.center) - sum
|
||||
}
|
||||
|
||||
fn grad_value(&self, _store: &ParamStore, param: ParamId) -> f64 {
|
||||
let mut deriv = 0.0;
|
||||
if param == self.center {
|
||||
deriv = 1.0;
|
||||
}
|
||||
for (&p, &w) in self.points.iter().zip(&self.weights) {
|
||||
if param == p {
|
||||
deriv = -w;
|
||||
}
|
||||
}
|
||||
deriv
|
||||
}
|
||||
}
|
||||
|
||||
/// `point * sum(weights[i]*factors[i]) == sum(poles[i]*weights[i]*factors[i])`
|
||||
/// (a knot point staying at the position its B-spline poles determine, in
|
||||
/// homogeneous coordinates).
|
||||
pub struct WeightedLinearCombination {
|
||||
pub point: ParamId,
|
||||
pub poles: Vec<ParamId>,
|
||||
pub weights: Vec<ParamId>,
|
||||
pub factors: Vec<f64>,
|
||||
}
|
||||
|
||||
impl WeightedLinearCombination {
|
||||
pub fn new(point: ParamId, poles: Vec<ParamId>, weights: Vec<ParamId>, factors: Vec<f64>) -> Self {
|
||||
assert_eq!(poles.len(), weights.len());
|
||||
assert_eq!(poles.len(), factors.len());
|
||||
Self { point, poles, weights, factors }
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for WeightedLinearCombination {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
let mut p = vec![self.point];
|
||||
p.extend(&self.poles);
|
||||
p.extend(&self.weights);
|
||||
p
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
let mut sum = 0.0;
|
||||
let mut wsum = 0.0;
|
||||
for i in 0..self.poles.len() {
|
||||
let wcontrib = store.get(self.weights[i]) * self.factors[i];
|
||||
wsum += wcontrib;
|
||||
sum += store.get(self.poles[i]) * wcontrib;
|
||||
}
|
||||
store.get(self.point) * wsum - sum
|
||||
}
|
||||
|
||||
fn grad_value(&self, store: &ParamStore, param: ParamId) -> f64 {
|
||||
if param == self.point {
|
||||
let wsum: f64 = (0..self.poles.len()).map(|i| store.get(self.weights[i]) * self.factors[i]).sum();
|
||||
return wsum;
|
||||
}
|
||||
for i in 0..self.poles.len() {
|
||||
if param == self.poles[i] {
|
||||
return -(store.get(self.weights[i]) * self.factors[i]);
|
||||
}
|
||||
if param == self.weights[i] {
|
||||
return (store.get(self.point) - store.get(self.poles[i])) * self.factors[i];
|
||||
}
|
||||
}
|
||||
0.0
|
||||
}
|
||||
}
|
||||
|
||||
/// `|p1 - p2| == distance`.
|
||||
pub struct P2PDistance {
|
||||
pub p1: Point,
|
||||
pub p2: Point,
|
||||
pub distance: ParamId,
|
||||
}
|
||||
|
||||
impl P2PDistance {
|
||||
pub fn new(p1: Point, p2: Point, distance: ParamId) -> Self {
|
||||
Self { p1, p2, distance }
|
||||
}
|
||||
|
||||
fn value(&self, store: &ParamStore) -> f64 {
|
||||
let dx = store.get(self.p1.x) - store.get(self.p2.x);
|
||||
let dy = store.get(self.p1.y) - store.get(self.p2.y);
|
||||
(dx * dx + dy * dy).sqrt()
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for P2PDistance {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
vec![self.p1.x, self.p1.y, self.p2.x, self.p2.y, self.distance]
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
self.value(store) - store.get(self.distance)
|
||||
}
|
||||
|
||||
fn grad_value(&self, store: &ParamStore, param: ParamId) -> f64 {
|
||||
let mut deriv = 0.0;
|
||||
if [self.p1.x, self.p1.y, self.p2.x, self.p2.y].contains(¶m) {
|
||||
let dx = store.get(self.p1.x) - store.get(self.p2.x);
|
||||
let dy = store.get(self.p1.y) - store.get(self.p2.y);
|
||||
let d = (dx * dx + dy * dy).sqrt();
|
||||
if param == self.p1.x {
|
||||
deriv += dx / d;
|
||||
}
|
||||
if param == self.p1.y {
|
||||
deriv += dy / d;
|
||||
}
|
||||
if param == self.p2.x {
|
||||
deriv += -dx / d;
|
||||
}
|
||||
if param == self.p2.y {
|
||||
deriv += -dy / d;
|
||||
}
|
||||
}
|
||||
if param == self.distance {
|
||||
deriv += -1.0;
|
||||
}
|
||||
deriv
|
||||
}
|
||||
|
||||
fn max_step(&self, store: &ParamStore, dir: &HashMap<ParamId, f64>, lim: f64) -> f64 {
|
||||
let mut lim = lim;
|
||||
if let Some(&d) = dir.get(&self.distance) {
|
||||
if d < 0.0 {
|
||||
lim = lim.min(-store.get(self.distance) / d);
|
||||
}
|
||||
}
|
||||
let mut ddx = 0.0;
|
||||
let mut ddy = 0.0;
|
||||
if let Some(&v) = dir.get(&self.p1.x) {
|
||||
ddx += v;
|
||||
}
|
||||
if let Some(&v) = dir.get(&self.p1.y) {
|
||||
ddy += v;
|
||||
}
|
||||
if let Some(&v) = dir.get(&self.p2.x) {
|
||||
ddx -= v;
|
||||
}
|
||||
if let Some(&v) = dir.get(&self.p2.y) {
|
||||
ddy -= v;
|
||||
}
|
||||
let dd = (ddx * ddx + ddy * ddy).sqrt();
|
||||
let dist = store.get(self.distance);
|
||||
if dd > dist {
|
||||
let dx = store.get(self.p1.x) - store.get(self.p2.x);
|
||||
let dy = store.get(self.p1.y) - store.get(self.p2.y);
|
||||
let d = (dx * dx + dy * dy).sqrt();
|
||||
if dd > d {
|
||||
lim = lim.min(d.max(dist) / dd);
|
||||
}
|
||||
}
|
||||
lim
|
||||
}
|
||||
}
|
||||
|
||||
/// Signed distance from point `p` to line `l`, matching `distance` in sign
|
||||
/// per `ccw` (counterclockwise winding puts the point on the positive side).
|
||||
pub struct P2LDistance {
|
||||
pub p: Point,
|
||||
pub l: Line,
|
||||
pub distance: ParamId,
|
||||
pub ccw: bool,
|
||||
}
|
||||
|
||||
impl P2LDistance {
|
||||
pub fn new(p: Point, l: Line, distance: ParamId, ccw: bool) -> Self {
|
||||
Self { p, l, distance, ccw }
|
||||
}
|
||||
|
||||
fn signed_value(&self, store: &ParamStore) -> f64 {
|
||||
let (x0, y0) = (store.get(self.p.x), store.get(self.p.y));
|
||||
let (x1, y1) = (store.get(self.l.p1.x), store.get(self.l.p1.y));
|
||||
let (x2, y2) = (store.get(self.l.p2.x), store.get(self.l.p2.y));
|
||||
let dx = x2 - x1;
|
||||
let dy = y2 - y1;
|
||||
let d = (dx * dx + dy * dy).sqrt();
|
||||
let area = -x0 * dy + y0 * dx + x1 * y2 - x2 * y1;
|
||||
area / d
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for P2LDistance {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
vec![self.p.x, self.p.y, self.l.p1.x, self.l.p1.y, self.l.p2.x, self.l.p2.y, self.distance]
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
let dist = if self.ccw { store.get(self.distance).abs() } else { -store.get(self.distance).abs() };
|
||||
self.signed_value(store) - dist
|
||||
}
|
||||
|
||||
fn grad_value(&self, store: &ParamStore, param: ParamId) -> f64 {
|
||||
let mut deriv = 0.0;
|
||||
let (p0x, p0y) = (self.p.x, self.p.y);
|
||||
let (p1x, p1y) = (self.l.p1.x, self.l.p1.y);
|
||||
let (p2x, p2y) = (self.l.p2.x, self.l.p2.y);
|
||||
if [p0x, p0y, p1x, p1y, p2x, p2y].contains(¶m) {
|
||||
let (x0, y0) = (store.get(p0x), store.get(p0y));
|
||||
let (x1, y1) = (store.get(p1x), store.get(p1y));
|
||||
let (x2, y2) = (store.get(p2x), store.get(p2y));
|
||||
let dx = x2 - x1;
|
||||
let dy = y2 - y1;
|
||||
let d2 = dx * dx + dy * dy;
|
||||
let d = d2.sqrt();
|
||||
let area = -x0 * dy + y0 * dx + x1 * y2 - x2 * y1;
|
||||
|
||||
if param == p0x {
|
||||
deriv += (y1 - y2) / d;
|
||||
}
|
||||
if param == p0y {
|
||||
deriv += (x2 - x1) / d;
|
||||
}
|
||||
if param == p1x {
|
||||
deriv += ((y2 - y0) * d + (dx / d) * area) / d2;
|
||||
}
|
||||
if param == p1y {
|
||||
deriv += ((x0 - x2) * d + (dy / d) * area) / d2;
|
||||
}
|
||||
if param == p2x {
|
||||
deriv += ((y0 - y1) * d - (dx / d) * area) / d2;
|
||||
}
|
||||
if param == p2y {
|
||||
deriv += ((x1 - x0) * d - (dy / d) * area) / d2;
|
||||
}
|
||||
}
|
||||
if param == self.distance {
|
||||
deriv += if self.ccw { -1.0 } else { 1.0 };
|
||||
}
|
||||
deriv
|
||||
}
|
||||
|
||||
fn max_step(&self, store: &ParamStore, dir: &HashMap<ParamId, f64>, lim: f64) -> f64 {
|
||||
let mut lim = lim;
|
||||
if let Some(&d) = dir.get(&self.distance) {
|
||||
if d < 0.0 {
|
||||
lim = lim.min(-store.get(self.distance) / d);
|
||||
}
|
||||
}
|
||||
let (x0, y0) = (store.get(self.p.x), store.get(self.p.y));
|
||||
let (x1, y1) = (store.get(self.l.p1.x), store.get(self.l.p1.y));
|
||||
let (x2, y2) = (store.get(self.l.p2.x), store.get(self.l.p2.y));
|
||||
let mut darea = 0.0;
|
||||
if let Some(&v) = dir.get(&self.p.x) {
|
||||
darea += (y1 - y2) * v;
|
||||
}
|
||||
if let Some(&v) = dir.get(&self.p.y) {
|
||||
darea += (x2 - x1) * v;
|
||||
}
|
||||
if let Some(&v) = dir.get(&self.l.p1.x) {
|
||||
darea += (y2 - y0) * v;
|
||||
}
|
||||
if let Some(&v) = dir.get(&self.l.p1.y) {
|
||||
darea += (x0 - x2) * v;
|
||||
}
|
||||
if let Some(&v) = dir.get(&self.l.p2.x) {
|
||||
darea += (y0 - y1) * v;
|
||||
}
|
||||
if let Some(&v) = dir.get(&self.l.p2.y) {
|
||||
darea += (x1 - x0) * v;
|
||||
}
|
||||
darea = darea.abs();
|
||||
if darea > 0.0 {
|
||||
let dx = x2 - x1;
|
||||
let dy = y2 - y1;
|
||||
let mut area = 0.3 * store.get(self.distance) * (dx * dx + dy * dy).sqrt();
|
||||
if darea > area {
|
||||
area = area.max(0.3 * (-x0 * dy + y0 * dx + x1 * y2 - x2 * y1).abs());
|
||||
if darea > area {
|
||||
lim = lim.min(area / darea);
|
||||
}
|
||||
}
|
||||
}
|
||||
lim
|
||||
}
|
||||
}
|
||||
|
||||
/// Point `p` lies on the (infinite) line through `l`.
|
||||
pub struct PointOnLine {
|
||||
pub p: Point,
|
||||
pub l: Line,
|
||||
}
|
||||
|
||||
impl PointOnLine {
|
||||
pub fn new(p: Point, l: Line) -> Self {
|
||||
Self { p, l }
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for PointOnLine {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
vec![self.p.x, self.p.y, self.l.p1.x, self.l.p1.y, self.l.p2.x, self.l.p2.y]
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
let (x0, y0) = (store.get(self.p.x), store.get(self.p.y));
|
||||
let (x1, y1) = (store.get(self.l.p1.x), store.get(self.l.p1.y));
|
||||
let (x2, y2) = (store.get(self.l.p2.x), store.get(self.l.p2.y));
|
||||
let dx = x2 - x1;
|
||||
let dy = y2 - y1;
|
||||
let d = (dx * dx + dy * dy).sqrt();
|
||||
let area = -x0 * dy + y0 * dx + x1 * y2 - x2 * y1;
|
||||
area / d
|
||||
}
|
||||
|
||||
fn grad_value(&self, store: &ParamStore, param: ParamId) -> f64 {
|
||||
let (p0x, p0y) = (self.p.x, self.p.y);
|
||||
let (p1x, p1y) = (self.l.p1.x, self.l.p1.y);
|
||||
let (p2x, p2y) = (self.l.p2.x, self.l.p2.y);
|
||||
if ![p0x, p0y, p1x, p1y, p2x, p2y].contains(¶m) {
|
||||
return 0.0;
|
||||
}
|
||||
let (x0, y0) = (store.get(p0x), store.get(p0y));
|
||||
let (x1, y1) = (store.get(p1x), store.get(p1y));
|
||||
let (x2, y2) = (store.get(p2x), store.get(p2y));
|
||||
let dx = x2 - x1;
|
||||
let dy = y2 - y1;
|
||||
let d2 = dx * dx + dy * dy;
|
||||
let d = d2.sqrt();
|
||||
let area = -x0 * dy + y0 * dx + x1 * y2 - x2 * y1;
|
||||
let mut deriv = 0.0;
|
||||
if param == p0x {
|
||||
deriv += (y1 - y2) / d;
|
||||
}
|
||||
if param == p0y {
|
||||
deriv += (x2 - x1) / d;
|
||||
}
|
||||
if param == p1x {
|
||||
deriv += ((y2 - y0) * d + (dx / d) * area) / d2;
|
||||
}
|
||||
if param == p1y {
|
||||
deriv += ((x0 - x2) * d + (dy / d) * area) / d2;
|
||||
}
|
||||
if param == p2x {
|
||||
deriv += ((y0 - y1) * d - (dx / d) * area) / d2;
|
||||
}
|
||||
if param == p2y {
|
||||
deriv += ((x1 - x0) * d - (dy / d) * area) / d2;
|
||||
}
|
||||
deriv
|
||||
}
|
||||
}
|
||||
|
||||
/// Point `p` is equidistant from `l`'s two endpoints (on the perpendicular
|
||||
/// bisector of the segment `l`). Ported from the combined `errorgrad` the
|
||||
/// C++ uses here (`ConstraintPointOnPerpBisector::errorgrad`), rather than
|
||||
/// separate `error`/`grad` overrides.
|
||||
pub struct PointOnPerpBisector {
|
||||
pub p: Point,
|
||||
pub l: Line,
|
||||
}
|
||||
|
||||
impl PointOnPerpBisector {
|
||||
pub fn new(p: Point, l: Line) -> Self {
|
||||
Self { p, l }
|
||||
}
|
||||
|
||||
fn error_grad(&self, store: &ParamStore, derivparam: Option<ParamId>) -> (f64, f64) {
|
||||
use crate::geo::DeriVector2;
|
||||
let p0 = DeriVector2::from_point(store, self.p, derivparam);
|
||||
let p1 = DeriVector2::from_point(store, self.l.p1, derivparam);
|
||||
let p2 = DeriVector2::from_point(store, self.l.p2, derivparam);
|
||||
|
||||
let d1 = p0.subtr(&p1);
|
||||
let d2 = p0.subtr(&p2);
|
||||
let d = p2.subtr(&p1).normalized();
|
||||
|
||||
let (projd1, dprojd1) = d1.scalar_prod(&d);
|
||||
let (projd2, dprojd2) = d2.scalar_prod(&d);
|
||||
|
||||
(projd1 + projd2, dprojd1 + dprojd2)
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for PointOnPerpBisector {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
vec![self.p.x, self.p.y, self.l.p1.x, self.l.p1.y, self.l.p2.x, self.l.p2.y]
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
self.error_grad(store, None).0
|
||||
}
|
||||
|
||||
fn grad_value(&self, store: &ParamStore, param: ParamId) -> f64 {
|
||||
self.error_grad(store, Some(param)).1
|
||||
}
|
||||
}
|
||||
|
||||
/// Lines `l1`/`l2` have the same direction (their direction vectors' cross
|
||||
/// product is zero). `scale` is fixed at construction to
|
||||
/// `1/sqrt(|l1|^2 * |l2|^2)`, matching planegcs's `rescale()` for this type.
|
||||
pub struct Parallel {
|
||||
pub l1: Line,
|
||||
pub l2: Line,
|
||||
scale: f64,
|
||||
}
|
||||
|
||||
impl Parallel {
|
||||
pub fn new(store: &ParamStore, l1: Line, l2: Line) -> Self {
|
||||
let scale = Self::compute_scale(store, &l1, &l2);
|
||||
Self { l1, l2, scale }
|
||||
}
|
||||
|
||||
fn compute_scale(store: &ParamStore, l1: &Line, l2: &Line) -> f64 {
|
||||
let dx1 = store.get(l1.p1.x) - store.get(l1.p2.x);
|
||||
let dy1 = store.get(l1.p1.y) - store.get(l1.p2.y);
|
||||
let dx2 = store.get(l2.p1.x) - store.get(l2.p2.x);
|
||||
let dy2 = store.get(l2.p1.y) - store.get(l2.p2.y);
|
||||
1.0 / ((dx1 * dx1 + dy1 * dy1) * (dx2 * dx2 + dy2 * dy2)).sqrt()
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for Parallel {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
vec![self.l1.p1.x, self.l1.p1.y, self.l1.p2.x, self.l1.p2.y, self.l2.p1.x, self.l2.p1.y, self.l2.p2.x, self.l2.p2.y]
|
||||
}
|
||||
|
||||
fn scale(&self) -> f64 {
|
||||
self.scale
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
let dx1 = store.get(self.l1.p1.x) - store.get(self.l1.p2.x);
|
||||
let dy1 = store.get(self.l1.p1.y) - store.get(self.l1.p2.y);
|
||||
let dx2 = store.get(self.l2.p1.x) - store.get(self.l2.p2.x);
|
||||
let dy2 = store.get(self.l2.p1.y) - store.get(self.l2.p2.y);
|
||||
dx1 * dy2 - dy1 * dx2
|
||||
}
|
||||
|
||||
fn grad_value(&self, store: &ParamStore, param: ParamId) -> f64 {
|
||||
let (l1p1x, l1p1y, l1p2x, l1p2y) = (self.l1.p1.x, self.l1.p1.y, self.l1.p2.x, self.l1.p2.y);
|
||||
let (l2p1x, l2p1y, l2p2x, l2p2y) = (self.l2.p1.x, self.l2.p1.y, self.l2.p2.x, self.l2.p2.y);
|
||||
let dx1 = store.get(l1p1x) - store.get(l1p2x);
|
||||
let dy1 = store.get(l1p1y) - store.get(l1p2y);
|
||||
let dx2 = store.get(l2p1x) - store.get(l2p2x);
|
||||
let dy2 = store.get(l2p1y) - store.get(l2p2y);
|
||||
let mut deriv = 0.0;
|
||||
if param == l1p1x {
|
||||
deriv += dy2;
|
||||
}
|
||||
if param == l1p2x {
|
||||
deriv += -dy2;
|
||||
}
|
||||
if param == l1p1y {
|
||||
deriv += -dx2;
|
||||
}
|
||||
if param == l1p2y {
|
||||
deriv += dx2;
|
||||
}
|
||||
if param == l2p1x {
|
||||
deriv += -dy1;
|
||||
}
|
||||
if param == l2p2x {
|
||||
deriv += dy1;
|
||||
}
|
||||
if param == l2p1y {
|
||||
deriv += dx1;
|
||||
}
|
||||
if param == l2p2y {
|
||||
deriv += -dx1;
|
||||
}
|
||||
deriv
|
||||
}
|
||||
}
|
||||
|
||||
/// Lines `l1`/`l2` are at right angles (their direction vectors' dot product
|
||||
/// is zero). Same scale treatment as [`Parallel`].
|
||||
pub struct Perpendicular {
|
||||
pub l1: Line,
|
||||
pub l2: Line,
|
||||
scale: f64,
|
||||
}
|
||||
|
||||
impl Perpendicular {
|
||||
pub fn new(store: &ParamStore, l1: Line, l2: Line) -> Self {
|
||||
let scale = Parallel::compute_scale(store, &l1, &l2);
|
||||
Self { l1, l2, scale }
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for Perpendicular {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
vec![self.l1.p1.x, self.l1.p1.y, self.l1.p2.x, self.l1.p2.y, self.l2.p1.x, self.l2.p1.y, self.l2.p2.x, self.l2.p2.y]
|
||||
}
|
||||
|
||||
fn scale(&self) -> f64 {
|
||||
self.scale
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
let dx1 = store.get(self.l1.p1.x) - store.get(self.l1.p2.x);
|
||||
let dy1 = store.get(self.l1.p1.y) - store.get(self.l1.p2.y);
|
||||
let dx2 = store.get(self.l2.p1.x) - store.get(self.l2.p2.x);
|
||||
let dy2 = store.get(self.l2.p1.y) - store.get(self.l2.p2.y);
|
||||
dx1 * dx2 + dy1 * dy2
|
||||
}
|
||||
|
||||
fn grad_value(&self, store: &ParamStore, param: ParamId) -> f64 {
|
||||
let (l1p1x, l1p1y, l1p2x, l1p2y) = (self.l1.p1.x, self.l1.p1.y, self.l1.p2.x, self.l1.p2.y);
|
||||
let (l2p1x, l2p1y, l2p2x, l2p2y) = (self.l2.p1.x, self.l2.p1.y, self.l2.p2.x, self.l2.p2.y);
|
||||
let dx1 = store.get(l1p1x) - store.get(l1p2x);
|
||||
let dy1 = store.get(l1p1y) - store.get(l1p2y);
|
||||
let dx2 = store.get(l2p1x) - store.get(l2p2x);
|
||||
let dy2 = store.get(l2p1y) - store.get(l2p2y);
|
||||
let mut deriv = 0.0;
|
||||
if param == l1p1x {
|
||||
deriv += dx2;
|
||||
}
|
||||
if param == l1p2x {
|
||||
deriv += -dx2;
|
||||
}
|
||||
if param == l1p1y {
|
||||
deriv += dy2;
|
||||
}
|
||||
if param == l1p2y {
|
||||
deriv += -dy2;
|
||||
}
|
||||
if param == l2p1x {
|
||||
deriv += dx1;
|
||||
}
|
||||
if param == l2p2x {
|
||||
deriv += -dx1;
|
||||
}
|
||||
if param == l2p1y {
|
||||
deriv += dy1;
|
||||
}
|
||||
if param == l2p2y {
|
||||
deriv += -dy1;
|
||||
}
|
||||
deriv
|
||||
}
|
||||
}
|
||||
|
||||
/// The midpoint of `l1` lies on the (infinite) line through `l2`.
|
||||
pub struct MidpointOnLine {
|
||||
pub l1: Line,
|
||||
pub l2: Line,
|
||||
}
|
||||
|
||||
impl MidpointOnLine {
|
||||
pub fn new(l1: Line, l2: Line) -> Self {
|
||||
Self { l1, l2 }
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for MidpointOnLine {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
vec![self.l1.p1.x, self.l1.p1.y, self.l1.p2.x, self.l1.p2.y, self.l2.p1.x, self.l2.p1.y, self.l2.p2.x, self.l2.p2.y]
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
let x0 = (store.get(self.l1.p1.x) + store.get(self.l1.p2.x)) / 2.0;
|
||||
let y0 = (store.get(self.l1.p1.y) + store.get(self.l1.p2.y)) / 2.0;
|
||||
let (x1, y1) = (store.get(self.l2.p1.x), store.get(self.l2.p1.y));
|
||||
let (x2, y2) = (store.get(self.l2.p2.x), store.get(self.l2.p2.y));
|
||||
let dx = x2 - x1;
|
||||
let dy = y2 - y1;
|
||||
let d = (dx * dx + dy * dy).sqrt();
|
||||
let area = -x0 * dy + y0 * dx + x1 * y2 - x2 * y1;
|
||||
area / d
|
||||
}
|
||||
|
||||
fn grad_value(&self, store: &ParamStore, param: ParamId) -> f64 {
|
||||
let (l1p1x, l1p1y, l1p2x, l1p2y) = (self.l1.p1.x, self.l1.p1.y, self.l1.p2.x, self.l1.p2.y);
|
||||
let (l2p1x, l2p1y, l2p2x, l2p2y) = (self.l2.p1.x, self.l2.p1.y, self.l2.p2.x, self.l2.p2.y);
|
||||
if ![l1p1x, l1p1y, l1p2x, l1p2y, l2p1x, l2p1y, l2p2x, l2p2y].contains(¶m) {
|
||||
return 0.0;
|
||||
}
|
||||
let x0 = (store.get(l1p1x) + store.get(l1p2x)) / 2.0;
|
||||
let y0 = (store.get(l1p1y) + store.get(l1p2y)) / 2.0;
|
||||
let (x1, y1) = (store.get(l2p1x), store.get(l2p1y));
|
||||
let (x2, y2) = (store.get(l2p2x), store.get(l2p2y));
|
||||
let dx = x2 - x1;
|
||||
let dy = y2 - y1;
|
||||
let d2 = dx * dx + dy * dy;
|
||||
let d = d2.sqrt();
|
||||
let area = -x0 * dy + y0 * dx + x1 * y2 - x2 * y1;
|
||||
let mut deriv = 0.0;
|
||||
if param == l1p1x {
|
||||
deriv += (y1 - y2) / (2.0 * d);
|
||||
}
|
||||
if param == l1p1y {
|
||||
deriv += (x2 - x1) / (2.0 * d);
|
||||
}
|
||||
if param == l1p2x {
|
||||
deriv += (y1 - y2) / (2.0 * d);
|
||||
}
|
||||
if param == l1p2y {
|
||||
deriv += (x2 - x1) / (2.0 * d);
|
||||
}
|
||||
if param == l2p1x {
|
||||
deriv += ((y2 - y0) * d + (dx / d) * area) / d2;
|
||||
}
|
||||
if param == l2p1y {
|
||||
deriv += ((x0 - x2) * d + (dy / d) * area) / d2;
|
||||
}
|
||||
if param == l2p2x {
|
||||
deriv += ((y0 - y1) * d - (dx / d) * area) / d2;
|
||||
}
|
||||
if param == l2p2y {
|
||||
deriv += ((x1 - x0) * d - (dy / d) * area) / d2;
|
||||
}
|
||||
deriv
|
||||
}
|
||||
}
|
||||
|
||||
/// `l1` and `l2` have equal length. Ported from the combined `errorgrad`
|
||||
/// the C++ uses here (`ConstraintEqualLineLength::errorgrad`), including its
|
||||
/// surrogate-gradient fallback: when both lines are axis-aligned the same
|
||||
/// way, the true gradient underflows toward zero and the solver's diagnosis
|
||||
/// pass would misread that as "fully constrained" rather than "at a
|
||||
/// horizontal/vertical extremum" — so a tiny signed surrogate derivative is
|
||||
/// substituted whenever the true one drops below `1e-10`.
|
||||
pub struct EqualLineLength {
|
||||
pub l1: Line,
|
||||
pub l2: Line,
|
||||
}
|
||||
|
||||
impl EqualLineLength {
|
||||
pub fn new(l1: Line, l2: Line) -> Self {
|
||||
Self { l1, l2 }
|
||||
}
|
||||
|
||||
fn error_grad(&self, store: &ParamStore, derivparam: Option<ParamId>) -> (f64, f64) {
|
||||
use crate::geo::DeriVector2;
|
||||
let p1 = DeriVector2::from_point(store, self.l1.p1, derivparam);
|
||||
let p2 = DeriVector2::from_point(store, self.l1.p2, derivparam);
|
||||
let p3 = DeriVector2::from_point(store, self.l2.p1, derivparam);
|
||||
let p4 = DeriVector2::from_point(store, self.l2.p2, derivparam);
|
||||
|
||||
let v1 = p1.subtr(&p2);
|
||||
let v2 = p3.subtr(&p4);
|
||||
|
||||
let (length1, dlength1) = v1.length_deriv();
|
||||
let (length2, dlength2) = v2.length_deriv();
|
||||
|
||||
let err = length2 - length1;
|
||||
let mut grad = dlength2 - dlength1;
|
||||
|
||||
if grad.abs() < 1e-10 {
|
||||
const SURROGATE: f64 = 1e-10;
|
||||
if let Some(param) = derivparam {
|
||||
if param == self.l1.p1.x {
|
||||
grad = if v1.x > 0.0 { SURROGATE } else { -SURROGATE };
|
||||
}
|
||||
if param == self.l1.p1.y {
|
||||
grad = if v1.y > 0.0 { SURROGATE } else { -SURROGATE };
|
||||
}
|
||||
if param == self.l1.p2.x {
|
||||
grad = if v1.x > 0.0 { -SURROGATE } else { SURROGATE };
|
||||
}
|
||||
if param == self.l1.p2.y {
|
||||
grad = if v1.y > 0.0 { -SURROGATE } else { SURROGATE };
|
||||
}
|
||||
if param == self.l2.p1.x {
|
||||
grad = if v2.x > 0.0 { SURROGATE } else { -SURROGATE };
|
||||
}
|
||||
if param == self.l2.p1.y {
|
||||
grad = if v2.y > 0.0 { SURROGATE } else { -SURROGATE };
|
||||
}
|
||||
if param == self.l2.p2.x {
|
||||
grad = if v2.x > 0.0 { -SURROGATE } else { SURROGATE };
|
||||
}
|
||||
if param == self.l2.p2.y {
|
||||
grad = if v2.y > 0.0 { -SURROGATE } else { SURROGATE };
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
(err, grad)
|
||||
}
|
||||
}
|
||||
|
||||
impl Constraint for EqualLineLength {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
vec![self.l1.p1.x, self.l1.p1.y, self.l1.p2.x, self.l1.p2.y, self.l2.p1.x, self.l2.p1.y, self.l2.p2.x, self.l2.p2.y]
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
self.error_grad(store, None).0
|
||||
}
|
||||
|
||||
fn grad_value(&self, store: &ParamStore, param: ParamId) -> f64 {
|
||||
self.error_grad(store, Some(param)).1
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::constraints::test_support::assert_grad_matches_finite_difference;
|
||||
|
||||
fn line(store: &mut ParamStore, x1: f64, y1: f64, x2: f64, y2: f64) -> Line {
|
||||
Line {
|
||||
p1: Point::new(store.add(x1, false), store.add(y1, false)),
|
||||
p2: Point::new(store.add(x2, false), store.add(y2, false)),
|
||||
}
|
||||
}
|
||||
|
||||
fn point(store: &mut ParamStore, x: f64, y: f64) -> Point {
|
||||
Point::new(store.add(x, false), store.add(y, false))
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn equal_is_zero_when_params_match_and_gradient_checks_out_at_ratio_one() {
|
||||
let mut store = ParamStore::new();
|
||||
let a = store.add(3.0, false);
|
||||
let b = store.add(3.0, false);
|
||||
let c = Equal::new(a, b, 1.0);
|
||||
assert!(c.error_value(&store).abs() < 1e-12);
|
||||
assert_grad_matches_finite_difference(&c, &mut store);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn difference_matches_hand_computation() {
|
||||
let mut store = ParamStore::new();
|
||||
let a = store.add(5.0, false);
|
||||
let b = store.add(8.0, false);
|
||||
let d = store.add(3.0, false);
|
||||
let c = Difference::new(a, b, d);
|
||||
assert!(c.error_value(&store).abs() < 1e-12); // 8 - 5 == 3
|
||||
assert_grad_matches_finite_difference(&c, &mut store);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn center_of_gravity_matches_hand_computation() {
|
||||
let mut store = ParamStore::new();
|
||||
let p1 = store.add(0.0, false);
|
||||
let p2 = store.add(10.0, false);
|
||||
let center = store.add(5.0, false); // midpoint == center of gravity at equal weights
|
||||
let c = CenterOfGravity::new(center, vec![p1, p2], vec![0.5, 0.5]);
|
||||
assert!(c.error_value(&store).abs() < 1e-12);
|
||||
assert_grad_matches_finite_difference(&c, &mut store);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn weighted_linear_combination_matches_hand_computation() {
|
||||
let mut store = ParamStore::new();
|
||||
let pole0 = store.add(0.0, false);
|
||||
let pole1 = store.add(10.0, false);
|
||||
let w0 = store.add(1.0, false);
|
||||
let w1 = store.add(1.0, false);
|
||||
// factors [0.5, 0.5] both weight 1 -> point should equal (0*1*0.5 + 10*1*0.5) / (1*0.5+1*0.5) = 5
|
||||
let point_param = store.add(5.0, false);
|
||||
let c = WeightedLinearCombination::new(point_param, vec![pole0, pole1], vec![w0, w1], vec![0.5, 0.5]);
|
||||
assert!(c.error_value(&store).abs() < 1e-12);
|
||||
assert_grad_matches_finite_difference(&c, &mut store);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn p2p_distance_matches_pythagoras() {
|
||||
let mut store = ParamStore::new();
|
||||
let p1 = point(&mut store, 0.0, 0.0);
|
||||
let p2 = point(&mut store, 3.0, 4.0);
|
||||
let d = store.add(5.0, false);
|
||||
let c = P2PDistance::new(p1, p2, d);
|
||||
assert!(c.error_value(&store).abs() < 1e-12);
|
||||
assert_grad_matches_finite_difference(&c, &mut store);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn p2l_distance_matches_hand_computation() {
|
||||
let mut store = ParamStore::new();
|
||||
let p = point(&mut store, 0.0, 5.0);
|
||||
let l = line(&mut store, -10.0, 0.0, 10.0, 0.0); // the x axis
|
||||
let d = store.add(5.0, false);
|
||||
let c = P2LDistance::new(p, l, d, true);
|
||||
assert!(c.error_value(&store).abs() < 1e-9);
|
||||
assert_grad_matches_finite_difference(&c, &mut store);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn point_on_line_is_zero_for_a_collinear_point() {
|
||||
let mut store = ParamStore::new();
|
||||
let p = point(&mut store, 5.0, 5.0);
|
||||
let l = line(&mut store, 0.0, 0.0, 10.0, 10.0);
|
||||
let c = PointOnLine::new(p, l);
|
||||
assert!(c.error_value(&store).abs() < 1e-9);
|
||||
assert_grad_matches_finite_difference(&c, &mut store);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn point_on_perp_bisector_is_zero_for_an_equidistant_point() {
|
||||
let mut store = ParamStore::new();
|
||||
let p = point(&mut store, 5.0, 100.0); // equidistant from (0,0) and (10,0)
|
||||
let l = line(&mut store, 0.0, 0.0, 10.0, 0.0);
|
||||
let c = PointOnPerpBisector::new(p, l);
|
||||
assert!(c.error_value(&store).abs() < 1e-9);
|
||||
assert_grad_matches_finite_difference(&c, &mut store);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn parallel_is_zero_for_parallel_lines_and_gradient_checks_out() {
|
||||
let mut store = ParamStore::new();
|
||||
let l1 = line(&mut store, 0.0, 0.0, 10.0, 5.0);
|
||||
let l2 = line(&mut store, 1.0, 1.0, 11.0, 6.0);
|
||||
let c = Parallel::new(&store, l1, l2);
|
||||
assert!(c.error_value(&store).abs() < 1e-9);
|
||||
assert_grad_matches_finite_difference(&c, &mut store);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn perpendicular_is_zero_for_perpendicular_lines_and_gradient_checks_out() {
|
||||
let mut store = ParamStore::new();
|
||||
let l1 = line(&mut store, 0.0, 0.0, 10.0, 0.0);
|
||||
let l2 = line(&mut store, 0.0, 0.0, 0.0, 10.0);
|
||||
let c = Perpendicular::new(&store, l1, l2);
|
||||
assert!(c.error_value(&store).abs() < 1e-9);
|
||||
assert_grad_matches_finite_difference(&c, &mut store);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn midpoint_on_line_is_zero_when_satisfied() {
|
||||
let mut store = ParamStore::new();
|
||||
let l1 = line(&mut store, 0.0, 10.0, 10.0, 10.0); // midpoint (5, 10)
|
||||
let l2 = line(&mut store, 5.0, 0.0, 5.0, 20.0); // vertical line through x=5
|
||||
let c = MidpointOnLine::new(l1, l2);
|
||||
assert!(c.error_value(&store).abs() < 1e-9);
|
||||
assert_grad_matches_finite_difference(&c, &mut store);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn equal_line_length_is_zero_for_equal_length_lines() {
|
||||
let mut store = ParamStore::new();
|
||||
let l1 = line(&mut store, 0.0, 0.0, 3.0, 4.0); // length 5
|
||||
let l2 = line(&mut store, 0.0, 0.0, 5.0, 0.0); // length 5
|
||||
let c = EqualLineLength::new(l1, l2);
|
||||
assert!(c.error_value(&store).abs() < 1e-9);
|
||||
assert_grad_matches_finite_difference(&c, &mut store);
|
||||
}
|
||||
}
|
||||
@@ -1,265 +0,0 @@
|
||||
//! Degrees-of-freedom and redundant-constraint diagnosis.
|
||||
//!
|
||||
//! Ported in spirit, not line-by-line, from `System::diagnose` and
|
||||
//! `identifyConflictingRedundantConstraints` (`GCS.cpp`). The C++ version is
|
||||
//! substantially more machinery than the rest of this port: it runs two QR
|
||||
//! decompositions in parallel threads, switches between dense and sparse QR
|
||||
//! by system size, tracks per-constraint "tags" (so a caller can mark
|
||||
//! certain constraints high-priority or exempt from the conflict report),
|
||||
//! and distinguishes *which* rows of a rank-deficient Jacobian are
|
||||
//! genuinely redundant (safe to drop) via Eigen's `FullPivHouseholderQR`
|
||||
//! column tracking.
|
||||
//!
|
||||
//! This gets to the same two numbers a caller actually needs — the
|
||||
//! remaining degrees of freedom, and a set of constraints that could be
|
||||
//! removed without changing what the rest of the system can satisfy — via
|
||||
//! plain rank-revealing SVD (already used the same way in `qp_eq.rs`) and a
|
||||
//! straightforward incremental-rank scan, at the cost of two simplifications
|
||||
//! worth knowing about before relying on this for anything but a UI hint:
|
||||
//!
|
||||
//! - **No tag system.** Every constraint in the subsystem is considered;
|
||||
//! there's no way yet to exempt a particular constraint from the report.
|
||||
//! - **Redundant vs. conflicting is a separate, opt-in step.** [`diagnose`]
|
||||
//! itself only identifies constraint rows that are *linearly dependent* on
|
||||
//! the others (the Jacobian doesn't have full row rank) — safe to drop
|
||||
//! without changing the solvable configuration space, but not yet telling
|
||||
//! apart a harmless duplicate from a genuinely *conflicting* constraint
|
||||
//! (dependent AND infeasible together, e.g. two different fixed distances
|
||||
//! between the same two points). [`classify_redundant`] answers that,
|
||||
//! for exactly the rows `diagnose` flagged, by attempting a solve on the
|
||||
//! reduced system and checking whether the removed row's own error can
|
||||
//! still reach zero there — kept separate from `diagnose` because it costs
|
||||
//! one extra solve per redundant row, which `diagnose`'s own "cheap enough
|
||||
//! for every edit" contract doesn't afford.
|
||||
|
||||
use std::rc::Rc;
|
||||
|
||||
use nalgebra::DMatrix;
|
||||
|
||||
use crate::constraints::Constraint;
|
||||
use crate::subsystem::SubSystem;
|
||||
use crate::util::ParamStore;
|
||||
|
||||
pub struct Diagnosis {
|
||||
/// Remaining degrees of freedom: free parameters minus the Jacobian's
|
||||
/// rank. Zero means fully constrained.
|
||||
pub dof: usize,
|
||||
/// Row indices into [`SubSystem::constraints`] that are linearly
|
||||
/// dependent on the rest — candidates to drop when over-constrained.
|
||||
pub redundant: Vec<usize>,
|
||||
}
|
||||
|
||||
impl Diagnosis {
|
||||
/// The actual redundant [`Constraint`]s, resolved from `redundant`
|
||||
/// against `sub`'s own constraint list.
|
||||
pub fn redundant_constraints<'a>(&self, sub: &'a SubSystem) -> Vec<&'a Rc<dyn Constraint>> {
|
||||
self.redundant.iter().map(|&i| &sub.constraints()[i]).collect()
|
||||
}
|
||||
}
|
||||
|
||||
/// Diagnoses `sub` at the store's current parameter values. Cheap enough to
|
||||
/// call after every edit for a live DOF readout — the UX research behind
|
||||
/// this port's plan flagged that as something every incumbent CAD tool
|
||||
/// under-serves.
|
||||
pub fn diagnose(sub: &SubSystem, store: &ParamStore) -> Diagnosis {
|
||||
let jacobi = sub.calc_jacobi(store);
|
||||
let psize = sub.p_size();
|
||||
let csize = sub.c_size();
|
||||
|
||||
let full_rank = matrix_rank(&jacobi);
|
||||
let dof = psize.saturating_sub(full_rank);
|
||||
|
||||
let mut kept_rows: Vec<usize> = Vec::with_capacity(csize);
|
||||
let mut redundant: Vec<usize> = Vec::new();
|
||||
let mut current_rank = 0usize;
|
||||
|
||||
for i in 0..csize {
|
||||
let mut trial = kept_rows.clone();
|
||||
trial.push(i);
|
||||
let trial_rank = matrix_rank(&row_submatrix(&jacobi, &trial));
|
||||
if trial_rank > current_rank {
|
||||
kept_rows.push(i);
|
||||
current_rank = trial_rank;
|
||||
} else {
|
||||
redundant.push(i);
|
||||
}
|
||||
}
|
||||
|
||||
Diagnosis { dof, redundant }
|
||||
}
|
||||
|
||||
/// Whether a redundant (linearly-dependent) constraint is safe to drop or
|
||||
/// actively disagrees with the rest of the subsystem — the distinction
|
||||
/// [`diagnose`] itself deliberately doesn't make (see the module doc
|
||||
/// comment) and [`classify_redundant`] exists to add on top of it.
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum RedundancyKind {
|
||||
/// Removing this constraint doesn't change what the rest can satisfy —
|
||||
/// its own condition is already implied by the others' solution (e.g.
|
||||
/// the same Equal constraint added twice).
|
||||
Redundant,
|
||||
/// Removing this constraint lets the rest solve to a configuration that
|
||||
/// does *not* satisfy this constraint's own error function — it
|
||||
/// disagrees with the rest, not just duplicates one of them (e.g. two
|
||||
/// different fixed distances between the same two points).
|
||||
Conflicting,
|
||||
}
|
||||
|
||||
/// Below this, a redundant constraint's own error at the reduced system's
|
||||
/// solved position counts as "satisfied" (genuinely redundant) rather than
|
||||
/// "violated" (conflicting). Matches the tolerance `solvers` use to call a
|
||||
/// solve `Success` at sketch scale.
|
||||
const CONFLICT_TOLERANCE: f64 = 1e-6;
|
||||
|
||||
/// For each `redundant` row `diagnose` flagged on `sub` (linearly dependent
|
||||
/// on the rest, so dropping it doesn't reduce DOF), determines whether it's
|
||||
/// genuinely [`RedundancyKind::Redundant`] or actually
|
||||
/// [`RedundancyKind::Conflicting`] with the rest — the distinction the
|
||||
/// module doc comment describes: remove it, solve what remains (from a
|
||||
/// clone of `store`, so the caller's real parameter values are untouched),
|
||||
/// and check whether the *removed* constraint's own error function reaches
|
||||
/// ~0 at that solved position. If the rest of the subsystem naturally
|
||||
/// settles somewhere that already satisfies it, it was redundant; if the
|
||||
/// rest settles somewhere that still violates it, the two disagree.
|
||||
///
|
||||
/// Deliberately a separate, opt-in call from [`diagnose`] rather than folded
|
||||
/// into it: this does one extra solve per redundant row, which [`diagnose`]'s
|
||||
/// own "cheap enough for every edit" contract doesn't afford — but the
|
||||
/// common case (no redundant rows at all) never pays for it, since a caller
|
||||
/// only reaches for this once `diagnose` has already reported something to
|
||||
/// classify (e.g. the plan's SketchXpert-style resolver UI).
|
||||
pub fn classify_redundant(sub: &SubSystem, store: &ParamStore, redundant: &[usize]) -> Vec<(usize, RedundancyKind)> {
|
||||
redundant
|
||||
.iter()
|
||||
.map(|&index| {
|
||||
let reduced: Vec<Rc<dyn Constraint>> = sub
|
||||
.constraints()
|
||||
.iter()
|
||||
.enumerate()
|
||||
.filter_map(|(i, c)| (i != index).then(|| c.clone()))
|
||||
.collect();
|
||||
let reduced_sub = SubSystem::new(reduced, sub.plist());
|
||||
let mut reduced_store = store.clone();
|
||||
let _ = crate::solvers::dogleg::solve_dl(&reduced_sub, &mut reduced_store);
|
||||
let residual = sub.constraints()[index].error(&reduced_store).abs();
|
||||
let kind = if residual < CONFLICT_TOLERANCE { RedundancyKind::Redundant } else { RedundancyKind::Conflicting };
|
||||
(index, kind)
|
||||
})
|
||||
.collect()
|
||||
}
|
||||
|
||||
fn row_submatrix(m: &DMatrix<f64>, rows: &[usize]) -> DMatrix<f64> {
|
||||
let owned_rows: Vec<_> = rows.iter().map(|&r| m.row(r).clone_owned()).collect();
|
||||
DMatrix::from_rows(&owned_rows)
|
||||
}
|
||||
|
||||
fn matrix_rank(m: &DMatrix<f64>) -> usize {
|
||||
if m.nrows() == 0 || m.ncols() == 0 {
|
||||
return 0;
|
||||
}
|
||||
let singular_values = m.clone().singular_values();
|
||||
let tol = singular_values.max() * f64::EPSILON * (m.nrows().max(m.ncols()) as f64);
|
||||
singular_values.iter().filter(|&&s| s > tol).count()
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::constraints::point_line::{Difference, Equal};
|
||||
use crate::geo::Point;
|
||||
use crate::util::ParamStore;
|
||||
|
||||
#[test]
|
||||
fn a_fully_constrained_two_point_distance_has_zero_dof() {
|
||||
let mut store = ParamStore::new();
|
||||
let p1 = Point::new(store.add(0.0, true), store.add(0.0, true));
|
||||
let p2 = Point::new(store.add(3.0, false), store.add(4.0, false));
|
||||
let distance = store.add(5.0, true);
|
||||
let c: Rc<dyn Constraint> = Rc::new(crate::constraints::point_line::P2PDistance::new(p1, p2, distance));
|
||||
// Only one constraint on two free params (p2.x, p2.y): one DOF left
|
||||
// (free to slide around the circle), matching a real sketch's math,
|
||||
// not "fully constrained" -- this test is really about the *shape*
|
||||
// of the result, exercised precisely by the redundancy tests below.
|
||||
let sub = SubSystem::new(vec![c], &[p2.x, p2.y]);
|
||||
let diag = diagnose(&sub, &store);
|
||||
assert_eq!(diag.dof, 1);
|
||||
assert!(diag.redundant.is_empty());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn a_duplicated_constraint_is_flagged_redundant() {
|
||||
let mut store = ParamStore::new();
|
||||
let a = store.add(3.0, false);
|
||||
let b = store.add(3.0, false);
|
||||
|
||||
let c1: Rc<dyn Constraint> = Rc::new(Equal::new(a, b, 1.0));
|
||||
let c2: Rc<dyn Constraint> = Rc::new(Equal::new(a, b, 1.0)); // identical constraint again
|
||||
let sub = SubSystem::new(vec![c1, c2], &[a, b]);
|
||||
|
||||
let diag = diagnose(&sub, &store);
|
||||
assert_eq!(diag.redundant.len(), 1, "one of the two identical constraints should be redundant");
|
||||
assert_eq!(diag.dof, 1); // Equal removes exactly one DOF regardless of duplication
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn independent_constraints_are_never_flagged_redundant() {
|
||||
let mut store = ParamStore::new();
|
||||
let a = store.add(1.0, false);
|
||||
let b = store.add(2.0, false);
|
||||
let d1 = store.add(0.0, false);
|
||||
let d2 = store.add(0.0, false);
|
||||
|
||||
// Two different constraints touching disjoint pairs -- both needed.
|
||||
let c1: Rc<dyn Constraint> = Rc::new(Difference::new(a, b, d1));
|
||||
let c2: Rc<dyn Constraint> = Rc::new(Difference::new(a, b, d2));
|
||||
// Same params, but only redundant if their gradients are parallel --
|
||||
// Difference's gradient w.r.t. (a,b) is always (-1,1) regardless of
|
||||
// the target, so these ARE linearly dependent rows; use this to
|
||||
// confirm the flagged set resolves back to real constraints.
|
||||
let sub = SubSystem::new(vec![c1, c2], &[a, b]);
|
||||
let diag = diagnose(&sub, &store);
|
||||
assert_eq!(diag.redundant.len(), 1);
|
||||
let resolved = diag.redundant_constraints(&sub);
|
||||
assert_eq!(resolved.len(), 1);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn a_duplicated_constraint_classifies_as_genuinely_redundant() {
|
||||
let mut store = ParamStore::new();
|
||||
let a = store.add(3.0, false);
|
||||
let b = store.add(3.0, false);
|
||||
let c1: Rc<dyn Constraint> = Rc::new(Equal::new(a, b, 1.0));
|
||||
let c2: Rc<dyn Constraint> = Rc::new(Equal::new(a, b, 1.0)); // identical constraint again
|
||||
let sub = SubSystem::new(vec![c1, c2], &[a, b]);
|
||||
|
||||
let diag = diagnose(&sub, &store);
|
||||
assert_eq!(diag.redundant.len(), 1);
|
||||
let classified = classify_redundant(&sub, &store, &diag.redundant);
|
||||
assert_eq!(
|
||||
classified,
|
||||
vec![(diag.redundant[0], RedundancyKind::Redundant)],
|
||||
"the duplicate should classify as redundant, not conflicting"
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn two_different_fixed_targets_on_the_same_pair_classify_as_conflicting() {
|
||||
let mut store = ParamStore::new();
|
||||
let a = store.add(0.0, false);
|
||||
let b = store.add(1.0, false);
|
||||
let d1 = store.add(0.0, true); // wants b - a == 0
|
||||
let d2 = store.add(5.0, true); // wants b - a == 5 -- can't both hold
|
||||
let c1: Rc<dyn Constraint> = Rc::new(Difference::new(a, b, d1));
|
||||
let c2: Rc<dyn Constraint> = Rc::new(Difference::new(a, b, d2));
|
||||
let sub = SubSystem::new(vec![c1, c2], &[a, b]);
|
||||
|
||||
let diag = diagnose(&sub, &store);
|
||||
assert_eq!(diag.redundant.len(), 1, "the two Difference rows are linearly dependent regardless of target");
|
||||
let classified = classify_redundant(&sub, &store, &diag.redundant);
|
||||
assert_eq!(
|
||||
classified,
|
||||
vec![(diag.redundant[0], RedundancyKind::Conflicting)],
|
||||
"two incompatible fixed targets on the same pair must classify as conflicting, not redundant"
|
||||
);
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large.
Load diff
@@ -1,140 +0,0 @@
|
||||
//! Partitions a constraint list into independent, decoupled groups so a
|
||||
//! solver can solve each separately.
|
||||
//!
|
||||
//! planegcs does this (`GCS.cpp`, `System::initSolution`) by building a
|
||||
//! bipartite graph — one vertex per free parameter, one per constraint, an
|
||||
//! edge from a constraint to each parameter it touches — and running
|
||||
//! Boost.Graph's `connected_components` over it. The result is exactly the
|
||||
//! partition a plain union-find over parameters gives directly: two
|
||||
//! constraints end up in the same group iff they share a parameter,
|
||||
//! transitively through a chain of other constraints. That's what this does
|
||||
//! instead, without pulling in a graph crate for one algorithm.
|
||||
|
||||
use std::collections::HashMap;
|
||||
|
||||
use crate::constraints::Constraint;
|
||||
use crate::util::ParamId;
|
||||
|
||||
struct UnionFind {
|
||||
parent: Vec<usize>,
|
||||
}
|
||||
|
||||
impl UnionFind {
|
||||
fn new(n: usize) -> Self {
|
||||
Self { parent: (0..n).collect() }
|
||||
}
|
||||
|
||||
fn find(&mut self, x: usize) -> usize {
|
||||
if self.parent[x] != x {
|
||||
self.parent[x] = self.find(self.parent[x]);
|
||||
}
|
||||
self.parent[x]
|
||||
}
|
||||
|
||||
fn union(&mut self, a: usize, b: usize) {
|
||||
let (ra, rb) = (self.find(a), self.find(b));
|
||||
if ra != rb {
|
||||
self.parent[ra] = rb;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Groups constraint indices (`0..constraints.len()`) by shared parameters.
|
||||
/// A constraint with no parameters gets its own singleton group. Group
|
||||
/// order, and the order of indices within each group, are unspecified.
|
||||
pub fn partition_by_shared_params(constraints: &[std::rc::Rc<dyn Constraint>]) -> Vec<Vec<usize>> {
|
||||
let n = constraints.len();
|
||||
let mut uf = UnionFind::new(n);
|
||||
let mut first_seen: HashMap<ParamId, usize> = HashMap::new();
|
||||
|
||||
for (i, constr) in constraints.iter().enumerate() {
|
||||
for param in constr.params() {
|
||||
match first_seen.get(¶m) {
|
||||
Some(&j) => uf.union(i, j),
|
||||
None => {
|
||||
first_seen.insert(param, i);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
let mut groups: HashMap<usize, Vec<usize>> = HashMap::new();
|
||||
for i in 0..n {
|
||||
groups.entry(uf.find(i)).or_default().push(i);
|
||||
}
|
||||
groups.into_values().collect()
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::constraints::point_line::Difference;
|
||||
use crate::util::ParamStore;
|
||||
use std::rc::Rc;
|
||||
|
||||
fn sorted(mut groups: Vec<Vec<usize>>) -> Vec<Vec<usize>> {
|
||||
for g in &mut groups {
|
||||
g.sort();
|
||||
}
|
||||
groups.sort();
|
||||
groups
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn constraints_sharing_a_param_land_in_one_group() {
|
||||
let mut store = ParamStore::new();
|
||||
let a = store.add(1.0, false);
|
||||
let b = store.add(2.0, false);
|
||||
let c = store.add(3.0, false);
|
||||
let d1 = store.add(0.0, false);
|
||||
let d2 = store.add(0.0, false);
|
||||
|
||||
let constraints: Vec<Rc<dyn Constraint>> = vec![
|
||||
Rc::new(Difference::new(a, b, d1)), // touches a, b
|
||||
Rc::new(Difference::new(b, c, d2)), // touches b, c -- links to the first via b
|
||||
];
|
||||
|
||||
let groups = sorted(partition_by_shared_params(&constraints));
|
||||
assert_eq!(groups, vec![vec![0, 1]]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn disjoint_constraints_land_in_separate_groups() {
|
||||
let mut store = ParamStore::new();
|
||||
let a = store.add(1.0, false);
|
||||
let b = store.add(2.0, false);
|
||||
let c = store.add(3.0, false);
|
||||
let e = store.add(4.0, false);
|
||||
let d1 = store.add(0.0, false);
|
||||
let d2 = store.add(0.0, false);
|
||||
|
||||
let constraints: Vec<Rc<dyn Constraint>> = vec![
|
||||
Rc::new(Difference::new(a, b, d1)),
|
||||
Rc::new(Difference::new(c, e, d2)),
|
||||
];
|
||||
|
||||
let groups = sorted(partition_by_shared_params(&constraints));
|
||||
assert_eq!(groups, vec![vec![0], vec![1]]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn a_chain_of_shared_params_transitively_joins_three_constraints() {
|
||||
let mut store = ParamStore::new();
|
||||
let a = store.add(1.0, false);
|
||||
let b = store.add(2.0, false);
|
||||
let c = store.add(3.0, false);
|
||||
let d = store.add(4.0, false);
|
||||
let d1 = store.add(0.0, false);
|
||||
let d2 = store.add(0.0, false);
|
||||
let d3 = store.add(0.0, false);
|
||||
|
||||
let constraints: Vec<Rc<dyn Constraint>> = vec![
|
||||
Rc::new(Difference::new(a, b, d1)), // a, b
|
||||
Rc::new(Difference::new(c, d, d2)), // c, d -- separate so far
|
||||
Rc::new(Difference::new(b, c, d3)), // b, c -- bridges the two groups above
|
||||
];
|
||||
|
||||
let groups = sorted(partition_by_shared_params(&constraints));
|
||||
assert_eq!(groups, vec![vec![0, 1, 2]]);
|
||||
}
|
||||
}
|
||||
@@ -1,15 +0,0 @@
|
||||
//! 2D geometric constraint solver for OpenCADStudio sketches.
|
||||
//!
|
||||
//! A Rust port of FreeCAD's planegcs (LGPL-2.1-or-later), staged bottom-up:
|
||||
//! see `/Users/felix/.claude/plans/snoopy-finding-matsumoto.md` for the full
|
||||
//! port plan and stage order.
|
||||
|
||||
pub mod constraints;
|
||||
pub mod diagnosis;
|
||||
pub mod geo;
|
||||
pub mod graph;
|
||||
pub mod qp_eq;
|
||||
pub mod solvers;
|
||||
pub mod subsystem;
|
||||
pub mod system;
|
||||
pub mod util;
|
||||
@@ -1,182 +0,0 @@
|
||||
//! Equality-constrained quadratic program: minimizes `0.5*xᵀHx + gᵀx`
|
||||
//! subject to `Ax + c = 0`.
|
||||
//!
|
||||
//! Ported from planegcs's `qp_eq.h`/`qp_eq.cpp`. The C++ takes a
|
||||
//! `FullPivHouseholderQR` of `Aᵀ` and reads its *full* square `Q`, splitting
|
||||
//! its columns by rank into a basis for `A`'s row space and one for its null
|
||||
//! space. nalgebra's `QR` (like its `SVD`) only ever returns the *thin*
|
||||
//! Q/U — `params_num × rank`, not `params_num × params_num` — so there are
|
||||
//! no extra columns to split off directly. This gets the same two bases a
|
||||
//! different way: `Y` (the row-space part) via the equivalent minimum-norm
|
||||
//! right inverse `Y = Aᵀ(AAᵀ)⁻¹` (Cholesky, since `AAᵀ` is symmetric
|
||||
//! positive-definite for full-row-rank `A`), and `Z` (the null-space part)
|
||||
//! by extending the thin Q's `rank` orthonormal columns to a full
|
||||
//! orthonormal basis of R^params_num via Gram-Schmidt against the standard
|
||||
//! basis, keeping only the newly-added vectors. Both give the same
|
||||
//! projectors the C++'s single QR produces — the KKT system solved here has
|
||||
//! a unique minimizer regardless of which orthonormal basis the row/null
|
||||
//! space happen to be expressed in.
|
||||
|
||||
use nalgebra::{DMatrix, DVector};
|
||||
|
||||
/// Solution of the equality-constrained QP: the minimizer `x`, the row-space
|
||||
/// basis `y`, and the null-space basis `z` of `A` (mirroring the C++'s
|
||||
/// output parameters `x`, `Y`, `Z`).
|
||||
pub struct QpEqSolution {
|
||||
pub x: DVector<f64>,
|
||||
pub y: DMatrix<f64>,
|
||||
pub z: DMatrix<f64>,
|
||||
}
|
||||
|
||||
/// Returns `None` where the C++ returns `-1`: `A` doesn't have full row rank,
|
||||
/// or has more constraints than parameters.
|
||||
pub fn qp_eq(h: &DMatrix<f64>, g: &DVector<f64>, a: &DMatrix<f64>, c: &DVector<f64>) -> Option<QpEqSolution> {
|
||||
let constr_num = a.nrows();
|
||||
let params_num = a.ncols();
|
||||
if constr_num > params_num {
|
||||
return None;
|
||||
}
|
||||
|
||||
// Rank check only needs the singular values, not the vectors — cheaper
|
||||
// than a full SVD and mirrors the C++'s `rank != constr_num` guard.
|
||||
let singular_values = a.clone().singular_values();
|
||||
let tol = singular_values.max() * f64::EPSILON * (params_num.max(constr_num) as f64);
|
||||
let rank = singular_values.iter().filter(|&&s| s > tol).count();
|
||||
if rank != constr_num {
|
||||
return None;
|
||||
}
|
||||
|
||||
// Thin QR of A^T: Q1 is params_num x rank, an orthonormal basis of A's
|
||||
// row space (A^T's column space) — well-posed since A's already-confirmed
|
||||
// full row rank means A^T has full column rank.
|
||||
let qr = a.transpose().qr();
|
||||
let q1 = qr.q();
|
||||
|
||||
// Z: null-space basis of A, found by extending Q1's columns to a full
|
||||
// orthonormal basis of R^params_num and keeping only the new vectors —
|
||||
// mirrors `Z = Q.rightCols(params_num - rank)` from the full square Q
|
||||
// the C++ has directly.
|
||||
let z = extend_to_orthonormal_basis(&q1, params_num);
|
||||
|
||||
// Y = A^T (A A^T)^-1, the minimum-norm right inverse of full-row-rank A
|
||||
// — mirrors `Y = Q1 * inv(R1^T) * P^T`, the same row-space projector.
|
||||
let aat = a * a.transpose();
|
||||
let aat_chol = nalgebra::linalg::Cholesky::new(aat)?;
|
||||
let aat_inv = aat_chol.solve(&DMatrix::identity(constr_num, constr_num));
|
||||
let y = a.transpose() * &aat_inv;
|
||||
|
||||
let x = if params_num == rank {
|
||||
-&y * c
|
||||
} else {
|
||||
let ztz = z.transpose() * h * &z;
|
||||
let rhs = z.transpose() * (h * &y * c - g);
|
||||
let y_step = ztz.lu().solve(&rhs)?;
|
||||
-&y * c + &z * y_step
|
||||
};
|
||||
|
||||
Some(QpEqSolution { x, y, z })
|
||||
}
|
||||
|
||||
/// Extends the orthonormal columns of `q1` (`dim x rank`) to a full
|
||||
/// orthonormal basis of R^dim via modified Gram-Schmidt against the standard
|
||||
/// basis vectors, returning only the newly-added `dim - rank` columns.
|
||||
fn extend_to_orthonormal_basis(q1: &DMatrix<f64>, dim: usize) -> DMatrix<f64> {
|
||||
const TOL: f64 = 1e-10;
|
||||
let rank = q1.ncols();
|
||||
if dim == rank {
|
||||
return DMatrix::zeros(dim, 0);
|
||||
}
|
||||
let mut basis: Vec<DVector<f64>> = (0..rank).map(|i| q1.column(i).into_owned()).collect();
|
||||
let mut extra: Vec<DVector<f64>> = Vec::with_capacity(dim - rank);
|
||||
|
||||
for i in 0..dim {
|
||||
if extra.len() == dim - rank {
|
||||
break;
|
||||
}
|
||||
let mut candidate = DVector::zeros(dim);
|
||||
candidate[i] = 1.0;
|
||||
for b in &basis {
|
||||
let proj = b.dot(&candidate);
|
||||
candidate -= b * proj;
|
||||
}
|
||||
let norm = candidate.norm();
|
||||
if norm > TOL {
|
||||
candidate /= norm;
|
||||
basis.push(candidate.clone());
|
||||
extra.push(candidate);
|
||||
}
|
||||
}
|
||||
|
||||
DMatrix::from_columns(&extra)
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
const EPS: f64 = 1e-8;
|
||||
|
||||
#[test]
|
||||
fn minimizes_sum_of_squares_on_a_line() {
|
||||
// minimize x1^2 + x2^2 s.t. x1 + x2 = 1 -> x1 = x2 = 0.5
|
||||
let h = DMatrix::identity(2, 2) * 2.0;
|
||||
let g = DVector::from_vec(vec![0.0, 0.0]);
|
||||
let a = DMatrix::from_row_slice(1, 2, &[1.0, 1.0]);
|
||||
let c = DVector::from_vec(vec![-1.0]);
|
||||
|
||||
let sol = qp_eq(&h, &g, &a, &c).expect("full row rank system should solve");
|
||||
assert!((sol.x[0] - 0.5).abs() < EPS);
|
||||
assert!((sol.x[1] - 0.5).abs() < EPS);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn fully_determined_system_matches_the_direct_solution() {
|
||||
// Square A: no null space (params_num == rank), x = -Y*c is the whole answer.
|
||||
// x1 + x2 = 3, x1 - x2 = 1 -> x1=2, x2=1
|
||||
let h = DMatrix::identity(2, 2);
|
||||
let g = DVector::from_vec(vec![0.0, 0.0]);
|
||||
let a = DMatrix::from_row_slice(2, 2, &[1.0, 1.0, 1.0, -1.0]);
|
||||
let c = DVector::from_vec(vec![-3.0, -1.0]);
|
||||
|
||||
let sol = qp_eq(&h, &g, &a, &c).expect("square full-rank system should solve");
|
||||
assert!((sol.x[0] - 2.0).abs() < EPS);
|
||||
assert!((sol.x[1] - 1.0).abs() < EPS);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn rank_deficient_constraints_return_none() {
|
||||
// Two identical rows: rank(A) = 1 != constr_num = 2.
|
||||
let h = DMatrix::identity(2, 2);
|
||||
let g = DVector::from_vec(vec![0.0, 0.0]);
|
||||
let a = DMatrix::from_row_slice(2, 2, &[1.0, 1.0, 1.0, 1.0]);
|
||||
let c = DVector::from_vec(vec![-1.0, -1.0]);
|
||||
|
||||
assert!(qp_eq(&h, &g, &a, &c).is_none());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn more_constraints_than_parameters_return_none() {
|
||||
let h = DMatrix::identity(1, 1);
|
||||
let g = DVector::from_vec(vec![0.0]);
|
||||
let a = DMatrix::from_row_slice(2, 1, &[1.0, 1.0]);
|
||||
let c = DVector::from_vec(vec![-1.0, -1.0]);
|
||||
|
||||
assert!(qp_eq(&h, &g, &a, &c).is_none());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn z_spans_the_null_space_of_a() {
|
||||
let h = DMatrix::identity(3, 3);
|
||||
let g = DVector::from_vec(vec![0.0, 0.0, 0.0]);
|
||||
let a = DMatrix::from_row_slice(1, 3, &[1.0, 1.0, 1.0]);
|
||||
let c = DVector::from_vec(vec![0.0]);
|
||||
|
||||
let sol = qp_eq(&h, &g, &a, &c).unwrap();
|
||||
assert_eq!(sol.z.nrows(), 3);
|
||||
assert_eq!(sol.z.ncols(), 2);
|
||||
let should_be_zero = &a * &sol.z;
|
||||
for v in should_be_zero.iter() {
|
||||
assert!(v.abs() < EPS, "A*Z should be zero, got {should_be_zero}");
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,169 +0,0 @@
|
||||
//! BFGS — quasi-Newton with a numerically-approximated inverse Hessian.
|
||||
//!
|
||||
//! Ported from `System::solve_BFGS` and the free function `lineSearch`
|
||||
//! (`GCS.cpp`). Defaults match planegcs's own: `convergence = 1e-10`
|
||||
//! (`System::System()`), `smallF = 1e-20` (`#define smallF 1e-20`,
|
||||
//! `GCS.h`), `max_iter = 100`. As with `dogleg.rs`/`lm.rs`, store snapshot/
|
||||
//! revert-on-failure replaces the C++'s scratch-copy-plus-`applySolution()`
|
||||
//! split — see `solvers/mod.rs`'s doc comment.
|
||||
|
||||
use nalgebra::DVector;
|
||||
|
||||
use crate::solvers::SolveStatus;
|
||||
use crate::subsystem::SubSystem;
|
||||
use crate::util::ParamStore;
|
||||
|
||||
const CONVERGENCE: f64 = 1e-10;
|
||||
const SMALL_F: f64 = 1e-20;
|
||||
const MAX_ITER: usize = 100;
|
||||
|
||||
/// Bracket-and-quadratic-fit line search along `xdir`, restricted to
|
||||
/// `subsys->maxStep`. Leaves `store` holding the params at the chosen step
|
||||
/// and returns that step's scale — ported from the free function
|
||||
/// `lineSearch` (`GCS.cpp`).
|
||||
fn line_search(sub: &SubSystem, store: &mut ParamStore, xdir: &DVector<f64>) -> f64 {
|
||||
let alpha_max = sub.max_step(store, xdir.as_slice(), 1e10);
|
||||
let x0 = sub.get_params(store);
|
||||
|
||||
let alpha1 = 0.0_f64;
|
||||
let f1 = sub.error(store);
|
||||
|
||||
let mut alpha2 = 1.0_f64;
|
||||
sub.set_params(store, &(&x0 + xdir * alpha2));
|
||||
let mut f2 = sub.error(store);
|
||||
|
||||
let mut alpha3 = alpha2 * 2.0;
|
||||
sub.set_params(store, &(&x0 + xdir * alpha3));
|
||||
let mut f3 = sub.error(store);
|
||||
|
||||
while f2 > f1 || f2 > f3 {
|
||||
if f2 > f1 {
|
||||
alpha3 = alpha2;
|
||||
f3 = f2;
|
||||
alpha2 /= 2.0;
|
||||
sub.set_params(store, &(&x0 + xdir * alpha2));
|
||||
f2 = sub.error(store);
|
||||
} else if f2 > f3 {
|
||||
if alpha3 >= alpha_max {
|
||||
break;
|
||||
}
|
||||
alpha2 = alpha3;
|
||||
f2 = f3;
|
||||
alpha3 *= 2.0;
|
||||
sub.set_params(store, &(&x0 + xdir * alpha3));
|
||||
f3 = sub.error(store);
|
||||
}
|
||||
}
|
||||
|
||||
let mut alpha_star = alpha2 + ((alpha2 - alpha1) * (f1 - f3)) / (3.0 * (f1 - 2.0 * f2 + f3));
|
||||
if alpha_star >= alpha3 || alpha_star <= alpha1 {
|
||||
alpha_star = alpha2;
|
||||
}
|
||||
if alpha_star > alpha_max {
|
||||
alpha_star = alpha_max;
|
||||
}
|
||||
if alpha_star.is_nan() {
|
||||
alpha_star = 0.0;
|
||||
}
|
||||
|
||||
sub.set_params(store, &(&x0 + xdir * alpha_star));
|
||||
alpha_star
|
||||
}
|
||||
|
||||
pub fn solve_bfgs(sub: &SubSystem, store: &mut ParamStore) -> SolveStatus {
|
||||
let xsize = sub.p_size();
|
||||
if xsize == 0 {
|
||||
return SolveStatus::Success;
|
||||
}
|
||||
|
||||
let snapshot = store.redirect();
|
||||
|
||||
let mut d = nalgebra::DMatrix::<f64>::identity(xsize, xsize);
|
||||
|
||||
let mut x = sub.get_params(store);
|
||||
let mut grad = sub.calc_grad(store);
|
||||
|
||||
let mut xdir = -&grad;
|
||||
line_search(sub, store, &xdir);
|
||||
let mut err = sub.error(store);
|
||||
|
||||
let mut h = &sub.get_params(store) - &x;
|
||||
x = sub.get_params(store);
|
||||
|
||||
let diverging_lim = 1e6 * err + 1e12;
|
||||
|
||||
for _iter in 1..MAX_ITER {
|
||||
let h_norm = h.norm();
|
||||
if h_norm <= CONVERGENCE || err <= SMALL_F {
|
||||
break;
|
||||
}
|
||||
if err > diverging_lim || err.is_nan() {
|
||||
break;
|
||||
}
|
||||
|
||||
let grad_old = grad.clone();
|
||||
grad = sub.calc_grad(store);
|
||||
let y = &grad - &grad_old;
|
||||
|
||||
let mut hty = h.dot(&y);
|
||||
if hty == 0.0 {
|
||||
hty = 0.0000000001;
|
||||
}
|
||||
|
||||
let dy = &d * &y;
|
||||
let yt_dy = y.dot(&dy);
|
||||
|
||||
d += (&h * h.transpose()) * ((1.0 + yt_dy / hty) / hty);
|
||||
d -= (&h * dy.transpose() + &dy * h.transpose()) * (1.0 / hty);
|
||||
|
||||
xdir = -(&d * &grad);
|
||||
line_search(sub, store, &xdir);
|
||||
err = sub.error(store);
|
||||
|
||||
h = &sub.get_params(store) - &x;
|
||||
x = sub.get_params(store);
|
||||
}
|
||||
|
||||
if err <= SMALL_F {
|
||||
return SolveStatus::Success;
|
||||
}
|
||||
if h.norm() <= CONVERGENCE {
|
||||
return SolveStatus::Converged;
|
||||
}
|
||||
store.revert(&snapshot);
|
||||
SolveStatus::Failed
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::constraints::point_line::P2PDistance;
|
||||
use crate::geo::Point;
|
||||
use std::rc::Rc;
|
||||
|
||||
#[test]
|
||||
fn solves_a_single_p2p_distance_by_moving_the_free_point() {
|
||||
let mut store = ParamStore::new();
|
||||
let p1 = Point::new(store.add(0.0, true), store.add(0.0, true));
|
||||
let p2 = Point::new(store.add(1.0, false), store.add(0.0, false));
|
||||
let distance = store.add(5.0, true);
|
||||
|
||||
let constr: Rc<dyn crate::constraints::Constraint> = Rc::new(P2PDistance::new(p1, p2, distance));
|
||||
let sub = SubSystem::new(vec![constr], &[p2.x, p2.y]);
|
||||
|
||||
let status = solve_bfgs(&sub, &mut store);
|
||||
assert!(matches!(status, SolveStatus::Success | SolveStatus::Converged), "expected Success or Converged, got {status:?}");
|
||||
assert!(sub.error(&store).abs() < 1e-6);
|
||||
|
||||
let dx = store.get(p1.x) - store.get(p2.x);
|
||||
let dy = store.get(p1.y) - store.get(p2.y);
|
||||
assert!(((dx * dx + dy * dy).sqrt() - 5.0).abs() < 1e-4);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn a_zero_parameter_subsystem_trivially_succeeds() {
|
||||
let mut store = ParamStore::new();
|
||||
let sub = SubSystem::new(vec![], &[]);
|
||||
assert_eq!(solve_bfgs(&sub, &mut store), SolveStatus::Success);
|
||||
}
|
||||
}
|
||||
@@ -1,209 +0,0 @@
|
||||
//! Powell's Dogleg trust-region method — planegcs's default solver.
|
||||
//!
|
||||
//! Ported from `System::solve_DL` (`GCS.cpp`). Tolerances and iteration cap
|
||||
//! match planegcs's own defaults (`System::System()`'s member-initializer
|
||||
//! list): `tolg = 1e-80`, `tolx = 1e-80`, `tolf = 1e-10`, `max_iter = 100`
|
||||
//! (the "redundant-solving" variants with different tolerances, and the
|
||||
//! `sketchSizeMultiplier`-scaled iteration cap, aren't exposed — both are
|
||||
//! System-level solver *configuration* the later `diagnosis.rs`/`system.rs`
|
||||
//! work can thread through if it turns out to matter).
|
||||
//!
|
||||
//! One deliberate substitution: the Gauss-Newton step `h_gn` solves
|
||||
//! `Jx * h_gn = -fx`. The C++ defaults to Eigen's `FullPivLU`, chosen
|
||||
//! because it handles `Jx` being rectangular and possibly rank-deficient.
|
||||
//! nalgebra has no equivalent decomposition; this uses its SVD-based
|
||||
//! least-squares solve instead, which handles the same rectangular/
|
||||
//! rank-deficient cases (via a small-singular-value cutoff) and serves the
|
||||
//! same purpose — a robust direction when the Jacobian isn't a nice square
|
||||
//! invertible matrix — without replicating the C++'s three-way
|
||||
//! `dogLegGaussStep` switch between LU variants.
|
||||
|
||||
use nalgebra::DVector;
|
||||
|
||||
use crate::solvers::SolveStatus;
|
||||
use crate::subsystem::SubSystem;
|
||||
use crate::util::ParamStore;
|
||||
|
||||
const TOLG: f64 = 1e-80;
|
||||
const TOLX: f64 = 1e-80;
|
||||
const TOLF: f64 = 1e-10;
|
||||
const MAX_ITER: usize = 100;
|
||||
|
||||
pub fn solve_dl(sub: &SubSystem, store: &mut ParamStore) -> SolveStatus {
|
||||
let xsize = sub.p_size();
|
||||
if xsize == 0 {
|
||||
return SolveStatus::Success;
|
||||
}
|
||||
|
||||
let snapshot = store.redirect();
|
||||
|
||||
let mut x = sub.get_params(store);
|
||||
let mut fx = sub.calc_residual(store);
|
||||
let mut jx = sub.calc_jacobi(store);
|
||||
let mut err = sub.error(store);
|
||||
|
||||
let mut g = jx.transpose() * (-fx.clone());
|
||||
let mut g_inf = g.amax();
|
||||
let mut fx_inf = fx.amax();
|
||||
|
||||
let diverging_lim = 1e6 * err + 1e12;
|
||||
let mut delta = 0.1_f64;
|
||||
let mut nu = 2.0_f64;
|
||||
let mut iter = 0usize;
|
||||
let mut reduce = 0i32;
|
||||
let mut stop = 0u8;
|
||||
|
||||
loop {
|
||||
if fx_inf <= TOLF {
|
||||
stop = 1;
|
||||
break;
|
||||
} else if g_inf <= TOLG {
|
||||
stop = 2;
|
||||
break;
|
||||
} else if delta <= TOLX * (TOLX + x.norm()) {
|
||||
stop = 2;
|
||||
break;
|
||||
} else if iter >= MAX_ITER {
|
||||
stop = 4;
|
||||
break;
|
||||
} else if err > diverging_lim || err.is_nan() {
|
||||
stop = 6;
|
||||
break;
|
||||
}
|
||||
|
||||
let alpha = g.norm_squared() / (&jx * &g).norm_squared();
|
||||
let h_sd = &g * alpha;
|
||||
|
||||
let h_gn = jx
|
||||
.clone()
|
||||
.svd(true, true)
|
||||
.solve(&(-fx.clone()), 1e-12)
|
||||
.unwrap_or_else(|_| DVector::zeros(xsize));
|
||||
|
||||
let rel_error = (&jx * &h_gn + &fx).norm() / fx.norm();
|
||||
if rel_error > 1e15 {
|
||||
break; // stop stays 0 -> Failed, matching the C++'s bare `break;` here
|
||||
}
|
||||
|
||||
let h_dl;
|
||||
if h_gn.norm() < delta {
|
||||
h_dl = h_gn.clone();
|
||||
if h_dl.norm() <= TOLX * (TOLX + x.norm()) {
|
||||
stop = 5;
|
||||
break;
|
||||
}
|
||||
} else if alpha * g.norm() >= delta {
|
||||
h_dl = &h_sd * (delta / (alpha * g.norm()));
|
||||
} else {
|
||||
let b = &h_gn - &h_sd;
|
||||
let bb = b.norm_squared();
|
||||
let gb = h_sd.dot(&b).abs();
|
||||
let c = (delta + h_sd.norm()) * (delta - h_sd.norm());
|
||||
let beta = if gb > 0.0 { c / (gb + (gb * gb + c * bb).sqrt()) } else { ((gb * gb + c * bb).sqrt() - gb) / bb };
|
||||
h_dl = &h_sd + &b * beta;
|
||||
}
|
||||
|
||||
let x_new = &x + &h_dl;
|
||||
sub.set_params(store, &x_new);
|
||||
let fx_new = sub.calc_residual(store);
|
||||
let err_new = sub.error(store);
|
||||
let jx_new = sub.calc_jacobi(store);
|
||||
|
||||
let dl_pred = err - 0.5 * (&fx + &jx * &h_dl).norm_squared();
|
||||
let df = err - err_new;
|
||||
let rho = dl_pred / df;
|
||||
|
||||
if df > 0.0 && dl_pred > 0.0 {
|
||||
x = x_new;
|
||||
jx = jx_new;
|
||||
fx = fx_new;
|
||||
err = err_new;
|
||||
g = jx.transpose() * (-fx.clone());
|
||||
g_inf = g.amax();
|
||||
fx_inf = fx.amax();
|
||||
} else {
|
||||
// The trial step wasn't accepted: undo it. The C++ never wrote
|
||||
// it anywhere but its own scratch copy in the first place; this
|
||||
// port has no such scratch, so the store needs restoring to the
|
||||
// last accepted `x` explicitly.
|
||||
sub.set_params(store, &x);
|
||||
}
|
||||
|
||||
if (rho - 1.0).abs() < 0.2 && h_dl.norm() > delta / 3.0 && reduce <= 0 {
|
||||
delta *= 3.0;
|
||||
nu = 2.0;
|
||||
reduce = 0;
|
||||
} else if rho < 0.25 {
|
||||
delta /= nu;
|
||||
nu *= 2.0;
|
||||
reduce = 2;
|
||||
} else {
|
||||
reduce -= 1;
|
||||
}
|
||||
|
||||
iter += 1;
|
||||
}
|
||||
|
||||
if stop != 1 {
|
||||
store.revert(&snapshot);
|
||||
return SolveStatus::Failed;
|
||||
}
|
||||
SolveStatus::Success
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::constraints::point_line::P2PDistance;
|
||||
use crate::geo::Point;
|
||||
use std::rc::Rc;
|
||||
|
||||
#[test]
|
||||
fn solves_a_single_p2p_distance_by_moving_the_free_point() {
|
||||
let mut store = ParamStore::new();
|
||||
let p1 = Point::new(store.add(0.0, true), store.add(0.0, true)); // fixed anchor
|
||||
let p2 = Point::new(store.add(1.0, false), store.add(0.0, false)); // free point, wrong distance
|
||||
let distance = store.add(5.0, true); // driven target
|
||||
|
||||
let constr: Rc<dyn crate::constraints::Constraint> = Rc::new(P2PDistance::new(p1, p2, distance));
|
||||
let sub = SubSystem::new(vec![constr], &[p2.x, p2.y]);
|
||||
|
||||
assert!(sub.error(&store) > 0.0, "should start unsatisfied");
|
||||
let status = solve_dl(&sub, &mut store);
|
||||
assert_eq!(status, SolveStatus::Success);
|
||||
assert!(sub.error(&store) < 1e-15, "residual should be ~zero after solving");
|
||||
|
||||
let dx = store.get(p1.x) - store.get(p2.x);
|
||||
let dy = store.get(p1.y) - store.get(p2.y);
|
||||
assert!(((dx * dx + dy * dy).sqrt() - 5.0).abs() < 1e-7);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn solves_a_two_constraint_triangle() {
|
||||
// Two points free, each constrained to a fixed distance from a
|
||||
// fixed anchor and from each other -- a small but non-trivial
|
||||
// coupled system.
|
||||
let mut store = ParamStore::new();
|
||||
let anchor = Point::new(store.add(0.0, true), store.add(0.0, true));
|
||||
let p = Point::new(store.add(1.0, false), store.add(1.0, false));
|
||||
let d_anchor = store.add(10.0, true);
|
||||
|
||||
let c1: Rc<dyn crate::constraints::Constraint> = Rc::new(P2PDistance::new(anchor, p, d_anchor));
|
||||
let sub = SubSystem::new(vec![c1], &[p.x, p.y]);
|
||||
|
||||
let status = solve_dl(&sub, &mut store);
|
||||
assert_eq!(status, SolveStatus::Success);
|
||||
|
||||
let dx = store.get(anchor.x) - store.get(p.x);
|
||||
let dy = store.get(anchor.y) - store.get(p.y);
|
||||
assert!(((dx * dx + dy * dy).sqrt() - 10.0).abs() < 1e-7);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn a_zero_parameter_subsystem_trivially_succeeds() {
|
||||
let store = ParamStore::new();
|
||||
let sub = SubSystem::new(vec![], &[]);
|
||||
let mut store = store;
|
||||
assert_eq!(solve_dl(&sub, &mut store), SolveStatus::Success);
|
||||
}
|
||||
}
|
||||
@@ -1,176 +0,0 @@
|
||||
//! Levenberg-Marquardt — damped Gauss-Newton least squares.
|
||||
//!
|
||||
//! Ported from `System::solve_LM` (`GCS.cpp`). Defaults match planegcs's
|
||||
//! own (`System::System()`): `eps = 1e-10`, `eps1 = 1e-80`, `tau = 1e-3`,
|
||||
//! `max_iter = 100` (as with `dogleg.rs`, the "redundant-solving" tolerance
|
||||
//! variants and the `sketchSizeMultiplier`-scaled iteration cap aren't
|
||||
//! exposed here). Store snapshot/revert-on-failure replaces the C++'s
|
||||
//! scratch-copy-plus-separate-`applySolution()` pattern exactly as in
|
||||
//! `dogleg.rs` — see that module's doc comment and `subsystem.rs`'s for why.
|
||||
//!
|
||||
//! One faithful-but-odd detail kept as-is: the inner damping-adjustment loop
|
||||
//! counts rejected steps in `k` and stops after `k` reaches 50, but the
|
||||
//! C++'s `if (k > 50) { stop = 7; ... }` check after that loop can never be
|
||||
//! true (`k` never exceeds 50, only reaches it) — so that branch is
|
||||
//! effectively dead code upstream, and stays dead code here rather than
|
||||
//! being "corrected" into the probably-intended `k >= 50`.
|
||||
|
||||
use crate::solvers::SolveStatus;
|
||||
use crate::subsystem::SubSystem;
|
||||
use crate::util::ParamStore;
|
||||
|
||||
const EPS: f64 = 1e-10;
|
||||
const EPS1: f64 = 1e-80;
|
||||
const TAU: f64 = 1e-3;
|
||||
const MAX_ITER: usize = 100;
|
||||
|
||||
pub fn solve_lm(sub: &SubSystem, store: &mut ParamStore) -> SolveStatus {
|
||||
let xsize = sub.p_size();
|
||||
if xsize == 0 {
|
||||
return SolveStatus::Success;
|
||||
}
|
||||
|
||||
let snapshot = store.redirect();
|
||||
|
||||
let mut x = sub.get_params(store);
|
||||
let mut e = -sub.calc_residual(store);
|
||||
|
||||
let diverging_lim = 1e6 * e.norm_squared() + 1e12;
|
||||
let mut nu = 2.0_f64;
|
||||
let mut mu = 0.0_f64;
|
||||
let mut stop = 0u8;
|
||||
|
||||
'outer: for iter in 0..MAX_ITER {
|
||||
let err = e.norm_squared();
|
||||
if err <= EPS * EPS {
|
||||
stop = 1;
|
||||
break;
|
||||
} else if err > diverging_lim || err.is_nan() {
|
||||
stop = 6;
|
||||
break;
|
||||
}
|
||||
|
||||
let j = sub.calc_jacobi(store);
|
||||
let mut a = j.transpose() * &j;
|
||||
let g = j.transpose() * &e;
|
||||
|
||||
let g_inf = g.amax();
|
||||
let diag_a = a.diagonal().clone_owned();
|
||||
|
||||
if g_inf <= EPS1 {
|
||||
stop = 2;
|
||||
break;
|
||||
}
|
||||
|
||||
if iter == 0 {
|
||||
mu = TAU * diag_a.amax();
|
||||
}
|
||||
|
||||
let mut k = 0u32;
|
||||
loop {
|
||||
if k >= 50 {
|
||||
break;
|
||||
}
|
||||
|
||||
for i in 0..xsize {
|
||||
a[(i, i)] += mu;
|
||||
}
|
||||
|
||||
let h = a.clone().lu().solve(&g).unwrap_or_else(|| nalgebra::DVector::zeros(xsize));
|
||||
let rel_error = (&a * &h - &g).norm() / g.norm();
|
||||
|
||||
if rel_error < 1e-5 {
|
||||
let scale = sub.max_step(store, h.as_slice(), 1e10);
|
||||
let h = if scale < 1.0 { &h * scale } else { h };
|
||||
|
||||
let x_new = &x + &h;
|
||||
let h_norm = h.norm_squared();
|
||||
|
||||
if h_norm <= EPS1 * EPS1 * x.norm() {
|
||||
stop = 3;
|
||||
break;
|
||||
} else if h_norm >= (x.norm() + EPS1) / (f64::EPSILON * f64::EPSILON) {
|
||||
stop = 4;
|
||||
break;
|
||||
}
|
||||
|
||||
sub.set_params(store, &x_new);
|
||||
let e_new = -sub.calc_residual(store);
|
||||
|
||||
let df = e.norm_squared() - e_new.norm_squared();
|
||||
let dl = h.dot(&(&h * mu + &g));
|
||||
|
||||
if df > 0.0 && dl > 0.0 {
|
||||
let tmp = 2.0 * df / dl - 1.0;
|
||||
mu *= (1.0 / 3.0_f64).max(1.0 - tmp * tmp * tmp);
|
||||
nu = 2.0;
|
||||
x = x_new;
|
||||
e = e_new;
|
||||
break;
|
||||
}
|
||||
// Rejected: undo the trial write into the store (no
|
||||
// separate scratch copy here to have kept it out of in the
|
||||
// first place -- see the module doc).
|
||||
sub.set_params(store, &x);
|
||||
}
|
||||
|
||||
// Rejected (or the linear solve was unreliable): back off and
|
||||
// retry with a fresh damping factor.
|
||||
mu *= nu;
|
||||
nu *= 2.0;
|
||||
for i in 0..xsize {
|
||||
a[(i, i)] = diag_a[i];
|
||||
}
|
||||
k += 1;
|
||||
}
|
||||
|
||||
if stop != 0 {
|
||||
break 'outer;
|
||||
}
|
||||
if k > 50 {
|
||||
// Dead code upstream (see module doc) -- kept for fidelity.
|
||||
stop = 7;
|
||||
break 'outer;
|
||||
}
|
||||
}
|
||||
|
||||
if stop != 1 {
|
||||
store.revert(&snapshot);
|
||||
return SolveStatus::Failed;
|
||||
}
|
||||
SolveStatus::Success
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::constraints::point_line::P2PDistance;
|
||||
use crate::geo::Point;
|
||||
use std::rc::Rc;
|
||||
|
||||
#[test]
|
||||
fn solves_a_single_p2p_distance_by_moving_the_free_point() {
|
||||
let mut store = ParamStore::new();
|
||||
let p1 = Point::new(store.add(0.0, true), store.add(0.0, true));
|
||||
let p2 = Point::new(store.add(1.0, false), store.add(0.0, false));
|
||||
let distance = store.add(5.0, true);
|
||||
|
||||
let constr: Rc<dyn crate::constraints::Constraint> = Rc::new(P2PDistance::new(p1, p2, distance));
|
||||
let sub = SubSystem::new(vec![constr], &[p2.x, p2.y]);
|
||||
|
||||
let status = solve_lm(&sub, &mut store);
|
||||
assert_eq!(status, SolveStatus::Success);
|
||||
assert!(sub.error(&store).abs() < 1e-9);
|
||||
|
||||
let dx = store.get(p1.x) - store.get(p2.x);
|
||||
let dy = store.get(p1.y) - store.get(p2.y);
|
||||
assert!(((dx * dx + dy * dy).sqrt() - 5.0).abs() < 1e-6);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn a_zero_parameter_subsystem_trivially_succeeds() {
|
||||
let mut store = ParamStore::new();
|
||||
let sub = SubSystem::new(vec![], &[]);
|
||||
assert_eq!(solve_lm(&sub, &mut store), SolveStatus::Success);
|
||||
}
|
||||
}
|
||||
@@ -1,38 +0,0 @@
|
||||
//! Iterative solvers that drive a [`SubSystem`](crate::subsystem::SubSystem)'s
|
||||
//! residual toward zero by moving its free parameters.
|
||||
//!
|
||||
//! Ported from planegcs's three algorithms in `GCS.cpp`
|
||||
//! (`System::solve_DL`/`solve_LM`/`solve_BFGS`), one per submodule. Common
|
||||
//! to all of them: the C++ solves against its own scratch parameter copy
|
||||
//! (`SubSystem::pvals`, populated by `redirectParams()`) and leaves the
|
||||
//! *real* parameters untouched until a caller separately calls
|
||||
//! `applySolution()` after checking the return status. This port has no
|
||||
//! such scratch copy (see `subsystem.rs`'s module doc) — `SubSystem::
|
||||
//! set_params` always writes straight into the live [`ParamStore`
|
||||
//! ](crate::util::ParamStore) — so each solver here takes that snapshot
|
||||
//! itself, via [`ParamStore::redirect`](crate::util::ParamStore::redirect)
|
||||
//! before iterating, and either leaves the store holding the converged
|
||||
//! values (success) or calls
|
||||
//! [`ParamStore::revert`](crate::util::ParamStore::revert) to undo every
|
||||
//! trial step (failure) — the same commit-on-success/discard-on-failure
|
||||
//! contract, just self-contained instead of split across two calls.
|
||||
|
||||
pub mod bfgs;
|
||||
pub mod dogleg;
|
||||
pub mod lm;
|
||||
|
||||
/// Mirrors planegcs's `SolveStatus` (`GCS.h`). Dogleg and LM only ever
|
||||
/// produce two of its three C++ values — every non-1 stop code (max-
|
||||
/// iterations, no-progress, diverging, NaN) collapses to `Failed`, matching
|
||||
/// their own `return (stop == 1) ? Success : Failed;`. BFGS is the one
|
||||
/// solver that distinguishes a genuine `Converged` (step size below
|
||||
/// tolerance, but the residual itself isn't yet below `smallF`) from a full
|
||||
/// `Success` — see its own return statement in `GCS.cpp` — so this keeps
|
||||
/// that third variant rather than folding it into `Failed`, which would
|
||||
/// misrepresent what is usually still a usable result.
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum SolveStatus {
|
||||
Success,
|
||||
Converged,
|
||||
Failed,
|
||||
}
|
||||
@@ -1,268 +0,0 @@
|
||||
//! Assembles a set of constraints' residual, Jacobian, and gradient against
|
||||
//! a chosen set of free parameters.
|
||||
//!
|
||||
//! Ported from planegcs's `SubSystem.h`/`.cpp`, with one deliberate
|
||||
//! simplification: the C++ `SubSystem` keeps its own working copy of
|
||||
//! parameter values (`pvals`) and temporarily redirects each constraint's
|
||||
//! `double*`s to point into it (`redirectParams`/`revertParams`), so solving
|
||||
//! can be undone by reverting those pointers, with `applySolution()` copying
|
||||
//! the working copy back at the end. Here, constraints always address
|
||||
//! [`ParamStore`] directly through [`ParamId`]s (see `util.rs`), so there is
|
||||
//! no separate working copy to redirect into or apply back — a solver rolls
|
||||
//! back a bad iteration via [`ParamStore::redirect`]/[`ParamStore::revert`]
|
||||
//! on the store itself instead. That drops `pvals`/`pmap`/`redirectParams`/
|
||||
//! `revertParams`/`applySolution`/`getParamMap` as having no counterpart
|
||||
//! here, and likewise drops the reduction-map constructor variant (used in
|
||||
//! the C++ to merge coincident-point parameters), deferred to whichever
|
||||
//! later stage needs that specific optimization.
|
||||
|
||||
use std::collections::HashSet;
|
||||
use std::rc::Rc;
|
||||
|
||||
use nalgebra::{DMatrix, DVector};
|
||||
|
||||
use crate::constraints::Constraint;
|
||||
use crate::util::{ParamId, ParamStore};
|
||||
|
||||
pub struct SubSystem {
|
||||
clist: Vec<Rc<dyn Constraint>>,
|
||||
/// The free parameters this subsystem solves for: the intersection of
|
||||
/// the candidate `params` passed to [`SubSystem::new`] with the union of
|
||||
/// every constraint's own parameters — mirrors the C++'s `tmpplist`
|
||||
/// (`s1 ∩ s2`) becoming `plist` in the no-reduction-map path.
|
||||
plist: Vec<ParamId>,
|
||||
}
|
||||
|
||||
impl SubSystem {
|
||||
pub fn new(clist: Vec<Rc<dyn Constraint>>, params: &[ParamId]) -> Self {
|
||||
let candidates: HashSet<ParamId> = params.iter().copied().collect();
|
||||
let mut referenced: HashSet<ParamId> = HashSet::new();
|
||||
for constr in &clist {
|
||||
referenced.extend(constr.params());
|
||||
}
|
||||
// Preserve `params`' order, matching `std::set_intersection` over
|
||||
// ordered sets in the C++ (plist's order is otherwise unobserved by
|
||||
// any caller here, but a stable order keeps Jacobian columns and
|
||||
// solver output reproducible run to run).
|
||||
let plist: Vec<ParamId> = params.iter().copied().filter(|p| referenced.contains(p) && candidates.contains(p)).collect();
|
||||
|
||||
Self { clist, plist }
|
||||
}
|
||||
|
||||
pub fn p_size(&self) -> usize {
|
||||
self.plist.len()
|
||||
}
|
||||
|
||||
pub fn c_size(&self) -> usize {
|
||||
self.clist.len()
|
||||
}
|
||||
|
||||
pub fn plist(&self) -> &[ParamId] {
|
||||
&self.plist
|
||||
}
|
||||
|
||||
/// This subsystem's constraints, in the same order [`calc_residual`]/
|
||||
/// [`calc_jacobi`]'s rows use — lets a caller (e.g. `diagnosis.rs`) map
|
||||
/// a Jacobian row index back to the constraint it came from.
|
||||
pub fn constraints(&self) -> &[Rc<dyn Constraint>] {
|
||||
&self.clist
|
||||
}
|
||||
|
||||
/// `0.5 * sum(constraint.error()^2)` — the scalar objective a solver
|
||||
/// minimizes.
|
||||
pub fn error(&self, store: &ParamStore) -> f64 {
|
||||
let mut err = 0.0;
|
||||
for constr in &self.clist {
|
||||
let tmp = constr.error(store);
|
||||
err += tmp * tmp;
|
||||
}
|
||||
0.5 * err
|
||||
}
|
||||
|
||||
/// One residual entry per constraint, in `clist` order.
|
||||
pub fn calc_residual(&self, store: &ParamStore) -> DVector<f64> {
|
||||
DVector::from_iterator(self.c_size(), self.clist.iter().map(|c| c.error(store)))
|
||||
}
|
||||
|
||||
/// Jacobian of the residual w.r.t. `params`: `csize x params.len()`.
|
||||
pub fn calc_jacobi_for(&self, store: &ParamStore, params: &[ParamId]) -> DMatrix<f64> {
|
||||
let mut jacobi = DMatrix::zeros(self.c_size(), params.len());
|
||||
for (j, ¶m) in params.iter().enumerate() {
|
||||
for (i, constr) in self.clist.iter().enumerate() {
|
||||
jacobi[(i, j)] = constr.grad(store, param);
|
||||
}
|
||||
}
|
||||
jacobi
|
||||
}
|
||||
|
||||
/// [`calc_jacobi_for`](Self::calc_jacobi_for) against this subsystem's
|
||||
/// own [`plist`](Self::plist) — mirrors the C++'s no-argument
|
||||
/// `calcJacobi(Eigen::MatrixXd&)` overload.
|
||||
pub fn calc_jacobi(&self, store: &ParamStore) -> DMatrix<f64> {
|
||||
self.calc_jacobi_for(store, &self.plist.clone())
|
||||
}
|
||||
|
||||
/// Gradient of [`error`](Self::error) w.r.t. `params`:
|
||||
/// `sum(constraint.error() * constraint.grad(param))` per parameter.
|
||||
///
|
||||
/// The C++ restricts this sum to constraints adjacent to `param` via a
|
||||
/// `p2c` map; here every constraint is checked for every parameter
|
||||
/// instead, relying on [`Constraint::grad`]'s own membership check to
|
||||
/// contribute `0.0` for unrelated pairs — same result, no adjacency
|
||||
/// bookkeeping to maintain yet. Revisit if profiling ever shows this
|
||||
/// O(csize * params.len()) scan mattering at real sketch sizes.
|
||||
pub fn calc_grad_for(&self, store: &ParamStore, params: &[ParamId]) -> DVector<f64> {
|
||||
let mut grad = DVector::zeros(params.len());
|
||||
for (j, ¶m) in params.iter().enumerate() {
|
||||
for constr in &self.clist {
|
||||
grad[j] += constr.error(store) * constr.grad(store, param);
|
||||
}
|
||||
}
|
||||
grad
|
||||
}
|
||||
|
||||
pub fn calc_grad(&self, store: &ParamStore) -> DVector<f64> {
|
||||
self.calc_grad_for(store, &self.plist.clone())
|
||||
}
|
||||
|
||||
/// Largest step scale in `(0, lim]` every constraint accepts for a
|
||||
/// proposed direction, keyed the same way as `plist`/`dir`.
|
||||
pub fn max_step_for(&self, store: &ParamStore, params: &[ParamId], dir: &[f64], lim: f64) -> f64 {
|
||||
assert_eq!(params.len(), dir.len());
|
||||
let dir_map: std::collections::HashMap<ParamId, f64> =
|
||||
params.iter().copied().zip(dir.iter().copied()).collect();
|
||||
let mut alpha = lim;
|
||||
for constr in &self.clist {
|
||||
alpha = constr.max_step(store, &dir_map, alpha);
|
||||
}
|
||||
alpha
|
||||
}
|
||||
|
||||
pub fn max_step(&self, store: &ParamStore, dir: &[f64], lim: f64) -> f64 {
|
||||
self.max_step_for(store, &self.plist.clone(), dir, lim)
|
||||
}
|
||||
|
||||
/// Reads this subsystem's free parameters' current values from `store`,
|
||||
/// in [`plist`](Self::plist) order.
|
||||
pub fn get_params(&self, store: &ParamStore) -> DVector<f64> {
|
||||
DVector::from_iterator(self.p_size(), self.plist.iter().map(|&p| store.get(p)))
|
||||
}
|
||||
|
||||
/// Writes `x` into `store` at this subsystem's free parameters, in
|
||||
/// [`plist`](Self::plist) order.
|
||||
pub fn set_params(&self, store: &mut ParamStore, x: &DVector<f64>) {
|
||||
assert_eq!(x.len(), self.p_size());
|
||||
for (i, &p) in self.plist.iter().enumerate() {
|
||||
store.set(p, x[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
/// A minimal test-only constraint: `error = a - b` (so `a == b` at the
|
||||
/// zero). Keeps these tests independent of any concrete constraint from
|
||||
/// a later port stage.
|
||||
struct Difference {
|
||||
a: ParamId,
|
||||
b: ParamId,
|
||||
}
|
||||
|
||||
impl Constraint for Difference {
|
||||
fn params(&self) -> Vec<ParamId> {
|
||||
vec![self.a, self.b]
|
||||
}
|
||||
|
||||
fn error_value(&self, store: &ParamStore) -> f64 {
|
||||
store.get(self.a) - store.get(self.b)
|
||||
}
|
||||
|
||||
fn grad_value(&self, _store: &ParamStore, param: ParamId) -> f64 {
|
||||
if param == self.a {
|
||||
1.0
|
||||
} else if param == self.b {
|
||||
-1.0
|
||||
} else {
|
||||
0.0
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn plist_is_the_intersection_of_candidates_and_constraint_params() {
|
||||
let mut store = ParamStore::new();
|
||||
let a = store.add(1.0, false);
|
||||
let b = store.add(2.0, false);
|
||||
let unrelated = store.add(3.0, false);
|
||||
|
||||
let constr: Rc<dyn Constraint> = Rc::new(Difference { a, b });
|
||||
let sub = SubSystem::new(vec![constr], &[a, b, unrelated]);
|
||||
|
||||
assert_eq!(sub.plist(), &[a, b]);
|
||||
assert_eq!(sub.p_size(), 2);
|
||||
assert_eq!(sub.c_size(), 1);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn error_and_residual_reflect_the_constraint_error() {
|
||||
let mut store = ParamStore::new();
|
||||
let a = store.add(5.0, false);
|
||||
let b = store.add(2.0, false);
|
||||
let constr: Rc<dyn Constraint> = Rc::new(Difference { a, b });
|
||||
let sub = SubSystem::new(vec![constr], &[a, b]);
|
||||
|
||||
assert_eq!(sub.calc_residual(&store)[0], 3.0);
|
||||
assert_eq!(sub.error(&store), 0.5 * 3.0 * 3.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn jacobi_and_grad_match_the_hand_derived_formulas() {
|
||||
let mut store = ParamStore::new();
|
||||
let a = store.add(5.0, false);
|
||||
let b = store.add(2.0, false);
|
||||
let constr: Rc<dyn Constraint> = Rc::new(Difference { a, b });
|
||||
let sub = SubSystem::new(vec![constr], &[a, b]);
|
||||
|
||||
let jacobi = sub.calc_jacobi(&store);
|
||||
assert_eq!(jacobi.nrows(), 1);
|
||||
assert_eq!(jacobi.ncols(), 2);
|
||||
assert_eq!(jacobi[(0, 0)], 1.0);
|
||||
assert_eq!(jacobi[(0, 1)], -1.0);
|
||||
|
||||
// grad[j] = error * d(error)/d(param_j) = 3.0 * (+-1.0)
|
||||
let grad = sub.calc_grad(&store);
|
||||
assert_eq!(grad[0], 3.0);
|
||||
assert_eq!(grad[1], -3.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn get_and_set_params_round_trip_through_the_store() {
|
||||
let mut store = ParamStore::new();
|
||||
let a = store.add(5.0, false);
|
||||
let b = store.add(2.0, false);
|
||||
let constr: Rc<dyn Constraint> = Rc::new(Difference { a, b });
|
||||
let sub = SubSystem::new(vec![constr], &[a, b]);
|
||||
|
||||
let x = sub.get_params(&store);
|
||||
assert_eq!(x[0], 5.0);
|
||||
assert_eq!(x[1], 2.0);
|
||||
|
||||
sub.set_params(&mut store, &DVector::from_vec(vec![10.0, 4.0]));
|
||||
assert_eq!(store.get(a), 10.0);
|
||||
assert_eq!(store.get(b), 4.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn max_step_defaults_to_the_limit_when_no_constraint_restricts_it() {
|
||||
let mut store = ParamStore::new();
|
||||
let a = store.add(5.0, false);
|
||||
let b = store.add(2.0, false);
|
||||
let constr: Rc<dyn Constraint> = Rc::new(Difference { a, b });
|
||||
let sub = SubSystem::new(vec![constr], &[a, b]);
|
||||
|
||||
let alpha = sub.max_step(&store, &[1.0, 1.0], 42.0);
|
||||
assert_eq!(alpha, 42.0);
|
||||
}
|
||||
}
|
||||
@@ -1,155 +0,0 @@
|
||||
//! The top-level solver system: owns the parameter store and the full
|
||||
//! constraint list, and partitions them into independent [`SubSystem`]s.
|
||||
//!
|
||||
//! Ported from planegcs's `GCS::System` (`GCS.h`/`.cpp`), scoped to what
|
||||
//! this port stage needs: parameter/constraint bookkeeping and partitioning.
|
||||
//! The C++'s per-constraint-type `addConstraintXxx(...)` factory methods
|
||||
//! aren't ported as named wrappers yet — `add_constraint` takes any already-
|
||||
//! constructed [`Constraint`] (from `constraints::point_line`,
|
||||
//! `::angle_distance`, `::circle_arc`) directly, which is equivalent for
|
||||
//! everything actually ported so far, just less convenient at a call site
|
||||
//! than a per-type method would be. The three solving algorithms
|
||||
//! (`solve_BFGS`/`solve_LM`/`solve_DL`) and redundant/conflicting-constraint
|
||||
//! diagnosis are separate, later port stages (`solvers/`, `diagnosis.rs`).
|
||||
|
||||
use std::rc::Rc;
|
||||
|
||||
use crate::constraints::Constraint;
|
||||
use crate::graph;
|
||||
use crate::subsystem::SubSystem;
|
||||
use crate::util::{ParamId, ParamStore};
|
||||
|
||||
pub struct System {
|
||||
store: ParamStore,
|
||||
constraints: Vec<Rc<dyn Constraint>>,
|
||||
}
|
||||
|
||||
impl System {
|
||||
pub fn new() -> Self {
|
||||
Self { store: ParamStore::new(), constraints: Vec::new() }
|
||||
}
|
||||
|
||||
pub fn store(&self) -> &ParamStore {
|
||||
&self.store
|
||||
}
|
||||
|
||||
pub fn store_mut(&mut self) -> &mut ParamStore {
|
||||
&mut self.store
|
||||
}
|
||||
|
||||
pub fn add_param(&mut self, value: f64, driven: bool) -> ParamId {
|
||||
self.store.add(value, driven)
|
||||
}
|
||||
|
||||
pub fn add_constraint(&mut self, constraint: Rc<dyn Constraint>) {
|
||||
self.constraints.push(constraint);
|
||||
}
|
||||
|
||||
pub fn constraints(&self) -> &[Rc<dyn Constraint>] {
|
||||
&self.constraints
|
||||
}
|
||||
|
||||
/// Splits the current constraint list into independent [`SubSystem`]s —
|
||||
/// mirrors `System::initSolution`'s partitioning step (`GCS.cpp`),
|
||||
/// minus the equality-constraint parameter-reduction and
|
||||
/// redundant/conflicting-constraint exclusion that step also does
|
||||
/// (those belong to the later `diagnosis.rs` stage; every constraint
|
||||
/// here is treated as driving and undiagnosed).
|
||||
///
|
||||
/// A `SubSystem`'s free-parameter list is every one of its constraints'
|
||||
/// referenced params *excluding* the ones marked `driven` in the store
|
||||
/// (a dimensional constraint's target value, or any point the caller
|
||||
/// added with `driven: true` to keep fixed) — the solver moves the
|
||||
/// former to satisfy error = 0, and must never move the latter. Every
|
||||
/// hand-built `SubSystem::new(..., candidates)` call elsewhere in this
|
||||
/// port (and every one-shot bridge in the app crate) passes its own
|
||||
/// explicit non-driven candidate list for exactly this reason; this is
|
||||
/// `System`'s equivalent, computed automatically from the store instead
|
||||
/// of by the caller.
|
||||
pub fn partition(&self) -> Vec<SubSystem> {
|
||||
graph::partition_by_shared_params(&self.constraints)
|
||||
.into_iter()
|
||||
.map(|indices| {
|
||||
let clist: Vec<Rc<dyn Constraint>> = indices.iter().map(|&i| self.constraints[i].clone()).collect();
|
||||
let mut params: Vec<ParamId> =
|
||||
clist.iter().flat_map(|c| c.params()).filter(|&p| !self.store.is_driven(p)).collect();
|
||||
params.sort();
|
||||
params.dedup();
|
||||
SubSystem::new(clist, ¶ms)
|
||||
})
|
||||
.collect()
|
||||
}
|
||||
}
|
||||
|
||||
impl Default for System {
|
||||
fn default() -> Self {
|
||||
Self::new()
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::constraints::point_line::{Difference, P2PDistance};
|
||||
use crate::geo::Point;
|
||||
|
||||
#[test]
|
||||
fn a_two_point_distance_sketch_builds_partitions_and_round_trips() {
|
||||
// Two independent constraints on disjoint geometry: a P2PDistance
|
||||
// between two points, and an unrelated Difference — should land in
|
||||
// two separate subsystems.
|
||||
let mut sys = System::new();
|
||||
|
||||
let p1 = Point::new(sys.add_param(0.0, false), sys.add_param(0.0, false));
|
||||
let p2 = Point::new(sys.add_param(3.0, false), sys.add_param(4.0, false));
|
||||
let distance = sys.add_param(5.0, true); // driven: the target distance
|
||||
sys.add_constraint(Rc::new(P2PDistance::new(p1, p2, distance)));
|
||||
|
||||
let a = sys.add_param(1.0, false);
|
||||
let b = sys.add_param(2.0, false);
|
||||
let d = sys.add_param(1.0, true);
|
||||
sys.add_constraint(Rc::new(Difference::new(a, b, d)));
|
||||
|
||||
assert_eq!(sys.constraints().len(), 2);
|
||||
|
||||
let mut subsystems = sys.partition();
|
||||
assert_eq!(subsystems.len(), 2, "the two constraints share no parameters");
|
||||
|
||||
// "Stub solve": no actual iterative solver yet (stage 8) — just
|
||||
// confirm the assembled pipeline (partition -> residual/jacobi ->
|
||||
// get/set params) round-trips correctly end to end.
|
||||
subsystems.sort_by_key(|s| s.c_size().max(s.p_size()));
|
||||
for sub in &subsystems {
|
||||
let residual_before = sub.calc_residual(sys.store());
|
||||
let x = sub.get_params(sys.store());
|
||||
sub.set_params(sys.store_mut(), &x); // no-op round trip
|
||||
let residual_after = sub.calc_residual(sys.store());
|
||||
assert_eq!(residual_before, residual_after);
|
||||
}
|
||||
|
||||
// The P2PDistance subsystem: 4 free params (p1.x, p1.y, p2.x, p2.y)
|
||||
// — its `distance` target is `driven`, so `partition` must exclude
|
||||
// it from the free list despite `P2PDistance::params()` returning
|
||||
// it alongside the point coordinates.
|
||||
let p2p_sub = subsystems.iter().find(|s| s.p_size() == 4).expect("the 4-param P2PDistance subsystem");
|
||||
// Points already satisfy distance=5 (3-4-5 triangle), so its
|
||||
// residual should already be ~zero.
|
||||
assert!(p2p_sub.error(sys.store()).abs() < 1e-9);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn partition_excludes_driven_params_from_each_subsystems_free_list() {
|
||||
let mut sys = System::new();
|
||||
let p1 = Point::new(sys.add_param(0.0, true), sys.add_param(0.0, true)); // fixed anchor
|
||||
let p2 = Point::new(sys.add_param(1.0, false), sys.add_param(0.0, false)); // free, wrong distance
|
||||
let distance = sys.add_param(5.0, true); // driven target
|
||||
sys.add_constraint(Rc::new(P2PDistance::new(p1, p2, distance)));
|
||||
|
||||
let subsystems = sys.partition();
|
||||
assert_eq!(subsystems.len(), 1);
|
||||
// Only p2.x/p2.y are free — p1's coords and the distance target are
|
||||
// all `driven`, so despite being referenced by `P2PDistance::params()`
|
||||
// they must not appear in the subsystem's free-parameter list.
|
||||
assert_eq!(subsystems[0].p_size(), 2);
|
||||
}
|
||||
}
|
||||
@@ -1,161 +0,0 @@
|
||||
//! Parameter storage.
|
||||
//!
|
||||
//! planegcs represents every solver parameter as a raw `double*` aliased
|
||||
//! between geometry storage and the solver's working copy, with
|
||||
//! `redirectParams`/`revertParams` swapping what those pointers point at
|
||||
//! during an iteration. Rust disallows that aliasing, so parameters are
|
||||
//! plain indices (`ParamId`) into a [`ParamStore`], and "redirect" becomes
|
||||
//! snapshotting the store's values before an iterative solve mutates them
|
||||
//! in place, with "revert" restoring that snapshot on failure.
|
||||
|
||||
/// An index into a [`ParamStore`]. Cheap to copy, holds no value itself.
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, PartialOrd, Ord)]
|
||||
pub struct ParamId(u32);
|
||||
|
||||
/// The solver's parameter values, addressed by [`ParamId`].
|
||||
///
|
||||
/// A parameter marked `driven` is a fixed input (e.g. a dimension value)
|
||||
/// the solver must read but never write.
|
||||
#[derive(Debug, Clone, Default)]
|
||||
pub struct ParamStore {
|
||||
values: Vec<f64>,
|
||||
driven: Vec<bool>,
|
||||
}
|
||||
|
||||
/// A saved copy of a [`ParamStore`]'s values, for [`ParamStore::revert`].
|
||||
///
|
||||
/// Only valid against the store it was taken from; reverting into a store
|
||||
/// with a different parameter count is a programmer error and panics.
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct Snapshot(Vec<f64>);
|
||||
|
||||
impl ParamStore {
|
||||
pub fn new() -> Self {
|
||||
Self::default()
|
||||
}
|
||||
|
||||
pub fn len(&self) -> usize {
|
||||
self.values.len()
|
||||
}
|
||||
|
||||
pub fn is_empty(&self) -> bool {
|
||||
self.values.is_empty()
|
||||
}
|
||||
|
||||
/// Adds a parameter and returns the id to address it by.
|
||||
pub fn add(&mut self, value: f64, driven: bool) -> ParamId {
|
||||
let id = ParamId(self.values.len() as u32);
|
||||
self.values.push(value);
|
||||
self.driven.push(driven);
|
||||
id
|
||||
}
|
||||
|
||||
pub fn get(&self, id: ParamId) -> f64 {
|
||||
self.values[id.0 as usize]
|
||||
}
|
||||
|
||||
/// Overwrites a parameter's value regardless of its `driven` flag.
|
||||
///
|
||||
/// The solver itself must not call this on a driven parameter — that
|
||||
/// check belongs to the constraint/subsystem layer, which knows which
|
||||
/// parameters it is allowed to move. This method stays unconditional so
|
||||
/// setting up or re-driving a sketch (the one place driven values do
|
||||
/// change) does not need a separate code path.
|
||||
pub fn set(&mut self, id: ParamId, value: f64) {
|
||||
self.values[id.0 as usize] = value;
|
||||
}
|
||||
|
||||
pub fn is_driven(&self, id: ParamId) -> bool {
|
||||
self.driven[id.0 as usize]
|
||||
}
|
||||
|
||||
pub fn set_driven(&mut self, id: ParamId, driven: bool) {
|
||||
self.driven[id.0 as usize] = driven;
|
||||
}
|
||||
|
||||
/// Captures the current values so a failed solve can [`revert`](Self::revert).
|
||||
pub fn redirect(&self) -> Snapshot {
|
||||
Snapshot(self.values.clone())
|
||||
}
|
||||
|
||||
/// Restores values captured by [`redirect`](Self::redirect).
|
||||
///
|
||||
/// Panics if `snapshot` was not taken from this store (parameter count
|
||||
/// mismatch) — that pairing is a solver-internal invariant, not
|
||||
/// something callers should recover from.
|
||||
pub fn revert(&mut self, snapshot: &Snapshot) {
|
||||
assert_eq!(
|
||||
self.values.len(),
|
||||
snapshot.0.len(),
|
||||
"snapshot taken from a store with a different parameter count"
|
||||
);
|
||||
self.values.copy_from_slice(&snapshot.0);
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn add_get_set_round_trip() {
|
||||
let mut store = ParamStore::new();
|
||||
let a = store.add(1.0, false);
|
||||
let b = store.add(2.0, true);
|
||||
assert_eq!(store.get(a), 1.0);
|
||||
assert_eq!(store.get(b), 2.0);
|
||||
assert!(!store.is_driven(a));
|
||||
assert!(store.is_driven(b));
|
||||
|
||||
store.set(a, 5.0);
|
||||
assert_eq!(store.get(a), 5.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn redirect_then_revert_restores_original_values() {
|
||||
let mut store = ParamStore::new();
|
||||
let a = store.add(1.0, false);
|
||||
let b = store.add(2.0, false);
|
||||
|
||||
let snapshot = store.redirect();
|
||||
store.set(a, 100.0);
|
||||
store.set(b, 200.0);
|
||||
assert_eq!(store.get(a), 100.0);
|
||||
assert_eq!(store.get(b), 200.0);
|
||||
|
||||
store.revert(&snapshot);
|
||||
assert_eq!(store.get(a), 1.0);
|
||||
assert_eq!(store.get(b), 2.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn revert_after_no_mutation_is_a_no_op() {
|
||||
let mut store = ParamStore::new();
|
||||
let a = store.add(3.5, false);
|
||||
let snapshot = store.redirect();
|
||||
store.revert(&snapshot);
|
||||
assert_eq!(store.get(a), 3.5);
|
||||
}
|
||||
|
||||
#[test]
|
||||
#[should_panic(expected = "different parameter count")]
|
||||
fn revert_with_mismatched_snapshot_panics() {
|
||||
let mut store = ParamStore::new();
|
||||
store.add(1.0, false);
|
||||
let snapshot = store.redirect();
|
||||
|
||||
let mut other = ParamStore::new();
|
||||
other.add(1.0, false);
|
||||
other.add(2.0, false);
|
||||
other.revert(&snapshot);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn set_driven_can_toggle_the_flag() {
|
||||
let mut store = ParamStore::new();
|
||||
let a = store.add(1.0, false);
|
||||
assert!(!store.is_driven(a));
|
||||
store.set_driven(a, true);
|
||||
assert!(store.is_driven(a));
|
||||
}
|
||||
}
|
||||
Reference in new issue
Block a user