Files
solidcraft/crates/sketch/src/cursor.rs
PaddleStroke 863b0777da Sketch: dragging a constrained line by its middle no longer stalls (#11696) (#11714)
* claim: #11696

* Sketch: move constrained lines that stall during dragging
2026-11-04 18:19:46 +00:00

400 lines
17 KiB
Rust

//! Feasible cursor motion: optimize only within the design Jacobian's null space, then
//! retract to the constraint manifold. Mouse error never competes with design residuals.
use super::{
ACCEPT, Hold, Layout, State, TOL, build_blocks, columns, initial_x, layout, lm_capped_with_budget, local_system, max_abs, points_on_curves,
residual_work, system, within_budget,
};
use crate::{
Budget, CurveKind, Sketch,
linalg::{Mat, rref},
union_find::{find, union},
};
use solidcore_record::Vec2;
use solidcraft_geom::prelude::*;
const MAX_VARIABLES: usize = 400;
const MAX_STEPS: usize = 96;
const CORRECTION_ITERS: usize = 16;
const TOTAL_CORRECTION_ITERS: usize = 256;
/// Pivot tolerance of the null space, on the Jacobian equilibrated to unit rows and columns.
/// Redundant constraints (a parallelogram held both parallel and equal on each pair of sides)
/// are dependent only at an exact solution; a pose corrected to `TOL` leaves their rows
/// independent by about that much, and a tighter tolerance counts that as rank and finds no
/// freedom (#4585). The solver's rank analysis, which reports the sketch's degrees of freedom,
/// uses 1e-7 too.
const NULL_SPACE_TOL: f64 = 1e-7;
/// Move a point toward `target` using freedom left by the design constraints. `max_step`
/// bounds the full variable displacement before constraint correction; `valid` can reject
/// a corrected candidate (for example, an arc crossing its own start). Only feasible,
/// cursor-improving candidates are committed. False leaves the sketch unchanged when the
/// input is unsolved, invalid or outside the dense-work limit. Work is bounded; true may
/// retain the best feasible pose when the iteration or optional shared frame-work budget is
/// exhausted. The work budget includes Jacobian, null-space and LM arithmetic, not wall time.
pub fn move_point_towards(
sk: &mut Sketch,
point: usize,
target: Vec2,
max_step: f64,
budget: Option<&mut Budget>,
valid: impl Fn(&Sketch, &Sketch) -> bool,
) -> bool {
move_cursor_towards(sk, &[(point, Vec2::X)], target, max_step, budget, valid)
}
/// Move a grabbed position attached to a line toward the cursor. `grab.x` is its fraction
/// along the line (0 at its start, 1 at its end); `grab.y` is the signed perpendicular offset,
/// also divided by the line's length. The offset rotates and scales with the line.
/// The line may rotate: its endpoints are not independently pinned to a translation.
/// Feasibility, work limits and the return value have the same meaning as [`move_point_towards`].
pub fn move_line_towards(
sk: &mut Sketch,
line: usize,
grab: Vec2,
target: Vec2,
max_step: f64,
budget: Option<&mut Budget>,
valid: impl Fn(&Sketch, &Sketch) -> bool,
) -> bool {
if !grab.is_finite() {
return false;
}
let Some(CurveKind::Line { a, b }) = sk.curves.get(line).map(|c| &c.kind) else { return false };
let (Some(pa), Some(pb)) = (sk.point(*a), sk.point(*b)) else { return false };
let d = pb - pa;
if !(d.dot(d) > 1e-18 && crate::model::in_range(pa + weighted(d, grab))) {
return false;
}
move_cursor_towards(sk, &[(*a, Vec2::new(1.0 - grab.x, -grab.y)), (*b, grab)], target, max_step, budget, valid)
}
// Both callers' endpoint weights sum to (1, 0): translating the anchors translates the grab.
fn move_cursor_towards(
sk: &mut Sketch,
points: &[(usize, Vec2)],
target: Vec2,
max_step: f64,
mut budget: Option<&mut Budget>,
valid: impl Fn(&Sketch, &Sketch) -> bool,
) -> bool {
let setup_work = sk.points.len().saturating_add(sk.curves.len()).saturating_add(sk.constraints.len()).max(1);
if !within_budget(&mut budget, setup_work) {
return false;
}
if !crate::model::in_range(target) || !max_step.is_finite() || max_step <= 0.0 || !in_range(sk) {
return false;
}
let problem = sk.clone();
let lay = layout(&problem, &Hold::default());
let active: Vec<_> =
points.iter().filter(|(_, w)| w.x != 0.0 || w.y != 0.0).filter_map(|(p, w)| Some((lay.pvar.get(*p).copied().flatten()?, *w))).collect();
let Some(&(px, _)) = active.first() else { return false };
let (blocks, _) = build_blocks(&problem, &lay, &points_on_curves(&problem));
let mut parent: Vec<usize> = (0..lay.n).collect();
for &(v, _) in &active {
union(&mut parent, px, v);
union(&mut parent, px, v + 1);
}
for (_, vars) in &blocks {
if let Some(&first) = vars.first() {
for &v in vars.iter().skip(1) {
union(&mut parent, first, v);
}
}
}
let root = find(&mut parent, px);
let vars: Vec<usize> = (0..lay.n).filter(|v| find(&mut parent, *v) == root).collect();
if vars.len() > MAX_VARIABLES {
return false;
}
let sub: Vec<_> =
blocks.iter().filter(|(_, v)| v.iter().any(|i| vars.binary_search(i).is_ok())).map(|(b, v)| (super::copy_block(b), v.clone())).collect();
// Reserve both initial and final all-block validation before starting cursor work.
// An exhausted search can then keep its best feasible pose, including unrelated blocks.
if !within_budget(&mut budget, residual_work(&lay, &blocks).saturating_mul(2)) {
return false;
}
let mut x = initial_x(&problem, &lay);
let initial_residual = max_abs(&system(&problem, &lay, &blocks, &mut x, false).0);
if initial_residual >= ACCEPT {
return false;
}
let mut col = vec![usize::MAX; lay.n];
columns(&mut col, &vars, &vars);
let axes: Vec<_> = active.iter().filter_map(|(v, w)| Some((vars.binary_search(v).ok()?, *w))).collect();
let position = |x: &[f64]| {
let state = State { sk: &problem, lay: &lay, x };
// Relative coordinates avoid cancellation when a small line is far from the origin.
let base = points.first().map_or(Vec2::ZERO, |(p, _)| state.p(*p));
points.iter().fold(base, |at, (p, w)| at + weighted(state.p(*p) - base, *w))
};
let mut best = problem.clone();
let mut spent = 0;
// Held feasibility solves accept up to ACCEPT. A tiny, nearly feasible hard pin can
// precede a failed step; refine it before constructing a tangent, rather than losing
// the cursor objective just because the previous pose is between TOL and ACCEPT.
// An ill-conditioned sketch (a near-zero-length construction line makes its angle row
// unsolvable to TOL, #7635) stalls that refinement between TOL and ACCEPT. The solver
// itself calls such a pose solved, so the cursor keeps working from it and accepts its
// corrected trials at the same tolerance the final validation uses.
let mut trial_tol = TOL;
if initial_residual >= TOL {
let Some((its, residual)) = lm_capped_with_budget(&problem, &lay, &sub, &vars, &mut x, &col, CORRECTION_ITERS, budget.as_deref_mut()) else {
return false;
};
spent += its.max(1);
if residual >= ACCEPT {
return false;
}
if residual >= TOL {
trial_tol = ACCEPT;
}
apply(&mut best, &lay, &x);
if !in_range(&best) || !valid(&problem, &best) {
return false;
}
}
let mut gain = 1.0_f64;
for _ in 0..MAX_STEPS {
let at = position(&x);
let error = target - at;
if error.len() < 1e-7 || spent >= TOTAL_CORRECTION_ITERS {
break;
}
let n = vars.len();
let jacobian_work = residual_work(&lay, &sub).saturating_mul(n.saturating_mul(2).saturating_add(1));
if !within_budget(&mut budget, jacobian_work) {
break;
}
let (_, j, _) = local_system(&problem, &lay, &sub, &mut x, &col, n);
let Some(z) = null_space(j, &mut budget) else { break };
if z.is_empty() {
break;
}
let mut direction = target_step(&z, &axes, error);
let norm = length(&direction);
// Adapt to curvature of the reachable locus. Inside a circle the Gauss-Newton
// cursor step can be arbitrarily small although useful freedom remains.
let scale = if norm > 0.0 { gain.min(max_step / norm) } else { gain };
for v in &mut direction {
*v *= scale;
}
let mut directions = vec![(direction, false)];
// A cursor across a circular locus has (nearly) zero first derivative at the
// farthest point. If line search stalls, probe both tangent directions. Testing
// only an exactly zero gradient would strand reversed drags after rounding.
let influence = |v: &&Vec<f64>| cursor_direction(v, &axes).len();
let mut probes: Vec<_> = z.iter().collect();
probes.sort_by(|a, b| influence(b).total_cmp(&influence(a)));
for v in probes.into_iter().take(2) {
directions.push((v.iter().map(|x| x * max_step).collect(), true));
directions.push((v.iter().map(|x| -x * max_step).collect(), true));
}
let old_cost = error.dot(error);
let mut accepted = None;
'search: for (direction, probe) in directions {
if length(&direction) < 1e-12 {
continue;
}
for backtrack in 0..if probe { 1 } else { 8 } {
if spent >= TOTAL_CORRECTION_ITERS {
break 'search;
}
let alpha = 0.5_f64.powi(backtrack);
let mut trial = x.clone();
for (&v, d) in vars.iter().zip(&direction) {
if let Some(t) = trial.get_mut(v) {
*t += alpha * d;
}
}
let Some((its, residual)) = lm_capped_with_budget(
&problem,
&lay,
&sub,
&vars,
&mut trial,
&col,
CORRECTION_ITERS.min(TOTAL_CORRECTION_ITERS - spent),
budget.as_deref_mut(),
) else {
break 'search;
};
spent += its.max(1);
if !(residual < trial_tol) {
if budget.as_ref().is_some_and(|b| b.exhausted()) {
break 'search;
}
continue;
}
let next = position(&trial);
let cost = (target - next).dot(target - next);
// The cursor itself must move. Retracting onto the constraints moves it by
// rounding even along freedom that can't reach it (the far end of a line held only
// by its direction): taking such a step for the rounding moved that end a whole
// step, and step after step it walked away or collapsed its line to a point (#8944).
if !cost.is_finite() || cost >= old_cost - 1e-14 * old_cost.max(1.0) || next.dist(at) < trial_tol {
continue;
}
if !within_budget(&mut budget, setup_work.saturating_mul(4)) {
break 'search;
}
let mut candidate = best.clone();
apply(&mut candidate, &lay, &trial);
if !in_range(&candidate) || !valid(&best, &candidate) {
continue;
}
accepted = Some((trial, candidate, ((if probe { 1.0 } else { scale }) * alpha * 1.5).clamp(1e-8, 1e8)));
break 'search;
}
}
let Some((next, candidate, next_gain)) = accepted else { break };
x = next;
best = candidate;
gain = next_gain;
}
// Full constraint validation includes constant blocks and disconnected components.
if max_abs(&system(&best, &lay, &blocks, &mut x, false).0) >= ACCEPT {
return false;
}
super::refresh_driven_dimensions(&mut best);
*sk = best;
true
}
fn length(v: &[f64]) -> f64 {
v.iter().map(|x| x * x).sum::<f64>().sqrt()
}
fn in_range(sk: &Sketch) -> bool {
sk.points.iter().all(|p| crate::model::in_range(p.pos))
&& sk
.curves
.iter()
.all(|c| !matches!(c.kind, CurveKind::Circle { r, .. } | CurveKind::Ellipse { r, .. } if !crate::model::radius_in_range(r)))
}
fn null_space(j: Mat, budget: &mut Option<&mut Budget>) -> Option<Vec<Vec<f64>>> {
// A pivot processes at most all rows and columns. A rectangular matrix has at
// most as many pivots as its smaller dimension.
let reduction_work = j.rows.saturating_mul(j.cols).saturating_mul(j.rows.min(j.cols));
if !within_budget(budget, reduction_work) {
return None;
}
// Equilibrate before the rank analysis. A near-zero-length construction line makes its
// angle row's entries on that line ~1e7 while the same row's entries on the line it
// measures are ~1: per-row scaling alone hides those (physically meaningful) small
// entries below the pivot tolerance, and the null space then lets the measured line
// rotate freely (#7635). Scaling each column by its largest entry first brings the
// row's entries onto one scale; the null vectors are mapped back afterwards.
let mut col = vec![0.0_f64; j.cols];
for (c, m) in col.iter_mut().enumerate() {
for r in 0..j.rows {
*m = m.max(j.get(r, c).abs());
}
}
let inv: Vec<f64> = col.iter().map(|m| 1.0 / m.max(1.0)).collect();
let mut a = Mat::zeros(j.rows, j.cols);
for r in 0..j.rows {
let scale = (0..j.cols).map(|c| j.get(r, c).abs() * inv[c]).fold(0.0_f64, f64::max);
if scale > 0.0 {
for (c, &ic) in inv.iter().enumerate() {
a.set(r, c, j.get(r, c) * ic / scale);
}
}
}
let pivots = rref(&mut a, NULL_SPACE_TOL);
// Only the free directions are orthogonalized. In a nearly determined sketch,
// reserving work for a full basis could consume the frame before any motion.
let free = a.cols.saturating_sub(pivots.len());
let basis_work = a.cols.saturating_mul(free).saturating_mul(free);
if !within_budget(budget, basis_work) {
return None;
}
let mut z: Vec<Vec<f64>> = Vec::new();
for free in (0..a.cols).filter(|c| !pivots.contains(c)) {
let mut v = vec![0.0; a.cols];
if let Some(x) = v.get_mut(free) {
*x = 1.0;
}
for (row, &pivot) in pivots.iter().enumerate() {
if let Some(x) = v.get_mut(pivot) {
*x = -a.get(row, free);
}
}
for (x, &ic) in v.iter_mut().zip(&inv) {
*x *= ic;
}
for _ in 0..2 {
for q in &z {
let d: f64 = v.iter().zip(q).map(|(a, b)| a * b).sum();
for (a, b) in v.iter_mut().zip(q) {
*a -= d * b;
}
}
}
let norm = length(&v);
if norm.is_finite() && norm > 1e-10 {
for x in &mut v {
*x /= norm;
}
z.push(v);
}
}
Some(z)
}
/// The second weight component multiplies a 90-degree rotation of the endpoint.
fn weighted(v: Vec2, w: Vec2) -> Vec2 {
if w.y == 0.0 { v * w.x } else { Vec2::new(v.x * w.x - v.y * w.y, v.y * w.x + v.x * w.y) }
}
fn cursor_direction(v: &[f64], axes: &[(usize, Vec2)]) -> Vec2 {
axes.iter().fold(Vec2::ZERO, |at, (i, w)| at + weighted(Vec2::new(v.get(*i).copied().unwrap_or(0.0), v.get(*i + 1).copied().unwrap_or(0.0)), *w))
}
fn target_step(z: &[Vec<f64>], axes: &[(usize, Vec2)], error: Vec2) -> Vec<f64> {
let mut a = 0.0;
let mut b = 0.0;
let mut c = 0.0;
for q in z {
let Vec2 { x, y } = cursor_direction(q, axes);
a += x * x;
b += x * y;
c += y * y;
}
let high = 0.5 * (a + c + (a - c).hypot(2.0 * b));
let low = if high > 0.0 { (a * c - b * b).max(0.0) / high } else { 0.0 };
let axis = if a >= c { Vec2::new(high - c, b) } else { Vec2::new(b, high - a) }.normalized().unwrap_or(if a >= c { Vec2::X } else { Vec2::Y });
let other = Vec2::new(-axis.y, axis.x);
let mut u = if high > 1e-12 { axis * (axis.dot(error) / high) } else { Vec2::ZERO };
if low > high * 1e-8 {
u += other * (other.dot(error) / low);
}
let mut step = vec![0.0; z.first().map_or(0, Vec::len)];
for q in z {
let d = cursor_direction(q, axes).dot(u);
for (x, v) in step.iter_mut().zip(q) {
*x += d * v;
}
}
step
}
fn apply(sk: &mut Sketch, lay: &Layout, x: &[f64]) {
for (i, p) in sk.points.iter_mut().enumerate() {
if let Some(v) = lay.pvar.get(i).copied().flatten()
&& let (Some(a), Some(b)) = (x.get(v), x.get(v + 1))
{
p.pos = Vec2::new(*a, *b);
}
}
for (i, c) in sk.curves.iter_mut().enumerate() {
if let Some(v) = lay.rvar.get(i).copied().flatten()
&& let Some(value) = x.get(v)
&& let CurveKind::Circle { r, .. } | CurveKind::Ellipse { r, .. } = &mut c.kind
{
*r = *value;
}
}
}