Changeset 18148

Timestamp:

06/12/14 14:38:13 (11 years ago)

Author:

Mathieu Morlighem

Message:

NEW: implemeted matrix inversion

Location:

issm/trunk-jpl/src/c/shared/Matrix

Files:

• MatrixUtils.cpp (modified) (5 diffs)
• matrix.h (modified) (1 diff)

issm/trunk-jpl/src/c/shared/Matrix/MatrixUtils.cpp

@@ -3 +3 @@
  */ 
 
+#include "../Elements/elements.h"
 /*Headers*/
 /*{{{*/
@@ -309 +310 @@
         return noerr;
 }/*}}}*/
+
 void Matrix2x2Determinant(IssmDouble* Adet,IssmDouble* A){/*{{{*/
         /*Compute determinant of a 2x2 matrix*/
@@ -332 +334 @@
 
 }/*}}}*/
+
 void Matrix3x3Determinant(IssmDouble* Adet,IssmDouble* A){/*{{{*/
         /*Compute determinant of a 3x3 matrix*/
@@ -337 +340 @@
         /*det = a*(e*i-f*h)-b*(d*i-f*g)+c*(d*h-e*g)*/
         *Adet= A[0]*A[4]*A[8]-A[0]*A[5]*A[7]-A[3]*A[1]*A[8]+A[3]*A[2]*A[7]+A[6]*A[1]*A[5]-A[6]*A[2]*A[4];
+}
+/*}}}*/
+IssmDouble Matrix3x3Determinant(IssmDouble a1,IssmDouble a2,IssmDouble a3, IssmDouble b1,IssmDouble b2,IssmDouble b3, IssmDouble c1,IssmDouble c2,IssmDouble c3){/*{{{*/
+        /*Compute determinant of a 3x3 matrix*/
+
+        /*det = a*(e*i-f*h)-b*(d*i-f*g)+c*(d*h-e*g)
+         * a b c   a1 a2 a3
+         * d e f   b1 b2 b3
+         * g h i   c1 c2 c3 */
+        return a1*b2*c3-a1*b3*c2-b1*a2*c3+b1*a3*c2+c1*a2*b3-c1*a3*b2;
 }
 /*}}}*/
@@ -368 +381 @@
 
 }/*}}}*/
+
+void Matrix4x4Determinant(IssmDouble* Adet,IssmDouble* A){/*{{{*/
+        /*Compute determinant of a 4x4 matrix*/
+
+        IssmDouble a1 = A[0*4+0];
+        IssmDouble b1 = A[0*4+1]; 
+        IssmDouble c1 = A[0*4+2];
+        IssmDouble d1 = A[0*4+3];
+
+        IssmDouble a2 = A[1*4+0];
+        IssmDouble b2 = A[1*4+1]; 
+        IssmDouble c2 = A[1*4+2];
+        IssmDouble d2 = A[1*4+3];
+
+        IssmDouble a3 = A[2*4+0]; 
+        IssmDouble b3 = A[2*4+1];
+        IssmDouble c3 = A[2*4+2];
+        IssmDouble d3 = A[2*4+3];
+
+        IssmDouble a4 = A[3*4+0];
+        IssmDouble b4 = A[3*4+1]; 
+        IssmDouble c4 = A[3*4+2];
+        IssmDouble d4 = A[3*4+3];
+
+        *Adet= a1 * Matrix3x3Determinant(b2, b3, b4, c2, c3, c4, d2, d3, d4)
+                  - b1 * Matrix3x3Determinant(a2, a3, a4, c2, c3, c4, d2, d3, d4)
+                  + c1 * Matrix3x3Determinant(a2, a3, a4, b2, b3, b4, d2, d3, d4)
+                  - d1 * Matrix3x3Determinant(a2, a3, a4, b2, b3, b4, c2, c3, c4);
+}
+/*}}}*/
+void Matrix4x4Adjoint(IssmDouble* Aadj,IssmDouble* A){/*{{{*/
+
+    IssmDouble a1 = A[0*4+0];
+    IssmDouble b1 = A[0*4+1]; 
+    IssmDouble c1 = A[0*4+2];
+    IssmDouble d1 = A[0*4+3];
+
+    IssmDouble a2 = A[1*4+0];
+    IssmDouble b2 = A[1*4+1]; 
+    IssmDouble c2 = A[1*4+2];
+    IssmDouble d2 = A[1*4+3];
+
+    IssmDouble a3 = A[2*4+0];
+    IssmDouble b3 = A[2*4+1];
+    IssmDouble c3 = A[2*4+2];
+    IssmDouble d3 = A[2*4+3];
+
+    IssmDouble a4 = A[3*4+0];
+    IssmDouble b4 = A[3*4+1]; 
+    IssmDouble c4 = A[3*4+2];
+    IssmDouble d4 = A[3*4+3];
+
+    /* Row column labeling reversed since we transpose rows & columns*/
+    Aadj[0*4+0]  =   Matrix3x3Determinant(b2, b3, b4, c2, c3, c4, d2, d3, d4);
+    Aadj[1*4+0]  = - Matrix3x3Determinant(a2, a3, a4, c2, c3, c4, d2, d3, d4);
+    Aadj[2*4+0]  =   Matrix3x3Determinant(a2, a3, a4, b2, b3, b4, d2, d3, d4);
+    Aadj[3*4+0]  = - Matrix3x3Determinant(a2, a3, a4, b2, b3, b4, c2, c3, c4);
+
+    Aadj[0*4+1]  = - Matrix3x3Determinant(b1, b3, b4, c1, c3, c4, d1, d3, d4);
+    Aadj[1*4+1]  =   Matrix3x3Determinant(a1, a3, a4, c1, c3, c4, d1, d3, d4);
+    Aadj[2*4+1]  = - Matrix3x3Determinant(a1, a3, a4, b1, b3, b4, d1, d3, d4);
+    Aadj[3*4+1]  =   Matrix3x3Determinant(a1, a3, a4, b1, b3, b4, c1, c3, c4);
+
+    Aadj[0*4+2]  =   Matrix3x3Determinant(b1, b2, b4, c1, c2, c4, d1, d2, d4);
+    Aadj[1*4+2]  = - Matrix3x3Determinant(a1, a2, a4, c1, c2, c4, d1, d2, d4);
+    Aadj[2*4+2]  =   Matrix3x3Determinant(a1, a2, a4, b1, b2, b4, d1, d2, d4);
+    Aadj[3*4+2]  = - Matrix3x3Determinant(a1, a2, a4, b1, b2, b4, c1, c2, c4);
+
+    Aadj[0*4+3]  = - Matrix3x3Determinant(b1, b2, b3, c1, c2, c3, d1, d2, d3);
+    Aadj[1*4+3]  =   Matrix3x3Determinant(a1, a2, a3, c1, c2, c3, d1, d2, d3);
+    Aadj[2*4+3]  = - Matrix3x3Determinant(a1, a2, a3, b1, b2, b3, d1, d2, d3);
+    Aadj[3*4+3]  =   Matrix3x3Determinant(a1, a2, a3, b1, b2, b3, c1, c2, c3);
+}/*}}}*/
+void Matrix4x4Invert(IssmDouble* Ainv,IssmDouble* A){/*{{{*/
+
+        /*Intermediaries*/
+        IssmDouble det;
+
+        /*Compute determinant*/
+        Matrix4x4Determinant(&det,A);
+        if(fabs(det) < DBL_EPSILON){
+                printarray(A,4,4);
+                _error_("Determinant smaller than machine epsilon");
+        }
+
+        /*Compute adjoint matrix*/
+        Matrix4x4Adjoint(Ainv,A);
+
+        /*Scalte adjoint matrix to get inverse*/
+        for(int i=0;i<4*4;i++) Ainv[i] = Ainv[i]/det;
+}/*}}}*/
 void Matrix4x4Solve(IssmDouble* X,IssmDouble* A,IssmDouble *B){/*{{{*/
-        _error_("STOP");
-}/*}}}*/
+        IssmDouble Ainv[4][4];
+
+        Matrix4x4Invert(&Ainv[0][0],A);
+        for(int i=0;i<4;i++) X[i]=Ainv[i][0]*B[0] + Ainv[i][1]*B[1] + Ainv[i][2]*B[2] + Ainv[i][3]*B[3];
+}/*}}}*/

issm/trunk-jpl/src/c/shared/Matrix/matrix.h

@@ -11 +11 @@
 int  MatrixMultiply( IssmDouble* a, int nrowa, int ncola, int itrna, IssmDouble* b, int nrowb, int ncolb, int itrnb, IssmDouble* c, int iaddc );
 int  MatrixInverse( IssmDouble* a, int ndim, int nrow, IssmDouble* b, int nvec, IssmDouble* pdet );
+
 void Matrix2x2Invert(IssmDouble* Ainv, IssmDouble* A);
 void Matrix2x2Determinant(IssmDouble* Adet,IssmDouble* A);
+
 void Matrix3x3Invert(IssmDouble* Ainv, IssmDouble* A);
 void Matrix3x3Determinant(IssmDouble* Adet,IssmDouble* A);
+IssmDouble Matrix3x3Determinant( IssmDouble a1,IssmDouble a2,IssmDouble a3, IssmDouble b1,IssmDouble b2,IssmDouble b3, IssmDouble c1,IssmDouble c2,IssmDouble c3);
 void Matrix3x3Solve(IssmDouble* X,IssmDouble* A,IssmDouble* B);
+
+void Matrix4x4Adjoint(IssmDouble* Aadj, IssmDouble* A);
+void Matrix4x4Invert(IssmDouble* Ainv, IssmDouble* A);
+void Matrix4x4Determinant(IssmDouble* Adet,IssmDouble* A);
 void Matrix4x4Solve(IssmDouble* X,IssmDouble* A,IssmDouble* B);