Files
solidcraft/crates/engine/src/cmd/component_matrix.rs
T

122 lines
5.1 KiB
Rust
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
//! 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<f64> {
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<Mat> {
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<Vec<f64>> {
let a = v.as_array()?;
let flat: Vec<f64> = 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::<Option<_>>()?
};
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)))
}