You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
362 lines
10 KiB
362 lines
10 KiB
#ifndef _MAT_H_
|
|
#define _MAT_H_
|
|
#include <iostream>
|
|
#include "vec.h"
|
|
|
|
template <class T, unsigned SIZE> class CVector;
|
|
|
|
// template square matrix class for SIMPLE data types
|
|
template <class T, unsigned SIZE> class Mat
|
|
{
|
|
public:
|
|
|
|
Mat<T, SIZE> ()
|
|
{
|
|
for (unsigned int j=0; j<SIZE; j++)
|
|
{
|
|
for (unsigned int i=0; i<SIZE; i++)
|
|
{
|
|
m_aatData[i][j] = T(0);
|
|
}
|
|
}
|
|
}
|
|
|
|
~Mat<T, SIZE> ()
|
|
{
|
|
// nothing to do here ...
|
|
}
|
|
|
|
Mat<T, SIZE> (const T aatData[SIZE][SIZE])
|
|
{
|
|
for (unsigned int j=0; j<SIZE; j++)
|
|
{
|
|
for (unsigned int i=0; i<SIZE; i++)
|
|
{
|
|
m_aatData[i][j] = aatData[i][j];
|
|
}
|
|
}
|
|
}
|
|
|
|
Mat<T, SIZE> (const Mat<T, SIZE> &mat)
|
|
{
|
|
for (unsigned int j=0; j<SIZE; j++)
|
|
{
|
|
for (unsigned int i=0; i<SIZE; i++)
|
|
{
|
|
m_aatData[i][j] = mat.m_aatData[i][j];
|
|
}
|
|
}
|
|
}
|
|
|
|
T &operator () (unsigned i, unsigned j)
|
|
{
|
|
if (i>=SIZE)
|
|
i = SIZE-1;
|
|
if (j>=SIZE)
|
|
j = SIZE-1;
|
|
return m_aatData[i][j];
|
|
}
|
|
|
|
T operator () (unsigned i, unsigned j) const
|
|
{
|
|
if (i>=SIZE)
|
|
i = SIZE-1;
|
|
if (j>=SIZE)
|
|
j = SIZE-1;
|
|
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 Mat<T, SIZE> &mat)
|
|
{
|
|
Mat<T, SIZE> buf;
|
|
for (unsigned int i=0; i<SIZE; i++) // ZEILE i
|
|
for (unsigned int j=0; j<SIZE; j++) // Spalte j
|
|
for (unsigned int a=0; a<SIZE; a++) // a
|
|
buf(i,j) += m_aatData[i][a] * mat(a,j);
|
|
return buf;
|
|
}
|
|
|
|
Vec<T, SIZE> operator * (const Vec<T, SIZE> &vec)
|
|
{
|
|
Vec<T, SIZE> buf;
|
|
for (unsigned int j=0; j<SIZE; j++) // SPALTE j
|
|
for (unsigned int i=0; i<SIZE; i++) // ZEILE i
|
|
buf(i) += m_aatData[i][j]*vec(j);
|
|
return buf;
|
|
}
|
|
Mat<float, 4> 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;
|
|
}
|
|
|
|
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 (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;
|
|
|
|
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;
|
|
}
|
|
|
|
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>
|
|
static std::ostream& operator<< (std::ostream& output,const Mat<T,SIZE> &mat)
|
|
{
|
|
output << "( ";
|
|
for(unsigned int j =0; j< SIZE; ++j)
|
|
{
|
|
output << "( ";
|
|
for(unsigned int i =0; i< SIZE; ++i)
|
|
{
|
|
output << mat(j,i);
|
|
if (i<4-1)
|
|
output << " , ";
|
|
else
|
|
output << " )";
|
|
}
|
|
if (j<SIZE-1)
|
|
output << " ) ; ";
|
|
else
|
|
output << " ) )";
|
|
}
|
|
return output;
|
|
}
|
|
static Mat<float, 4> getRotationMatrix(float x_angle,float y_angle, float z_angle)
|
|
{
|
|
Mat<float,4> tempx,tempy,tempz;
|
|
float temp_x[4][4] = { { 1, 0, 0, 0},
|
|
{ 0, cos(x_angle), -sin(x_angle), 0},
|
|
{ 0, sin(x_angle), cos(x_angle), 0},
|
|
{ 0, 0, 0, 1}
|
|
};
|
|
|
|
float temp_y[4][4] = { { cos(y_angle), 0, -sin(y_angle), 0},
|
|
{ 0, 1, 0, 0},
|
|
{ sin(y_angle), 0, cos(y_angle), 0},
|
|
{ 0, 0, 0, 1}
|
|
};
|
|
float temp_z[4][4] = { { cos(z_angle), -sin(z_angle), 0, 0},
|
|
{ sin(z_angle), cos(z_angle), 0, 0},
|
|
{ 0, 0, 1, 0},
|
|
{ 0, 0, 0, 1}
|
|
};
|
|
|
|
tempx=temp_x;
|
|
tempy=temp_y;
|
|
tempz=temp_z;
|
|
return tempz*tempy*tempx;
|
|
}
|
|
#endif
|