Matrix.hpp

Go to the documentation of this file.

#ifndef SPIDAR_MATRIX_HPP
#define SPIDAR_MATRIX_HPP

#include <cmath>
#include "Vector.hpp"

namespace Spidar
{
namespace Math
{
template <class T, size_t M, size_t N>
struct Matrix
{
    typedef T ValueType;                

    static const size_t row = M;        
    static const size_t column = N;     

    ValueType element[M][N];            

    // コンストラクタ
    Matrix(void)
    {
        for (size_t i=0; i<M; ++i)
        {
            for (size_t j=0; j<N; ++j)
            {
                element[i][j] = 0;
            }
        }
    }

    Matrix(const Matrix &other)
    {
        for (size_t i=0; i<M; ++i)
        {
            for (size_t j=0; j<N; ++j)
            {
                element[i][j] = other.element[i][j];
            }
        }
    }

    Matrix& operator=(const Matrix &other)
    {
        for (size_t i=0; i<M; ++i)
        {
            for (size_t j=0; j<N; ++j)
            {
                element[i][j] = other.element[i][j];
            }
        }
        return *this;
    }

    //
    ValueType* operator[](size_t i)
    {
        return element[i];
    }

    //
    const ValueType* operator[](size_t i) const
    {
        return element[i];
    }

    //
    Matrix& operator+=(const Matrix &other)
    {
        for (size_t i=0; i<M; ++i)
        {
            for (size_t j=0; j<N; ++j)
            {
                element[i][j] += other.element[i][j];
            }
        }
        return *this;
    }

    //
    Matrix& operator-=(const Matrix &other)
    {
        for (size_t i=0; i<M; ++i)
        {
            for (size_t j=0; j<N; ++j)
            {
                element[i][j] -= other.element[i][j];
            }
        }
        return *this;
    }

    //
    Matrix& operator*=(double value)
    {
        for (size_t i=0; i<M; ++i)
        {
            for (size_t j=0; j<N; ++j)
            {
                element[i][j] = static_cast<ValueType>(value * element[i][j]);
            }
        }
        return *this;
    }

    //
    Matrix& operator/=(double value)
    {
        for (size_t i=0; i<M; ++i)
        {
            for (size_t j=0; j<N; ++j)
            {
                element[i][j] = static_cast<ValueType>(element[i][j]/value);
            }
        }
        return *this;
    }

    //
    void clear(void)
    {
        for (size_t i=0; i<M; ++i)
        {
            for (size_t j=0; j<N; ++j)
            {
                element[i][j] = 0;
            }
        }
    }

    //
    Matrix<T,N,M> trans(void) const
    {
        Matrix<T,N,M> mat;
    
        for (size_t i=0; i<M; ++i)
        {
            for (size_t j=0; j<N; ++j)
            {
                mat.element[j][i] = element[i][j];
            }
        }
        return mat;
    }

}; // end of struct Matrix.

//
template <class T, size_t M, size_t N>
std::ostream& operator<<(std::ostream &os, const Matrix<T,M,N> &m)
{
    os << "[";

    for (size_t i=0; i<M; ++i)
    {
        if (i>0) os << " ";
        for (size_t j=0; j<N; ++j)
        {
            os << m.element[i][j];
            if (j<N-1) os << " ";
        }
        if (i<M-1) os << std::endl;
    }

    os << "]";

    return os;
}

//
template <class T, size_t N>
void Identity(Matrix<T,N,N>& mat)
{
    mat.clear();
    for (size_t i=0; i<N; ++i)  mat[i][i] = 1;
}

// 
template <class T, size_t M, size_t N>
const Matrix<T,M,N> operator+(Matrix<T,M,N> lhs, const Matrix<T,M,N> &rhs)
{    
    return lhs += rhs;
}

// 
template <class T, size_t M, size_t N>
const Matrix<T,M,N> operator-(Matrix<T,M,N> lhs, const Matrix<T,M,N> &rhs)
{
    return lhs -= rhs;
}

// 
template <class T, size_t M, size_t N>
const Matrix<T,M,N> operator*(Matrix<T,M,N> lhs, double rhs)
{   
    return lhs *= rhs;
}

// 
template <class T, size_t M, size_t N>
const Matrix<T,M,N> operator*(double lhs, Matrix<T,M,N> rhs)
{
    return rhs *= lhs;
}

//
template <class T, size_t L, size_t M, size_t N>
const Matrix<T,L,N> operator*(const Matrix<T,L,M> &lhs, const Matrix<T,M,N> &rhs)
{
    Matrix<T,L,N> temp;

    for (size_t i=0; i<L; ++i)
    {
        for (size_t j=0; j<N; ++j)
        {
            temp[i][j] = 0;

            for(size_t k=0; k<M; ++k)
            {
                temp[i][j] += lhs[i][k] * rhs[k][j];
            }
        }
    }
    return temp;
}

//
template <class T, size_t M, size_t N>
const Vector<T,M> operator*(const Matrix<T,M,N> &mat, const Vector<T,N> &vec)
{
    Vector<T,M> temp;

    for (size_t i=0; i<M; ++i)
    {
        temp[i] = 0;

        for (size_t j=0; j<N; ++j)
        {
            temp[i] += mat[i][j] * vec[j];
        }
    }
    return temp;
}

//
template <class T, size_t M, size_t N>
const Matrix<T,M,N> operator/(Matrix<T,M,N> lhs, double rhs)
{   
    return lhs /= rhs;
}

} // end of namespace Math.
} // end of namespace Spidar.

#endif // SPIDAR_MATRIX_HPP

// end of file.