bherbruckandClaude Opus 5.5
296683229d
* WIP deep rename (not for push) * WIP2 * WIP3 * WIP4 wxs fix * rustfmt * Rename the remaining identifiers in merged crash-report code Co-Authored-By: Claude Opus 5.5 <[email protected]> Claude-Session: https://claude.ai/code/session_01RjGw4gA5ZvKrU8MQP6sBgT --------- Co-authored-by: Claude Opus 5.5 <[email protected]>
1054 lines
47 KiB
Rust
1054 lines
47 KiB
Rust
//! Cluster decomposition: places points that are rigid relative to known geometry but that no
|
||
//! point-by-point construction reaches.
|
||
//!
|
||
//! Owen (1991, §3) splits a well-constrained constraint graph at articulation pairs into
|
||
//! triangles whose sides are original dimensions or *virtual bonds*, solves the triangles with
|
||
//! square roots and reassembles the pieces (§3.3.2). Fudos and Hoffmann (1997) run the same
|
||
//! reduction bottom-up, and that is the form implemented here (Hoffmann 2005 gives the
|
||
//! summary):
|
||
//!
|
||
//! - a **cluster** is a set of points whose relative positions are known, stored in the
|
||
//! cluster's own frame. Every dimension between two points starts as a two-point cluster;
|
||
//! the known geometry is the *ground* cluster, in world coordinates;
|
||
//! - three clusters that pairwise share one point know the three distances between the
|
||
//! shared points (Owen's virtual bonds). The triangle is constructed in one cluster's frame
|
||
//! (circle ∩ circle) and the other two are moved onto it rigidly: the three become one;
|
||
//! - a rigid cluster sharing two points with another is moved onto it;
|
||
//! - the ground absorbs everything it reaches, so placing the root cluster is the rigid
|
||
//! transform onto the shared known points.
|
||
//!
|
||
//! Horizontal/vertical relations and DistanceX/Y are not invariant under rotation. Two of them
|
||
//! between the same pair of points (or one with a distance) fix the pair's offset vector: an
|
||
//! *oriented* cluster, known up to a translation. Two oriented clusters (the ground counts as
|
||
//! one) sharing a point merge by translation. A rigid cluster holding both ends of a single
|
||
//! horizontal/vertical relation turns until it holds and becomes oriented; one pinned to an
|
||
//! oriented cluster at a single point turns about the pin until a relation between the two
|
||
//! holds (circle ∩ axis, the hinge step).
|
||
//!
|
||
//! Branches follow Owen's rule (§2.4.3, §5): every constructed triangle keeps the orientation
|
||
//! (clockwise or counter-clockwise) its three points have in `reference`, the geometry before
|
||
//! the solve (or before a drag). A cluster is never reflected when it is moved.
|
||
//!
|
||
//! The relations come from [`crate::loci`]'s reading, as for propagation: those between two
|
||
//! points build the starting clusters, and a point that is not in a cluster but whose
|
||
//! relations and ties to the cluster's points (a point on a line through two of them, a
|
||
//! midpoint, a mirrored point, a tangent circle's centre, a direction derived from a line of
|
||
//! the cluster…) cross at one place joins it (Owen's single-point merge), placed by the same
|
||
//! [`crate::loci::place`] as propagation, in the cluster's frame. Relations fixed in the
|
||
//! sketch's frame (horizontal, vertical, line angles) count only in a ground or oriented
|
||
//! cluster.
|
||
//!
|
||
//! A rigid cluster that never reaches the ground (no known point, or a single one it can still
|
||
//! turn about) has no pose the constraints decide. Once nothing more is placed,
|
||
//! [`Decomposition::floats`] places such a cluster where its reference pose puts it, if that
|
||
//! moves it at all (its pin moved, or one of its relations misses at the reference): free, the
|
||
//! rotation and translation that fit its points to their reference positions best in the
|
||
//! least-squares sense; pinned, its reference pose about the pin moved onto the pin (a large
|
||
//! move of the pin carries it along unturned); an oriented cluster by translation only. It is
|
||
//! never reflected. The pose is a guess at free motion: the caller seeds the solver with it but
|
||
//! builds nothing on it, and the solver keeps every degree of freedom.
|
||
//!
|
||
//! What remains unplaced is left to the numerical solver: triconnected remainders, which no
|
||
//! sequence of square roots solves (Owen §3.3.1 step 4), and anything held by constraints this
|
||
//! module does not read. The output only seeds the solver; it verifies every constraint and
|
||
//! its rank analysis decides the degrees of freedom. Every intersection goes through [`crate::loci`]: a near-tangent one pins nothing
|
||
//! down well and a branch the reference does not decide is unknown, so neither is merged
|
||
//! (the solver starts those points where they are).
|
||
|
||
use std::collections::btree_map::Entry;
|
||
use std::collections::{BTreeMap, BTreeSet, VecDeque};
|
||
|
||
use solidcraft_geom::Vec2;
|
||
|
||
use crate::loci::{Locus, Reading, Rel, Span, View, crossing, mag, place, side};
|
||
use crate::union_find::{find, union};
|
||
|
||
/// More starting clusters than this (one per dimensioned point pair) and the sketch is left
|
||
/// to the numerical solver unchanged.
|
||
const MAX_CLUSTERS: usize = 20_000;
|
||
/// Bound on the membership scans of the merge search, so a hostile sketch can't stall it
|
||
/// (over the life of one graph, whatever is added to it): about 0.3 s.
|
||
pub(crate) const MAX_WORK: usize = 20_000_000;
|
||
/// The bound for all the solves of one live drag frame together: about 16 ms. What a solve's
|
||
/// decomposition leaves unplaced once it is spent starts where it is, for LM, so that solve keeps
|
||
/// its branch only by continuation: a drag that runs out takes short steps for the rest of the
|
||
/// frame (see [`Budget::exhausted`]).
|
||
pub(crate) const FRAME_WORK: usize = 1_000_000;
|
||
|
||
/// Work the seed's cluster decomposition may do, shared by the solves it is passed to (the held
|
||
/// solves of one drag frame): each decomposition is bounded by what is left, and charges what it
|
||
/// did.
|
||
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
|
||
pub struct Budget {
|
||
left: usize,
|
||
spent: usize,
|
||
cut: bool,
|
||
}
|
||
|
||
impl Budget {
|
||
/// A budget of `work` units (a unit is one membership scan of the merge search).
|
||
pub fn new(work: usize) -> Budget {
|
||
Budget { left: work, spent: 0, cut: false }
|
||
}
|
||
/// The budget of one solve: about 0.3 s, so a hostile sketch can't stall it.
|
||
pub fn full() -> Budget {
|
||
Budget::new(MAX_WORK)
|
||
}
|
||
/// The budget of one live drag frame: about 16 ms.
|
||
pub fn frame() -> Budget {
|
||
Budget::new(FRAME_WORK)
|
||
}
|
||
/// Some decomposition stopped because the budget ran out: what it would have placed was
|
||
/// left to LM, from where it was.
|
||
pub fn exhausted(&self) -> bool {
|
||
self.cut
|
||
}
|
||
/// The work charged so far (it may pass the budget by one scan).
|
||
pub fn spent(&self) -> usize {
|
||
self.spent
|
||
}
|
||
pub(crate) fn left(&self) -> usize {
|
||
self.left
|
||
}
|
||
/// Charge a decomposition's `work`; `cut` when it stopped at the budget.
|
||
pub(crate) fn charge(&mut self, work: usize, cut: bool) {
|
||
self.left = self.left.saturating_sub(work);
|
||
self.spent = self.spent.saturating_add(work);
|
||
self.cut |= cut;
|
||
}
|
||
}
|
||
/// Shorter than this (mm), a pair of points gives no direction to align on.
|
||
const DEGENERATE: f64 = 1e-9;
|
||
|
||
/// The constraint graph of a sketch reduced by cluster merging, relative to the known points.
|
||
/// More points can become known later (placed by propagation); the graph is extended and
|
||
/// reduced further rather than rebuilt.
|
||
pub(crate) struct Decomposition<'a> {
|
||
/// `None` when nothing can be placed or the sketch is too large.
|
||
graph: Option<Graph<'a>>,
|
||
}
|
||
|
||
impl<'a> Decomposition<'a> {
|
||
/// Reduce the graph of the reading's relations relative to the known points (there may be
|
||
/// none), unless it can place nothing (see [`may_place`]): then no graph is built.
|
||
///
|
||
/// `known[i]` is the position of point `i` when it is already fixed (fixed, held or placed
|
||
/// by propagation; a missing or non-finite entry is unknown). The reading's reference
|
||
/// layout picks the branch of every construction (where it has no position for a point,
|
||
/// every branch it would decide is left to the solver). `edits`: the solve applies edits
|
||
/// (no drag step): a floating part whose relations miss at the reference may float.
|
||
///
|
||
/// `limit` bounds the merge search's work over the decomposition's life (what is left of
|
||
/// the solve's [`Budget`]).
|
||
pub(crate) fn new(reading: &'a Reading, known: &[Option<Vec2>], edits: bool, limit: usize) -> Self {
|
||
let known_at = |i: usize| known.get(i).copied().flatten().filter(|p| p.is_finite());
|
||
if !may_place(reading, &known_at, edits) {
|
||
return Decomposition { graph: None };
|
||
}
|
||
let mut graph = Graph::build(reading, &known_at, limit);
|
||
if let Some(g) = &mut graph {
|
||
g.edits = edits;
|
||
g.reduce();
|
||
}
|
||
Decomposition { graph }
|
||
}
|
||
|
||
/// A graph was built: the decomposition may place points.
|
||
pub(crate) fn built(&self) -> bool {
|
||
self.graph.is_some()
|
||
}
|
||
|
||
/// The points are now known at their positions: merge what that lets merge.
|
||
pub(crate) fn know(&mut self, points: &[(usize, Vec2)]) {
|
||
let Some(g) = &mut self.graph else { return };
|
||
for (i, at) in points.iter().filter(|(_, at)| at.is_finite()) {
|
||
g.know(*i, *at);
|
||
}
|
||
g.reduce();
|
||
}
|
||
|
||
/// Where the ground cluster has point `i` (known points included, at their known
|
||
/// positions); None when the decomposition did not reach it or built no graph.
|
||
pub(crate) fn pos(&mut self, i: usize) -> Option<Vec2> {
|
||
let g = self.graph.as_mut()?;
|
||
let node = g.node(i);
|
||
g.cluster(g.ground?)?.pts.get(&node).copied().filter(|p| p.is_finite())
|
||
}
|
||
|
||
/// The work the merge search did, and whether it stopped at its limit.
|
||
pub(crate) fn work(&self) -> (usize, bool) {
|
||
self.graph.as_ref().map_or((0, false), |g| (g.work, g.work > g.limit))
|
||
}
|
||
|
||
/// The merge search's budget is spent: nothing more will merge.
|
||
pub(crate) fn exhausted(&self) -> bool {
|
||
self.graph.as_ref().is_none_or(|g| g.work > g.limit)
|
||
}
|
||
|
||
/// The rigid clusters that are free or pinned at a single anchored point, each placed by its
|
||
/// reference pose (see the module doc) as its points but the pin with their positions;
|
||
/// pinned ones first, larger ones first. Only those the pose moves: pinned at a point that
|
||
/// moved from its reference, or with a relation among its points that misses at the
|
||
/// reference. A cluster holding a point of a pair whose readings disagree is left to the
|
||
/// solver. `anchor(i)`: where the seed placed point `i`, if it builds on it (pins come only
|
||
/// from there).
|
||
pub(crate) fn floats(&mut self, anchor: &dyn Fn(usize) -> Option<Vec2>) -> Vec<Vec<(usize, Vec2)>> {
|
||
self.graph.as_mut().map_or_else(Vec::new, |g| g.floats(anchor))
|
||
}
|
||
}
|
||
|
||
/// Can a decomposition place anything? A point it places is rigid relative to known points or
|
||
/// to the other points of a cluster of three or more (or of an oriented pair), so it has two
|
||
/// relations (a coincidence counts twice, a tie relates each of its points to each other one)
|
||
/// to known points and to points that are themselves rigid. Peeling unknown points with fewer,
|
||
/// until none is left, keeps every point a decomposition could place; it places something
|
||
/// only if one is kept next to a known point, or (`edits`) a kept point misses a relation or
|
||
/// tie at the reference, so a floating cluster may float. A drag step moves only held points:
|
||
/// a floating part it does not reach is left as it is. Linear in the relations, so it is cheap
|
||
/// next to building the graph; this is where most drag frames of an under-constrained sketch
|
||
/// stop (chains of dimensioned links peel away entirely).
|
||
fn may_place(reading: &Reading, known: &dyn Fn(usize) -> Option<Vec2>, edits: bool) -> bool {
|
||
let n = reading.points();
|
||
let weight = |rel: Rel| if rel.joins() { 2 } else { 1 };
|
||
let mut adj: Vec<Vec<(usize, usize)>> = vec![Vec::new(); n];
|
||
let mut degree = vec![0usize; n];
|
||
let ties = reading.tie_point_lists().flat_map(|pts| pts.iter().flat_map(move |p| pts.iter().filter(move |q| *q > p).map(move |q| (*p, *q, 1))));
|
||
for (p, q, w) in reading.rels.iter().map(|&(p, q, rel)| (p, q, weight(rel))).chain(ties) {
|
||
for (a, b) in [(p, q), (q, p)] {
|
||
if let Some(list) = adj.get_mut(a) {
|
||
list.push((b, w));
|
||
}
|
||
if let Some(d) = degree.get_mut(a) {
|
||
*d += w;
|
||
}
|
||
}
|
||
}
|
||
let mut alive: Vec<bool> = (0..n).map(|i| known(i).is_none()).collect();
|
||
let mut peel: Vec<usize> = (0..n).filter(|i| alive.get(*i).copied().unwrap_or(false) && degree.get(*i).is_some_and(|d| *d < 2)).collect();
|
||
while let Some(i) = peel.pop() {
|
||
if let Some(a) = alive.get_mut(i) {
|
||
*a = false;
|
||
}
|
||
for &(o, w) in adj.get(i).into_iter().flatten() {
|
||
if alive.get(o).copied().unwrap_or(false)
|
||
&& let Some(d) = degree.get_mut(o)
|
||
{
|
||
let was = *d;
|
||
*d = d.saturating_sub(w);
|
||
if was >= 2 && *d < 2 {
|
||
peel.push(o);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
let kept = |i: usize| alive.get(i).copied().unwrap_or(false);
|
||
let next_to_known = |i: usize| adj.get(i).into_iter().flatten().any(|(o, _)| known(*o).is_some());
|
||
let before = |i: usize| reading.before(i);
|
||
let view = View { at: &before, oriented: true };
|
||
let misses = |i: usize| before(i).is_none_or(|at| !reading.holds(i, at, &view));
|
||
(0..n).any(|i| kept(i) && (next_to_known(i) || (edits && misses(i))))
|
||
}
|
||
|
||
#[cfg(test)]
|
||
thread_local! {
|
||
static BUILDS: std::cell::Cell<usize> = const { std::cell::Cell::new(0) };
|
||
static LAST_WORK: std::cell::Cell<usize> = const { std::cell::Cell::new(0) };
|
||
}
|
||
|
||
/// Graphs built on this thread so far (tests count rebuilds with it).
|
||
#[cfg(test)]
|
||
pub(crate) fn builds() -> usize {
|
||
BUILDS.with(std::cell::Cell::get)
|
||
}
|
||
|
||
/// The work charged to the last decomposition dropped on this thread (0 when it built no
|
||
/// graph).
|
||
#[cfg(test)]
|
||
pub(crate) fn last_work() -> usize {
|
||
LAST_WORK.with(std::cell::Cell::get)
|
||
}
|
||
|
||
#[cfg(test)]
|
||
impl Drop for Decomposition<'_> {
|
||
fn drop(&mut self) {
|
||
let work = self.graph.as_ref().map_or(0, |g| g.work);
|
||
LAST_WORK.with(|w| w.set(work));
|
||
}
|
||
}
|
||
|
||
/// How much of a cluster's placement is still free, least free first: a merge keeps the
|
||
/// frame of its least free cluster.
|
||
#[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord)]
|
||
enum Frame {
|
||
/// World coordinates.
|
||
Ground,
|
||
/// World coordinates up to a translation.
|
||
Oriented,
|
||
/// World coordinates up to a rotation and a translation.
|
||
Rigid,
|
||
}
|
||
|
||
#[derive(Clone, Debug)]
|
||
struct Cluster {
|
||
frame: Frame,
|
||
/// Node → position in the cluster's frame.
|
||
pts: BTreeMap<usize, Vec2>,
|
||
/// Node → the point the cluster's constraints are written on there. Coincident points
|
||
/// may be apart in the reference; branches are picked from this point's side.
|
||
via: BTreeMap<usize, usize>,
|
||
/// Nodes whose neighbours [`Graph::grow`] has not tried yet.
|
||
fresh: Vec<usize>,
|
||
}
|
||
|
||
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
|
||
enum Axis {
|
||
X,
|
||
Y,
|
||
}
|
||
|
||
impl Axis {
|
||
/// (the coordinate along the axis, the other one).
|
||
fn split(self, v: Vec2) -> (f64, f64) {
|
||
match self {
|
||
Axis::X => (v.x, v.y),
|
||
Axis::Y => (v.y, v.x),
|
||
}
|
||
}
|
||
fn join(self, along: f64, other: f64) -> Vec2 {
|
||
match self {
|
||
Axis::X => Vec2::new(along, other),
|
||
Axis::Y => Vec2::new(other, along),
|
||
}
|
||
}
|
||
}
|
||
|
||
/// One horizontal/vertical relation alone between two nodes: `axis(hi) − axis(lo) = delta`.
|
||
/// `ends` are the points the constraint is written on (on the `lo` and `hi` side), whose
|
||
/// reference positions pick branches: coincident points may be apart in the reference.
|
||
#[derive(Clone, Copy, Debug)]
|
||
struct AxisBond {
|
||
lo: usize,
|
||
hi: usize,
|
||
ends: (usize, usize),
|
||
axis: Axis,
|
||
delta: f64,
|
||
}
|
||
|
||
/// The constraints this module reads between one pair of nodes, signed from `lo` to `hi`;
|
||
/// `ends` as in [`AxisBond`] (the lowest pair of points written on, so that nothing depends
|
||
/// on the order of the constraints).
|
||
#[derive(Clone, Copy, Debug)]
|
||
struct Bundle {
|
||
ends: (usize, usize),
|
||
dist: Option<Span>,
|
||
dx: Option<Span>,
|
||
dy: Option<Span>,
|
||
}
|
||
|
||
impl Bundle {
|
||
/// The agreed (distance, dx, dy); None when readings of one kind disagree, or a distance
|
||
/// disagrees with both offsets.
|
||
fn agreed(&self) -> Option<(Option<f64>, Option<f64>, Option<f64>)> {
|
||
let one = |s: Option<Span>| s.map_or(Some(None), |s| s.agreed().map(Some));
|
||
let (dist, dx, dy) = (one(self.dist)?, one(self.dx)?, one(self.dy)?);
|
||
if let (Some(len), Some(x), Some(y)) = (dist, dx, dy)
|
||
&& !Rel::Dist(len).holds(Vec2::ZERO, Vec2::new(x, y), 0.0)
|
||
{
|
||
return None;
|
||
}
|
||
Some((dist, dx, dy))
|
||
}
|
||
}
|
||
|
||
/// Rotation then translation taking `from` to `to`; never a reflection.
|
||
#[derive(Clone, Copy, Debug)]
|
||
struct Motion {
|
||
/// (cos, sin) of the rotation.
|
||
rot: Vec2,
|
||
from: Vec2,
|
||
to: Vec2,
|
||
}
|
||
|
||
impl Motion {
|
||
const IDENTITY: Motion = Motion { rot: Vec2::X, from: Vec2::ZERO, to: Vec2::ZERO };
|
||
|
||
fn translate(from: Vec2, to: Vec2) -> Self {
|
||
Motion { rot: Vec2::X, from, to }
|
||
}
|
||
|
||
/// The motion taking `ua` to `wa` and the direction `ua→ub` onto `wa→wb`.
|
||
fn onto(ua: Vec2, ub: Vec2, wa: Vec2, wb: Vec2) -> Option<Self> {
|
||
let (u, w) = (ub - ua, wb - wa);
|
||
if !(u.len() > DEGENERATE && w.len() > DEGENERATE) {
|
||
return None;
|
||
}
|
||
let rot = Vec2::new(u.dot(w), u.cross(w)).normalized()?;
|
||
Some(Motion { rot, from: ua, to: wa })
|
||
}
|
||
|
||
fn apply(&self, p: Vec2) -> Vec2 {
|
||
let d = p - self.from;
|
||
self.to + Vec2::new(self.rot.x * d.x - self.rot.y * d.y, self.rot.y * d.x + self.rot.x * d.y)
|
||
}
|
||
}
|
||
|
||
/// (cos, sin) of the rotation turning the vectors `u` onto their `w` best in the least-squares
|
||
/// sense (never a reflection); None when it is undecided (they are all zero, or the fit is as
|
||
/// good at every angle).
|
||
fn best_turn(pairs: impl Iterator<Item = (Vec2, Vec2)>) -> Option<Vec2> {
|
||
let (mut sum, mut scale) = (Vec2::ZERO, 0.0);
|
||
for (u, w) in pairs {
|
||
sum += Vec2::new(u.dot(w), u.cross(w));
|
||
scale += u.len() * w.len();
|
||
}
|
||
if !(sum.len() > 1e-9 * scale) {
|
||
return None;
|
||
}
|
||
sum.normalized()
|
||
}
|
||
|
||
/// A merge: the clusters taking part, each with the motion into the merged frame (the first
|
||
/// one's positions win where they overlap), and the merged frame.
|
||
struct Merge {
|
||
frame: Frame,
|
||
parts: Vec<(usize, Motion)>,
|
||
}
|
||
|
||
/// The point at distance `r` from `centre` whose `axis` coordinate is `along` past
|
||
/// `centre`'s (circle ∩ axis line), on the side of the axis through `ref_centre` that
|
||
/// `ref_point` is on.
|
||
fn on_axis(axis: Axis, centre: Vec2, r: f64, along: f64, ref_centre: Option<Vec2>, ref_point: Option<Vec2>) -> Option<Vec2> {
|
||
let loci = |c: Vec2| (Locus::Circle { c, r }, Locus::Line { o: c + axis.join(along, 0.0), d: axis.join(0.0, 1.0) });
|
||
crossing(loci(centre), ref_centre.map(loci), ref_point)
|
||
}
|
||
|
||
/// The constraint graph reduced by cluster merging.
|
||
struct Graph<'a> {
|
||
/// What the constraints say, and the reference layout (per point, not per node).
|
||
reading: &'a Reading,
|
||
/// Point → representative node (coincident points share one; final once built).
|
||
node_of: Vec<usize>,
|
||
/// Node → its points.
|
||
points_of: Vec<Vec<usize>>,
|
||
/// `None` once merged into another cluster; a merge keeps its base cluster's slot.
|
||
clusters: Vec<Option<Cluster>>,
|
||
/// Per cluster: waiting in `queue`.
|
||
queued: Vec<bool>,
|
||
/// Per node: in a pair whose readings disagree (left to the solver, never floated).
|
||
conflicted: Vec<bool>,
|
||
/// The solve applies edits (see [`Decomposition::new`]): a cluster whose relations miss at
|
||
/// the reference floats; otherwise only one whose pin moved.
|
||
edits: bool,
|
||
/// Node → clusters containing it. Merged-away clusters are dropped lazily (they stay
|
||
/// `None` for good), so a merge touches only the nodes that change cluster.
|
||
member: Vec<Vec<usize>>,
|
||
bonds: Vec<AxisBond>,
|
||
/// Node → axis bonds touching it.
|
||
bonds_of: Vec<Vec<usize>>,
|
||
queue: VecDeque<usize>,
|
||
/// Every scan of the merge search is charged here; past `limit` the search stops.
|
||
work: usize,
|
||
/// The work budget.
|
||
limit: usize,
|
||
/// The ground cluster's slot (a merge never moves it: the ground is always the base);
|
||
/// always set once built, possibly empty.
|
||
ground: Option<usize>,
|
||
}
|
||
|
||
impl<'a> Graph<'a> {
|
||
fn build(reading: &'a Reading, known: &dyn Fn(usize) -> Option<Vec2>, limit: usize) -> Option<Self> {
|
||
#[cfg(test)]
|
||
BUILDS.with(|b| b.set(b.get() + 1));
|
||
let n = reading.points();
|
||
let rels = &reading.rels;
|
||
// Coincident points, and points at distance zero, are one node: the lowest point of
|
||
// its group.
|
||
let mut parent: Vec<usize> = (0..n).collect();
|
||
for &(p, q, rel) in rels {
|
||
if rel.joins() {
|
||
let (a, b) = (find(&mut parent, p), find(&mut parent, q));
|
||
union(&mut parent, a.max(b), a.min(b));
|
||
}
|
||
}
|
||
let node_of: Vec<usize> = (0..n).map(|i| find(&mut parent, i)).collect();
|
||
let mut points_of = vec![Vec::new(); n];
|
||
for (i, node) in node_of.iter().enumerate() {
|
||
if let Some(list) = points_of.get_mut(*node) {
|
||
list.push(i);
|
||
}
|
||
}
|
||
let mut g = Graph {
|
||
reading,
|
||
node_of,
|
||
points_of,
|
||
clusters: Vec::new(),
|
||
queued: Vec::new(),
|
||
conflicted: vec![false; n],
|
||
edits: false,
|
||
member: vec![Vec::new(); n],
|
||
bonds: Vec::new(),
|
||
bonds_of: vec![Vec::new(); n],
|
||
queue: VecDeque::new(),
|
||
work: 0,
|
||
limit,
|
||
ground: None,
|
||
};
|
||
let mut ground = Cluster { frame: Frame::Ground, pts: BTreeMap::new(), via: BTreeMap::new(), fresh: Vec::new() };
|
||
for i in 0..n {
|
||
if let Some(p) = known(i) {
|
||
let node = g.node(i);
|
||
if let Entry::Vacant(slot) = ground.pts.entry(node) {
|
||
slot.insert(p);
|
||
ground.via.insert(node, i);
|
||
}
|
||
}
|
||
}
|
||
|
||
let mut bundles: BTreeMap<(usize, usize), Bundle> = BTreeMap::new();
|
||
for &(p, q, rel) in rels {
|
||
let (axis, value) = match rel {
|
||
Rel::Same => continue,
|
||
Rel::Dist(v) => (None, v),
|
||
Rel::SameX => (Some(Axis::X), 0.0),
|
||
Rel::SameY => (Some(Axis::Y), 0.0),
|
||
Rel::OffsetX(v) => (Some(Axis::X), v),
|
||
Rel::OffsetY(v) => (Some(Axis::Y), v),
|
||
};
|
||
let (a, b) = (g.node(p), g.node(q));
|
||
if a == b {
|
||
continue;
|
||
}
|
||
let ((lo, lp), (hi, hp)) = if a < b { ((a, p), (b, q)) } else { ((b, q), (a, p)) };
|
||
// Signed offsets take the side the constrained points are on before the solve; a
|
||
// nonzero offset whose side is unknown is left to the solver.
|
||
let signed = |axis: Axis| {
|
||
if value == 0.0 {
|
||
return Some(0.0);
|
||
}
|
||
let (from, to) = (g.at(lp)?, g.at(hp)?);
|
||
let (c, _) = axis.split(to - from);
|
||
side(c, value, mag(from).max(mag(to))).map(|s| s * value)
|
||
};
|
||
let Some(v) = axis.map_or(Some(value), signed) else { continue };
|
||
let bundle = bundles.entry((lo, hi)).or_insert(Bundle { ends: (lp, hp), dist: None, dx: None, dy: None });
|
||
bundle.ends = bundle.ends.min((lp, hp));
|
||
Span::add(
|
||
match axis {
|
||
None => &mut bundle.dist,
|
||
Some(Axis::X) => &mut bundle.dx,
|
||
Some(Axis::Y) => &mut bundle.dy,
|
||
},
|
||
v,
|
||
);
|
||
}
|
||
|
||
let mut pairs = Vec::new();
|
||
for ((lo, hi), bundle) in bundles {
|
||
// Readings that disagree are left to the solver, which reports the conflict.
|
||
let Some((dist, dx, dy)) = bundle.agreed() else {
|
||
for node in [lo, hi] {
|
||
if let Some(c) = g.conflicted.get_mut(node) {
|
||
*c = true;
|
||
}
|
||
}
|
||
continue;
|
||
};
|
||
let ends = bundle.ends;
|
||
if ground.pts.contains_key(&lo) && ground.pts.contains_key(&hi) {
|
||
continue;
|
||
}
|
||
let dir = g.at(ends.1).zip(g.at(ends.0)).and_then(|(b, a)| (b - a).normalized());
|
||
let offset = match (dx, dy, dist) {
|
||
(Some(x), Some(y), _) => Some((Vec2::new(x, y), Frame::Oriented)),
|
||
(Some(x), None, Some(len)) => on_axis(Axis::X, Vec2::ZERO, len, x, g.at(ends.0), g.at(ends.1)).map(|v| (v, Frame::Oriented)),
|
||
(None, Some(y), Some(len)) => on_axis(Axis::Y, Vec2::ZERO, len, y, g.at(ends.0), g.at(ends.1)).map(|v| (v, Frame::Oriented)),
|
||
(None, None, Some(len)) => Some((dir.unwrap_or(Vec2::X) * len, Frame::Rigid)),
|
||
(Some(delta), None, None) => {
|
||
g.bond(AxisBond { lo, hi, ends, axis: Axis::X, delta });
|
||
None
|
||
}
|
||
(None, Some(delta), None) => {
|
||
g.bond(AxisBond { lo, hi, ends, axis: Axis::Y, delta });
|
||
None
|
||
}
|
||
(None, None, None) => None,
|
||
};
|
||
if let Some((v, frame)) = offset {
|
||
let base = g.at(lo).unwrap_or_default();
|
||
let pts = BTreeMap::from([(lo, base), (hi, base + v)]);
|
||
pairs.push(Some(Cluster { frame, pts, via: BTreeMap::from([(lo, ends.0), (hi, ends.1)]), fresh: vec![lo, hi] }));
|
||
}
|
||
if pairs.len() > MAX_CLUSTERS {
|
||
return None;
|
||
}
|
||
}
|
||
// Kept even when nothing is known yet: floating clusters join it once placed.
|
||
g.ground = Some(g.clusters.len());
|
||
g.clusters.push(Some(ground));
|
||
g.clusters.extend(pairs);
|
||
for (id, c) in g.clusters.iter().enumerate() {
|
||
for node in c.iter().flat_map(|c| c.pts.keys()) {
|
||
if let Some(list) = g.member.get_mut(*node) {
|
||
list.push(id);
|
||
}
|
||
}
|
||
}
|
||
g.queued = vec![true; g.clusters.len()];
|
||
g.queue = (0..g.clusters.len()).collect();
|
||
Some(g)
|
||
}
|
||
|
||
/// Point `i` is known at `at`: add it to the ground and queue the other clusters holding it
|
||
/// (they find the ground through it).
|
||
fn know(&mut self, i: usize, at: Vec2) {
|
||
let node = self.node(i);
|
||
let Some(id) = self.ground else { return };
|
||
let Some(Some(ground)) = self.clusters.get_mut(id) else { return };
|
||
let Entry::Vacant(slot) = ground.pts.entry(node) else { return };
|
||
slot.insert(at);
|
||
ground.via.insert(node, i);
|
||
self.join(node, id);
|
||
}
|
||
|
||
/// `node` joined cluster `id`: queue the other clusters holding it (they meet `id` through
|
||
/// it).
|
||
fn join(&mut self, node: usize, id: usize) {
|
||
let Graph { member, queued, queue, .. } = self;
|
||
if let Some(list) = member.get_mut(node) {
|
||
for c in list.iter() {
|
||
enqueue(queued, queue, *c);
|
||
}
|
||
list.push(id);
|
||
}
|
||
}
|
||
|
||
/// Record an axis bond (it makes no cluster of its own).
|
||
fn bond(&mut self, b: AxisBond) {
|
||
let id = self.bonds.len();
|
||
self.bonds.push(b);
|
||
for e in [b.lo, b.hi] {
|
||
if let Some(list) = self.bonds_of.get_mut(e) {
|
||
list.push(id);
|
||
}
|
||
}
|
||
}
|
||
|
||
/// Representative node of point `i`.
|
||
fn node(&self, i: usize) -> usize {
|
||
self.node_of.get(i).copied().unwrap_or(i)
|
||
}
|
||
|
||
/// Reference position of a point (`None`: unknown).
|
||
fn at(&self, point: usize) -> Option<Vec2> {
|
||
self.reading.before(point)
|
||
}
|
||
|
||
/// Reference position of `node` as cluster `id` constrains it.
|
||
fn at_node(&self, id: usize, node: usize) -> Option<Vec2> {
|
||
self.at(self.cluster(id).and_then(|c| c.via.get(&node)).copied().unwrap_or(node))
|
||
}
|
||
|
||
fn cluster(&self, id: usize) -> Option<&Cluster> {
|
||
self.clusters.get(id).and_then(Option::as_ref)
|
||
}
|
||
|
||
fn pos(&self, id: usize, node: usize) -> Option<Vec2> {
|
||
self.cluster(id)?.pts.get(&node).copied()
|
||
}
|
||
|
||
fn frame(&self, id: usize) -> Frame {
|
||
self.cluster(id).map_or(Frame::Rigid, |c| c.frame)
|
||
}
|
||
|
||
/// Nodes `x` and `y` share.
|
||
fn common(&self, x: usize, y: usize) -> Vec<usize> {
|
||
let (Some(a), Some(b)) = (self.cluster(x), self.cluster(y)) else { return Vec::new() };
|
||
let (small, big) = if a.pts.len() <= b.pts.len() { (a, b) } else { (b, a) };
|
||
small.pts.keys().copied().filter(|n| big.pts.contains_key(n)).collect()
|
||
}
|
||
|
||
/// The cost of scanning a rigid cluster's nodes and their axis bonds (only rigid
|
||
/// clusters turn on bonds).
|
||
fn bond_load(&self, id: usize) -> usize {
|
||
self.cluster(id).filter(|c| c.frame == Frame::Rigid).map_or(0, |c| c.pts.keys().map(|n| 1 + self.bonds_of.get(*n).map_or(0, Vec::len)).sum())
|
||
}
|
||
|
||
/// Merge and grow clusters until none of the steps applies. A cluster is queued at most
|
||
/// once at a time, and only again after a step changed it or a cluster it shares a node
|
||
/// with; every pop and scan is charged to the work budget, which bounds the loop.
|
||
fn reduce(&mut self) {
|
||
while let Some(id) = self.queue.pop_front() {
|
||
if let Some(q) = self.queued.get_mut(id) {
|
||
*q = false;
|
||
}
|
||
self.work += 1;
|
||
if self.work > self.limit {
|
||
return;
|
||
}
|
||
if let Some(m) = self.find_merge(id) {
|
||
self.absorb(m);
|
||
} else if self.grow(id) {
|
||
enqueue(&mut self.queued, &mut self.queue, id);
|
||
}
|
||
}
|
||
}
|
||
|
||
/// The first step `id` takes part in.
|
||
fn find_merge(&mut self, id: usize) -> Option<Merge> {
|
||
self.work += self.bond_load(id);
|
||
if let Some(m) = self.orient(id) {
|
||
return Some(m);
|
||
}
|
||
let nodes: Vec<usize> = self.cluster(id)?.pts.keys().copied().collect();
|
||
// Pairs first, stopping at the first merge: a busy node's member list is read only
|
||
// up to the first partner.
|
||
let (mut tried, mut work) = (BTreeSet::new(), 0);
|
||
let found = 'pairs: {
|
||
for other in nodes.iter().flat_map(|n| self.member.get(*n).into_iter().flatten()) {
|
||
work += 1;
|
||
if self.work + work > self.limit {
|
||
break 'pairs None;
|
||
}
|
||
let Some(size) = self.cluster(*other).map(|c| c.pts.len()) else { continue };
|
||
if *other == id || !tried.insert(*other) {
|
||
continue;
|
||
}
|
||
work += size.min(nodes.len());
|
||
if let Some(m) = self.pair(id, *other, &self.common(id, *other)) {
|
||
break 'pairs Some(m);
|
||
}
|
||
}
|
||
None
|
||
};
|
||
self.work += work;
|
||
if found.is_some() || self.work > self.limit {
|
||
return found;
|
||
}
|
||
// Neighbours with the nodes they share (dropping merged-away clusters on the way).
|
||
let mut shared: BTreeMap<usize, Vec<usize>> = BTreeMap::new();
|
||
for node in &nodes {
|
||
let clusters = &self.clusters;
|
||
let Some(list) = self.member.get_mut(*node) else { continue };
|
||
list.retain(|c| clusters.get(*c).is_some_and(Option::is_some));
|
||
self.work += list.len();
|
||
for other in list.iter().filter(|c| **c != id) {
|
||
shared.entry(*other).or_default().push(*node);
|
||
}
|
||
}
|
||
// Triangles through the node index: y shares `a` with x, z holds a node `b` of y
|
||
// and shares another node `c` with x.
|
||
let mut work = 0;
|
||
let found = 'search: {
|
||
let xc = self.cluster(id)?;
|
||
for (y, ys) in &shared {
|
||
let (Some(&a), Some(yc)) = (ys.first(), self.cluster(*y)) else { continue };
|
||
for b in yc.pts.keys().filter(|b| !xc.pts.contains_key(b)) {
|
||
work += 1;
|
||
for z in self.member.get(*b).into_iter().flatten() {
|
||
work += 1;
|
||
if self.work + work > self.limit {
|
||
break 'search None;
|
||
}
|
||
let Some(c) = shared.get(z).filter(|_| z != y).and_then(|zs| zs.iter().find(|c| **c != a)) else { continue };
|
||
if let Some(m) = self.triangle([id, *y, *z], [a, *b, *c]) {
|
||
break 'search Some(m);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
None
|
||
};
|
||
self.work += work;
|
||
if found.is_some() || self.work > self.limit {
|
||
return found;
|
||
}
|
||
for (other, nodes) in &shared {
|
||
if let [pivot] = nodes.as_slice() {
|
||
self.work += self.bond_load(id) + self.bond_load(*other);
|
||
if self.work > self.limit {
|
||
return None;
|
||
}
|
||
if let Some(m) = self.hinge(id, *other, *pivot) {
|
||
return Some(m);
|
||
}
|
||
}
|
||
}
|
||
None
|
||
}
|
||
|
||
/// A rigid cluster holding both ends of an axis bond turns until the bond holds and is
|
||
/// then oriented. Of the two turns, the one keeping the bond's other component on its
|
||
/// reference side. Bonds whose ends coincide in the cluster can't turn it; the next is
|
||
/// tried.
|
||
fn orient(&self, id: usize) -> Option<Merge> {
|
||
let c = self.cluster(id).filter(|c| c.frame == Frame::Rigid)?;
|
||
c.pts.keys().flat_map(|n| self.bonds_of.get(*n).into_iter().flatten()).filter_map(|b| self.bonds.get(*b)).find_map(|bond| {
|
||
let (lo, hi) = (*c.pts.get(&bond.lo)?, *c.pts.get(&bond.hi)?);
|
||
let target = on_axis(bond.axis, lo, lo.dist(hi), bond.delta, self.at(bond.ends.0), self.at(bond.ends.1))?;
|
||
let m = Motion::onto(lo, hi, lo, target)?;
|
||
Some(Merge { frame: Frame::Oriented, parts: vec![(id, m)] })
|
||
})
|
||
}
|
||
|
||
/// Two clusters sharing points: two non-rigid ones by translation, a rigid one onto two
|
||
/// shared points.
|
||
fn pair(&self, x: usize, y: usize, shared: &[usize]) -> Option<Merge> {
|
||
// The least free frame is kept; between equals the larger cluster stays put.
|
||
let key = |c: usize| (self.frame(c), std::cmp::Reverse(self.cluster(c).map_or(0, |c| c.pts.len())));
|
||
let (base, moved) = if key(y) < key(x) { (y, x) } else { (x, y) };
|
||
let a = *shared.first()?;
|
||
let (wa, ua) = (self.pos(base, a)?, self.pos(moved, a)?);
|
||
let m = if self.frame(moved) != Frame::Rigid {
|
||
Motion::translate(ua, wa)
|
||
} else {
|
||
// The other shared point farthest from `a` gives the best-conditioned direction.
|
||
let far = |n: &usize| self.pos(base, *n).map_or(0.0, |w| w.dist(wa));
|
||
let b = shared.iter().skip(1).max_by(|p, q| far(p).total_cmp(&far(q)))?;
|
||
Motion::onto(ua, self.pos(moved, *b)?, wa, self.pos(base, *b)?)?
|
||
};
|
||
Some(Merge { frame: self.frame(base), parts: vec![(base, Motion::IDENTITY), (moved, m)] })
|
||
}
|
||
|
||
/// Owen's triangle step: clusters `[x, y, z]` hold the distinct nodes `[a, b, c]` as
|
||
/// x ∋ c, a; y ∋ a, b; z ∋ b, c, so the distances between them are known. Construct the
|
||
/// triangle in the frame of the least free cluster, keeping its reference orientation,
|
||
/// and move the other two (rigid) onto it.
|
||
fn triangle(&self, [x, y, z]: [usize; 3], [a, b, c]: [usize; 3]) -> Option<Merge> {
|
||
// Each cluster with the two shared nodes it holds, in cyclic order (previous, next).
|
||
let held = [(x, c, a), (y, a, b), (z, b, c)];
|
||
let base = (0..3).min_by_key(|i| held.get(*i).map(|h| self.frame(h.0)))?;
|
||
let (kp, p, r) = *held.get(base)?;
|
||
let (kr, _, q) = *held.get((base + 1) % 3)?;
|
||
let (kq, ..) = *held.get((base + 2) % 3)?;
|
||
if self.frame(kr) != Frame::Rigid || self.frame(kq) != Frame::Rigid {
|
||
return None;
|
||
}
|
||
let (wp, wr) = (self.pos(kp, p)?, self.pos(kp, r)?);
|
||
let (rr, rq) = (self.pos(kr, r)?, self.pos(kr, q)?);
|
||
let (qq, qp) = (self.pos(kq, q)?, self.pos(kq, p)?);
|
||
let (to_p, to_r) = (qp.dist(qq), rr.dist(rq));
|
||
let circles = |p: Vec2, r: Vec2| (Locus::Circle { c: p, r: to_p }, Locus::Circle { c: r, r: to_r });
|
||
let then = self.at_node(kp, p).zip(self.at_node(kp, r)).map(|(p, r)| circles(p, r));
|
||
// q keeps the turn p→r→q it has in the reference.
|
||
let wq = crossing(circles(wp, wr), then, self.at_node(kr, q))?;
|
||
let (mr, mq) = (Motion::onto(rr, rq, wr, wq)?, Motion::onto(qq, qp, wq, wp)?);
|
||
Some(Merge { frame: self.frame(kp), parts: vec![(kp, Motion::IDENTITY), (kr, mr), (kq, mq)] })
|
||
}
|
||
|
||
/// A rigid cluster pinned at one point of a non-rigid one turns about the pin until an
|
||
/// axis bond between them holds (circle ∩ axis line). The branch keeps the side of the
|
||
/// pin the bonded point is on in the reference.
|
||
fn hinge(&self, x: usize, y: usize, pivot: usize) -> Option<Merge> {
|
||
let (base, moved) = if self.frame(y) < self.frame(x) { (y, x) } else { (x, y) };
|
||
if self.frame(base) == Frame::Rigid || self.frame(moved) != Frame::Rigid {
|
||
return None;
|
||
}
|
||
let (bc, mc) = (self.cluster(base)?, self.cluster(moved)?);
|
||
let (centre, local_pivot) = (*bc.pts.get(&pivot)?, *mc.pts.get(&pivot)?);
|
||
let pin = self.at_node(moved, pivot);
|
||
mc.pts.iter().filter(|(n, _)| **n != pivot).find_map(|(u, local_u)| {
|
||
let radius = local_u.dist(local_pivot);
|
||
self.bonds_of.get(*u).into_iter().flatten().filter_map(|b| self.bonds.get(*b)).find_map(|bond| {
|
||
let (other, delta, end) = if bond.hi == *u { (bond.lo, bond.delta, bond.ends.1) } else { (bond.hi, -bond.delta, bond.ends.0) };
|
||
let (along, _) = bond.axis.split(*bc.pts.get(&other)? - centre);
|
||
let target = on_axis(bond.axis, centre, radius, along + delta, pin, self.at(end))?;
|
||
let m = Motion::onto(local_pivot, *local_u, centre, target)?;
|
||
Some(Merge { frame: self.frame(base), parts: vec![(base, Motion::IDENTITY), (moved, m)] })
|
||
})
|
||
})
|
||
}
|
||
|
||
/// Owen's single-point merge: every point outside cluster `id` that its relations and
|
||
/// ties to the cluster's points place at one crossing joins it (in the cluster's frame),
|
||
/// and the clusters holding it are queued to meet the grown one. Only the neighbours of
|
||
/// nodes not tried before are tried (a point's placement changes only when a point it is
|
||
/// related to joins). The ground grows by propagation instead. True when a point joined.
|
||
fn grow(&mut self, id: usize) -> bool {
|
||
if self.ground == Some(id) {
|
||
return false;
|
||
}
|
||
let Some(c) = self.clusters.get_mut(id).and_then(Option::as_mut) else { return false };
|
||
let oriented = c.frame != Frame::Rigid;
|
||
let mut scan = std::mem::take(&mut c.fresh);
|
||
let mut grown = false;
|
||
// Out of budget: the nodes not scanned stay fresh (nothing more merges anyway).
|
||
let stop = |g: &mut Self, node: usize, scan: Vec<usize>| {
|
||
if let Some(Some(c)) = g.clusters.get_mut(id) {
|
||
c.fresh.extend(scan);
|
||
c.fresh.push(node);
|
||
}
|
||
};
|
||
while let Some(node) = scan.pop() {
|
||
// Points outside the cluster related to a point of `node`.
|
||
let mut candidates = BTreeSet::new();
|
||
for p in self.points_of.get(node).into_iter().flatten() {
|
||
let woken = self.reading.woken(*p);
|
||
self.work += woken.len();
|
||
for o in self.reading.related(*p).chain(woken) {
|
||
self.work += 1;
|
||
if self.work > self.limit {
|
||
stop(self, node, scan);
|
||
return grown;
|
||
}
|
||
if self.cluster(id).is_some_and(|c| !c.pts.contains_key(&self.node(o))) {
|
||
candidates.insert(o);
|
||
}
|
||
}
|
||
}
|
||
for t in candidates {
|
||
if self.work > self.limit {
|
||
stop(self, node, scan);
|
||
return grown;
|
||
}
|
||
let joining = self.node(t);
|
||
let Some(c) = self.cluster(id).filter(|c| !c.pts.contains_key(&joining)) else { continue };
|
||
let at = |i: usize| c.pts.get(&self.node(i)).copied();
|
||
let items = self.reading.items(t, &View { at: &at, oriented });
|
||
let fix = place(&items, self.at(t)).filter(|f| f.transversal);
|
||
self.work += 1 + self.reading.adj(t).len() + self.reading.tie_count_of(t) + items.len().saturating_mul(items.len());
|
||
let Some(fix) = fix else { continue };
|
||
if let Some(Some(c)) = self.clusters.get_mut(id) {
|
||
c.pts.insert(joining, fix.at);
|
||
c.via.insert(joining, t);
|
||
}
|
||
self.join(joining, id);
|
||
scan.push(joining);
|
||
grown = true;
|
||
}
|
||
}
|
||
grown
|
||
}
|
||
|
||
/// See [`Decomposition::floats`]. Charged a scan of the clusters and their relations.
|
||
fn floats(&mut self, anchor: &dyn Fn(usize) -> Option<Vec2>) -> Vec<Vec<(usize, Vec2)>> {
|
||
let node_at = |n: usize| self.points_of.get(n).into_iter().flatten().find_map(|i| anchor(*i));
|
||
let mut work = 0;
|
||
// (pinned first, larger first, lower slot first), slot, pin with its position.
|
||
let mut picks = Vec::new();
|
||
for (id, c) in self.clusters.iter().enumerate() {
|
||
let Some(c) = c.as_ref().filter(|_| self.ground != Some(id)) else { continue };
|
||
work += c.pts.len();
|
||
if self.work + work > self.limit {
|
||
self.work += work;
|
||
return Vec::new();
|
||
}
|
||
if c.pts.keys().any(|n| self.conflicted.get(*n).copied().unwrap_or(false)) {
|
||
continue;
|
||
}
|
||
let mut pins = c.pts.keys().filter_map(|n| Some((*n, node_at(*n)?)));
|
||
let (pin, more) = (pins.next(), pins.next().is_some());
|
||
let eligible = match c.frame {
|
||
Frame::Rigid => c.pts.len() >= 3 && !more,
|
||
Frame::Oriented => c.pts.len() >= 2 && pin.is_none(),
|
||
Frame::Ground => false,
|
||
};
|
||
if eligible && (pin.is_some_and(|(n, at)| self.at_node(id, n) != Some(at)) || (self.edits && self.misses(id, &mut work))) {
|
||
picks.push(((pin.is_none(), std::cmp::Reverse(c.pts.len()), id), pin));
|
||
}
|
||
}
|
||
self.work += work;
|
||
picks.sort_by_key(|(key, _)| *key);
|
||
picks.into_iter().filter_map(|((.., id), pin)| self.pose(id, pin)).collect()
|
||
}
|
||
|
||
/// A relation or tie among cluster `id`'s points misses at the reference (or a point has
|
||
/// no reference position). Charges `work`.
|
||
fn misses(&self, id: usize, work: &mut usize) -> bool {
|
||
let Some(c) = self.cluster(id) else { return false };
|
||
let inside = |i: usize| c.pts.contains_key(&self.node(i)).then(|| self.at(i)).flatten();
|
||
let view = View { at: &inside, oriented: true };
|
||
c.pts.keys().flat_map(|n| self.points_of.get(*n).into_iter().flatten()).any(|i| {
|
||
*work += 1 + self.reading.adj(*i).len() + self.reading.tie_count_of(*i);
|
||
self.at(*i).is_none_or(|at| !self.reading.holds(*i, at, &view))
|
||
})
|
||
}
|
||
|
||
/// Cluster `id` where its reference pose puts it (see the module doc), pinned at a node
|
||
/// placed at a position if any: each of its points but the pin's, with its position. None
|
||
/// when the reference decides no pose.
|
||
fn pose(&self, id: usize, pin: Option<(usize, Vec2)>) -> Option<Vec<(usize, Vec2)>> {
|
||
let c = self.cluster(id)?;
|
||
let moved = || c.pts.iter().filter(move |(n, _)| pin.is_none_or(|(p, _)| **n != p));
|
||
// (position in the cluster's frame, reference position) of each node but the pin.
|
||
let pairs: Vec<(Vec2, Vec2)> = moved().filter_map(|(n, p)| Some((*p, self.at_node(id, *n)?))).collect();
|
||
let centroids = || {
|
||
let (u, w) = pairs.iter().fold((Vec2::ZERO, Vec2::ZERO), |(su, sw), (u, w)| (su + *u, sw + *w));
|
||
(u / pairs.len() as f64, w / pairs.len() as f64)
|
||
};
|
||
// The point the cluster turns about, in its frame and at its reference, where that
|
||
// point goes, and whether it turns.
|
||
let (from, about, to, turns) = match (c.frame, pin) {
|
||
(Frame::Oriented, None) if !pairs.is_empty() => {
|
||
let (from, to) = centroids();
|
||
(from, to, to, false)
|
||
}
|
||
(Frame::Rigid, Some((pin, at))) => (*c.pts.get(&pin)?, self.at_node(id, pin).unwrap_or(at), at, true),
|
||
(Frame::Rigid, None) if pairs.len() >= 2 => {
|
||
let (from, to) = centroids();
|
||
(from, to, to, true)
|
||
}
|
||
_ => return None,
|
||
};
|
||
let rot = if turns { best_turn(pairs.iter().map(|(u, w)| (*u - from, *w - about)))? } else { Vec2::X };
|
||
let motion = Motion { rot, from, to };
|
||
let out: Vec<(usize, Vec2)> =
|
||
moved().flat_map(|(n, p)| self.points_of.get(*n).into_iter().flatten().map(move |i| (*i, motion.apply(*p)))).collect();
|
||
out.iter().all(|(_, at)| at.is_finite()).then_some(out)
|
||
}
|
||
|
||
/// Replace the merged clusters by their union, kept in the first one's slot (so only the
|
||
/// nodes joining it change membership), and queue it.
|
||
fn absorb(&mut self, m: Merge) {
|
||
let Some(&(base, base_motion)) = m.parts.first() else { return };
|
||
let Some(mut merged) = self.clusters.get_mut(base).and_then(Option::take) else { return };
|
||
merged.frame = m.frame;
|
||
// Only `orient` moves the base (a merge's base stays put, and may be the ground); an
|
||
// oriented cluster reads relations along the axes, so every node is tried again.
|
||
if m.parts.len() == 1 {
|
||
self.work += merged.pts.len();
|
||
for p in merged.pts.values_mut() {
|
||
*p = base_motion.apply(*p);
|
||
}
|
||
merged.fresh = merged.pts.keys().copied().collect();
|
||
}
|
||
for (id, motion) in m.parts.iter().skip(1) {
|
||
let Some(c) = self.clusters.get_mut(*id).and_then(Option::take) else { continue };
|
||
self.work += c.pts.len();
|
||
for (node, p) in c.pts {
|
||
if let Entry::Vacant(slot) = merged.pts.entry(node) {
|
||
slot.insert(motion.apply(p));
|
||
merged.fresh.push(node);
|
||
if let Some(point) = c.via.get(&node) {
|
||
merged.via.entry(node).or_insert(*point);
|
||
}
|
||
if let Some(list) = self.member.get_mut(node) {
|
||
list.push(base);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
// The ground grows by propagation: it keeps no nodes to try.
|
||
if merged.frame == Frame::Ground {
|
||
merged.fresh.clear();
|
||
}
|
||
if let Some(slot) = self.clusters.get_mut(base) {
|
||
*slot = Some(merged);
|
||
}
|
||
enqueue(&mut self.queued, &mut self.queue, base);
|
||
}
|
||
}
|
||
|
||
/// Push `i` onto a work queue unless `queued[i]` says it is waiting there already.
|
||
pub(crate) fn enqueue(queued: &mut [bool], queue: &mut VecDeque<usize>, i: usize) {
|
||
if let Some(q) = queued.get_mut(i)
|
||
&& !*q
|
||
{
|
||
*q = true;
|
||
queue.push_back(i);
|
||
}
|
||
}
|
||
|
||
#[cfg(test)]
|
||
#[path = "decompose_tests.rs"]
|
||
mod tests;
|