use super::{
BLANK_DOMINATION_FACTOR, DEFAULT_PLAUSIBLE_SCALE, EPS_INCIDENT, LEVEL_TOL,
MAX_CONSTRUCTIVE_SWEEPS, MAX_PLANES, MAX_REFINE_SWEEPS, MeetConstraint, MeetTierInput,
SolveStrategy, SolvedTier,
blocks::{Block, classify_blocks, tier_sides},
candidates::{
CandidateVertex, SolvePlane, blank_planes, enumerate_candidate_vertices,
filter_levels_by_instance_support, group_levels, tier_normals,
},
names::MeetNameResolver,
phase1_cache::{CachedCandidate, Phase1Cache},
};
use crate::geometry::stone_metrics::{ExternalProportions, measure_solid};
use glam::DVec3;
use std::collections::BTreeMap;
#[derive(Clone, Copy, PartialEq, Eq)]
enum Origin {
Unset,
Anchor,
ConstructiveNamed,
ConstructiveRank1,
Estimated,
Refined,
}
fn block_estimate(
members: &[(f64, f64, f64)], cos_t: f64,
sin_t: f64,
fallback: f64,
) -> f64 {
if members.len() >= 2 {
let (mut cc, mut cs, mut ss, mut cm, mut sm) = (0.0_f64, 0.0, 0.0, 0.0, 0.0);
for &(c, s, m) in members {
cc = c.mul_add(c, cc);
cs = c.mul_add(s, cs);
ss = s.mul_add(s, ss);
cm = c.mul_add(m, cm);
sm = s.mul_add(m, sm);
}
let det = cs.mul_add(-cs, cc * ss);
if det.abs() > 1e-9 {
let a = sm.mul_add(-cs, cm * ss) / det;
let b = cm.mul_add(-cs, sm * cc) / det;
let est = a.mul_add(cos_t, b * sin_t);
if est.is_finite() && est > 1e-6 {
return est;
}
}
}
let usable: Vec<&(f64, f64, f64)> = members.iter().filter(|(_, s, _)| *s > 0.1).collect();
if !usable.is_empty() {
let b = usable.iter().map(|(_, s, m)| m / s).sum::<f64>() / usable.len() as f64;
let est = b * sin_t;
if est.is_finite() && est > 1e-6 {
return est;
}
}
fallback
}
pub(super) struct PipelineResult {
pub(super) mast: Vec<f64>,
origin: Vec<Origin>,
pub(super) last_pick: Vec<Option<(usize, usize)>>,
named_cause: Vec<Option<&'static str>>,
refine_sweeps: usize,
converged: bool,
}
#[must_use]
pub fn solve_meet_points(gear_teeth_abs: u32, tiers: &[MeetTierInput]) -> Vec<SolvedTier> {
let ctx = SolveContext::new(gear_teeth_abs, tiers);
if ctx.total_planes > MAX_PLANES {
return ctx.failed_solved();
}
let result = ctx.run_pipeline(&BTreeMap::new(), &BTreeMap::new());
ctx.to_solved(&result)
}
pub(super) struct SolveContext<'a> {
pub(super) tiers: &'a [MeetTierInput],
normals: Vec<Vec<DVec3>>,
blocks: Vec<Block>,
resolved_named: Vec<Vec<usize>>,
pub(super) is_anchor: Vec<bool>,
scale_prior: f64,
domination_limit: f64,
pub(super) total_planes: usize,
}
impl<'a> SolveContext<'a> {
pub(super) fn new(gear_teeth_abs: u32, tiers: &'a [MeetTierInput]) -> Self {
let sides = tier_sides(tiers);
let normals: Vec<Vec<DVec3>> = tiers
.iter()
.zip(&sides)
.map(|(t, &crown)| tier_normals(gear_teeth_abs, t.angle_deg, &t.indices, crown))
.collect();
let blocks = classify_blocks(tiers);
let resolver = MeetNameResolver::new(tiers);
let resolved_named: Vec<Vec<usize>> = tiers
.iter()
.enumerate()
.map(|(i, t)| match &t.constraint {
MeetConstraint::MeetNamed(names) => {
let mut refs = resolver.resolve_names(names).refs;
refs.retain(|&r| r != i);
refs
}
_ => Vec::new(),
})
.collect();
let is_anchor: Vec<bool> = tiers
.iter()
.map(|t| matches!(t.constraint, MeetConstraint::ScaleReference(_)))
.collect();
let scale_prior = tiers
.iter()
.filter_map(|t| match &t.constraint {
MeetConstraint::ScaleReference(v) => Some(v.abs()),
_ => None,
})
.fold(0.0_f64, f64::max);
let scale_prior = if scale_prior > 1e-9 {
scale_prior
} else {
DEFAULT_PLAUSIBLE_SCALE
};
let domination_limit = BLANK_DOMINATION_FACTOR * scale_prior;
let total_planes = 6 + normals.iter().map(Vec::len).sum::<usize>();
Self {
tiers,
normals,
blocks,
resolved_named,
is_anchor,
scale_prior,
domination_limit,
total_planes,
}
}
pub(super) fn failed_solved(&self) -> Vec<SolvedTier> {
let total_planes = self.total_planes;
self.tiers
.iter()
.map(|t| match &t.constraint {
MeetConstraint::ScaleReference(v) => SolvedTier {
mast: v.abs(),
strategy: SolveStrategy::ScaleReference,
detail: "given (scale reference)".to_string(),
},
_ => SolvedTier {
mast: self.scale_prior,
strategy: SolveStrategy::Failed,
detail: format!(
"design has {total_planes} planes, above the {MAX_PLANES}-plane cap \
for candidate-vertex enumeration"
),
},
})
.collect()
}
#[expect(
clippy::too_many_lines,
reason = "three sequential phases of one algorithm (constructive pass, \
per-block estimate, nearest-level refinement -- see the module docs' \
walkthrough) sharing one set of per-tier working vectors (mast, \
origin, settled, last_pick); splitting the phases into separate \
functions would just turn those shared locals into a too-many-\
arguments (or a bespoke context struct) problem at each call boundary"
)]
pub(super) fn run_pipeline(
&self,
overrides: &BTreeMap<usize, usize>,
anchor_values: &BTreeMap<usize, f64>,
) -> PipelineResult {
let tiers: &[MeetTierInput] = self.tiers;
let n = tiers.len();
let normals = &self.normals;
let blocks = &self.blocks;
let resolved_named = &self.resolved_named;
let is_anchor = &self.is_anchor;
let scale_prior = self.scale_prior;
let domination_limit = self.domination_limit;
let mut mast: Vec<f64> = tiers
.iter()
.enumerate()
.map(|(i, t)| match &t.constraint {
MeetConstraint::ScaleReference(v) => {
anchor_values.get(&i).copied().unwrap_or_else(|| v.abs())
}
_ => scale_prior,
})
.collect();
let mut origin: Vec<Origin> = is_anchor
.iter()
.map(|&a| if a { Origin::Anchor } else { Origin::Unset })
.collect();
let mut last_pick: Vec<Option<(usize, usize)>> = vec![None; n];
let mut named_cause: Vec<Option<&'static str>> = vec![None; n];
let mut settled: Vec<bool> = is_anchor.clone();
let mut named_release = false;
let mut phase1_cache = Phase1Cache::new();
for (j, &s) in settled.iter().enumerate() {
if s {
phase1_cache.add_tier(j, &normals[j], mast[j]);
}
}
for _pass in 0..MAX_CONSTRUCTIVE_SWEEPS {
let mut progress = false;
for i in 0..n {
if settled[i] {
continue;
}
let refs = &resolved_named[i];
let refs_settled: Vec<usize> =
refs.iter().copied().filter(|&t| settled[t]).collect();
if !refs.is_empty() && refs_settled.len() < refs.len() && !named_release {
continue;
}
let n0 = normals[i][0];
let feasible: Vec<&CachedCandidate> = phase1_cache
.candidates
.iter()
.filter(|c| c.violated.is_none())
.collect();
let vals: Vec<f64> = feasible.iter().map(|c| n0.dot(c.v)).collect();
let levels = group_levels(vals.iter().copied().filter(|v| *v > 1e-9).collect());
if levels.is_empty() {
continue;
}
let mut choice: Option<(usize, Origin)> = None;
if let Some(&forced) = overrides.get(&i) {
choice = Some((forced.min(levels.len() - 1), Origin::ConstructiveRank1));
} else if !refs_settled.is_empty() {
let found = levels.iter().position(|&head| {
feasible.iter().zip(&vals).any(|(c, &val)| {
(val - head).abs() <= LEVEL_TOL
&& refs_settled.iter().all(|&t| {
normals[t]
.iter()
.any(|&nr| (nr.dot(c.v) - mast[t]).abs() < EPS_INCIDENT)
})
})
});
if let Some(idx) = found {
choice = Some((idx, Origin::ConstructiveNamed));
}
}
let (li, orig) = choice
.unwrap_or_else(|| (usize::from(levels.len() >= 2), Origin::ConstructiveRank1));
let value = levels[li];
if value.is_finite() && value > 1e-9 && value <= domination_limit {
if orig == Origin::ConstructiveRank1
&& !refs.is_empty()
&& !overrides.contains_key(&i)
{
named_cause[i] = Some(if refs_settled.len() < refs.len() {
"refs not yet settled at release"
} else {
"no feasible level incident to all settled refs"
});
}
mast[i] = value;
origin[i] = orig;
last_pick[i] = Some((li, levels.len()));
settled[i] = true;
phase1_cache.add_tier(i, &normals[i], mast[i]);
progress = true;
}
}
if !progress {
if named_release {
break;
}
named_release = true;
} else if settled.iter().all(|&s| s) {
break;
}
}
for i in 0..n {
if settled[i] {
continue;
}
let members: Vec<(f64, f64, f64)> = (0..n)
.filter(|&j| settled[j] && blocks[j] == blocks[i])
.map(|j| {
let y = normals[j][0].y.abs();
(y, y.mul_add(-y, 1.0).max(0.0).sqrt(), mast[j])
})
.collect();
let y = normals[i][0].y.abs();
mast[i] = block_estimate(&members, y, y.mul_add(-y, 1.0).max(0.0).sqrt(), scale_prior);
origin[i] = Origin::Estimated;
}
let mut refine_sweeps = 0usize;
let mut converged = false;
for _ in 0..MAX_REFINE_SWEEPS {
refine_sweeps += 1;
let mut planes = blank_planes();
for (i, ns) in normals.iter().enumerate() {
for &nv in ns {
planes.push(SolvePlane {
n: nv,
m: mast[i],
owner: i,
});
}
}
let cands = enumerate_candidate_vertices(&planes, 6);
let mut new_mast = mast.clone();
for i in 0..n {
if is_anchor[i] {
continue;
}
let mut usable: Vec<&CandidateVertex> = Vec::with_capacity(cands.len());
usable.extend(cands.iter().filter(|c| {
!c.owners.contains(&i) && (c.violated.is_none() || c.violated == Some(i))
}));
if usable.is_empty() {
continue;
}
let n0 = normals[i][0];
let vals: Vec<f64> = usable.iter().map(|c| n0.dot(c.v)).collect();
let levels = group_levels(vals.iter().copied().filter(|v| *v > 1e-9).collect());
let levels = filter_levels_by_instance_support(levels, &normals[i], &usable);
if levels.is_empty() {
continue;
}
let refs = &resolved_named[i];
let nearest_of = || -> Option<usize> {
levels
.iter()
.enumerate()
.min_by(|&(_, a), &(_, b)| {
(a - mast[i])
.abs()
.partial_cmp(&(b - mast[i]).abs())
.unwrap_or(std::cmp::Ordering::Equal)
})
.map(|(idx, _)| idx)
};
let named_pool: Vec<usize> = if refs.is_empty() || overrides.contains_key(&i) {
Vec::new()
} else {
levels
.iter()
.enumerate()
.filter(|&(_, &head)| {
usable.iter().zip(&vals).any(|(c, &val)| {
(val - head).abs() <= LEVEL_TOL
&& refs.iter().all(|&t| {
normals[t]
.iter()
.any(|&nr| (nr.dot(c.v) - mast[t]).abs() < EPS_INCIDENT)
})
})
})
.map(|(idx, _)| idx)
.collect()
};
let pick = named_pool.first().copied().or_else(nearest_of);
if let Some(li) = pick {
let v = levels[li];
if v.is_finite() && v > 1e-9 && v <= domination_limit {
new_mast[i] = v;
last_pick[i] = Some((li, levels.len()));
if origin[i] == Origin::Estimated {
origin[i] = Origin::Refined;
}
}
}
}
let max_rel_change = mast
.iter()
.zip(&new_mast)
.map(|(&old, &new)| (new - old).abs() / old.abs().max(1e-9))
.fold(0.0_f64, f64::max);
mast = new_mast;
if max_rel_change < 1e-12 {
converged = true;
break;
}
}
PipelineResult {
mast,
origin,
last_pick,
named_cause,
refine_sweeps,
converged,
}
}
pub(super) fn to_solved(&self, result: &PipelineResult) -> Vec<SolvedTier> {
let variant = "file-order";
let refine_sweeps = result.refine_sweeps;
let conv = if result.converged {
"converged"
} else {
"sweep cap"
};
(0..self.tiers.len())
.map(|i| {
let pick_str = result.last_pick[i].map_or_else(
|| "no vertex pick".to_string(),
|(li, nl)| format!("level {li} of {nl}"),
);
match result.origin[i] {
Origin::Anchor => SolvedTier {
mast: result.mast[i],
strategy: SolveStrategy::ScaleReference,
detail: "given (scale reference)".to_string(),
},
Origin::ConstructiveNamed => SolvedTier {
mast: result.mast[i],
strategy: SolveStrategy::DependencyOrder,
detail: format!(
"constructive vertex incidence ({variant} pipeline) with {} resolved \
named reference(s), then {pick_str} after {refine_sweeps} refinement \
sweep(s) ({conv})",
self.resolved_named[i].len()
),
},
Origin::ConstructiveRank1 => SolvedTier {
mast: result.mast[i],
strategy: SolveStrategy::DependencyOrder,
detail: {
let cause = result.named_cause[i]
.map_or_else(String::new, |c| format!(" [named fallback: {c}]"));
format!(
"constructive vertex incidence ({variant} pipeline, rank-1 \
prior{cause}), then {pick_str} after {refine_sweeps} refinement \
sweep(s) ({conv})"
)
},
},
Origin::Refined => SolvedTier {
mast: result.mast[i],
strategy: SolveStrategy::JointGroup,
detail: format!(
"mutually-dependent remainder settled by nearest-level refinement \
({variant} pipeline): {pick_str} after {refine_sweeps} sweep(s) ({conv})"
),
},
Origin::Estimated => SolvedTier {
mast: result.mast[i],
strategy: SolveStrategy::LeastSquaresFallback,
detail: "no usable candidate vertex; mast is the per-block \
a*cos(theta)+b*sin(theta) estimate"
.to_string(),
},
Origin::Unset => SolvedTier {
mast: result.mast[i],
strategy: SolveStrategy::Failed,
detail: "no vertex and no estimate; mast is the scale prior".to_string(),
},
}
})
.collect()
}
pub(super) fn config_score(&self, masts: &[f64], targets: &ExternalProportions) -> f64 {
let planes: Vec<(DVec3, f64)> = self
.normals
.iter()
.zip(masts)
.flat_map(|(ns, &m)| ns.iter().map(move |&n| (n, m)))
.collect();
measure_solid(&planes)
.and_then(|m| targets.combined_deviation(&m))
.unwrap_or(f64::INFINITY)
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn selects_the_rank1_vertex_level_against_a_capped_box() {
let gear = 4;
let tiers = vec![
MeetTierInput {
angle_deg: 90.0,
indices: vec![0.0, 1.0, 2.0, 3.0],
constraint: MeetConstraint::ScaleReference(1.0),
names: vec![],
},
MeetTierInput {
angle_deg: 0.0,
indices: vec![],
constraint: MeetConstraint::ScaleReference(0.6),
names: vec![],
},
MeetTierInput {
angle_deg: -0.0,
indices: vec![],
constraint: MeetConstraint::ScaleReference(0.6),
names: vec![],
},
MeetTierInput {
angle_deg: 45.0,
indices: vec![0.5],
constraint: MeetConstraint::MeetExisting,
names: vec![],
},
];
let solved = solve_meet_points(gear, &tiers);
assert_eq!(solved.len(), 4);
for s in &solved[..3] {
assert_eq!(s.strategy, SolveStrategy::ScaleReference);
}
assert_eq!(
solved[3].strategy,
SolveStrategy::DependencyOrder,
"detail: {}",
solved[3].detail
);
let expected = std::f64::consts::FRAC_1_SQRT_2.mul_add(-0.6, 1.0);
assert!(
(solved[3].mast - expected).abs() < 1e-6,
"expected the rank-1 vertex level {expected}, solver produced {} (detail: {})",
solved[3].mast,
solved[3].detail
);
}
#[test]
fn a_stated_named_meet_overrides_the_rank1_prior() {
let gear = 4;
let tiers = vec![
MeetTierInput {
angle_deg: 90.0,
indices: vec![0.0, 1.0, 2.0, 3.0],
constraint: MeetConstraint::ScaleReference(1.0),
names: vec!["girdle".to_string()],
},
MeetTierInput {
angle_deg: 0.0,
indices: vec![],
constraint: MeetConstraint::ScaleReference(0.6),
names: vec!["table".to_string()],
},
MeetTierInput {
angle_deg: -0.0,
indices: vec![],
constraint: MeetConstraint::ScaleReference(0.6),
names: vec!["culet".to_string()],
},
MeetTierInput {
angle_deg: 45.0,
indices: vec![0.5],
constraint: MeetConstraint::MeetNamed(vec![
"Y".to_string(),
"girdle".to_string(),
"table".to_string(),
]),
names: vec!["X".to_string()],
},
MeetTierInput {
angle_deg: 60.0,
indices: vec![0.5],
constraint: MeetConstraint::MeetExisting,
names: vec!["Y".to_string()],
},
];
let solved = solve_meet_points(gear, &tiers);
assert_eq!(solved.len(), 5);
let x = &solved[3];
assert_eq!(
x.strategy,
SolveStrategy::DependencyOrder,
"X should have solved exactly, got {:?}: {}",
x.strategy,
x.detail
);
assert!(
x.detail.contains("named reference"),
"X should have used its stated named references, detail: {}",
x.detail
);
let y = &solved[4];
let theta_x = 45.0_f64.to_radians();
let theta_y = 60.0_f64.to_radians();
let phi = std::f64::consts::FRAC_PI_4; let n_x = DVec3::new(
theta_x.sin() * phi.cos(),
theta_x.cos(),
theta_x.sin() * phi.sin(),
);
let n_y = DVec3::new(
theta_y.sin() * phi.cos(),
theta_y.cos(),
theta_y.sin() * phi.sin(),
);
let m = glam::DMat3::from_cols(n_y, DVec3::X, DVec3::Y).transpose();
let v = m.inverse() * DVec3::new(y.mast, 1.0, 0.6);
assert!(
(n_x.dot(v) - x.mast).abs() < 1e-6,
"X's mast {} should equal n_x . v = {} at the named meet vertex",
x.mast,
n_x.dot(v)
);
}
#[test]
fn solving_is_deterministic() {
let schedule = indicatrix_formats::asc::parse_asc(
"GemCad 5.0\n\
g 96 0.0\n\
y 6 y\n\
I 1.72\n\
H PC 45.149 Round Trichecker-12\n\
a -41.000000 0.64991234 92 n 1 84 76 68 60 52 44 36 28 20 12 4\n\
a -90.000000 1.07325092 92 n 2 84 76 68 60 52 44 36 28 20 12 4\n\
a 29.730000 0.65249790 4 n A 12 20 28 36 44 52 60 68 76 84 92\n\
a 25.000000 0.59508784 96 n B 16 32 48 64 80\n\
a 10.000000 0.48799664 96 n C 16 32 48 64 80\n",
)
.expect("must parse");
let mut tiers = super::super::meet_tier_inputs_from_asc(&schedule);
tiers[0].constraint = MeetConstraint::ScaleReference(schedule.tiers[0].mast);
tiers[1].constraint = MeetConstraint::ScaleReference(schedule.tiers[1].mast);
tiers[2].constraint = MeetConstraint::ScaleReference(schedule.tiers[2].mast);
let a = solve_meet_points(schedule.gear_teeth_abs(), &tiers);
let b = solve_meet_points(schedule.gear_teeth_abs(), &tiers);
assert_eq!(a.len(), b.len());
for (x, y) in a.iter().zip(&b) {
assert!(
x.mast.to_bits() == y.mast.to_bits(),
"nondeterministic mast"
);
assert_eq!(x.strategy, y.strategy);
assert_eq!(x.detail, y.detail);
}
}
}