149 lines
3.7 KiB
C
149 lines
3.7 KiB
C
#include <stdio.h>
|
|
#include <math.h>
|
|
#include <assert.h>
|
|
#include <stdlib.h>
|
|
|
|
#define EPS 1e-100
|
|
#define ISZERO(e) (abs(e) < EPS)
|
|
|
|
/*** 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 glsSet(GLS m, size_t row, size_t col, float v) {
|
|
assert(row < m.size);
|
|
assert(col < m.size);
|
|
m.data[row][col] = v;
|
|
}
|
|
|
|
float glsGet(GLS m, size_t row, size_t col) {
|
|
assert(row < m.size);
|
|
assert(col < m.size);
|
|
return m.data[row][col];
|
|
}
|
|
|
|
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);
|
|
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 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("%3.1f ", glsGet(m,row,col));
|
|
}
|
|
printf("| %3.1f\n", m.rs[row]);
|
|
}
|
|
}
|
|
|
|
void glsFree(GLS m) {
|
|
for(unsigned int row = 0; row < m.size; row++) free(m.data[row]);
|
|
free(m.data);
|
|
free(m.rs);
|
|
}
|
|
|
|
/*** End GLS ***/
|
|
|
|
/*** Gauss ***/
|
|
int reorder_gls(GLS m, int rowCol) {
|
|
if(rowCol >= m.size) return 1;
|
|
if(!ISZERO(glsGet(m, rowCol, rowCol))) return reorder_gls(m,rowCol+1);
|
|
|
|
for(int candidate = rowCol+1; candidate < m.size; 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)))
|
|
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++) {
|
|
if(!ISZERO(glsGet(m,row,rowCol)))
|
|
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[] ) {
|
|
|
|
GLS m = glsCreate(3);
|
|
glsSet(m,0,0,3); glsSet(m,0,1,-1); glsSet(m,0,2,-1); glsSetRS(m,0, 0);
|
|
glsSet(m,1,0,1); glsSet(m,1,1,4); glsSet(m,1,2,2); glsSetRS(m,1, 4);
|
|
glsSet(m,2,0,0); glsSet(m,2,1,2); glsSet(m,2,2,1); glsSetRS(m,2, 1);
|
|
|
|
printf("Input: \n");
|
|
glsPrint(m);
|
|
if(solve(m)) {
|
|
printf("Output: \n");
|
|
} else {
|
|
printf("Failed to solve. :(\n");
|
|
}
|
|
glsPrint(m);
|
|
|
|
return 0;
|
|
}
|