From 656041187948ebcce6d914d12397d88c82953141 Mon Sep 17 00:00:00 2001
From: Makogan <camilotalero96@hotmail.com>
Date: Tue, 15 Aug 2023 13:25:11 -0700
Subject: [PATCH] Implement reduced row echelon form function

---
 nalgebra-lapack/tests/linalg/rref.rs | 34 +++++++++++++++
 src/linalg/mod.rs                    |  1 +
 src/linalg/rref.rs                   | 64 ++++++++++++++++++++++++++++
 3 files changed, 99 insertions(+)
 create mode 100644 nalgebra-lapack/tests/linalg/rref.rs
 create mode 100644 src/linalg/rref.rs

diff --git a/nalgebra-lapack/tests/linalg/rref.rs b/nalgebra-lapack/tests/linalg/rref.rs
new file mode 100644
index 00000000..89b832fe
--- /dev/null
+++ b/nalgebra-lapack/tests/linalg/rref.rs
@@ -0,0 +1,34 @@
+use na::rref;
+
+use std::cmp;
+
+use na::{DMatrix, DVector, Matrix4x3, Vector4};
+use nl::Cholesky;
+
+use crate::proptest::*;
+use proptest::{prop_assert, proptest};
+
+proptest! {
+    #[test]
+    pub fn rref_test() {
+        let mat: Mat4 = Mat4::Identity();
+        let res = rref(&mat);
+
+        assert_eq!(mat, res);
+
+        let m = Matrix3x4::<f64>::new(
+             1.0, 2.0, -1.0, -4.0,
+             2.0, 3.0, -1.0, -11.0,
+            -2.0, 0.0, -3.0,  22.0,
+        );
+
+        let expected = Matrix3x4::<f64>::new(
+            1.0, 0.0, -1.0, -8.0,
+            0.0, 1.0,  0.0, 1.0,
+            0.0, 0.0,  1.0, -2.0,
+       );
+
+        let res = rref(&mat);
+        prop_assert!(relative_eq!(res, expected, epsilon = 1.0e-5));
+    }
+}
diff --git a/src/linalg/mod.rs b/src/linalg/mod.rs
index 0209f9b1..b801f48f 100644
--- a/src/linalg/mod.rs
+++ b/src/linalg/mod.rs
@@ -21,6 +21,7 @@ mod lu;
 mod permutation_sequence;
 mod pow;
 mod qr;
+mod rref;
 mod schur;
 mod solve;
 mod svd;
diff --git a/src/linalg/rref.rs b/src/linalg/rref.rs
new file mode 100644
index 00000000..e7b16ba1
--- /dev/null
+++ b/src/linalg/rref.rs
@@ -0,0 +1,64 @@
+//! Functions for computing the rref of a matrix.
+
+use core::ops::{Mul, Sub, SubAssign};
+use simba::scalar::{ClosedMul, RealField};
+use std::ops::{DivAssign, MulAssign};
+
+use crate::allocator::Allocator;
+use crate::base::dimension::Dim;
+use crate::base::{Const, DefaultAllocator, OMatrix, OVector, Scalar};
+
+// Implementation of:
+// https://rosettacode.org/wiki/Reduced_row_echelon_form
+/// Compute the reduced row echelon form of a matrix.
+pub fn rref<T: RealField, D: Dim>(matrix: &OMatrix<T, D, D>) -> OMatrix<T, D, D>
+where
+    DefaultAllocator: Allocator<T, D, D> + Allocator<T, D>,
+    DefaultAllocator: Allocator<T, crate::base::dimension::Const<1>, D> + Allocator<T, D>,
+    T: Scalar + ClosedMul,
+{
+    let mut matrix = matrix.clone();
+    let mut lead = 0;
+    let row_count = matrix.nrows();
+    let column_count = matrix.ncols();
+
+    for r in 0..row_count {
+        if column_count <= lead {
+            break;
+        }
+
+        let mut i = r;
+
+        // Should this have an epsilon comparison instead?
+        while matrix[(i, lead)] == crate::convert(0.0) {
+            i += 1;
+            if row_count == i {
+                i = r;
+                lead += 1;
+                if column_count == lead {
+                    break;
+                }
+            }
+        }
+
+        matrix.swap_rows(i, r);
+
+        if matrix[(r, lead)] > T::default_epsilon() {
+            let t = matrix[(r, lead)].clone();
+            matrix.row_mut(r).div_assign(t);
+        }
+
+        for i in 0..row_count {
+            if i != r {
+                let lv = -matrix[(i, lead)].clone();
+                let r_row = matrix.row(r).clone() * lv;
+
+                matrix.row_mut(i).sub_assign(r_row);
+            }
+        }
+
+        lead += 1;
+    }
+
+    matrix
+}