Skip to content

SolidCraft: an open-source parametric 3D CAD application built in pure Rust. Desktop, web, and automation through MCP.

16.8 KB
Raw Blame
//! 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;
        }
    }
}