From c83b1f14851d9075d0f91686d2a042133d521ce5 Mon Sep 17 00:00:00 2001
From: felipez <tifonzafel@gmail.com>
Date: Sun, 16 Feb 2020 10:08:07 +0100
Subject: [PATCH] first version of mgs #8

---
 src/modified_gram_schmidt.rs | 32 ++++++++++++++++++++++++++------
 1 file changed, 26 insertions(+), 6 deletions(-)

diff --git a/src/modified_gram_schmidt.rs b/src/modified_gram_schmidt.rs
index 853e0c3..4d3489d 100644
--- a/src/modified_gram_schmidt.rs
+++ b/src/modified_gram_schmidt.rs
@@ -9,7 +9,7 @@ on computers.
  */
 
 extern crate nalgebra as na;
-use na::DMatrix;
+use na::{DMatrix, DVector, DVectorSlice};
 
 pub struct MGS {
     pub basis: DMatrix<f64>,
@@ -19,15 +19,36 @@ impl MGS {
     /// The new static method takes a single argument:
     /// * `vectors` to diagonalize as columns of the matrix
     pub fn new(vectors: DMatrix<f64>) -> Result<Self, &'static str> {
-        let mut result = Err("Something when wrong!");
-        result
+        if !vectors.is_square() {
+            return Err("vectors must be an square matrix");
+        }
+        let dim = vectors.nrows();
+        let mut basis = DMatrix::<f64>::zeros(dim, dim);
+        // first vector of the basis
+        let first = vectors.column(0) / vectors.column(0).norm();
+        basis.set_column(0, &first);
+        // Iterate over the rest of the columns
+        for i in 1..basis.ncols() {
+            basis.set_column(i, &vectors.column(i));
+            for j in 0..i {
+                let proj = MGS::project(&basis.column(j), &basis.column(i));
+                basis.set_column(i, &(basis.column(i) - proj));
+            }
+            basis.set_column(i, &basis.column(i).normalize());
+        }
+        Ok(MGS { basis })
+    }
+
+    // Project
+    fn project(v1: &DVectorSlice<f64>, v2: &DVectorSlice<f64>) -> DVector<f64> {
+        let magnitud = v1.dot(&v2) / v1.dot(&v1);
+        return v1 * magnitud;
     }
 }
 
 #[cfg(test)]
 mod test {
     extern crate nalgebra as na;
-    use approx::relative_eq;
     use na::DMatrix;
 
     #[test]
@@ -41,7 +62,6 @@ mod test {
         };
 
         let result = basis.transpose() * &basis;
-        let diag = result.sum();
-        assert!(relative_eq!(diag, dim as f64, epsilon = 1e-8));
+        assert!(result.is_identity(1e-8));
     }
 }