geop_core_sketch/space/
solve.rs

1//! Solving a 3-D sketch the way a planar sketch is solved (see
2//! [`crate::solve`]): every constraint a [`Residual`] of the sketch's
3//! variables, each a length, computed as a [`geop_core_math::dual::Dual`] of
4//! the sketch's scalar so its gradient is exact and its value an enclosure;
5//! [`geop_core_math::solvers::system`] holds them, pulls a dragged point,
6//! and encloses the exact solution. Everything this module asks of the
7//! solver is in [`Problem`]'s methods and [`Sketch3d`]'s below them.
8//!
9//! **Variables.** Each class of coincident points (see
10//! [`Sketch3d::point_classes`]) is one `(x, y, z)`. A point on a line or on
11//! a reference curve adds where along it — its parameter there — so lying
12//! on it is three plain equations, `p = C(t)`, rather than a distance whose
13//! square root has no slope at its solution. A point on an arc needs no
14//! parameter: it lies in the arc's plane and as far from its center as its
15//! ends.
16//!
17//! **Directions.** "Parallel to `d`" is two equations, not one: the cross
18//! product of the two unit directions, measured along two vectors across
19//! `d` (see [`across`]) — a cross product's length, like a distance, has no
20//! slope where it vanishes.
21//!
22//! What is structural is no residual at all: coincidence (one class of
23//! points), and a spline end's direction, which the spline is built with
24//! (see [`Sketch3d::adopts`]).
25
26use std::collections::BTreeMap;
27
28use geop_core_geometry::nurb_curve::NurbCurve3D;
29use geop_core_math::solvers::system::{self, Phase};
30use geop_core_math::{
31    geop_error::{GeopError, GeopResult},
32    scalars::{Number, Scalar, as_f64},
33    vector::Vector3,
34};
35use serde::{Deserialize, Serialize};
36
37use super::{
38    Constraint3d, CurveKind3d, Enclosure3d, End, Point3d, Sketch3d,
39    geometry::{Arc3, Helix3, across},
40};
41use crate::{
42    ConstraintId, CurveId, PointId,
43    variables::{PointLayout, Residuals, SketchPoint, Standing, Variables},
44};
45
46/// The most variables one constraint depends on: a tangency between two
47/// arcs, five points.
48const MAX_LOCAL_VARS: usize = 16;
49
50type Dual = geop_core_math::dual::Dual<MAX_LOCAL_VARS>;
51
52/// Maps the sketch's entities to solver variables: each class of points a
53/// triple (see [`PointLayout`]), then a parameter for each point on a line
54/// or a reference curve.
55struct Layout {
56    points: PointLayout<3>,
57    /// The parameter of each [`Constraint3d::OnCurve`] on a line or a
58    /// reference curve.
59    param_var: BTreeMap<ConstraintId, usize>,
60}
61
62impl SketchPoint<3> for Point3d {
63    fn fixed(&self) -> bool {
64        self.fixed
65    }
66
67    fn coordinates(&self) -> [Scalar; 3] {
68        self.at.to_array()
69    }
70
71    fn set_coordinates(&mut self, at: [Scalar; 3]) {
72        self.at = Vector3::from_array(at);
73    }
74}
75
76impl Layout {
77    fn new(sketch: &Sketch3d) -> Self {
78        let mut points = PointLayout::new(&sketch.points, &sketch.point_classes());
79        let mut param_var = BTreeMap::new();
80        for (&id, c) in &sketch.constraints {
81            if let Constraint3d::OnCurve { curve, .. } = c
82                && matches!(
83                    sketch.curves[curve].kind,
84                    CurveKind3d::Line { .. } | CurveKind3d::Reference
85                )
86            {
87                param_var.insert(id, points.push(true));
88            }
89        }
90        Layout { points, param_var }
91    }
92}
93
94/// One constraint, ready to evaluate.
95struct Prepared<'a> {
96    id: ConstraintId,
97    constraint: &'a Constraint3d,
98    vars: Vec<usize>,
99    /// Two vectors across the direction a "parallel" is measured from (see
100    /// [`across`]).
101    across: Option<[Vector3; 2]>,
102    /// The reference curve a point lies on.
103    reference: Option<&'a NurbCurve3D>,
104}
105
106/// Everything a solve evaluates, fixed for one solve.
107struct Problem<'a> {
108    sketch: &'a Sketch3d,
109    layout: Layout,
110    constraints: Vec<Prepared<'a>>,
111    /// The variables where the solve starts: the points as drawn, each
112    /// parameter where its point is nearest its curve.
113    start: Vec<Scalar>,
114    /// The sketch's characteristic length: the diagonal of its points' box,
115    /// at least one.
116    scale: Scalar,
117}
118
119/// The point `p` of `x`.
120fn point_of<T: Number>(layout: &Layout, x: &[T], p: PointId) -> Vector3<T> {
121    Vector3::from_array(layout.points.point(x, p))
122}
123
124/// The sketch's curve `c` as an arc, at the variables `x`; `None` for
125/// another kind.
126fn arc_of<T: Number>(sketch: &Sketch3d, layout: &Layout, x: &[T], c: CurveId) -> Option<Arc3<T>> {
127    match sketch.curves[&c].kind {
128        CurveKind3d::Arc {
129            start,
130            through,
131            end,
132        } => Some(Arc3 {
133            s: point_of(layout, x, start),
134            m: point_of(layout, x, through),
135            e: point_of(layout, x, end),
136        }),
137        _ => None,
138    }
139}
140
141/// The ends of the line `c`, at the variables `x`.
142fn line_of<T: Number>(
143    sketch: &Sketch3d,
144    layout: &Layout,
145    x: &[T],
146    c: CurveId,
147) -> (Vector3<T>, Vector3<T>) {
148    match sketch.curves[&c].kind {
149        CurveKind3d::Line { start, end } => (point_of(layout, x, start), point_of(layout, x, end)),
150        ref k => unreachable!("validated to be a line: {k:?}"),
151    }
152}
153
154/// The unit tangent of the line or arc `c` at its end `end`, in its own
155/// direction, at the variables `x`.
156fn tangent_of<T: Number>(
157    sketch: &Sketch3d,
158    layout: &Layout,
159    x: &[T],
160    c: CurveId,
161    end: End,
162) -> GeopResult<Vector3<T>> {
163    match arc_of(sketch, layout, x, c) {
164        Some(arc) => arc.tangent(end == End::End),
165        None => {
166            let (s, e) = line_of(sketch, layout, x, c);
167            e.sub(&s).normalize()
168        }
169    }
170}
171
172impl<'a> Problem<'a> {
173    fn new(sketch: &'a Sketch3d) -> GeopResult<Self> {
174        sketch.validate()?;
175        let layout = Layout::new(sketch);
176        let mut start = vec![Scalar::ZERO; layout.points.len()];
177        layout.points.read(&sketch.points, &mut start);
178
179        let span = |k: usize| {
180            let values = || sketch.points.values().map(move |p| p.at[k]);
181            let hi = values().reduce(Scalar::max).unwrap_or(Scalar::ZERO);
182            let lo = values().reduce(Scalar::min).unwrap_or(Scalar::ZERO);
183            hi.sub(lo)
184        };
185        let diagonal = (0..3)
186            .map(|k| span(k).mul(span(k)))
187            .fold(Scalar::ZERO, Scalar::add)
188            .sqrt()?;
189        let scale = if diagonal.is_finite() && diagonal.definitely_greater(Scalar::ONE) {
190            diagonal
191        } else {
192            Scalar::ONE
193        };
194
195        let mut constraints = Vec::new();
196        for (&id, c) in &sketch.constraints {
197            use Constraint3d::*;
198            let mut vars = Vec::new();
199            let mut across_ = None;
200            let mut reference = None;
201            let curve_vars = |c: CurveId, vars: &mut Vec<usize>| {
202                for p in sketch.curves[&c].points() {
203                    layout.points.point_vars(p, vars);
204                }
205            };
206            match c {
207                Coincident { a, b } if sketch.points[a].fixed && sketch.points[b].fixed => {}
208                // Built into the layout.
209                Coincident { .. } => continue,
210                Coordinate { point, .. } => layout.points.point_vars(*point, &mut vars),
211                Distance { a, b, .. } => {
212                    layout.points.point_vars(*a, &mut vars);
213                    layout.points.point_vars(*b, &mut vars);
214                }
215                Length { line: c, .. } | Radius { arc: c, .. } | HelixEnd { helix: c } => {
216                    curve_vars(*c, &mut vars);
217                }
218                Parallel { a, b } => {
219                    curve_vars(*a, &mut vars);
220                    curve_vars(*b, &mut vars);
221                    let (s, e) = line_of(sketch, &layout, &start, *a);
222                    across_ = Some(across(&e.sub(&s))?);
223                }
224                ParallelTo { line, direction } => {
225                    curve_vars(*line, &mut vars);
226                    across_ = Some(across(direction)?);
227                }
228                // A spline is built along it.
229                TangentTo { curve, .. }
230                    if matches!(sketch.curves[curve].kind, CurveKind3d::Spline { .. }) =>
231                {
232                    continue;
233                }
234                TangentTo {
235                    curve, direction, ..
236                } => {
237                    curve_vars(*curve, &mut vars);
238                    across_ = Some(across(direction)?);
239                }
240                OnCurve { point, curve } => {
241                    layout.points.point_vars(*point, &mut vars);
242                    curve_vars(*curve, &mut vars);
243                    if let Some(&t) = layout.param_var.get(&id) {
244                        vars.push(t);
245                        let p = point_of(&layout, &start, *point);
246                        let seed = match &sketch.curves[curve].kind {
247                            CurveKind3d::Line { .. } => {
248                                let (s, e) = line_of(sketch, &layout, &start, *curve);
249                                let d = e.sub(&s);
250                                p.sub(&s).prod_dot(&d).div(d.norm_sq())?
251                            }
252                            _ => {
253                                let curve = &sketch.references[curve];
254                                reference = Some(curve);
255                                // Nearest `p`: a point that lies on the
256                                // curve is found where it is, and a solve
257                                // does not move it along the curve to meet
258                                // a parameter merely near its own.
259                                curve.closest_point(&p)?.0
260                            }
261                        };
262                        // Where to start looking from: a free choice, sharp.
263                        start[t] = Scalar::from_f64(seed.to_f64());
264                    }
265                }
266                Tangent { a, b } => {
267                    if sketch.adopts(*a, *b)?.is_some() {
268                        continue;
269                    }
270                    curve_vars(*a, &mut vars);
271                    curve_vars(*b, &mut vars);
272                    let (ea, _) = sketch.shared_end(*a, *b)?.expect("validated");
273                    across_ = Some(across(&tangent_of(sketch, &layout, &start, *a, ea)?)?);
274                }
275            }
276            vars.sort_unstable();
277            vars.dedup();
278            if vars.len() > MAX_LOCAL_VARS {
279                return Err(GeopError::new(format!(
280                    "constraint {id} depends on {} variables, more than the supported {MAX_LOCAL_VARS}",
281                    vars.len()
282                )));
283            }
284            constraints.push(Prepared {
285                id,
286                constraint: c,
287                vars,
288                across: across_,
289                reference,
290            });
291        }
292        Ok(Problem {
293            sketch,
294            layout,
295            constraints,
296            start,
297            scale,
298        })
299    }
300
301    /// The residuals of `p` at the variables `x` — only those it depends on
302    /// are read.
303    fn residuals(&self, p: &Prepared<'_>, x: &[Dual], out: &mut Vec<Dual>) -> GeopResult<()> {
304        use Constraint3d::*;
305        let c = Dual::cst;
306        let scale = c(self.scale);
307        let (sketch, layout) = (self.sketch, &self.layout);
308        let point = |q: PointId| point_of(layout, x, q);
309        // `a` parallel to `b`, two lengths (see the module docs).
310        let parallel = |a: &Vector3<Dual>, b: &Vector3<Dual>, out: &mut Vec<Dual>| {
311            let cross = a.prod_cross(b);
312            for u in p.across.as_ref().expect("prepared") {
313                out.push(cross.prod_dot(&u.map(c)).mul(scale));
314            }
315        };
316        match *p.constraint {
317            Coincident { a, b } => {
318                let d = sketch.points[&b].at.sub(&sketch.points[&a].at);
319                out.extend(d.to_array().map(c));
320            }
321            Coordinate {
322                point: q,
323                axis,
324                value,
325            } => {
326                out.push(point(q)[axis.index()].sub(c(value)));
327            }
328            Distance { a, b, value } => out.push(point(b).sub(&point(a)).try_norm()?.sub(c(value))),
329            Length { line, value } => {
330                let (s, e) = line_of(sketch, layout, x, line);
331                out.push(e.sub(&s).try_norm()?.sub(c(value)));
332            }
333            Radius { arc, value } => {
334                let arc = arc_of(sketch, layout, x, arc).expect("validated");
335                out.push(arc.radius()?.sub(c(value)));
336            }
337            HelixEnd { helix } => {
338                let CurveKind3d::Helix {
339                    start,
340                    base,
341                    top,
342                    end,
343                    turns,
344                    left,
345                } = sketch.curves[&helix].kind
346                else {
347                    unreachable!("validated to be a helix");
348                };
349                let helix = Helix3 {
350                    start: point(start),
351                    base: point(base),
352                    top: point(top),
353                    turns,
354                    left,
355                };
356                out.extend(point(end).sub(&helix.end()?).to_array());
357            }
358            Parallel { a, b } => {
359                let (sa, ea) = line_of(sketch, layout, x, a);
360                let (sb, eb) = line_of(sketch, layout, x, b);
361                parallel(&ea.sub(&sa).normalize()?, &eb.sub(&sb).normalize()?, out);
362            }
363            ParallelTo { line, direction } => {
364                let (s, e) = line_of(sketch, layout, x, line);
365                parallel(&e.sub(&s).normalize()?, &direction.map(c).normalize()?, out);
366            }
367            TangentTo {
368                curve,
369                end,
370                direction,
371            } => {
372                let t = tangent_of(sketch, layout, x, curve, end)?;
373                parallel(&t, &direction.map(c).normalize()?, out);
374            }
375            OnCurve { point: q, curve } => {
376                let q = point(q);
377                let on = match (&sketch.curves[&curve].kind, layout.param_var.get(&p.id)) {
378                    (CurveKind3d::Arc { .. }, _) => {
379                        let arc = arc_of(sketch, layout, x, curve).expect("an arc");
380                        let center = arc.center()?;
381                        out.push(q.sub(&arc.m).prod_dot(&arc.normal()?));
382                        out.push(q.sub(&center).try_norm()?.sub(arc.radius()?));
383                        return Ok(());
384                    }
385                    (CurveKind3d::Line { .. }, Some(&t)) => {
386                        let (s, e) = line_of(sketch, layout, x, curve);
387                        s.add(&e.sub(&s).prod_scalar(x[t]))
388                    }
389                    (_, Some(&t)) => {
390                        // The curve at the parameter `t`, by the chain
391                        // rule: its point, moving along its tangent as
392                        // `t` moves.
393                        let curve = p.reference.expect("prepared");
394                        let (at, tangent) = (curve.evaluate(x[t].v)?, curve.tangent(x[t].v)?);
395                        Vector3::from_array([0, 1, 2].map(|i| x[t].chain(at[i], tangent[i])))
396                    }
397                    (k, None) => unreachable!("a point on {k:?} has a parameter"),
398                };
399                out.extend(q.sub(&on).to_array());
400            }
401            Tangent { a, b } => {
402                let (ea, eb) = sketch.shared_end(a, b)?.expect("validated");
403                let ta = tangent_of(sketch, layout, x, a, ea)?;
404                let tb = tangent_of(sketch, layout, x, b, eb)?;
405                parallel(&ta, &tb, out);
406            }
407        }
408        Ok(())
409    }
410
411    /// How the sketch stands at the variables `params` — from the
412    /// enclosure of the groups a solve that ended there solved, if given.
413    fn report(
414        &self,
415        params: Vec<Scalar>,
416        phases: Vec<Phase>,
417        solved: Option<system::Enclosure<Scalar>>,
418    ) -> GeopResult<Solve3dReport> {
419        let variables = Spatial::new(self, params, Vec::new());
420        let Standing {
421            held,
422            dof,
423            free: free_vars,
424            unproven,
425        } = variables.standing(&variables.start(), solved)?;
426        let failed_constraints = (held.failed.iter())
427            .map(|&i| self.constraints[i].id)
428            .collect::<Vec<_>>();
429        let points = &self.layout.points;
430        let free_points = (points.point_var.keys())
431            .map(|&p| (p, points.point(&free_vars, p).contains(&true)))
432            .collect();
433        Ok(Solve3dReport {
434            converged: failed_constraints.is_empty(),
435            max_residual: held.max_residual,
436            iterations: phases.iter().map(|p| p.iterations).sum(),
437            phases,
438            dof,
439            free_points,
440            failed_constraints,
441            unproven,
442        })
443    }
444}
445
446impl Residuals<MAX_LOCAL_VARS> for Problem<'_> {
447    fn offsets(&self) -> Vec<Option<usize>> {
448        self.layout.points.offsets()
449    }
450
451    fn constraints(&self) -> usize {
452        self.constraints.len()
453    }
454
455    fn reads(&self, c: usize) -> &[usize] {
456        &self.constraints[c].vars
457    }
458
459    fn residuals(&self, c: usize, values: &[Dual], out: &mut Vec<Dual>) -> GeopResult<()> {
460        let prepared = &self.constraints[c];
461        // Only the variables it depends on are read: the rest are zero.
462        let mut x = vec![Dual::cst(Scalar::ZERO); self.layout.points.len()];
463        for (&i, &v) in prepared.vars.iter().zip(values) {
464            x[i] = v;
465        }
466        Problem::residuals(self, prepared, &x, out)
467    }
468
469    fn name(&self, c: usize) -> String {
470        let c = &self.constraints[c];
471        format!("{} {:?}", c.id, c.constraint)
472    }
473
474    fn scale(&self) -> Scalar {
475        self.scale
476    }
477}
478
479/// The 3-D sketch's system (see [`Variables`]).
480type Spatial<'p, 'a> = Variables<'p, Problem<'a>, MAX_LOCAL_VARS>;
481
482/// The outcome of solving a 3-D sketch.
483#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
484pub struct Solve3dReport {
485    /// Every constraint holds, to
486    /// [`geop_core_math::solvers::system::RELATIVE_TOLERANCE`] of the
487    /// sketch's size.
488    pub converged: bool,
489    /// The largest remaining residual, a length: an upper bound.
490    #[serde(with = "as_f64")]
491    pub max_residual: Scalar,
492    pub iterations: usize,
493    pub phases: Vec<Phase>,
494    /// Degrees of freedom left.
495    pub dof: usize,
496    /// Per point: whether it can still move.
497    pub free_points: BTreeMap<PointId, bool>,
498    /// The constraints that do not hold.
499    pub failed_constraints: Vec<ConstraintId>,
500    /// Why no true solution near where the points are is proven, if none
501    /// is: the constraints that conflict there, or what is singular (see
502    /// [`Sketch3d::enclose`]).
503    #[serde(default)]
504    pub unproven: Vec<String>,
505}
506
507impl Sketch3d {
508    /// Moves the points so every constraint holds, as little as they allow.
509    /// The sketch is moved even if they cannot all hold, as near as they
510    /// come — the report says which do not.
511    pub fn solve(&mut self) -> GeopResult<Solve3dReport> {
512        self.solve_with_drag(&[])
513    }
514
515    /// Like [`Sketch3d::solve`], pulling each `(point, target)` towards its
516    /// target as far as the constraints allow: dragging.
517    pub fn solve_with_drag(&mut self, drags: &[(PointId, Vector3)]) -> GeopResult<Solve3dReport> {
518        self.seat_helix_ends()?;
519        let mut problem = Problem::new(self)?;
520        // A helix's end follows from the rest of it, and gives way to
521        // nothing: as a row among the others, a drag of the top of its axis
522        // moved its start and the bottom of its axis to share the move.
523        let helices = (problem.constraints.iter())
524            .any(|c| matches!(c.constraint, Constraint3d::HelixEnd { .. }));
525        (problem.constraints).retain(|c| !matches!(c.constraint, Constraint3d::HelixEnd { .. }));
526        let (_, solved, x) = Spatial::dragged(
527            &problem,
528            problem.start.clone(),
529            &problem.layout.points.point_var,
530            drags,
531        )?;
532        let report = problem.report(x.clone(), solved.phases.clone(), Some(solved.enclosure))?;
533        // Where the solve put the points is the sketch's state, and a free
534        // choice: sharp.
535        let x: Vec<Scalar> = x.into_iter().map(Scalar::sharpen).collect();
536        problem.layout.points.write(&mut self.points, &x);
537        if !helices {
538            return Ok(report);
539        }
540        self.seat_helix_ends()?;
541        let whole = Problem::new(self)?;
542        whole.report(whole.start.clone(), solved.phases, None)
543    }
544
545    /// Puts each helix's end where the rest of it says: a helix changed is
546    /// not a violation for its start and axis to give way to. Where a point
547    /// starts from is a free choice.
548    fn seat_helix_ends(&mut self) -> GeopResult<()> {
549        let helices: Vec<_> = (self.curves.values())
550            .filter_map(|curve| match curve.kind {
551                CurveKind3d::Helix {
552                    start,
553                    base,
554                    top,
555                    end,
556                    turns,
557                    left,
558                } => Some((start, base, top, end, turns, left)),
559                _ => None,
560            })
561            .collect();
562        for (start, base, top, end, turns, left) in helices {
563            let at = (Helix3 {
564                start: self.point(start)?.at,
565                base: self.point(base)?.at,
566                top: self.point(top)?.at,
567                turns,
568                left,
569            })
570            .end()?;
571            if let Some(end) = self.points.get_mut(&end)
572                && !end.fixed
573            {
574                end.at = at.sharpen();
575            }
576        }
577        Ok(())
578    }
579
580    /// Which constraints hold where the points are now, and what can still
581    /// move — without moving anything.
582    pub fn check(&self) -> GeopResult<Solve3dReport> {
583        let problem = Problem::new(self)?;
584        problem.report(problem.start.clone(), Vec::new(), None)
585    }
586
587    /// The sketch's geometry as the kernel builds on it: an enclosure of
588    /// the exact solution of its constraints near the solved points, every
589    /// coordinate they leave free exactly as drawn (see
590    /// [`geop_core_math::solvers::system::System::enclose`]). A sketch whose constraints do not hold is built
591    /// as drawn. Fails where the solution cannot be proven.
592    pub fn enclose(&self) -> GeopResult<Enclosure3d> {
593        let ctx = |e: GeopError| e.with_context("enclosing the 3-D sketch's solution");
594        let problem = Problem::new(self).map_err(ctx)?;
595        let variables = Spatial::new(&problem, problem.start.clone(), Vec::new());
596        let start = variables.start();
597        if !variables.system().held(&start)?.converged {
598            return Ok(Enclosure3d::as_drawn(self));
599        }
600        // What is fixed is as given; the rest as enclosed.
601        let x = variables.enclosed(&start).map_err(ctx)?;
602        let points = &problem.layout.points;
603        Ok(Enclosure3d {
604            points: (points.point_var.keys())
605                .map(|&p| (p, Vector3::from_array(points.point(&x, p))))
606                .collect(),
607        })
608    }
609}
geop_core_sketch/space/solve.rs

geop_core_sketch/space/
solve.rs
