Another project
1use bone_types::{
2 DegreesOfFreedom, Parameter, ParameterIndex, ResidualIndex, SolverResidual, SolverSeed,
3};
4use nalgebra::{DMatrix, DVector};
5
6use crate::jacobian::{assemble_jacobian, evaluate_residuals};
7use crate::system::ConstraintSystem;
8
9#[derive(Clone, Debug, PartialEq)]
10pub struct DofReport {
11 dof: DegreesOfFreedom,
12 under_constrained: Vec<ParameterIndex>,
13 over_constrained: Vec<ResidualIndex>,
14 redundant_consistent: Vec<ResidualIndex>,
15}
16
17impl DofReport {
18 #[must_use]
19 pub fn dof(&self) -> DegreesOfFreedom {
20 self.dof
21 }
22
23 #[must_use]
24 pub fn under_constrained(&self) -> &[ParameterIndex] {
25 &self.under_constrained
26 }
27
28 #[must_use]
29 pub fn over_constrained(&self) -> &[ResidualIndex] {
30 &self.over_constrained
31 }
32
33 #[must_use]
34 pub fn redundant_consistent(&self) -> &[ResidualIndex] {
35 &self.redundant_consistent
36 }
37
38 #[must_use]
39 pub fn display(&self) -> String {
40 format!(
41 "dof={} under=[{}] over=[{}] redundant=[{}]",
42 self.dof.value(),
43 self.under_constrained
44 .iter()
45 .map(ToString::to_string)
46 .collect::<Vec<_>>()
47 .join(","),
48 self.over_constrained
49 .iter()
50 .map(ToString::to_string)
51 .collect::<Vec<_>>()
52 .join(","),
53 self.redundant_consistent
54 .iter()
55 .map(ToString::to_string)
56 .collect::<Vec<_>>()
57 .join(","),
58 )
59 }
60}
61
62#[derive(Copy, Clone, Debug)]
63pub struct DofConfig {
64 pub seed: SolverSeed,
65 pub witness_scale: f64,
66 pub rank_tolerance: f64,
67 pub residual_tolerance: SolverResidual,
68}
69
70impl DofConfig {
71 pub const DEFAULT: Self = Self {
72 seed: SolverSeed::DEFAULT,
73 witness_scale: 0.5,
74 rank_tolerance: 1e-9,
75 residual_tolerance: SolverResidual::new(1e-7),
76 };
77}
78
79#[must_use]
80pub fn analyze_dof(system: &ConstraintSystem, cfg: DofConfig) -> DofReport {
81 analyze_dof_at(system, system.parameters(), cfg)
82}
83
84#[must_use]
85pub fn analyze_dof_at(
86 system: &ConstraintSystem,
87 params: &[Parameter],
88 cfg: DofConfig,
89) -> DofReport {
90 let current_params: Vec<f64> = params.iter().map(|p| p.value()).collect();
91 let n = system.parameter_count();
92 let m = system.row_count();
93 if m == 0 || n == 0 {
94 let Ok(dof_n) = u32::try_from(n) else {
95 unreachable!("parameter count fits in u32 via ParameterIndex")
96 };
97 return DofReport {
98 dof: DegreesOfFreedom::new(dof_n),
99 under_constrained: (0..dof_n).map(ParameterIndex::new).collect(),
100 over_constrained: Vec::new(),
101 redundant_consistent: Vec::new(),
102 };
103 }
104 let witness_params = perturb(¤t_params, cfg.seed, cfg.witness_scale);
105 let jacobian = assemble_jacobian(system, &witness_params);
106 let qr_cols = col_piv_qr(&jacobian, cfg.rank_tolerance);
107 let transpose = jacobian.transpose();
108 let qr_rows = col_piv_qr(&transpose, cfg.rank_tolerance);
109 let rank = qr_cols.rank;
110 let Ok(dof_n) = u32::try_from(n.saturating_sub(rank)) else {
111 unreachable!("rank <= n <= ParameterIndex::MAX")
112 };
113 let under_constrained: Vec<ParameterIndex> = qr_cols.col_perm[rank..]
114 .iter()
115 .map(|&c| {
116 let Ok(cv) = u32::try_from(c) else {
117 unreachable!("pivoted column index fits in u32 via ParameterIndex")
118 };
119 ParameterIndex::new(cv)
120 })
121 .collect();
122 let mut under_sorted = under_constrained;
123 under_sorted.sort_by_key(|p| p.value());
124
125 let residuals_at_current = evaluate_residuals(system, ¤t_params);
126 let tol = cfg.residual_tolerance.value();
127 let (over_constrained, redundant_consistent): (Vec<ResidualIndex>, Vec<ResidualIndex>) =
128 qr_rows.col_perm[rank..]
129 .iter()
130 .map(|&row| {
131 let Ok(rv) = u32::try_from(row) else {
132 unreachable!("dependent row index fits in u32 via ResidualIndex")
133 };
134 let value = residuals_at_current[row];
135 (ResidualIndex::new(rv), value.abs() <= tol)
136 })
137 .fold(
138 (Vec::new(), Vec::new()),
139 |(mut over, mut red), (idx, consistent)| {
140 if consistent {
141 red.push(idx);
142 } else {
143 over.push(idx);
144 }
145 (over, red)
146 },
147 );
148 let mut over_sorted = over_constrained;
149 let mut red_sorted = redundant_consistent;
150 over_sorted.sort_by_key(|r| r.value());
151 red_sorted.sort_by_key(|r| r.value());
152 DofReport {
153 dof: DegreesOfFreedom::new(dof_n),
154 under_constrained: under_sorted,
155 over_constrained: over_sorted,
156 redundant_consistent: red_sorted,
157 }
158}
159
160fn perturb(params: &[f64], seed: SolverSeed, scale: f64) -> Vec<f64> {
161 params
162 .iter()
163 .scan(WitnessRng::new(seed), |rng, &v| {
164 let (u, next) = rng.step();
165 *rng = next;
166 Some(v + (u - 0.5) * scale)
167 })
168 .collect()
169}
170
171#[derive(Copy, Clone, Debug)]
172struct WitnessRng {
173 state: u64,
174}
175
176impl WitnessRng {
177 fn new(seed: SolverSeed) -> Self {
178 Self {
179 state: seed.value(),
180 }
181 }
182
183 fn step(self) -> (f64, Self) {
184 let state = self.state.wrapping_add(0x9E37_79B9_7F4A_7C15);
185 let mixed = mix64(state);
186 let mantissa = mixed & 0x000F_FFFF_FFFF_FFFF;
187 let uniform = f64::from_bits(0x3FF0_0000_0000_0000 | mantissa) - 1.0;
188 (uniform, Self { state })
189 }
190}
191
192fn mix64(x: u64) -> u64 {
193 let a = (x ^ (x >> 30)).wrapping_mul(0xBF58_476D_1CE4_E5B9);
194 let b = (a ^ (a >> 27)).wrapping_mul(0x94D0_49BB_1331_11EB);
195 b ^ (b >> 31)
196}
197
198#[derive(Clone, Debug)]
199struct ColPivQr {
200 rank: usize,
201 col_perm: Vec<usize>,
202}
203
204fn col_piv_qr(input: &DMatrix<f64>, tol: f64) -> ColPivQr {
205 let m = input.nrows();
206 let n = input.ncols();
207 let initial_norms: Vec<f64> = (0..n).map(|j| input.column(j).norm_squared()).collect();
208 let initial = QrState {
209 a: input.clone(),
210 col_perm: (0..n).collect(),
211 col_norms: initial_norms,
212 rank: 0,
213 };
214 let max_steps = m.min(n);
215 let final_state = (0..max_steps).try_fold(initial, |state, k| state.step(k, tol));
216 let state = match final_state {
217 Ok(s) | Err(s) => s,
218 };
219 ColPivQr {
220 rank: state.rank,
221 col_perm: state.col_perm,
222 }
223}
224
225#[derive(Clone, Debug)]
226struct QrState {
227 a: DMatrix<f64>,
228 col_perm: Vec<usize>,
229 col_norms: Vec<f64>,
230 rank: usize,
231}
232
233impl QrState {
234 fn step(self, pivot_row: usize, tol: f64) -> Result<Self, Self> {
235 let QrState {
236 a: matrix,
237 mut col_perm,
238 mut col_norms,
239 rank,
240 } = self;
241 let rows = matrix.nrows();
242 let cols = matrix.ncols();
243 let (best_col, best_norm2) = col_norms.iter().enumerate().skip(pivot_row).fold(
244 (pivot_row, f64::NEG_INFINITY),
245 |(best, best_v), (j, &value)| {
246 if value > best_v {
247 (j, value)
248 } else {
249 (best, best_v)
250 }
251 },
252 );
253 if best_norm2 <= tol * tol {
254 return Err(QrState {
255 a: matrix,
256 col_perm,
257 col_norms,
258 rank,
259 });
260 }
261 let mut matrix = matrix;
262 if best_col != pivot_row {
263 matrix.swap_columns(pivot_row, best_col);
264 col_perm.swap(pivot_row, best_col);
265 col_norms.swap(pivot_row, best_col);
266 }
267 let tail = rows - pivot_row;
268 let column: DVector<f64> = (0..tail)
269 .map(|i| matrix[(pivot_row + i, pivot_row)])
270 .collect::<Vec<_>>()
271 .into();
272 let column_norm = column.norm();
273 if column_norm <= tol {
274 return Err(QrState {
275 a: matrix,
276 col_perm,
277 col_norms,
278 rank,
279 });
280 }
281 let alpha = if column[0] >= 0.0 {
282 -column_norm
283 } else {
284 column_norm
285 };
286 let reflector: DVector<f64> = column
287 .iter()
288 .enumerate()
289 .map(|(idx, &value)| if idx == 0 { value - alpha } else { value })
290 .collect::<Vec<_>>()
291 .into();
292 let reflector_dot = reflector.dot(&reflector);
293 let applied = if reflector_dot > 0.0 {
294 let beta = 2.0 / reflector_dot;
295 (pivot_row..cols).fold(matrix, |mut mat, col| {
296 let projection: f64 = (0..tail)
297 .map(|i| reflector[i] * mat[(pivot_row + i, col)])
298 .sum();
299 let coef = beta * projection;
300 (0..tail).for_each(|i| mat[(pivot_row + i, col)] -= coef * reflector[i]);
301 mat
302 })
303 } else {
304 matrix
305 };
306 let new_norms: Vec<f64> = col_norms
307 .iter()
308 .enumerate()
309 .map(|(col, &prev)| {
310 if col <= pivot_row || pivot_row + 1 >= rows {
311 prev
312 } else {
313 (0..(tail - 1))
314 .map(|i| {
315 let entry = applied[(pivot_row + 1 + i, col)];
316 entry * entry
317 })
318 .sum::<f64>()
319 }
320 })
321 .collect();
322 Ok(QrState {
323 a: applied,
324 col_perm,
325 col_norms: new_norms,
326 rank: rank + 1,
327 })
328 }
329}