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, p: &[f64]) -> Result, 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::>>(); let shape = x.iter().map(|i| i.len()).collect::>(); 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, Vec), 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 = (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, perm: &[usize]) -> Array2 { 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, 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::(); let sign = if inversions % 2 == 0 { 1.0 } else { -1.0 }; sign * (0..lu.nrows()).map(|k| lu[[k, k]]).product::() } #[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(()) } }