Gauss algorithm finished

gauss.c

@@ -1,82 +1,146 @@
 #include <stdio.h>
+#include <math.h>
 #include <assert.h>
 #include <stdlib.h>
 
-typedef struct {
-    int ** data;
-    int size;
-} Matrix;
+#define EPS 1e-100
+#define ISZERO(e) (abs(e) < EPS)
 
-Matrix matCreate(int size) {
-    int** data = malloc(sizeof(int*) * size);
-    for(unsigned int row = 0; row < size; row++) data[row] = malloc(sizeof(int)*size);
-    return (Matrix){data,size};
+/*** GLS ***/
+typedef struct {
+    float ** data;
+    float * rs;
+    int size;
+} GLS;
+
+GLS glsCreate(int size) {
+    float** data = malloc(sizeof(float*) * size);
+    for(unsigned int row = 0; row < size; row++) data[row] = malloc(sizeof(float)*size);
+    float* rs = malloc(sizeof(float) * size);
+    return (GLS){data,rs,size};
 }
 
-void matSet(Matrix m, size_t row, size_t col, int v) {
+void glsSet(GLS m, size_t row, size_t col, float v) {
     assert(row < m.size);
     assert(col < m.size);
     m.data[row][col] = v;
 }
 
-int matGet(Matrix m, size_t row, size_t col) {
+float glsGet(GLS m, size_t row, size_t col) {
     assert(row < m.size);
     assert(col < m.size);
     return m.data[row][col];
 }
 
-void matSwapRows(Matrix m, size_t row1, size_t row2) {
+float glsGetRS(GLS m, size_t row) {
+    assert(row < m.size);
+    return m.rs[row];
+}
+
+void glsSetRS(GLS m, size_t row, float value) {
+    assert(row < m.size);
+    m.rs[row] = value;
+}
+
+void glsSwapRows(GLS m, size_t row1, size_t row2) {
     assert(row1 < m.size);
     assert(row2 < m.size);
-    int* tmp = m.data[row1];
+    float* tmp = m.data[row1];
     m.data[row1] = m.data[row2];
     m.data[row2] = tmp;
+    float tmp2 = m.rs[row1];
+    m.rs[row1] = m.rs[row2];
+    m.rs[row2] = tmp2;
 }
 
-void matPrint(Matrix m) {
+void glsTransform(GLS m, size_t row1, float fac1, size_t row2, float fac2) {
+    assert(row1 < m.size);
+    assert(row2 < m.size);
+
+    for(unsigned int col = 0; col < m.size; col++) {
+        glsSet(m,row1,col, fac1 * glsGet(m,row1,col) + fac2 * glsGet(m,row2,col) );
+    }
+
+    glsSetRS(m, row1, fac1 * glsGetRS(m, row1) + fac2 * glsGetRS(m,row2));
+}
+
+void glsPrint(GLS m) {
     for(unsigned int row = 0; row < m.size; row++) {
         for(unsigned int col = 0; col < m.size; col++) {
-            printf("%02d ", matGet(m,row,col));
+            printf("%3.1f ", glsGet(m,row,col));
         }
-        printf("\n");
+        printf("| %3.1f\n", m.rs[row]);
     }
 }
 
-void matFree(Matrix m) {
+void glsFree(GLS m) {
     for(unsigned int row = 0; row < m.size; row++) free(m.data[row]);
     free(m.data);
+    free(m.rs);
 }
 
-int reorder_mat(Matrix m, int rowCol) {
+/*** End GLS ***/
+
+/*** Gauss ***/
+int reorder_gls(GLS m, int rowCol) {
     if(rowCol >= m.size) return 1;
-    if(matGet(m,rowCol,rowCol) != 0) return reorder_mat(m,rowCol+1);
+    if(!ISZERO(glsGet(m, rowCol, rowCol))) return reorder_gls(m,rowCol+1);
 
     for(int candidate = rowCol+1; candidate < m.size; candidate++) {
-        if(matGet(m,candidate, rowCol) != 0) {
-            matSwapRows(m, rowCol, candidate);
-            if(reorder_mat(m, rowCol+1)) return 1;
-            matSwapRows(m, rowCol, candidate);
+        if(!ISZERO(glsGet(m,candidate, rowCol))) {
+            glsSwapRows(m, rowCol, candidate);
+            if(reorder_gls(m, rowCol+1)) return 1;
+            glsSwapRows(m, rowCol, candidate);
         }
     }
 
     return 0;
 }
 
+int solve_gauss_rec(GLS m, int rowCol) {
+    if(rowCol >= m.size) return 1;
+    if( ISZERO(glsGet(m,rowCol,rowCol)) ) return 0;
+
+    for(size_t row = rowCol+1; row < m.size; row++) {
+        if(ISZERO(glsGet(m,row, rowCol))) continue;
+        glsTransform(m, row, glsGet(m,rowCol,rowCol), rowCol, - glsGet(m,row,rowCol) );
+        glsSet(m,row,rowCol, 0.0f);
+    }
+
+    if(!solve_gauss_rec(m, rowCol+1)) return 0;
+
+    for(size_t row = 0; row < rowCol; row--) {
+        glsTransform(m, row, glsGet(m,rowCol,rowCol), rowCol, - glsGet(m,row,rowCol) );
+        glsSet(m,row,rowCol, 0.0f);
+    }
+
+    glsTransform(m, rowCol, 1.0f / glsGet(m,rowCol,rowCol), rowCol, 0.0f );
+    glsSet(m, rowCol, rowCol, 1.0f);
+
+    return 1;
+}
+
+int solve(GLS m) {
+    if(!reorder_gls(m,0)) return 0;
+    if(!solve_gauss_rec(m,0)) return 0;
+
+    return 1;
+}
+
 int main( int argc, char *argv[] ) {
 
-    Matrix m = matCreate(2);
-    matSet(m,0,0,0);
-    matSet(m,0,1,1);
-    matSet(m,1,0,1);
-    matSet(m,1,1,0);
+    GLS m = glsCreate(2);
+    glsSet(m,0,0,2); glsSet(m,0,1,3); glsSetRS(m,0, 13);
+    glsSet(m,1,0,1); glsSet(m,1,1,2); glsSetRS(m,1, 8);
 
-    matPrint(m);
-
-    if(!reorder_mat(m,0)) {
-        printf("Failed\n");
+    printf("Input: \n");
+    glsPrint(m);
+    if(solve(m)) {
+        printf("Output: \n");
+        glsPrint(m);
+    } else {
+        printf("Failed to solve. :(\n");
     }
 
-    matPrint(m);
-
 	return 0;
 }