phschoen
/
lwr_server
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

add more simple inverse function capable to invert all matrix sizes

master
philipp schoenberger 10 years ago
parent
d9c7c92504
commit
9bae02d86f
2 changed files with 328 additions and 229 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 294
      lwrserv/include/mat.h
  2. 263
      lwrserv/test/test_mat.cpp

294
lwrserv/include/mat.h
View File

@ -1,9 +1,20 @@
#ifndef _MAT_H_
#define _MAT_H_
#include <iostream>
#include <math.h>
#include "vec.h"
template <class T, unsigned SIZE> class CVector;
template <class T, unsigned SIZE> class Vec;
template <class T, unsigned SIZE> class Mat;
// some common vector classes (abbr. names)
typedef Mat<float, 2> Mat2f;
typedef Mat<float, 3> Mat3f;
typedef Mat<float, 4> Mat4f;
typedef Mat<double, 2> Mat2d;
typedef Mat<double, 3> Mat3d;
typedef Mat<double, 4> Mat4d;
// template square matrix class for SIMPLE data types
template <class T, unsigned SIZE> class Mat
@ -36,6 +47,29 @@ public:
}
}
}
Mat<T, SIZE> (const T init_val,const T eye_val)
{
for (unsigned int j=0; j<SIZE; j++)
{
for (unsigned int i=0; i<SIZE; i++)
{
if (j == i )
m_aatData[i][j] = eye_val;
else
m_aatData[i][j] = init_val;
}
}
}
Mat<T, SIZE> (const T init_val)
{
for (unsigned int j=0; j<SIZE; j++)
{
for (unsigned int i=0; i<SIZE; i++)
{
m_aatData[i][j] = init_val;
}
}
}
Mat<T, SIZE> (const Mat<T, SIZE> &mat)
{
@ -66,10 +100,6 @@ public:
return m_aatData[i][j];
}
// CMatrix<T, SIZE>operator + (const Matrix<T, SIZE> &mat)
// CMatrix<T, SIZE>operator - (const Matrix<T, SIZE> &mat)
// ...
Mat<T, SIZE> operator + (const T &scalar)
{
Mat<T, SIZE> buf;
@ -78,6 +108,7 @@ public:
buf(i,j) += m_aatData[i][j] + scalar;
return buf;
}
Mat<T, SIZE> operator - (const T &scalar)
{
Mat<T, SIZE> buf;
@ -86,6 +117,7 @@ public:
buf(i,j) += m_aatData[i][j] - scalar;
return buf;
}
Mat<T, SIZE> operator * (const T &scalar)
{
Mat<T, SIZE> buf;
@ -94,6 +126,7 @@ public:
buf(i,j) += m_aatData[i][j] * scalar;
return buf;
}
Mat<T, SIZE> operator / (const T &scalar)
{
Mat<T, SIZE> buf;
@ -124,23 +157,30 @@ public:
}
#endif
Mat<T,SIZE> determinant()
T determinant()
{
T buf;
T buf = 0;
if (SIZE == 1)
return m_aatData[0][0];
if ( SIZE == 2)
return m_aatData[0][0] * m_aatData[1][1] - m_aatData[1][0]*m_aatData[0][1];
for (unsigned int j=0; j<SIZE; j++) // SPALTE j
for (unsigned int i=0; i<SIZE; i++) // SPALTE j
{
unsigned int i = j;
T multBuff;
multBuff = m_aatData[i][j];
multBuff *= m_aatData[(i+1)%SIZE][(i+1)%SIZE];
multBuff *= m_aatData[(i+2)%SIZE][(i+2)%SIZE];
multBuff = m_aatData[0][i];
multBuff *= m_aatData[1][(i+1)%SIZE];
multBuff *= m_aatData[2][(i+2)%SIZE];
std::cout <<"+("<< i << ", " << (i+1)%SIZE << ", " << (i+2)%SIZE << ") ";
buf += multBuff;
multBuff = m_aatData[i][j];
multBuff *= m_aatData[(i-1)%SIZE][(i-1)%SIZE];
multBuff *= m_aatData[(i-2)%SIZE][(i-2)%SIZE];
multBuff = m_aatData[0][i];
multBuff *= m_aatData[1][(i+SIZE-1)%SIZE];
multBuff *= m_aatData[2][(i+SIZE-2)%SIZE];
std::cout <<"-("<< i <<", " << (i+SIZE-1)%SIZE << ", " << (i+SIZE-2)%SIZE << ") ";
buf -= multBuff;
}
return buf;
}
Mat<T,SIZE> norm()
@ -158,231 +198,27 @@ public:
Mat<T, SIZE> buf;
for (unsigned int i=0; i<SIZE; i++) // ZEILE i
for (unsigned int j=0; j<SIZE; j++) // Spalte j
buf(i,j) += m_aatData[j][i];
buf.m_aatData[i][j] = m_aatData[j][i];
return buf;
}
Mat<float, 4> inv()
Mat<T,SIZE> inv()
{
Mat<float,4> invOut;
long indxc[4], indxr[4], ipiv[4];
long i, icol, irow, j, ir, ic;
long big, dum, pivinv,temp, bb;
ipiv[0] = -1;
ipiv[1] = -1;
ipiv[2] = -1;
ipiv[3] = -1;
invOut = *this;
for (i =0; i < 4 ; i++) {
big =0.0f;
for (j =0; j < 4 ; j++) {
if (ipiv[j] != 0) {
if (ipiv[0] == - 1) {
if ((bb = (float) fabs(invOut(j,0))) > big) {
big = bb;
irow = j;
icol = 0;
}
} else if (ipiv[0] > 0) {
goto end;
}
if (ipiv[1] == - 1) {
if ((bb = (float) fabs(invOut(j,1))) > big) {
big = bb;
irow = j;
icol = 1;
}
} else if (ipiv[1] > 0) {
goto end;
}
if (ipiv[2] == - 1) {
if ((bb = (float) fabs(invOut(j,2))) > big) {
big = bb;
irow = j;
icol = 2;
}
} else if (ipiv[2] > 0) {
goto end;
}
T det = determinant();
if (ipiv[3] == - 1) {
if ((bb = (float) fabs(invOut(j,3))) > big) {
big = bb;
irow = j;
icol = 3;
}
} else if (ipiv[3] > 0) {
goto end;
}
}
}
++(ipiv[icol]);
if (irow != icol) {
temp = invOut(irow,0);
invOut(irow, 0) = invOut(icol, 0);
invOut(icol, 0) = temp;
temp = invOut(irow,1);
invOut(irow, 1) = invOut(icol, 1);
invOut(icol,1) = temp;
temp = invOut(irow,2);
invOut(irow,2) = invOut(icol,2);
invOut(icol,2) = temp;
temp = invOut(irow,3);
invOut(irow,3) = invOut(icol,3);
invOut(icol,3) = temp;
}
indxr[i] = irow;
indxc[i] = icol;
if (invOut(icol,icol) ==0.0) {
goto end;
}
pivinv =1.0f / invOut(icol,icol);
invOut(icol,icol) =1.0f;
invOut(icol,0) *= pivinv;
invOut(icol,1) *= pivinv;
invOut(icol,2) *= pivinv;
invOut(icol,3) *= pivinv;
if (icol != 0) {
dum = invOut(0,icol);
invOut(0,icol) =0.0f;
invOut(0,0) -= invOut(icol,0) * dum;
invOut(0,1) -= invOut(icol,1) * dum;
invOut(0,2) -= invOut(icol,2) * dum;
invOut(0,3) -= invOut(icol,3) * dum;
}
if (icol != 1) {
dum = invOut(1,icol);
invOut(1,icol) =0.0f;
invOut(1,0) -= invOut(icol,0) * dum;
invOut(1,1) -= invOut(icol,1) * dum;
invOut(1,2) -= invOut(icol,2) * dum;
invOut(1,3) -= invOut(icol,3) * dum;
}
if (icol != 2) {
dum = invOut(2,icol);
invOut(2,icol) =0.0f;
invOut(2,0) -= invOut(icol,0) * dum;
invOut(2,1) -= invOut(icol,1) * dum;
invOut(2,2) -= invOut(icol,2) * dum;
invOut(2,3) -= invOut(icol,3) * dum;
}
if (icol != 3) {
dum = invOut(3,icol);
invOut(3,icol) =0.0f;
invOut(3,0) -= invOut(icol,0) * dum;
invOut(3,1) -= invOut(icol,1) * dum;
invOut(3,2) -= invOut(icol,2) * dum;
invOut(3,3) -= invOut(icol,3) * dum;
}
}
if (indxr[3] != indxc[3]) {
ir = indxr[3];
ic = indxc[3];
temp = invOut(0,ir);
invOut(0,ir) = invOut(0,ic);
invOut(0,ic) = temp;
temp = invOut(1,ir);
invOut(1,ir) = invOut(1,ic);
invOut(1,ic) = temp;
temp = invOut(2,ir);
invOut(2,ir) = invOut(2,ic);
invOut(2,ic) = temp;
temp = invOut(3,ir);
invOut(3,ir) = invOut(3,ic);
invOut(3,ic) = temp;
}
if (indxr[2] != indxc[2]) {
ir = indxr[2];
ic = indxc[2];
temp = invOut(0,ir);
invOut(0,ir) = invOut(0,ic);
invOut(0,ic) = temp;
temp = invOut(1,ir);
invOut(1,ir) = invOut(1,ic);
invOut(1,ic) = temp;
temp = invOut(2,ir);
invOut(2,ir) = invOut(2,ic);
invOut(2,ic) = temp;
temp = invOut(3,ir);
invOut(3,ir) = invOut(3,ic);
invOut(3,ic) = temp;
}
if (indxr[1] != indxc[1]) {
ir = indxr[1];
ic = indxc[1];
temp = invOut(0,ir);
invOut(0,ir) = invOut(0,ic);
invOut(0,ic) = temp;
temp = invOut(1,ir);
invOut(1,ir) = invOut(1,ic);
invOut(1,ic) = temp;
temp = invOut(2,ir);
invOut(2,ir) = invOut(2,ic);
invOut(2,ic) = temp;
temp = invOut(3,ir);
invOut(3,ir) = invOut(3,ic);
invOut(3,ic) = temp;
if (det== 0)
{
return *this;
}
if (indxr[0] != indxc[0]) {
ir = indxr[0];
ic = indxc[0];
temp = invOut(0,ir);
invOut(0,ir) = invOut(0,ic);
invOut(0,ic) = temp;
temp = invOut(1,ir);
invOut(1,ir) = invOut(1,ic);
invOut(1,ic) = temp;
Mat<float,4> thisTransposed = this->transpose();
thisTransposed/det;
temp = invOut(2,ir);
invOut(2,ir) = invOut(2,ic);
invOut(2,ic) = temp;
temp = invOut(3,ir);
invOut(3,ir) = invOut(3,ic);
invOut(3,ic) = temp;
}
end:
return invOut;
return thisTransposed;
}
private:
T m_aatData[SIZE][SIZE];
};
// some common vector classes (abbr. names)
typedef Mat<float, 2> Mat2f;
typedef Mat<float, 3> Mat3f;
typedef Mat<float, 4> Mat4f;
typedef Mat<double, 2> Mat2d;
typedef Mat<double, 3> Mat3d;
typedef Mat<double, 4> Mat4d;
template <class T, unsigned SIZE>

263
lwrserv/test/test_mat.cpp
View File

@ -8,6 +8,7 @@
#define TESTSIZE 4
int testMat [TESTSIZE][TESTSIZE];
int testVec [TESTSIZE];
float tolerance = 0.00001f;
TEST_GROUP(Matrix)
{
@ -29,6 +30,268 @@ TEST_GROUP(Matrix)
{
}
};
TEST(Matrix, vectorInvRotX)
{
Mat<float, TESTSIZE> a;
for(int x = 0 ; x < TESTSIZE ; ++x)
{
for (int y = 0; y <TESTSIZE ; ++y)
{
float val = 0.0f;
if (x == y )
val = 1.0f;
a(x,x) = val;
}
}
a(0,0) = 1.0f;
a(1,1) = cos(45.0f *M_PI/180.0f);
a(1,2) = -sin(45.0f *M_PI/180.0f);
a(2,2) = cos(45.0f *M_PI/180.0f);
a(2,1) = sin(45.0f *M_PI/180.0f);
float deg = 45.0f*M_PI/180.0f;
DOUBLES_EQUAL(1.0f,a(0,0), tolerance);
DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
DOUBLES_EQUAL(0.0f,a(0,2), tolerance);
DOUBLES_EQUAL(0.0f,a(0,3), tolerance);
DOUBLES_EQUAL(0.0f,a(1,0), tolerance);
DOUBLES_EQUAL(cos(deg),a(1,1), tolerance);
DOUBLES_EQUAL(-sin(deg),a(1,2), tolerance);
DOUBLES_EQUAL(0.0f,a(1,3), tolerance);
DOUBLES_EQUAL(0.0f,a(2,0), tolerance);
DOUBLES_EQUAL(sin(deg),a(2,1), tolerance);
DOUBLES_EQUAL(cos(deg),a(2,2), tolerance);
DOUBLES_EQUAL(0.0f,a(2,3), tolerance);
DOUBLES_EQUAL(0.0f,a(3,0), tolerance);
DOUBLES_EQUAL(0.0f,a(3,1), tolerance);
DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
a = a.inv();
deg = -45.0f*M_PI/180.0f;
DOUBLES_EQUAL(1.0f,a(0,0), tolerance);
DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
DOUBLES_EQUAL(0.0f,a(0,2), tolerance);
DOUBLES_EQUAL(0.0f,a(0,3), tolerance);
DOUBLES_EQUAL(0.0f,a(1,0), tolerance);
DOUBLES_EQUAL(cos(deg),a(1,1), tolerance);
DOUBLES_EQUAL(-sin(deg),a(1,2), tolerance);
DOUBLES_EQUAL(0.0f,a(1,3), tolerance);
DOUBLES_EQUAL(0.0f,a(2,0), tolerance);
DOUBLES_EQUAL(sin(deg),a(2,1), tolerance);
DOUBLES_EQUAL(cos(deg),a(2,2), tolerance);
DOUBLES_EQUAL(0.0f,a(2,3), tolerance);
DOUBLES_EQUAL(0.0f,a(3,0), tolerance);
DOUBLES_EQUAL(0.0f,a(3,1), tolerance);
DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
}
TEST(Matrix, vectorInvRotY)
{
Mat<float, TESTSIZE> a(0.0f);
float deg = 45.0f*M_PI/180.0f;
a(1,1) = 1.0f;
a(0,0) = cos(deg);
a(0,2) = sin(deg);
a(2,2) = cos(deg);
a(2,0) = -sin(deg);
a(3,3) = 1.0f;
std::cout << a;
a = a.inv();
deg = -45.0f*M_PI/180.0f;
DOUBLES_EQUAL(cos(deg),a(0,0), tolerance);
DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
DOUBLES_EQUAL(sin(deg),a(0,2), tolerance);
DOUBLES_EQUAL(0.0f,a(0,3), tolerance);
DOUBLES_EQUAL(0.0f,a(1,0), tolerance);
DOUBLES_EQUAL(1.0f,a(1,1), tolerance);
DOUBLES_EQUAL(0.0f,a(1,2), tolerance);
DOUBLES_EQUAL(0.0f,a(1,3), tolerance);
DOUBLES_EQUAL(-sin(deg),a(2,0), tolerance);
DOUBLES_EQUAL(0.0f,a(2,1), tolerance);
DOUBLES_EQUAL(cos(deg),a(2,2), tolerance);
DOUBLES_EQUAL(0.0f,a(2,3), tolerance);
DOUBLES_EQUAL(0.0f,a(3,0), tolerance);
DOUBLES_EQUAL(0.0f,a(3,1), tolerance);
DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
}
TEST(Matrix, matrixInvEye)
{
Mat<float, TESTSIZE> a(0.0f , 1.0f);
// do the invert
a = a.inv();
DOUBLES_EQUAL(1.0f,a(0,0), tolerance);
DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
DOUBLES_EQUAL(0.0f,a(0,2), tolerance);
DOUBLES_EQUAL(0.0f,a(0,3), tolerance);
DOUBLES_EQUAL(0.0f,a(1,0), tolerance);
DOUBLES_EQUAL(1.0f,a(1,1), tolerance);
DOUBLES_EQUAL(0.0f,a(1,2), tolerance);
DOUBLES_EQUAL(0.0f,a(1,3), tolerance);
DOUBLES_EQUAL(0.0f,a(2,0), tolerance);
DOUBLES_EQUAL(0.0f,a(2,1), tolerance);
DOUBLES_EQUAL(1.0f,a(2,2), tolerance);
DOUBLES_EQUAL(0.0f,a(2,3), tolerance);
DOUBLES_EQUAL(0.0f,a(3,0), tolerance);
DOUBLES_EQUAL(0.0f,a(3,1), tolerance);
DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
}
TEST(Matrix, matrixEye)
{
Mat<float, TESTSIZE> a(0.0f , 1.0f);
DOUBLES_EQUAL(1.0f,a(0,0), tolerance);
DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
DOUBLES_EQUAL(0.0f,a(0,2), tolerance);
DOUBLES_EQUAL(0.0f,a(0,3), tolerance);
DOUBLES_EQUAL(0.0f,a(1,0), tolerance);
DOUBLES_EQUAL(1.0f,a(1,1), tolerance);
DOUBLES_EQUAL(0.0f,a(1,2), tolerance);
DOUBLES_EQUAL(0.0f,a(1,3), tolerance);
DOUBLES_EQUAL(0.0f,a(2,0), tolerance);
DOUBLES_EQUAL(0.0f,a(2,1), tolerance);
DOUBLES_EQUAL(1.0f,a(2,2), tolerance);
DOUBLES_EQUAL(0.0f,a(2,3), tolerance);
DOUBLES_EQUAL(0.0f,a(3,0), tolerance);
DOUBLES_EQUAL(0.0f,a(3,1), tolerance);
DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
}
TEST(Matrix, matrixDeterminantEye)
{
//eye matrix
Mat<float, TESTSIZE> a(0.0f, 1.0f);
DOUBLES_EQUAL(1.0f,a.determinant(),tolerance);
}
TEST(Matrix, matrixDeterminantEye3)
{
//eye matrix
Mat<float, 3> a(0.0f, 1.0f);
DOUBLES_EQUAL(1.0f,a.determinant(),tolerance);
}
TEST(Matrix, matrixDeterminantSimple)
{
Mat<float, 1> a;
a(0,0) = 50.0f;
DOUBLES_EQUAL(50.0f,a.determinant(), tolerance);
}
TEST(Matrix, matrixDeterminantSimple2)
{
Mat<float, 2> a;
a(0,0) = 10.0f;
a(1,1) = 5.0f;
DOUBLES_EQUAL(50.0f,a.determinant(), tolerance);
}
TEST(Matrix, matrixDeterminantSimple3)
{
Mat<float, 2> a;
a(0,0) = 10.0f;
a(0,1) = 5.0f;
a(1,0) = 10.0f;
a(1,1) = 5.0f;
DOUBLES_EQUAL(0.0f,a.determinant(), tolerance);
}
TEST(Matrix, matrixDeterminantSimple4)
{
Mat<float, 2> a;
a(0,0) = 10.0f;
a(0,1) = 5.0f;
a(1,0) = 5.0f;
a(1,1) = 10.0f;
DOUBLES_EQUAL(75.0f,a.determinant(), tolerance);
}
TEST(Matrix, matrixTransposeEye)
{
Mat<float, TESTSIZE> a;
for(int x = 0 ; x < TESTSIZE ; ++x)
{
for (int y = 0; y <TESTSIZE ; ++y)
{
float val = 0.0f;
if (x == y )
val = 1.0f;
a(x,y) = val;
}
}
DOUBLES_EQUAL(1.0f,a(0,0), tolerance);
DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
DOUBLES_EQUAL(0.0f,a(0,2), tolerance);
DOUBLES_EQUAL(0.0f,a(0,3), tolerance);
DOUBLES_EQUAL(0.0f,a(1,0), tolerance);
DOUBLES_EQUAL(1.0f,a(1,1), tolerance);
DOUBLES_EQUAL(0.0f,a(1,2), tolerance);
DOUBLES_EQUAL(0.0f,a(1,3), tolerance);
DOUBLES_EQUAL(0.0f,a(2,0), tolerance);
DOUBLES_EQUAL(0.0f,a(2,1), tolerance);
DOUBLES_EQUAL(1.0f,a(2,2), tolerance);
DOUBLES_EQUAL(0.0f,a(2,3), tolerance);
DOUBLES_EQUAL(0.0f,a(3,0), tolerance);
DOUBLES_EQUAL(0.0f,a(3,1), tolerance);
DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
a = a.inv();
// should stille be the same
DOUBLES_EQUAL(1.0f,a(0,0), tolerance);
DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
DOUBLES_EQUAL(0.0f,a(0,2), tolerance);
DOUBLES_EQUAL(0.0f,a(0,3), tolerance);
DOUBLES_EQUAL(0.0f,a(1,0), tolerance);
DOUBLES_EQUAL(1.0f,a(1,1), tolerance);
DOUBLES_EQUAL(0.0f,a(1,2), tolerance);
DOUBLES_EQUAL(0.0f,a(1,3), tolerance);
DOUBLES_EQUAL(0.0f,a(2,0), tolerance);
DOUBLES_EQUAL(0.0f,a(2,1), tolerance);
DOUBLES_EQUAL(1.0f,a(2,2), tolerance);
DOUBLES_EQUAL(0.0f,a(2,3), tolerance);
DOUBLES_EQUAL(0.0f,a(3,0), tolerance);
DOUBLES_EQUAL(0.0f,a(3,1), tolerance);
DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
}
TEST(Matrix, vectorMultiply)
{
Mat<int, TESTSIZE> a = testMat;

Write Preview
Loading…
Cancel
Save