1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35

use crate::matrix::Matrix;

impl Matrix {
    /// # Solve equations with Conjugate Gradient Method
    /// for positiveDefinite matrix
    pub fn posvcgm(self, constants: Matrix) -> Result<Matrix, String> {
        if self.rows != constants.rows || constants.columns != 1 {
            return Err("dimension mismatch".to_owned());
        }

        let mut x = Matrix::zeros(constants.rows, 1);
        let mut r = constants;
        let mut p = r.clone();

        loop {
            let r_t = r.t();
            let a_p = &self * &p;
            let alpha = (&r_t * &p)[0][0] / (p.t() * &a_p)[0][0];

            let old_r = r.clone();
            x = x + p.clone() * alpha;
            r = r - a_p.clone() * alpha;

            let max_r = r.elements.iter().fold(0.0 / 0.0, |m, v| v.max(m));
            if max_r < 0.001 {
                break;
            }

            let beta = (r.t() * &r)[0][0] / (&r_t * &old_r)[0][0];
            p = r.clone() + p.clone() * beta;
        }

        Ok(x)
    }
}