use crate::Vec1;
use crate::linear::matrix::Matrix;
impl Matrix<f64> {
pub fn solve(&self, b: &Vec1<f64>) -> Vec1<f64> {
let n = self.rows();
assert_eq!(n, self.cols(), "solve: matrix must be square");
assert_eq!(n, b.len(), "solve: b length mismatch");
let mut m = self.clone();
let mut rhs = b.clone();
for k in 1..=n {
let mut max_row = k;
let mut max_val = m.get(k, k).abs();
for i in (k + 1)..=n {
if m.get(i, k).abs() > max_val {
max_val = m.get(i, k).abs();
max_row = i;
}
}
assert!(max_val > 1e-15, "solve: matrix is singular");
if max_row != k {
for j in 1..=n {
let tmp = m.get(k, j);
m.set(k, j, m.get(max_row, j));
m.set(max_row, j, tmp);
}
let tmp = rhs[k];
rhs[k] = rhs[max_row];
rhs[max_row] = tmp;
}
for i in (k + 1)..=n {
let factor = m.get(i, k) / m.get(k, k);
for j in k..=n {
m.set(i, j, m.get(i, j) - factor * m.get(k, j));
}
rhs[i] -= factor * rhs[k];
}
}
let mut x = Vec1::with_capacity(n);
for _ in 1..=n {
x.push(0.0);
}
for i in (1..=n).rev() {
let mut sum = rhs[i];
for j in (i + 1)..=n {
sum -= m.get(i, j) * x[j];
}
x[i] = sum / m.get(i, i);
}
x
}
pub fn solve_multiple(&self, b: &Matrix<f64>) -> Matrix<f64> {
let n = self.rows();
let k = b.cols();
assert_eq!(n, b.rows(), "solve_multiple: shape mismatch");
let mut out = Matrix::new(n, k, 0.0);
for j in 1..=k {
let col = b.col(j);
let sol = self.solve(&col);
for i in 1..=n {
out.set(i, j, sol[i]);
}
}
out
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::vec1;
#[test]
fn solve_2x2() {
let a = Matrix::from_row_major(vec1![2.0, 1.0, 1.0, 3.0], 2, 2);
let b = vec1![5.0, 10.0];
let x = a.solve(&b);
assert!((x[1] - 1.0).abs() < 1e-10);
assert!((x[2] - 3.0).abs() < 1e-10);
}
#[test]
fn solve_3x3() {
let a = Matrix::identity(3, 0.0, 1.0);
let b = vec1![1.0, 2.0, 3.0];
let x = a.solve(&b);
assert!((x[1] - 1.0).abs() < 1e-10);
assert!((x[2] - 2.0).abs() < 1e-10);
assert!((x[3] - 3.0).abs() < 1e-10);
}
#[test]
fn solve_verify_residual() {
let a = Matrix::from_row_major(vec1![4.0, 1.0, 1.0, 3.0, 2.0, 5.0, 1.0, 0.0, 3.0], 3, 3);
let b = vec1![1.0, 2.0, 3.0];
let x = a.solve(&b);
let ax = a.matvec(&x);
for i in 1..=3 {
assert!((ax[i] - b[i]).abs() < 1e-10);
}
}
#[test]
#[should_panic(expected = "singular")]
fn solve_singular_panics() {
let a = Matrix::from_row_major(vec1![1.0, 2.0, 2.0, 4.0], 2, 2);
let b = vec1![1.0, 2.0];
let _ = a.solve(&b);
}
#[test]
fn solve_multiple_basic() {
let a = Matrix::identity(2, 0.0, 1.0);
let b = Matrix::from_row_major(vec1![1.0, 2.0, 3.0, 4.0], 2, 2);
let x = a.solve_multiple(&b);
assert!((x.get(1, 1) - 1.0).abs() < 1e-10);
assert!((x.get(2, 2) - 4.0).abs() < 1e-10);
}
}