#[must_use]
pub(crate) fn solve_spd_3(matrix: &[f64; 6], rhs: &[f64; 3]) -> Option<[f64; 3]> {
let dense = [
matrix[0], matrix[1], matrix[2], matrix[1], matrix[3], matrix[4], matrix[2], matrix[4],
matrix[5],
];
let solution = solve_spd(&dense, rhs, 3)?;
Some([solution[0], solution[1], solution[2]])
}
#[must_use]
#[allow(clippy::many_single_char_names)] pub(crate) fn solve_spd(matrix: &[f64], rhs: &[f64], dimensions: usize) -> Option<Vec<f64>> {
if dimensions == 0
|| dimensions.checked_mul(dimensions)? != matrix.len()
|| rhs.len() != dimensions
|| matrix.iter().chain(rhs).any(|value| !value.is_finite())
{
return None;
}
let mut l = vec![0.0; matrix.len()];
for row in 0..dimensions {
for column in 0..=row {
let mut sum = matrix[row * dimensions + column];
for previous in 0..column {
sum -= l[row * dimensions + previous] * l[column * dimensions + previous];
}
if row == column {
if sum <= 0.0 || !sum.is_finite() {
return None;
}
l[row * dimensions + column] = sum.sqrt();
} else {
let pivot = l[column * dimensions + column];
if pivot == 0.0 {
return None;
}
l[row * dimensions + column] = sum / pivot;
}
}
}
let mut y = vec![0.0; dimensions];
for row in 0..dimensions {
let mut sum = rhs[row];
for column in 0..row {
sum -= l[row * dimensions + column] * y[column];
}
y[row] = sum / l[row * dimensions + row];
}
let mut x = vec![0.0; dimensions];
for row in (0..dimensions).rev() {
let mut sum = y[row];
for column in (row + 1)..dimensions {
sum -= l[column * dimensions + row] * x[column];
}
x[row] = sum / l[row * dimensions + row];
}
x.iter().all(|value| value.is_finite()).then_some(x)
}
#[cfg(test)]
#[allow(clippy::expect_used)]
mod tests {
use super::*;
#[test]
fn solves_identity() {
let solution = solve_spd_3(&[1.0, 0.0, 0.0, 1.0, 0.0, 1.0], &[2.0, 3.0, 5.0])
.expect("identity is positive definite");
assert!((solution[0] - 2.0).abs() < f64::EPSILON);
assert!((solution[1] - 3.0).abs() < f64::EPSILON);
assert!((solution[2] - 5.0).abs() < f64::EPSILON);
}
#[test]
fn solves_known_system() {
let matrix = [4.0, 1.0, 1.0, 3.0, 0.0, 2.0];
let rhs = [9.0, 7.0, 7.0];
let solution = solve_spd_3(&matrix, &rhs).expect("fixture matrix is positive definite");
assert!((solution[0] - 1.0).abs() < 1e-12);
assert!((solution[1] - 2.0).abs() < 1e-12);
assert!((solution[2] - 3.0).abs() < 1e-12);
}
#[test]
fn rejects_non_spd_nonfinite_and_bad_dimensions() {
assert!(solve_spd(&[1.0, 2.0, 2.0, 1.0], &[1.0, 1.0], 2).is_none());
assert!(solve_spd(&[1.0, 0.0, 0.0, 1.0], &[1.0], 2).is_none());
assert!(solve_spd(&[f64::NAN], &[1.0], 1).is_none());
assert!(solve_spd(&[], &[], 0).is_none());
}
}