:sparkles: added linear equation solver

might use for division later

src/types/matrix/generic.rs

@@ -217,6 +217,7 @@ impl<T: Numeric + Float + PartialOrd, const DIMENSION: usize> SquareMatrix<T, DI
         if !unsafe { self.lup_decompose(&mut pivot, tolerance) } {
             return None;
         }
+        debug_assert_eq!(pivot.len(), DIMENSION + 1);
 
         let mut result = self[(0, 0)];
         for i in 1..DIMENSION {
@@ -230,12 +231,44 @@ impl<T: Numeric + Float + PartialOrd, const DIMENSION: usize> SquareMatrix<T, DI
         }
     }
 
+    /// Solve the equation `self` * `x` = `b` for `x`.
+    pub fn solve(mut self, b: &ColumnVector<T, DIMENSION>, tolerance: T) -> Option<RowVector<T, DIMENSION>> {
+        let mut pivot = Vec::new();
+        if !unsafe { self.lup_decompose(&mut pivot, tolerance) } {
+            return None;
+        }
+        debug_assert_eq!(pivot.len(), DIMENSION + 1);
+
+        let mut result = RowVector::new();
+
+        for i in 0..DIMENSION {
+            result[(0, i)] = b[(pivot[i], 0)];
+
+            for k in 0..i {
+                let factor = result[(0, k)];
+                result[(0, i)] -= self[(i, k)] * factor;
+            }
+        }
+
+        for i in (0..DIMENSION).rev() {
+            for k in (i + 1)..DIMENSION {
+                let factor = result[(0, k)];
+                result[(0, i)] -= self[(i, k)] * factor;
+            }
+
+            result[(0, i)] /= self[(i, i)];
+        }
+
+        Some(result)
+    }
+
     /// Calculate the inverse of the input matrix.
     pub fn inverted(mut self, tolerance: T) -> Option<Self> {
         let mut pivot = Vec::new();
         if !unsafe { self.lup_decompose(&mut pivot, tolerance) } {
             return None;
         }
+        debug_assert_eq!(pivot.len(), DIMENSION + 1);
 
         let mut result = Self::new();