Files
smoll/src/gaussian.rs
T
2026-11-15 20:19:14 +01:00

187 lines
6.7 KiB
Rust

use crate::error::Error;
use itertools::Itertools;
use ndarray::{Array, Array2, ArrayView1, AsArray, Axis, Dimension, Ix1, Ix2};
const STP: f64 = 0.5 * std::f64::consts::FRAC_2_SQRT_PI * std::f64::consts::FRAC_1_SQRT_2;
/// p: I, x0, x1, ... xn, s00, s01, ..., s0n, s10, s11, ..., s1n, ..., sn0, sn1, ..., snn, o, o0, o1, ..., on
/// x: n 1d arrays, defining coordinates in each dimension
pub fn gaussian<'a, A, D>(x: Vec<A>, p: &[f64]) -> Result<Array<f64, D>, Error>
where
A: AsArray<'a, f64, Ix1>,
D: Dimension,
{
let n = x.len();
let n2 = n * n;
debug_assert_eq!(
p.len(),
2 + 2 * n + n2,
"number of parameters must be {}",
2 + 2 * n + n2
);
let m = ArrayView1::from(&p[1..1 + n]);
let s = &p[1 + n..1 + n + n2];
let o = p[1 + n + n2];
let q = ArrayView1::from(&p[2 + n + n2..]);
let c = Array2::from_shape_vec((n, n), s.to_vec())?.powi(2);
let (lu, perm) = lu_factor(&c)?;
let c_inv = matrix_inverse(&lu, &perm);
let c_det = matrix_determinant(&lu, &perm);
let a = p[0] * STP.powi(n as i32) / c_det.sqrt();
let x = x
.into_iter()
.map(|i| i.into())
.collect::<Vec<ArrayView1<f64>>>();
let shape = x.iter().map(|i| i.len()).collect::<Vec<_>>();
Ok(Array::from_shape_vec(
shape,
x.into_iter()
.zip(m)
.map(|(xi, &mi)| (&xi - mi).to_vec())
.multi_cartesian_product()
.map(|i| {
let xj = ArrayView1::from(&i).insert_axis(Axis(0));
a * (-0.5 * xj.dot(&c_inv).dot(&xj.t())).sum().exp() + o + q.dot(&xj.t()).sum()
})
.collect(),
)?
.into_dimensionality()?)
}
/// PA = LU factorization (Doolittle with partial pivoting).
///
/// Returns the packed LU matrix (lower unit triangular in columns
/// 0..k, upper triangular in rows k) and the row permutation, or `None`
/// if a pivot is singular or numerically rank-deficient.
fn lu_factor<'a, A>(matrix: A) -> Result<(Array2<f64>, Vec<usize>), Error>
where
A: AsArray<'a, f64, Ix2>,
{
let matrix = matrix.into();
let n = matrix.nrows();
debug_assert_eq!(n, matrix.ncols(), "matrix must be square");
// pivot magnitudes must exceed this relative threshold to count as significant
let max_abs = matrix.iter().fold(0.0f64, |m, &x| m.max(x.abs()));
let threshold = max_abs * (n as f64) * f64::EPSILON;
let mut lu = matrix.to_owned();
let mut perm: Vec<usize> = (0..n).collect();
for k in 0..n {
let mut pivot_row = k;
for i in (k + 1)..n {
if lu[[i, k]].abs() > lu[[pivot_row, k]].abs() {
pivot_row = i;
}
}
if lu[[pivot_row, k]].abs() <= threshold || !lu[[pivot_row, k]].is_finite() {
return Err(Error::MatrixNotInvertible);
}
if pivot_row != k {
for j in 0..n {
lu.swap([k, j], [pivot_row, j]);
}
perm.swap(k, pivot_row);
}
for i in (k + 1)..n {
let factor = lu[[i, k]] / lu[[k, k]];
lu[[i, k]] = factor;
for j in (k + 1)..n {
lu[[i, j]] -= factor * lu[[k, j]];
}
}
}
Ok((lu, perm))
}
/// Calculate the inverse of a square matrix using LU factorization with partial
/// pivoting.
///
/// Returns `None` if the matrix is singular, or numerically rank-deficient.
fn matrix_inverse(lu: &Array2<f64>, perm: &[usize]) -> Array2<f64> {
let n = lu.nrows();
// Solve A * inv = I column by column: first L*y = P*e_col, then U*x = y.
let mut inv = Array2::zeros((n, n));
for col in 0..n {
let mut y = vec![0.0; n];
for i in 0..n {
let rhs = if perm[i] == col { 1.0 } else { 0.0 };
let mut sum = rhs;
for j in 0..i {
sum -= lu[[i, j]] * y[j];
}
y[i] = sum;
}
for i in (0..n).rev() {
let mut sum = y[i];
for j in (i + 1)..n {
sum -= lu[[i, j]] * inv[[j, col]];
}
inv[[i, col]] = sum / lu[[i, i]];
}
}
inv
}
/// Calculate the determinant of a square matrix using LU factorization
/// with partial pivoting.
///
/// Returns `None` if the matrix is singular, or numerically rank-deficient.
fn matrix_determinant(lu: &Array2<f64>, perm: &[usize]) -> f64 {
// sign of the row permutation: parity of its inversion count
let inversions = (1..perm.len())
.map(|i| (0..i).filter(|&j| perm[j] > perm[i]).count())
.sum::<usize>();
let sign = if inversions % 2 == 0 { 1.0 } else { -1.0 };
sign * (0..lu.nrows()).map(|k| lu[[k, k]]).product::<f64>()
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn test_matrix_det() -> Result<(), Error> {
// trivial 1x1
let m = Array2::from_shape_vec((1, 1), vec![3.0])?;
let (lu, perm) = lu_factor(&m)?;
assert_eq!(matrix_determinant(&lu, &perm), 3.0);
// positive determinant, needs a row swap
let m = Array2::from_shape_vec((2, 2), vec![2.0, 1.0, 1.0, 3.0])?;
let (lu, perm) = lu_factor(&m)?;
assert!((matrix_determinant(&lu, &perm) - 5.0).abs() < 1e-14);
// negative determinant
let m = Array2::from_shape_vec((2, 2), vec![1.0, 2.0, 2.0, 1.0])?;
let (lu, perm) = lu_factor(&m)?;
assert!((matrix_determinant(&lu, &perm) + 3.0).abs() < 1e-14);
// 3x3 with det = 13
let m = Array2::from_shape_vec((3, 3), vec![2.0, 0.0, 1.0, 1.0, 2.0, 0.0, 0.0, 1.0, 3.0])?;
let (lu, perm) = lu_factor(&m)?;
assert!((matrix_determinant(&lu, &perm) - 13.0).abs() < 1e-13);
// 3x3 requiring row swaps (zero in the leading column), det = 2
let m = Array2::from_shape_vec((3, 3), vec![0.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 0.0])?;
let (lu, perm) = lu_factor(&m)?;
assert!((matrix_determinant(&lu, &perm) - 2.0).abs() < 1e-12);
// singular matrix
let m = Array2::from_shape_vec((2, 2), vec![1.0, 2.0, 2.0, 4.0])?;
assert!(lu_factor(&m).is_err());
// det(A * B) == det(A) * det(B)
let a = Array2::from_shape_vec((3, 3), vec![1.0, 2.0, 3.0, 0.0, 1.0, 4.0, 5.0, 6.0, 0.0])?;
let b =
Array2::from_shape_vec((3, 3), vec![0.0, 1.0, 2.0, -1.0, 0.0, 3.0, 2.0, -1.0, 0.0])?;
let prod = a.dot(&b);
let (lu, perm) = lu_factor(&prod)?;
let ab = matrix_determinant(&lu, &perm);
let (lu, perm) = lu_factor(&a)?;
let a_det = matrix_determinant(&lu, &perm);
let (lu, perm) = lu_factor(&b)?;
let b_det = matrix_determinant(&lu, &perm);
assert!((ab - a_det * b_det).abs() < 1e-10);
Ok(())
}
}