lwr_server/lwrserv/include/Mat.h

#ifndef _MAT_H_
#define _MAT_H_
#include <iostream>
#include <math.h>
#include "Vec.h"

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;

typedef Mat<float, 4> MatCarthesian;


// 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 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)
    {
        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];
    }

    Mat<T, SIZE> operator + (const T &scalar)
    {
        Mat<T, SIZE> buf;
        for (unsigned int i=0; i<SIZE; i++) // row i
            for (unsigned int j=0; j<SIZE; j++) // column j
                buf(i,j) += m_aatData[i][j] + scalar;
        return buf;
    }

    Mat<T, SIZE> operator - (const T &scalar)
    {
        Mat<T, SIZE> buf;
        for (unsigned int i=0; i<SIZE; i++) // row i
            for (unsigned int j=0; j<SIZE; j++) // column j
                buf(i,j) += m_aatData[i][j] - scalar;
        return buf;
    }

    Mat<T, SIZE> operator * (const T &scalar)
    {
        Mat<T, SIZE> buf;
        for (unsigned int row=0; row<SIZE; row++)
            for (unsigned int column=0; column<SIZE; column++)
                buf(row,column) += m_aatData[row][column] * scalar;
        return buf;
    }

    Mat<T, SIZE> operator / (const T &scalar)
    {
        Mat<T, SIZE> buf;
        for (unsigned int i=0; i<SIZE; i++) // row i
            for (unsigned int j=0; j<SIZE; j++) // column j
                buf(i,j) += m_aatData[i][j] / scalar;
        return buf;
    }

    Mat<T, SIZE> operator * (const Mat<T, SIZE> &mat)
    {
        Mat<T, SIZE> buf;
        for (unsigned int i=0; i<SIZE; i++) // row i
            for (unsigned int j=0; j<SIZE; j++) // column j
                for (unsigned int a=0; a<SIZE; a++) // a
                    buf(i,j) += m_aatData[i][a] * mat(a,j);
        return buf;
    }
    Mat<T, SIZE> &operator = (const T aatData[SIZE*SIZE]) 
    {
        for (unsigned int i=0; i<SIZE; i++) // row i
            for (unsigned int j=0; j<SIZE; j++) // column j
                m_aatData[i][j] = aatData[j+(i*4)];

        return (*this); // also an R-value in e.g.
                        // vec1  = vec2  +   (vec2=atData); // parenthesis () needed to evaluate expression vec2=atData
                        // L-Val = R-Val +       R-Val
                        //   "   =   "   + (L-Val = R-Val)
    }

#if 0
    Mat<T, SIZE> operator * (const Vec<T, SIZE> &vec)
    {
        Vec<T, SIZE> buf;
        for (unsigned int j=0; j<SIZE; j++) // column j
            for (unsigned int i=0; i<SIZE; i++) // row i
                buf(i) += m_aatData[i][j]*vec(j);
        return buf;
    }
#endif

    Mat<T,SIZE> abs()
    {
        Mat<T,SIZE> buf;
        for (unsigned int j=0; j<SIZE; j++) // column j
            for (unsigned int i=0; i<SIZE; i++) // row i
                buf.m_aatData[i][j] = abs(m_aatData[i][j]);
        return buf;
    }

    T determinant()
    {
        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 i=0; i<SIZE; i++) // column j
        {
            T multBuff;
            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[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()
    {
        T buf;
        for (unsigned int row=0; row < SIZE; row++)
            for (unsigned int column=0; column < SIZE; column++)
                buf +=  pow(abs(m_aatData[row][column]),2);
        sqrt(buf);
        return buf;
    }

    Mat<T,SIZE> transpose()
    {
        Mat<T, SIZE> buf;
        for (unsigned int i=0; i<SIZE; i++) // row i
            for (unsigned int j=0; j<SIZE; j++) // column j
                buf.m_aatData[i][j] = m_aatData[j][i];
        return buf;
    }
    Mat<T,SIZE> inv()
    {
        T det = determinant();

        if (det== 0)
        {
            return *this;
        }

        Mat<float,4> thisTransposed = this->transpose();
        thisTransposed/det;

        return thisTransposed;
    }
    void getData (T atData[SIZE*SIZE]) 
    {
        for (unsigned int row=0; row<SIZE; row++)
            for (unsigned int column=0; column<SIZE; column++)
                atData[row+column*SIZE] = m_aatData[row][column];
    }
    void setData (const T atData[SIZE*SIZE]) 
    {
        for (unsigned int row=0; row<SIZE; row++)
            for (unsigned int column=0; column<SIZE; column++)
                m_aatData[row][column] = atData[row+column*SIZE];
    }

private:
    T m_aatData[SIZE][SIZE];
};


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