Revision control
Copy as Markdown
Other Tools
// Copyright (c) the JPEG XL Project Authors. All rights reserved.
//
// Use of this source code is governed by a BSD-style
// license that can be found in the LICENSE file.
use crate::error::Error;
pub type Matrix3x3<T> = [[T; 3]; 3];
pub type Vector3<T> = [T; 3];
pub fn matmul3_vec(m: [f32; 9], v: [f32; 3]) -> [f32; 3] {
[
v[0] * m[0] + v[1] * m[1] + v[2] * m[2],
v[0] * m[3] + v[1] * m[4] + v[2] * m[5],
v[0] * m[6] + v[1] * m[7] + v[2] * m[8],
]
}
pub fn mul_3x3_vector(matrix: &Matrix3x3<f64>, vector: &Vector3<f64>) -> Vector3<f64> {
std::array::from_fn(|i| {
matrix[i]
.iter()
.zip(vector.iter())
.map(|(&matrix_element, &vector_element)| matrix_element * vector_element)
.sum()
})
}
pub fn mul_3x3_matrix(mat1: &Matrix3x3<f64>, mat2: &Matrix3x3<f64>) -> Matrix3x3<f64> {
std::array::from_fn(|i| std::array::from_fn(|j| (0..3).map(|k| mat1[i][k] * mat2[k][j]).sum()))
}
fn det2x2(a: f64, b: f64, c: f64, d: f64) -> f64 {
a * d - b * c
}
fn calculate_cofactor(m: &Matrix3x3<f64>, r: usize, c: usize) -> f64 {
// Determine the actual row and column indices for the 2x2 submatrix
// by excluding the current row 'r' and column 'c'.
// Ensure they are taken in ascending order to form the submatrix consistently.
let mut sub_rows = [0; 2];
let mut sub_cols = [0; 2];
let mut current_idx = 0;
for i in 0..3 {
if i != r {
sub_rows[current_idx] = i;
current_idx += 1;
}
}
current_idx = 0;
for i in 0..3 {
if i != c {
sub_cols[current_idx] = i;
current_idx += 1;
}
}
let minor_val = det2x2(
m[sub_rows[0]][sub_cols[0]],
m[sub_rows[0]][sub_cols[1]],
m[sub_rows[1]][sub_cols[0]],
m[sub_rows[1]][sub_cols[1]],
);
// Apply the checkerboard pattern sign for the cofactor
if (r + c).is_multiple_of(2) {
minor_val
} else {
-minor_val
}
}
/// Calculates the inverse of a 3x3 matrix.
pub fn inv_3x3_matrix(m: &Matrix3x3<f64>) -> Result<Matrix3x3<f64>, Error> {
let cofactor_matrix: [[f64; 3]; 3] = std::array::from_fn(|r_idx| {
std::array::from_fn(|c_idx| calculate_cofactor(m, r_idx, c_idx))
});
let det = m[0]
.iter()
.zip(cofactor_matrix[0].iter())
.map(|(&m_element, &cof_element)| m_element * cof_element)
.sum::<f64>();
// Check for numerical singularity.
const EPSILON: f64 = 1e-12;
if det.abs() < EPSILON {
return Err(Error::MatrixInversionFailed(det.abs()));
}
let inv_det = 1.0 / det;
let adjugate_matrix: [[f64; 3]; 3] =
std::array::from_fn(|r_idx| std::array::from_fn(|c_idx| cofactor_matrix[c_idx][r_idx]));
// Inverse matrix = (1/det) * Adjugate matrix.
Ok(std::array::from_fn(|r_idx| {
std::array::from_fn(|c_idx| adjugate_matrix[r_idx][c_idx] * inv_det)
}))
}
#[cfg(test)]
mod test {
use super::*;
fn assert_matrix_eq(a: &Matrix3x3<f64>, b: &Matrix3x3<f64>, epsilon: f64) {
for r in 0..3 {
for c in 0..3 {
assert!(
(a[r][c] - b[r][c]).abs() < epsilon,
"Matrices differ at [{}][{}]: expected {}, got {}. Diff: {}",
r,
c,
b[r][c],
a[r][c],
(a[r][c] - b[r][c]).abs()
);
}
}
}
#[test]
fn test_3x3_inverse() {
let m: Matrix3x3<f64> = [[1.0f64, -3.0, -2.0], [2.0, 2.0, 1.0], [2.0, 1.0, 1.0]];
let expected_inv: Matrix3x3<f64> = [[0.2, 0.2, 0.2], [0., 1., -1.], [-0.4, -1.4, 1.6]];
match inv_3x3_matrix(&m) {
Ok(inv_m) => {
assert_matrix_eq(&inv_m, &expected_inv, 1e-12);
}
Err(e) => {
panic!("Matrix inversion failed unexpectedly: {e:?}");
}
}
}
}