From 4146385e09fc95833ae6fada41dbb88039f75f9a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= <developer@crozet.re>
Date: Fri, 31 May 2013 18:35:48 +0200
Subject: [PATCH] Fix matrix inversion algorithm.

---
 src/ndim/dmat.rs | 56 ++++++++++++++++++++++++++++--------------------
 src/ndim/nmat.rs |  5 ++---
 src/tests/mat.rs |  5 ++++-
 3 files changed, 39 insertions(+), 27 deletions(-)

diff --git a/src/ndim/dmat.rs b/src/ndim/dmat.rs
index aee81c94..c5083bbc 100644
--- a/src/ndim/dmat.rs
+++ b/src/ndim/dmat.rs
@@ -2,6 +2,7 @@ use core::num::{One, Zero};
 use core::vec::{from_elem, swap, all, all2, len};
 use core::cmp::ApproxEq;
 use traits::inv::Inv;
+use traits::division_ring::DivisionRing;
 use traits::transpose::Transpose;
 use traits::workarounds::rlmul::{RMul, LMul};
 use ndim::dvec::{DVec, zero_vec_with_dim};
@@ -41,12 +42,19 @@ impl<T: Copy> DMat<T>
     assert!(j < self.dim);
     self.mij[self.offset(i, j)] = *t
   }
+
+  pub fn at(&self, i: uint, j: uint) -> T
+  {
+    assert!(i < self.dim);
+    assert!(j < self.dim);
+    self.mij[self.offset(i, j)]
+  }
 }
 
 impl<T: Copy> Index<(uint, uint), T> for DMat<T>
 {
   fn index(&self, &(i, j): &(uint, uint)) -> T
-  { self.mij[self.offset(i, j)] }
+  { self.at(i, j) }
 }
 
 impl<T: Copy + Mul<T, T> + Add<T, T> + Zero>
@@ -63,10 +71,10 @@ Mul<DMat<T>, DMat<T>> for DMat<T>
     {
       for uint::range(0u, dim) |j|
       {
-        let mut acc: T = Zero::zero();
+        let mut acc = Zero::zero::<T>();
 
         for uint::range(0u, dim) |k|
-        { acc += self[(i, k)] * other[(k, j)]; }
+        { acc += self.at(i, k) * other.at(k, j); }
 
         res.set(i, j, &acc);
       }
@@ -89,7 +97,7 @@ RMul<DVec<T>> for DMat<T>
     for uint::range(0u, dim) |i|
     {
       for uint::range(0u, dim) |j|
-      { res.at[i] = res.at[i] + other.at[j] * self[(i, j)]; }
+      { res.at[i] = res.at[i] + other.at[j] * self.at(i, j); }
     }
 
     res
@@ -109,15 +117,14 @@ LMul<DVec<T>> for DMat<T>
     for uint::range(0u, dim) |i|
     {
       for uint::range(0u, dim) |j|
-      { res.at[i] = res.at[i] + other.at[j] * self[(j, i)]; }
+      { res.at[i] = res.at[i] + other.at[j] * self.at(j, i); }
     }
 
     res
   }
 }
 
-impl<T: Clone + Copy + Eq + One + Zero +
-        Mul<T, T> + Div<T, T> + Sub<T, T> + Neg<T>>
+impl<T: Clone + Copy + Eq + DivisionRing>
 Inv for DMat<T>
 {
   fn inverse(&self) -> DMat<T>
@@ -142,14 +149,11 @@ Inv for DMat<T>
       // FIXME: would it be worth it to spend some more time searching for the
       // max instead?
 
-      // FIXME: this is kind of uggly…
-      // … but we cannot use position_between since we are iterating on one
-      // columns
-      let mut n0 = 0u; // index of a non-zero entry
+      let mut n0 = k; // index of a non-zero entry
 
       while (n0 != dim)
       {
-        if (self[(n0, k)] != _0T)
+        if (self.at(n0, k) != _0T)
         { break; }
 
         n0 += 1;
@@ -170,16 +174,17 @@ Inv for DMat<T>
         }
       }
 
-      let pivot = self[(k, k)];
+      let pivot = self.at(k, k);
 
       for uint::range(k, dim) |j|
       {
-        // FIXME: not to putting selfal exression directly on the nuction call
-        // is uggly but does not seem to compile any more…
-        let selfval = &(self[(k, j)] / pivot);
-        let resval  = &(res[(k, j)] / pivot);
-
+        let selfval = &(self.at(k, j) / pivot);
         self.set(k, j, selfval);
+      }
+
+      for uint::range(0u, dim) |j|
+      {
+        let resval  = &(res.at(k, j)   / pivot);
         res.set(k, j, resval);
       }
 
@@ -187,19 +192,24 @@ Inv for DMat<T>
       {
         if (l != k)
         {
-          let normalizer = self[(l, k)] / pivot;
+          let normalizer = self.at(l, k);
 
           for uint::range(k, dim) |j|
           {
-            let selfval = &(self[(l, j)] - self[(k, j)] * normalizer);
-            let resval  = &(res[(l, j)] - res[(k, j)] * normalizer);
+            let selfval = &(self.at(l, j) - self.at(k, j) * normalizer);
+            self.set(l, j, selfval);
+          }
 
-            self.set(k, j, selfval);
-            res.set(k, j, resval);
+          for uint::range(0u, dim) |j|
+          {
+            let resval  = &(res.at(l, j)   - res.at(k, j)   * normalizer);
+            res.set(l, j, resval);
           }
         }
       }
     }
+
+    *self = res;
   }
 }
 
diff --git a/src/ndim/nmat.rs b/src/ndim/nmat.rs
index 08c475eb..083e73bf 100644
--- a/src/ndim/nmat.rs
+++ b/src/ndim/nmat.rs
@@ -3,6 +3,7 @@ use core::rand::{Rand, Rng, RngUtil};
 use core::cmp::ApproxEq;
 use traits::dim::Dim;
 use traits::inv::Inv;
+use traits::division_ring::DivisionRing;
 use traits::transpose::Transpose;
 use traits::workarounds::rlmul::{RMul, LMul};
 use ndim::dmat::{DMat, one_mat_with_dim, zero_mat_with_dim, is_zero_mat};
@@ -113,9 +114,7 @@ LMul<NVec<D, T>> for NMat<D, T>
   }
 }
 
-impl<D: Dim,
-     T: Clone + Copy + Eq + One + Zero +
-        Mul<T, T> + Div<T, T> + Sub<T, T> + Neg<T>>
+impl<D: Dim, T: Clone + Copy + Eq + DivisionRing>
 Inv for NMat<D, T>
 {
   fn inverse(&self) -> NMat<D, T>
diff --git a/src/tests/mat.rs b/src/tests/mat.rs
index 8ddb4ae0..1e4fc21f 100644
--- a/src/tests/mat.rs
+++ b/src/tests/mat.rs
@@ -8,6 +8,8 @@ use core::cmp::ApproxEq;
 use traits::inv::Inv;
 #[test]
 use traits::rotation::Rotation;
+// #[test]
+// use traits::dim::d7;
 #[test]
 use dim1::vec1::vec1;
 #[test]
@@ -16,6 +18,8 @@ use dim1::mat1::Mat1;
 use dim2::mat2::Mat2;
 #[test]
 use dim3::mat3::Mat3;
+// #[test]
+// use ndim::nmat::NMat;
 #[test]
 use adaptors::rotmat::Rotmat;
 
@@ -42,7 +46,6 @@ fn test_inv_mat2()
 fn test_inv_mat3()
 { test_inv_mat_impl!(Mat3<f64>); }
 
-// FIXME: this one fails with an ICE: node_id_to_type: no type for node [...]
 // #[test]
 // fn test_inv_nmat()
 // { test_inv_mat_impl!(NMat<d7, f64>); }