//! Placement given as a matrix: a script's 3×4 (rotation rows and a translation column) or //! 4×4 transform, read as a rigid move. A rotation that is one only up to rounding is //! orthonormalised; a scale, shear or mirror is refused with the reason (a mirror is //! `solid.mirror`'s, not a placement's). use serde_json::Value; use solidcraft_doc::{IDENTITY, Mat}; use crate::Result; use crate::params::bad; /// The default largest departure from a rotation a matrix may show: |RᵀR − I| per entry. pub const TOLERANCE: f64 = 1e-9; /// The loosest tolerance a caller may ask for. const MAX_TOLERANCE: f64 = 1e-3; /// The `tolerance` parameter: the default, or a finite positive number no looser than 1e-3. pub fn tolerance(p: &Value, cmd: &str) -> Result { match p.get("tolerance") { None | Some(Value::Null) => Ok(TOLERANCE), Some(v) => v .as_f64() .filter(|t| t.is_finite() && *t > 0.0 && *t <= MAX_TOLERANCE) .ok_or_else(|| bad(cmd, format!("tolerance must be a number above 0 and at most {MAX_TOLERANCE}"))), } } /// Read `matrix` as a rigid transform. It is row-major: 12 numbers (three rows of rotation /// and translation, `[r00, r01, r02, tx, r10, …, tz]`) or 16 (a fourth row `0, 0, 0, 1`), flat /// or as rows. pub fn rigid(v: &Value, tolerance: f64, cmd: &str) -> Result { let numbers = numbers(v).ok_or_else(|| bad(cmd, "matrix must be 12 numbers (3x4, row by row) or 16 (4x4), flat or as rows"))?; if numbers.iter().any(|x| !x.is_finite()) { return Err(bad(cmd, "matrix must contain finite numbers")); } let at = |row: usize, col: usize| numbers.get(row * 4 + col).copied().unwrap_or(0.0); if numbers.len() == 16 { let last = [at(3, 0), at(3, 1), at(3, 2), at(3, 3) - 1.0]; if last.iter().any(|x| x.abs() > tolerance) { return Err(bad(cmd, "matrix is not affine: its fourth row must be 0, 0, 0, 1 (a projective transform is not a placement)")); } } if (0..3).any(|r| at(r, 3).abs() > 1e9) { return Err(bad(cmd, "matrix translation exceeds supported numeric range")); } // r[row][col], then |RᵀR − I| measures how far it is from a rotation. let r: [[f64; 3]; 3] = std::array::from_fn(|row| std::array::from_fn(|col| at(row, col))); let deviation = (0..3) .flat_map(|i| (0..3).map(move |j| (i, j))) .map(|(i, j)| { let dot: f64 = (0..3).map(|k| r[k][i] * r[k][j]).sum(); (dot - if i == j { 1.0 } else { 0.0 }).abs() }) .fold(0.0, f64::max); let det = r[0][0] * (r[1][1] * r[2][2] - r[1][2] * r[2][1]) - r[0][1] * (r[1][0] * r[2][2] - r[1][2] * r[2][0]) + r[0][2] * (r[1][0] * r[2][1] - r[1][1] * r[2][0]); if det < 0.0 { return Err(bad(cmd, "matrix mirrors (its determinant is negative): a placement only turns and moves; mirror bodies with solid.mirror")); } if !deviation.is_finite() || deviation > tolerance { return Err(bad( cmd, format!("matrix is not rigid: it scales or shears by {deviation:.3e} (|RᵀR − I|), above the tolerance {tolerance:.0e}"), )); } let rot = orthonormal(r); let mut m = IDENTITY; for (col, column) in m.iter_mut().enumerate().take(3) { for (row, cell) in column.iter_mut().enumerate().take(3) { *cell = rot[row][col]; } } m[3] = [at(0, 3), at(1, 3), at(2, 3), 1.0]; Ok(m) } /// The matrix's numbers, row by row: 12 or 16 of them, flat or nested as 3 or 4 rows of 4. fn numbers(v: &Value) -> Option> { let a = v.as_array()?; let flat: Vec = if a.iter().all(Value::is_array) { if !matches!(a.len(), 3 | 4) { return None; } let mut out = Vec::with_capacity(16); for row in a { let row = row.as_array().filter(|r| r.len() == 4)?; for x in row { out.push(x.as_f64()?); } } out } else { a.iter().map(Value::as_f64).collect::>()? }; matches!(flat.len(), 12 | 16).then_some(flat) } /// The rotation nearest to `r` (its polar factor), by Newton's iteration R ← (R + R⁻ᵀ) / 2, /// which converges quadratically from a near-rotation; a few rounds reach machine precision. fn orthonormal(mut r: [[f64; 3]; 3]) -> [[f64; 3]; 3] { for _ in 0..4 { let Some(inv_t) = inverse_transpose(&r) else { break }; for (row, inv_row) in r.iter_mut().zip(inv_t) { for (x, y) in row.iter_mut().zip(inv_row) { *x = 0.5 * (*x + y); } } } r } /// (R⁻¹)ᵀ: the cofactor matrix over the determinant. fn inverse_transpose(r: &[[f64; 3]; 3]) -> Option<[[f64; 3]; 3]> { let a = |i: usize, j: usize| r[i % 3][j % 3]; let cof: [[f64; 3]; 3] = std::array::from_fn(|i| std::array::from_fn(|j| a(i + 1, j + 1) * a(i + 2, j + 2) - a(i + 1, j + 2) * a(i + 2, j + 1))); let det: f64 = (0..3).map(|j| r[0][j] * cof[0][j]).sum(); if !det.is_finite() || det.abs() < 1e-12 { return None; } Some(cof.map(|row| row.map(|x| x / det))) }