#include <utils.h>

void r3x3_inv (r3x3_t *A, r3x3_t *R)
{ 
    r3x3_t RR;
    int i, j;
    r3x3_adj(A, &RR);

    double d = A->c[0][0]*RR.c[0][0] + A->c[0][1]*RR.c[1][0] + A->c[0][2]*RR.c[2][0];

    for (i = 0; i < mat_n; i++)
       for (j = 0; j < mat_n; j++)
          R->c[i][j] = RR.c[i][j]/d;
}

void r3x3_adj (r3x3_t *A, r3x3_t *R)
{ 
    double A00 = A->c[0][0];
    double A01 = A->c[0][1];
    double A02 = A->c[0][2];
    
    double A10 = A->c[1][0];
    double A11 = A->c[1][1];
    double A12 = A->c[1][2];
    
    double A20 = A->c[2][0];
    double A21 = A->c[2][1];
    double A22 = A->c[2][2];
    
    double A01_01 = A00 * A11 - A01 * A10;
    double A01_02 = A00 * A12 - A02 * A10;
    double A01_12 = A01 * A12 - A02 * A11;
    
    double A02_01 = A00 * A21 - A01 * A20;
    double A02_02 = A00 * A22 - A02 * A20;
    double A02_12 = A01 * A22 - A02 * A21;

    double A12_01 = A10 * A21 - A11 * A20;
    double A12_02 = A10 * A22 - A12 * A20;
    double A12_12 = A11 * A22 - A12 * A21;

    R->c[0][0] =  A12_12;
    R->c[0][1] = -A02_12;
    R->c[0][2] =  A01_12;

    R->c[1][0] = -A12_02;
    R->c[1][1] =  A02_02;
    R->c[1][2] = -A01_02;

    R->c[2][0] =  A12_01;
    R->c[2][1] = -A02_01;
    R->c[2][2] =  A01_01;
}

void r3x3_map_col (r3x3_t *A, r3_t *x, r3_t *r)
{ 
    r3_t rr;
    int i, k;
    for (i = 0; i < mat_n; i++) { 
        double s = 0.0;
        for (k = 0; k < mat_n; k++) {
            s += A->c[i][k] * x->c[k];
        }
        rr.c[i] = s;
    }
    for (i = 0; i < mat_n; i++) { 
        r->c[i] = rr.c[i]; 
    }
}