inverse tested

COPYING

@@ -1 +1 @@
-/usr/share/automake-1.15/COPYING
+/usr/share/automake-1.14/COPYING

INSTALL

@@ -1 +1 @@
-/usr/share/automake-1.15/INSTALL
+/usr/share/automake-1.14/INSTALL

src/matrix.hxx

@@ -9,9 +9,45 @@
 #include <cstring>
 #include <cassert>
 #include <type_traits>
+#include <limits>
+
+namespace math {
+
+  template<typename TYPE>
+      bool almostEqual(TYPE a, TYPE b) {
+    if (a<0)
+      if (b<0) {
+        a = -a;
+        b = -b;
+      } else return a==b; // different signs
+    else if (b<0) return a==b; // different signs
+    TYPE diff(abs(a-b));
+    a = abs(a);
+    b = abs(b);
+    TYPE max(a>b?a:b);
+    if (max<1) return diff<=std::numeric_limits<TYPE>::epsilon();
+    return diff<=max*std::numeric_limits<TYPE>::epsilon();
+  }
+
+  template<typename TYPE>
+      bool equal(const TYPE& a, const TYPE& b) {
+    return a==b;
+  }
+
+  template<>
+      bool equal(const double& a, const double& b) {
+    return almostEqual(a, b);
+  }
+
+  template<>
+      bool equal(const float& a, const float& b) {
+    return almostEqual(a, b);
+  }
+
+}
 
 template<typename TYPE, typename ARRAY=TYPE*> class MatrixBase {
-    
+
     //..............................................................constants
   public:
 
@@ -21,7 +57,7 @@ template<typename TYPE, typename ARRAY=TYPE*> class MatrixBase {
     const size_t MEM_SIZE;
 
     //...............................................................typedefs
-  public: 
+  public:
 
     /// @name Auxiliary Classes
     ///{
@@ -61,7 +97,7 @@ template<typename TYPE, typename ARRAY=TYPE*> class MatrixBase {
         const MatrixBase& _m;
         const TYPE *_v;
     };
-    
+
     ///}
 
     //................................................................methods
@@ -69,7 +105,7 @@ template<typename TYPE, typename ARRAY=TYPE*> class MatrixBase {
 
     /// @name construction
     ///{
-    
+
     MatrixBase(size_t rows, size_t columns):
         ROWS(rows), COLUMNS(columns),
         SIZE(ROWS*COLUMNS), MEM_SIZE(SIZE*sizeof(TYPE)) {
@@ -86,48 +122,48 @@ template<typename TYPE, typename ARRAY=TYPE*> class MatrixBase {
 
     /// @name element access
     ///{
-    
+
     TYPE& at(size_t row, size_t column) {
       assert(row<ROWS);
       assert(column<COLUMNS);
       return *((TYPE*)_c+row*COLUMNS+column);
     }
-    
+
     const TYPE& at(size_t row, size_t column) const {
       assert(row<ROWS);
       assert(column<COLUMNS);
       return *((TYPE*)_c+row*COLUMNS+column);
     }
-    
+
     TYPE& operator()(size_t row, size_t column) {
       return at(row, column);
     }
-    
+
     const TYPE& operator()(size_t row, size_t column) const {
       return at(row, column);
     }
-    
+
     RowVector operator[](size_t row) {
       assert(row<ROWS);
       return RowVector(*this, (TYPE*)_c+row*COLUMNS);
     }
-    
+
     const ConstRowVector operator[](size_t row) const {
       assert(row<ROWS);
       return ConstRowVector(*this, (TYPE*)_c+row*COLUMNS);
     }
-    
+
     ///}
 
     /// @name operators
     ///{
-    
+
     MatrixBase& operator=(TYPE oc[]) {
       assert(sizeof(oc)==MEM_SIZE);
       memcpy(_c, oc, MEM_SIZE);
       return *this;
     }
-    
+
     MatrixBase& operator=(const MatrixBase& o) {
       assert_check(o);
       memcpy(_c, o._c, MEM_SIZE);
@@ -137,7 +173,7 @@ template<typename TYPE, typename ARRAY=TYPE*> class MatrixBase {
     bool operator==(const MatrixBase& o) const {
       if (!check(o)) return false;
       TYPE *to((TYPE*)(_c)+SIZE), *from((TYPE*)(o._c)+SIZE);
-      while (to>(TYPE*)(_c)) if (*--to != *--from) return false;
+      while (to>(TYPE*)(_c)) if (!math::equal(*--to, *--from)) return false;
       return true;
     }
 
@@ -176,6 +212,13 @@ template<typename TYPE, typename ARRAY=TYPE*> class MatrixBase {
     /// @name special operations
     ///{
 
+    bool similar(const MatrixBase& o, const TYPE& diff) const {
+      if (!check(o)) return false;
+      TYPE *to((TYPE*)(_c)+SIZE), *from((TYPE*)(o._c)+SIZE);
+      while (to>(TYPE*)(_c)) if (abs(*--to - *--from)>diff) return false;
+      return true;
+    }
+
     TYPE det() {
       TYPE res(gauss());
       for (TYPE *p((TYPE*)(_c)+SIZE); --p>=(TYPE*)(_c); p-=COLUMNS) res *= *p;
@@ -204,7 +247,7 @@ template<typename TYPE, typename ARRAY=TYPE*> class MatrixBase {
     }
 
     ///}
-    
+
     //................................................................methods
   protected:
 
@@ -215,7 +258,7 @@ template<typename TYPE, typename ARRAY=TYPE*> class MatrixBase {
 
     //..............................................................variables
   protected:
-    
+
     ARRAY _c;
 
 };
@@ -235,7 +278,7 @@ template<typename TYPE, size_t TROWS=0, size_t TCOLUMNS=0> class Matrix:
 
     /// @name construction
     ///{
-    
+
     Matrix(): Parent(TROWS, TCOLUMNS) {
       memset(Parent::_c, 0, Parent::MEM_SIZE);
     }
@@ -253,12 +296,12 @@ template<typename TYPE, size_t TROWS=0, size_t TCOLUMNS=0> class Matrix:
 
     /// @name operators
     ///{
-    
+
     Matrix& operator=(TYPE oc[TROWS][TCOLUMNS]) {
       memcpy(Parent::_c, oc, Parent::MEM_SIZE);
       return *this;
     }
-    
+
     Matrix operator-() const {
       Matrix res;
       for (TYPE *to((TYPE*)(res._c)+Parent::SIZE); to>(TYPE*)(Parent::_c); --to)
@@ -278,7 +321,7 @@ template<typename TYPE, size_t TROWS=0, size_t TCOLUMNS=0> class Matrix:
     }
 
     ///}
-                    
+
     /// @name special operations
     ///{
 
@@ -302,27 +345,45 @@ template<typename TYPE, size_t TROWS=0, size_t TCOLUMNS=0> class Matrix:
       Matrix o(*this); // left side
       *this = i(); // right side
       /// 1. lower left part
-      for (size_t row(0); row<this->ROWS; ++row)
+      for (size_t column(0); column<this->COLUMNS; ++column) {
-        if (rows<this->COLUMNS) {
+        if (column<this->ROWS) {
-          /// 2. normalize first line to first value
+          /// 2. normalize pivot to one
-          TYPE pivot(o(row, row));
+          TYPE pivot(o(column, column));
           if (pivot!=1) {
-            o(row, row) = 1;
+            o(column, column) = 1;
-            for (size_t column(row+1); column<this->COLUMNS; ++column) o(row, column)/=pivot;
+            for (size_t pos(column+1); pos<this->COLUMNS; ++pos)
-            for (size_t column(row); column<this->COLUMNS; ++column) this->at(row, column)/=pivot;
+              o(column, pos)/=pivot;
+            for (size_t pos(0); pos<this->COLUMNS; ++pos)
+              this->at(column, pos)/=pivot;
           }
           /// 3. nullify lower triangle
           for (size_t row(column+1); row<this->ROWS; ++row) {
             TYPE pivot(o(row, column));
             if (pivot!=0) {
               o(row, column) = 0;
-              for (size_t pos(column+1); pos<this->COLUMNS; ++pos) o(row, pos) -= pivot*o(0, pos);
+              for (size_t pos(column+1); pos<this->COLUMNS; ++pos)
-              for (size_t pos(column); pos<this->COLUMNS; ++pos) this->at(row, pos) -= pivot*this->at(0, pos);
+                o(row, pos) -= pivot*o(column, pos);
+              for (size_t pos(0); pos<this->COLUMNS; ++pos)
+                this->at(row, pos) -= pivot*this->at(column, pos);
             }
           }
         }
+      }
       /// 4. nullify the upper triangle
-      
+      const size_t LASTCOL(this->COLUMNS-1);
+      const size_t LASTROW(this->ROWS-1);
+      for (size_t column(1); column<this->COLUMNS; ++column) {
+        for (size_t row(0); row<column && row<LASTROW; ++row) {
+          TYPE pivot(o(row, column));
+          if (pivot!=0) {
+            o(row, column) = 0;
+            for (size_t pos(column+1); pos<this->COLUMNS; ++pos)
+              o(row, pos) -= pivot*o(column, pos);
+            for (size_t pos(0); pos<this->COLUMNS; ++pos)
+              this->at(row, pos) -= pivot*this->at(column, pos);
+          }
+        }
+      }
       return *this;
     }
 
@@ -340,12 +401,12 @@ template<typename TYPE> class Matrix<TYPE, 0, 0>: public MatrixBase<TYPE> {
 
     //................................................................methods
   public:
-    
+
     /// @name construction
     ///{
 
     Matrix() = delete;
-    
+
     Matrix(size_t rows, size_t columns):
         Parent(rows, columns) {
       assert(rows>0);
@@ -357,7 +418,7 @@ template<typename TYPE> class Matrix<TYPE, 0, 0>: public MatrixBase<TYPE> {
     Matrix(const Matrix& o): Matrix(o.ROWS, o.Parent::COLUMNS) {
       memcpy(Parent::_c, o.Parent::_c, Parent::MEM_SIZE);
     }
-    
+
     template<typename ...ARGS>
         Matrix(size_t rows, size_t columns, ARGS...t):
             Matrix(rows, columns) {
@@ -373,24 +434,12 @@ template<typename TYPE> class Matrix<TYPE, 0, 0>: public MatrixBase<TYPE> {
     virtual ~Matrix() {
       delete[] Parent::_c;
     }
-    
+
     ///}
-    
+
     /// @name operators
     ///{
 
-    Matrix& operator=(const Matrix& o) {
-      if (o.ROWS!=Parent::ROWS&&o.COLUMNS!=Parent::COLUMNS) {
-        delete[] Parent::_c;
-        Parent::ROWS = o.ROWS;
-        Parent::COLUMNS = o.COLUMNS;
-        Parent::SIZE = o.SIZE;
-        Parent::MEM_SIZE = o.MEM_SIZE;
-        Parent::_c = new TYPE[Parent::SIZE];
-      }
-      return Parent::operator=(o);
-    }
-
     Matrix operator-() const {
       Matrix res(Parent::COLUMNS, Parent::ROWS);
       for (TYPE *to((TYPE*)(res._c)+Parent::SIZE); to>(TYPE*)(Parent::_c); --to)
@@ -412,7 +461,7 @@ template<typename TYPE> class Matrix<TYPE, 0, 0>: public MatrixBase<TYPE> {
 
     ///@name special operations
     ///{
-    
+
     Matrix t() const {
       Matrix res(this->COLUMNS, this->ROWS);
       for (size_t row(0); row<this->ROWS; ++row)
@@ -428,6 +477,54 @@ template<typename TYPE> class Matrix<TYPE, 0, 0>: public MatrixBase<TYPE> {
       return res;
     }
 
+    Matrix& inv() {
+      /// calculate using gauss-jordan algorithmus
+      /// @see http://www.mathebibel.de/inverse-matrix-berechnen-nach-gauss-jordan
+      Matrix o(*this); // left side
+      *this = i(); // right side
+      /// 1. lower left part
+      for (size_t column(0); column<this->COLUMNS; ++column) {
+        if (column<this->ROWS) {
+          /// 2. normalize pivot to one
+          TYPE pivot(o(column, column));
+          if (pivot!=1) {
+            o(column, column) = 1;
+            for (size_t pos(column+1); pos<this->COLUMNS; ++pos)
+              o(column, pos)/=pivot;
+            for (size_t pos(0); pos<this->COLUMNS; ++pos)
+              this->at(column, pos)/=pivot;
+          }
+          /// 3. nullify lower triangle
+          for (size_t row(column+1); row<this->ROWS; ++row) {
+            TYPE pivot(o(row, column));
+            if (pivot!=0) {
+              o(row, column) = 0;
+              for (size_t pos(column+1); pos<this->COLUMNS; ++pos)
+                o(row, pos) -= pivot*o(column, pos);
+              for (size_t pos(0); pos<this->COLUMNS; ++pos)
+                this->at(row, pos) -= pivot*this->at(column, pos);
+            }
+          }
+        }
+      }
+      /// 4. nullify the upper triangle
+      const size_t LASTCOL(this->COLUMNS-1);
+      const size_t LASTROW(this->ROWS-1);
+      for (size_t column(1); column<this->COLUMNS; ++column) {
+        for (size_t row(0); row<column && row<LASTROW; ++row) {
+          TYPE pivot(o(row, column));
+          if (pivot!=0) {
+            o(row, column) = 0;
+            for (size_t pos(column+1); pos<this->COLUMNS; ++pos)
+              o(row, pos) -= pivot*o(column, pos);
+            for (size_t pos(0); pos<this->COLUMNS; ++pos)
+              this->at(row, pos) -= pivot*this->at(column, pos);
+          }
+        }
+      }
+      return *this;
+    }
+
     ///}
 
     //................................................................methods
@@ -494,4 +591,3 @@ template<typename TYPE, size_t ROWS, size_t COLUMNS>
     s<<'\n';
   }
 }
-

test/basic.cxx

@@ -204,17 +204,41 @@ class TemplateMatrixTest: public CppUnit::TestFixture {
     }
     template<typename T>
         void inv() {
-      Matrix<T,3,3> m(2, -1, 0,
+      {
-                      1, 2, -2,
+        Matrix<T,3,3> m(2, -1, 0,
-                      0, -1, 1);
+                        1, 2, -2,
-      const Matrix<T,3,3> res(0.5, 0, 0,
+                        0, -1, 1);
-                              -0.2, 0.4, 0,
+        const Matrix<T,3,3> res(0, 1, 2,
-                              -1, 2, 5);
+                                -1, 2, 4,
-      // const Matrix<T,3,3> res(0, 1, 2,
+                                -1, 2, 5);
-      //                         -1, 2, 4,
+        m.inv();
-      //                         -1, 2, 5);
+        CPPUNIT_ASSERT(m.similar(res, 0.0001));
-      m.inv();
+      } {
-      CPPUNIT_ASSERT_EQUAL(res, m);
+        Matrix<T,3,3> m(1, 2, 3,
+                        0, 1, 4,
+                        5, 6, 0);
+        const Matrix<T,3,3> res(-24, 18, 5,
+                                20, -15, -4,
+                                -5, 4, 1);
+        m.inv();
+        CPPUNIT_ASSERT_EQUAL(res, m);
+      } {
+        Matrix<T,3,3> m(1, 2, 3,
+                        0, 4, 5,
+                        1, 0, 6);
+        const Matrix<T,3,3> res((T)12/11, (T)-6/11, (T)-1/11,
+                                (T)5/22, (T)3/22, (T)-5/22,
+                                (T)-2/11, (T)1/11, (T)2/11);
+        m.inv();
+        CPPUNIT_ASSERT_EQUAL(res, m);
+      } {
+        Matrix<T,2,2> m(4, 3,
+                        3, 2);
+        const Matrix<T,2,2> res(-2, 3,
+                                3, -4);
+        m.inv();
+        CPPUNIT_ASSERT_EQUAL(res, m);
+      }
     }
     CPPUNIT_TEST_SUITE(TemplateMatrixTest);
     CPPUNIT_TEST(initFromArray1<int>);
@@ -502,6 +526,32 @@ class VariableMatrixTest: public CppUnit::TestFixture {
       CPPUNIT_ASSERT_EQUAL(m2, m2.i());
       CPPUNIT_ASSERT_EQUAL(m3, m3.i());
     }
+    template<typename T>
+        void inv() {
+      {
+        Matrix<T> m(3, 3,
+                    2, -1, 0,
+                    1, 2, -2,
+                    0, -1, 1);
+        const Matrix<T> res(3, 3,
+                            0, 1, 2,
+                            -1, 2, 4,
+                            -1, 2, 5);
+        m.inv();
+        CPPUNIT_ASSERT(m.similar(res, 0.0001));
+      } {
+        Matrix<T> m(3, 3,
+                    1, 2, 3,
+                    0, 1, 4,
+                    5, 6, 0);
+        const Matrix<T> res(3, 3,
+                            -24, 18, 5,
+                            20, -15, -4,
+                            -5, 4, 1);
+        m.inv();
+        CPPUNIT_ASSERT_EQUAL(res, m);
+      }
+    }
     CPPUNIT_TEST_SUITE(VariableMatrixTest);
     CPPUNIT_TEST(initFromArray1<int>);
     CPPUNIT_TEST(initFromArray1<long>);
@@ -583,6 +633,8 @@ class VariableMatrixTest: public CppUnit::TestFixture {
     CPPUNIT_TEST(i<unsigned long>);
     CPPUNIT_TEST(i<float>);
     CPPUNIT_TEST(i<double>);
+    CPPUNIT_TEST(inv<float>);
+    CPPUNIT_TEST(inv<double>);
     CPPUNIT_TEST_SUITE_END();
 };
 CPPUNIT_TEST_SUITE_REGISTRATION(VariableMatrixTest);