Enter 2005 March/ENTER.ISO/files/fwp-0.0.6-win32-installer.exe/matrixmath.cpp

C/C++ Source or Header | 2004-12-18 | 4.6 KB | 221 lines

/*!
\file matrixmath.cpp
\author Karsten Schwenk

See header for description (matrixmath.h).

*/

#include "matrixmath.h"
#include <stdio.h>
#include <math.h>
#include <float.h>

#define SQUARE(x) ((x)*(x))
#define EPSILON FLT_EPSILON
#define EEQUAL(a,b) ( fabs(a-b)<EPSILON )

mat2x2_t id2x2={1,0,0,1};
mat3x3_t id3x3={1,0,0,0,1,0,0,0,1};
mat4x4_t id4x4={1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1};

//int matinv;

/*!
This function prints a matrix to stdout.
\param a matrix that should be printed
\param n dimension of \a a
*/
void matrixPrint(matbase_t *a, int n){
    int i,j;

    for(i=0;i<n;i++){
        for(j=0;j<n; j++){
            printf("%f ", a[i+j*n]);
        }
        printf("\n");
    }
}

/*!
This function multiplies vector \a b by a matrix \a a and stores the resulting vector in r (r=ab).
\param a matrix to multiply \a b by
\param b vector to be multiplied by \a a
\param n dimension of \a a and /a b
\param r the resulting vector is returned here
*/
void matrixMultVector(matbase_t *a, matbase_t *b, int n, float *r){
    int i, j;

    float *t=new matbase_t[n];

    for(i=0; i<n; i++){
        t[i] = 0.0;
        for(j=0; j<n; j++)
            t[i]+=a[i+j*n]*b[j];
    }

    for(i=0;i<n;i++)
        r[i]=t[i];

    delete[] t;
}

/*!
This function multiplies matrix \a a by a matrix \a b and stores the resulting vector in r (r=ab).
This is used mostly to concatenate transformations.
\param a matrix to multiply \a b by
\param b vector to be multiplied by \a a
\param n dimension of \a a and /a b
\param r the resulting matrix is returned here
*/
void matrixMultMatrix(matbase_t *a, matbase_t *b, int n, matbase_t *r){
    int i, j, k;
    float* ret=new matbase_t[n*n];


    for(i=0; i<n; i++){
        for(j=0; j<n; j++){
            ret[i+j*n]=0;

            for(k=0;k<n;k++)
                ret[i+j*n]+=a[i+k*n]*b[k+j*n];
        }
    }
    matrixCopy(ret, r, n);
    delete[] ret;
}

/*!
This function copies matrix \a a into matrix \a b.
\param a source matrix
\param b destination matrix
\param n dimension of \a a and \a b
*/
void matrixCopy(matbase_t *a, matbase_t *b, int n){
    for(int i=0;i<n*n;i++){
        b[i]=a[i];
    }
}



/*!
This function calculates the tranpose of matrix \a a and stores it in \a t.
This is although a very fast way to calculate the inverse of a (nearly) orthonormal matrix. (For orthonormal matrices the inverse IS the tranpose)
\param a matrix to be transposed
\param n dimension of \a a
\param t tranpose of \a a is returned here
*/
void matrixTranspose(matbase_t *a, int n, matbase_t *t){
    int i,j;

    for(i=0;i<n;i++){
        for(j=0;j<n;j++){
            t[i*n+j]=a[j*n+i];
        }
    }
}



/*!
This function calculates the inverse of matrix \a a and stores it in \a u.
Since this uses LR-decomposisition and solves a complete system of linear equations it isn't really a cheap function zu call.
If you have a orthonormal matrix you should use matrixTranspose() instead.
\param a matrix to be inverted
\param n dimension of \a a
\param u inverse of \a a is returned here
*/
void matrixInvert(matbase_t *a, int n, matbase_t *u){
//    matrixPrint(a, n);
    int i;
    float *t=new matbase_t[n*n];
    for(i=0; i<n*n; i++){
        t[i]=a[i];
    }
    matrixLU(t, n);
//    matrixPrint(t, n);
    for(i=0; i<n; i++){
        matrixSolve(t, &(id4x4[i*4]), n, &(u[i*n]));    //FIXME: nxn-E-mat
    }

//    mat4x4_t test;
//    matrixMultMatrix(a, u, 4, test);
//    matrixPrint(test, 4);

    delete[] t;
}



void matrixLU(matbase_t *a, int n){    // FIXME: irgendwie falsch!!!
    int k,i,j;
//    printf("Be!\n");
//    matrixPrint(a, n);
    for(k=0; k<n-1; k++){
        if( fabs(a[k+k*n]) < EPSILON ){
            //printf("AH!\n");
            a[k+k*n] = EPSILON;
        }

        for(i=k+1; i<n; i++){
            a[i+k*n]=a[i+k*n]/a[k+k*n];
            for(j=k+1; j<n; j++){
                a[i+j*n]=a[i+j*n]-a[i+k*n]*a[k+j*n];
            }
        }
    }
//    matrixPrint(a,n);
//    printf("\n");
}

void matrixSolveForward(matbase_t *a, matbase_t *b, int n, matbase_t *c){
    int i,j;

    for(i=0; i<n; i++){
        c[i]=b[i];
        for(j=0; j<i; j++){
            c[i]=c[i]-a[i+j*n]*c[j];
        }
    }
}


void matrixSolveBackward(matbase_t *a, matbase_t *c, int n, matbase_t *x){
    int i,k;
    float s;

    for(i=n-1; i>=0; i--){
        s=c[i];
        for(k=i+1; k<n; k++){
            s=s-a[i+k*n]*x[k];
        }
        x[i]=s/a[i+i*n];
    }
}

/*!
This functions solves the system of linear equations Ax=b.
It uses matrixLR(), matrixSolveForward() and matrixSolveBackward().
\param a matrix describing the linear equations
\param b the vector b (right side)
\param n dimension of \a a and \a b
\param x the solution (if there is any) is returned here
*/
void matrixSolve(matbase_t *a, matbase_t *b, int n, matbase_t *x){
    float *c=new matbase_t[n];

    matrixSolveForward(a, b, n, c);
    matrixSolveBackward(a, c, n, x);

    delete[] c;
}