From d24331376ad905917fb58a22bb55b609e7b68b0c Mon Sep 17 00:00:00 2001
From: Andreas Longva <andreas.b.longva@gmail.com>
Date: Tue, 14 Feb 2017 15:58:54 +0100
Subject: [PATCH] Implement Cholesky::solve

---
 src/matrix/decomposition/cholesky.rs | 52 +++++++++++++++++++++++++++-
 1 file changed, 51 insertions(+), 1 deletion(-)

diff --git a/src/matrix/decomposition/cholesky.rs b/src/matrix/decomposition/cholesky.rs
index 9c22f1d..4e9efa1 100644
--- a/src/matrix/decomposition/cholesky.rs
+++ b/src/matrix/decomposition/cholesky.rs
@@ -1,6 +1,7 @@
 use matrix::{Matrix, BaseMatrix};
 use error::{Error, ErrorKind};
 use matrix::decomposition::Decomposition;
+use matrix::forward_substitution;
 use vector::Vector;
 use utils::dot;
 
@@ -14,7 +15,7 @@ pub struct Cholesky<T> {
     l: Matrix<T>
 }
 
-impl<T> Cholesky<T> where T: Float {
+impl<T> Cholesky<T> where T: 'static + Float {
     /// TODO
     pub fn decompose(matrix: Matrix<T>) -> Result<Self, Error> {
         assert!(matrix.rows() == matrix.cols(),
@@ -79,6 +80,19 @@ impl<T> Cholesky<T> where T: Float {
                           .fold(T::one(), |a, b| a * b);
         l_det * l_det
     }
+
+    /// TODO
+    pub fn solve(&self, b: Vector<T>) -> Vector<T> {
+        assert!(self.l.rows() == b.size(),
+            "RHS vector and coefficient matrix must be
+             dimensionally compatible.");
+        // Solve Ly = b
+        let y = forward_substitution(&self.l, b)
+                    .expect("Internal error: L should be invertible.");
+        // Solve L^T x = y
+        transpose_back_substitution(&self.l, y)
+            .expect("Internal error: L^T should be invertible.")
+    }
 }
 
 impl<T: Zero> Decomposition for Cholesky<T> {
@@ -302,6 +316,42 @@ mod tests {
         }
     }
 
+    #[test]
+    fn cholesky_solve_examples() {
+        {
+            // TODO: Enable this test
+            // It is currently commented out because
+            // backward/forward substitution don't handle
+            // empty matrices. See PR #152 for more details.
+
+            // let a: Matrix<f64> = matrix![];
+            // let b: Vector<f64> = vector![];
+            // let expected: Vector<f64> = vector![];
+            // let cholesky = Cholesky::decompose(a).unwrap();
+            // let x = cholesky.solve(b);
+            // assert_eq!(x, expected);
+        }
+
+        {
+            let a = matrix![ 1.0 ];
+            let b = vector![ 4.0 ];
+            let expected = vector![ 4.0 ];
+            let cholesky = Cholesky::decompose(a).unwrap();
+            let x = cholesky.solve(b);
+            assert_vector_eq!(x, expected, comp = float);
+        }
+
+        {
+            let a = matrix![ 4.0,  6.0;
+                             6.0, 25.0];
+            let b = vector![ 2.0,  4.0];
+            let expected = vector![ 0.40625,  0.0625 ];
+            let cholesky = Cholesky::decompose(a).unwrap();
+            let x = cholesky.solve(b);
+            assert_vector_eq!(x, expected, comp = float);
+        }
+    }
+
     quickcheck! {
         fn property_cholesky_of_identity_is_identity(n: usize) -> TestResult {
             if n > 30 {