Serious-Engine/Sources/Engine/Math/Matrix.h
2016-03-11 18:20:51 -06:00

490 lines
17 KiB
C++

/* Copyright (c) 2002-2012 Croteam Ltd.
This program is free software; you can redistribute it and/or modify
it under the terms of version 2 of the GNU General Public License as published by
the Free Software Foundation
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License along
with this program; if not, write to the Free Software Foundation, Inc.,
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */
#ifndef SE_INCL_MATRIX_H
#define SE_INCL_MATRIX_H
#ifdef PRAGMA_ONCE
#pragma once
#endif
/*
* Template class for matrix of arbitrary dimensions and arbitrary type of members
*/
template<class Type, int iRows, int iColumns>
class Matrix {
public:
Type matrix[iRows][iColumns]; // array that holds the members
public:
/* Default constructor. */
__forceinline Matrix(void);
/* Constructor that sets the whole matrix to same number. */
__forceinline Matrix(const Type x);
/* Reference matrix member by it's row and column indices (1-based indices!). */
__forceinline Type &operator()(int iRow, int iColumn);
__forceinline const Type &operator()(int iRow, int iColumn) const;
/* Make a transposed matrix. */
__forceinline Matrix<Type, iRows, iColumns> operator!(void) const;
__forceinline Matrix<Type, iRows, iColumns> &operator!=(const Matrix<Type, iRows, iColumns> &matrix2);
/* Mathematical operators. */
// between matrices
__forceinline Matrix<Type, iRows, iColumns> operator+(const Matrix<Type, iRows, iColumns> &matrix2) const;
__forceinline Matrix<Type, iRows, iColumns> &operator+=(const Matrix<Type, iRows, iColumns> &matrix2);
__forceinline Matrix<Type, iRows, iColumns> operator-(const Matrix<Type, iRows, iColumns> &matrix2) const;
__forceinline Matrix<Type, iRows, iColumns> &operator-=(const Matrix<Type, iRows, iColumns> &matrix2);
__forceinline Matrix<Type, iRows, iColumns> operator*(const Matrix<Type, iRows, iColumns> &matrix2) const;
__forceinline Matrix<Type, iRows, iColumns> &operator*=(const Matrix<Type, iRows, iColumns> &matrix2);
// matrices and scalars
__forceinline Matrix<Type, iRows, iColumns> operator*(const Type tMul) const;
__forceinline Matrix<Type, iRows, iColumns> &operator*=(const Type tMul);
__forceinline Matrix<Type, iRows, iColumns> operator/(const Type tMul) const;
__forceinline Matrix<Type, iRows, iColumns> &operator/=(const Type tMul);
/* Set matrix main diagonal. */
void Diagonal(Type x);
void Diagonal(const Vector<Type, iRows> &v);
// get main vectors of matrix
Vector<Type, iColumns> GetRow(Type iRow) const;
Vector<Type, iRows> GetColumn(Type iColumn) const;
/* Stream operations */
friend __forceinline CTStream &operator>>(CTStream &strm, Matrix<Type, iRows, iColumns> &matrix)
{
strm.Read_t(&matrix, sizeof(matrix));
return strm;
}
friend __forceinline CTStream &operator<<(CTStream &strm, Matrix<Type, iRows, iColumns> &matrix)
{
strm.Write_t(&matrix, sizeof(matrix));
return strm;
}
};
// inline functions implementation
/*
* Default constructor.
*/
template<class Type, int iRows, int iColumns>
__forceinline Matrix<Type, iRows, iColumns>::Matrix(void)
{
#ifndef NDEBUG
// set whole matrix to trash
ULONG ulTrash = 0xCDCDCDCDul;
for(int iRow=1; iRow<=iRows; iRow++) {
for(int iColumn=1; iColumn<=iColumns; iColumn++) {
(*this)(iRow, iColumn) = *reinterpret_cast<Type *>(&ulTrash);
}
}
#endif
}
/*
* Constructor that sets the whole matrix to same number.
*/
// set FLOAT 3x3
Matrix<FLOAT,3,3>::Matrix(const FLOAT x /*= Type(0)*/)
{
// set whole matrix to constant
(*this)(1,1)=x; (*this)(1,2)=x; (*this)(1,3)=x;
(*this)(2,1)=x; (*this)(2,2)=x; (*this)(2,3)=x;
(*this)(3,1)=x; (*this)(3,2)=x; (*this)(3,3)=x;
}
// set DOUBLE 3x3
Matrix<DOUBLE,3,3>::Matrix(const DOUBLE x /*= Type(0)*/)
{
// set whole matrix to constant
(*this)(1,1)=x; (*this)(1,2)=x; (*this)(1,3)=x;
(*this)(2,1)=x; (*this)(2,2)=x; (*this)(2,3)=x;
(*this)(3,1)=x; (*this)(3,2)=x; (*this)(3,3)=x;
}
template<class Type, int iRows, int iColumns>
Matrix<Type, iRows, iColumns>::Matrix(const Type x /*= Type(0)*/)
{
ASSERT( iRows!=3 && iColumns!=3); // 3 is optimized special case
// set whole matrix to constant
for(int iRow=1; iRow<=iRows; iRow++) {
for(int iColumn=1; iColumn<=iColumns; iColumn++) {
(*this)(iRow, iColumn) = x;
}
}
}
/*
* Reference matrix member by it's row and column indices.
*/
template<class Type, int iRows, int iColumns>
__forceinline Type &Matrix<Type, iRows, iColumns>::operator()(int iRow, int iColumn)
{
// check boundaries (indices start at 1, not at 0)
ASSERT(iRow>=1 && iRow<=iRows && iColumn>=1 && iColumn<=iColumns);
// return member reference
return matrix[iRow-1][iColumn-1];
}
template<class Type, int iRows, int iColumns>
__forceinline const Type &Matrix<Type, iRows, iColumns>::operator()(int iRow, int iColumn) const
{
// check boundaries (indices start at 1, not at 0)
ASSERT(iRow>=1 && iRow<=iRows && iColumn>=1 && iColumn<=iColumns);
// return member reference
return matrix[iRow-1][iColumn-1];
}
/* Mathematical operators. */
// transposed FLOAT 3x3
__forceinline Matrix<FLOAT,3,3> &Matrix<FLOAT,3,3>::operator!=(const Matrix<FLOAT,3,3> &matrix2)
{
(*this)(1,1)=matrix2(1,1); (*this)(1,2)=matrix2(2,1); (*this)(1,3)=matrix2(3,1);
(*this)(2,1)=matrix2(1,2); (*this)(2,2)=matrix2(2,2); (*this)(2,3)=matrix2(3,2);
(*this)(3,1)=matrix2(1,3); (*this)(3,2)=matrix2(2,3); (*this)(3,3)=matrix2(3,3);
return *this;
}
// transposed DOUBLE 3x3
__forceinline Matrix<DOUBLE,3,3> &Matrix<DOUBLE,3,3>::operator!=(const Matrix<DOUBLE,3,3> &matrix2)
{
(*this)(1,1)=matrix2(1,1); (*this)(1,2)=matrix2(2,1); (*this)(1,3)=matrix2(3,1);
(*this)(2,1)=matrix2(1,2); (*this)(2,2)=matrix2(2,2); (*this)(2,3)=matrix2(3,2);
(*this)(3,1)=matrix2(1,3); (*this)(3,2)=matrix2(2,3); (*this)(3,3)=matrix2(3,3);
return *this;
}
// transposed matrix
template<class Type, int iRows, int iColumns>
__forceinline Matrix<Type, iRows, iColumns> Matrix<Type, iRows, iColumns>::operator!(void) const
{
return Matrix<Type, iRows, iColumns>() != *this;
}
template<class Type, int iRows, int iColumns>
__forceinline Matrix<Type, iRows, iColumns> &Matrix<Type, iRows, iColumns>::operator!=(const Matrix<Type, iRows, iColumns> &matrix2)
{
// transpose member by member
ASSERT( iRows!=3 && iColumns!=3); // 3 is optimized special case
for(int iRow=1; iRow<=iRows; iRow++) {
for(int iColumn=1; iColumn<=iColumns; iColumn++) {
(*this)(iColumn, iRow) = matrix2(iRow, iColumn);
}
}
return *this;
}
// sum of two FLOATs 3x3
__forceinline Matrix<FLOAT,3,3> &Matrix<FLOAT,3,3>::operator+=(const Matrix<FLOAT,3,3> &matrix2)
{
(*this)(1,1)+=matrix2(1,1); (*this)(1,2)+=matrix2(1,2); (*this)(1,3)+=matrix2(1,3);
(*this)(2,1)+=matrix2(2,1); (*this)(2,2)+=matrix2(2,2); (*this)(2,3)+=matrix2(2,3);
(*this)(3,1)+=matrix2(3,1); (*this)(3,2)+=matrix2(3,2); (*this)(3,3)+=matrix2(3,3);
return *this;
}
// sum of two DOUBLEs 3x3
__forceinline Matrix<DOUBLE,3,3> &Matrix<DOUBLE,3,3>::operator+=(const Matrix<DOUBLE,3,3> &matrix2)
{
(*this)(1,1)+=matrix2(1,1); (*this)(1,2)+=matrix2(1,2); (*this)(1,3)+=matrix2(1,3);
(*this)(2,1)+=matrix2(2,1); (*this)(2,2)+=matrix2(2,2); (*this)(2,3)+=matrix2(2,3);
(*this)(3,1)+=matrix2(3,1); (*this)(3,2)+=matrix2(3,2); (*this)(3,3)+=matrix2(3,3);
return *this;
}
// sum of two matrices
template<class Type, int iRows, int iColumns>
__forceinline Matrix<Type, iRows, iColumns> &Matrix<Type, iRows, iColumns>::operator+=(const Matrix<Type, iRows, iColumns> &matrix2)
{
// add member by member
ASSERT( iRows!=3 && iColumns!=3); // 3 is optimized special case
for(int iRow=1; iRow<=iRows; iRow++) {
for(int iColumn=1; iColumn<=iColumns; iColumn++) {
(*this)(iRow, iColumn) += matrix2(iRow, iColumn);
}
}
return *this;
}
template<class Type, int iRows, int iColumns>
__forceinline Matrix<Type, iRows, iColumns> Matrix<Type, iRows, iColumns>::operator+(const Matrix<Type, iRows, iColumns> &matrix2) const
{
return Matrix<Type, iRows, iColumns>(*this)+=matrix2;
}
// difference of two FLOATs 3x3
__forceinline Matrix<FLOAT,3,3> &Matrix<FLOAT,3,3>::operator-=(const Matrix<FLOAT,3,3> &matrix2)
{
(*this)(1,1)-=matrix2(1,1); (*this)(1,2)-=matrix2(1,2); (*this)(1,3)-=matrix2(1,3);
(*this)(2,1)-=matrix2(2,1); (*this)(2,2)-=matrix2(2,2); (*this)(2,3)-=matrix2(2,3);
(*this)(3,1)-=matrix2(3,1); (*this)(3,2)-=matrix2(3,2); (*this)(3,3)-=matrix2(3,3);
return *this;
}
// difference of two DOUBLEs 3x3
__forceinline Matrix<DOUBLE,3,3> &Matrix<DOUBLE,3,3>::operator-=(const Matrix<DOUBLE,3,3> &matrix2)
{
(*this)(1,1)-=matrix2(1,1); (*this)(1,2)-=matrix2(1,2); (*this)(1,3)-=matrix2(1,3);
(*this)(2,1)-=matrix2(2,1); (*this)(2,2)-=matrix2(2,2); (*this)(2,3)-=matrix2(2,3);
(*this)(3,1)-=matrix2(3,1); (*this)(3,2)-=matrix2(3,2); (*this)(3,3)-=matrix2(3,3);
return *this;
}
// difference of two matrices
template<class Type, int iRows, int iColumns>
__forceinline Matrix<Type, iRows, iColumns> &Matrix<Type, iRows, iColumns>::operator-=(const Matrix<Type, iRows, iColumns> &matrix2)
{
// sub member by member
ASSERT( iRows!=3 && iColumns!=3); // 3 is optimized special case
for(int iRow=1; iRow<=iRows; iRow++) {
for(int iColumn=1; iColumn<=iColumns; iColumn++) {
(*this)(iRow, iColumn) -= matrix2(iRow, iColumn);
}
}
return *this;
}
template<class Type, int iRows, int iColumns>
__forceinline Matrix<Type, iRows, iColumns> Matrix<Type, iRows, iColumns>::operator-(const Matrix<Type, iRows, iColumns> &matrix2) const
{
return Matrix<Type, iRows, iColumns>(*this)-=matrix2;
}
// multiplication of two square matrices of same dimensions
/*
* Dimensions: A(n,k), B(k,p), C(n,p)
*
* Formula: C=AxB --> Cij=Sum(s=1..k)(Ais*Bsj)
*/
// FLOAT 3x3
__forceinline Matrix<FLOAT,3,3> Matrix<FLOAT,3,3>::operator*(const Matrix<FLOAT,3,3> &matrix2) const
{
Matrix<FLOAT,3,3> result;
result(1,1) = (*this)(1,1) * matrix2(1,1) + (*this)(1,2) * matrix2(2,1) + (*this)(1,3) * matrix2(3,1);
result(1,2) = (*this)(1,1) * matrix2(1,2) + (*this)(1,2) * matrix2(2,2) + (*this)(1,3) * matrix2(3,2);
result(1,3) = (*this)(1,1) * matrix2(1,3) + (*this)(1,2) * matrix2(2,3) + (*this)(1,3) * matrix2(3,3);
result(2,1) = (*this)(2,1) * matrix2(1,1) + (*this)(2,2) * matrix2(2,1) + (*this)(2,3) * matrix2(3,1);
result(2,2) = (*this)(2,1) * matrix2(1,2) + (*this)(2,2) * matrix2(2,2) + (*this)(2,3) * matrix2(3,2);
result(2,3) = (*this)(2,1) * matrix2(1,3) + (*this)(2,2) * matrix2(2,3) + (*this)(2,3) * matrix2(3,3);
result(3,1) = (*this)(3,1) * matrix2(1,1) + (*this)(3,2) * matrix2(2,1) + (*this)(3,3) * matrix2(3,1);
result(3,2) = (*this)(3,1) * matrix2(1,2) + (*this)(3,2) * matrix2(2,2) + (*this)(3,3) * matrix2(3,2);
result(3,3) = (*this)(3,1) * matrix2(1,3) + (*this)(3,2) * matrix2(2,3) + (*this)(3,3) * matrix2(3,3);
return result;
}
// DOUBLE 3x3
__forceinline Matrix<DOUBLE,3,3> Matrix<DOUBLE,3,3>::operator*(const Matrix<DOUBLE,3,3> &matrix2) const
{
Matrix<DOUBLE,3,3> result;
result(1,1) = (*this)(1,1) * matrix2(1,1) + (*this)(1,2) * matrix2(2,1) + (*this)(1,3) * matrix2(3,1);
result(1,2) = (*this)(1,1) * matrix2(1,2) + (*this)(1,2) * matrix2(2,2) + (*this)(1,3) * matrix2(3,2);
result(1,3) = (*this)(1,1) * matrix2(1,3) + (*this)(1,2) * matrix2(2,3) + (*this)(1,3) * matrix2(3,3);
result(2,1) = (*this)(2,1) * matrix2(1,1) + (*this)(2,2) * matrix2(2,1) + (*this)(2,3) * matrix2(3,1);
result(2,2) = (*this)(2,1) * matrix2(1,2) + (*this)(2,2) * matrix2(2,2) + (*this)(2,3) * matrix2(3,2);
result(2,3) = (*this)(2,1) * matrix2(1,3) + (*this)(2,2) * matrix2(2,3) + (*this)(2,3) * matrix2(3,3);
result(3,1) = (*this)(3,1) * matrix2(1,1) + (*this)(3,2) * matrix2(2,1) + (*this)(3,3) * matrix2(3,1);
result(3,2) = (*this)(3,1) * matrix2(1,2) + (*this)(3,2) * matrix2(2,2) + (*this)(3,3) * matrix2(3,2);
result(3,3) = (*this)(3,1) * matrix2(1,3) + (*this)(3,2) * matrix2(2,3) + (*this)(3,3) * matrix2(3,3);
return result;
}
// general
template<class Type, int iRows, int iColumns>
__forceinline Matrix<Type, iRows, iColumns> Matrix<Type, iRows, iColumns>::operator*(const Matrix<Type, iRows, iColumns> &matrix2) const
{
Matrix<Type, iRows, iColumns> result;
// check that the matrices have square dimensions
ASSERT(iRows==iColumns);
ASSERT( iRows!=3 && iColumns!=3); // 3 is optimized special case
// multiply
for(int iRow=1; iRow<=iRows; iRow++) {
for(int iColumn=1; iColumn<=iColumns; iColumn++) {
result(iRow, iColumn) = (Type)0;
for(int s=1; s<=iRows; s++) {
result(iRow, iColumn) += (*this)(iRow, s) * matrix2(s, iColumn);
}
}
}
return result;
}
// general
template<class Type, int iRows, int iColumns>
__forceinline Matrix<Type, iRows, iColumns> &Matrix<Type, iRows, iColumns>::operator*=(const Matrix<Type, iRows, iColumns> &matrix2)
{
*this = *this * matrix2;
return *this;
}
// multiply FLOAT 3x3 with scalar
__forceinline Matrix<FLOAT,3,3> &Matrix<FLOAT,3,3>::operator*=(const FLOAT tMul)
{
(*this)(1,1)*=tMul; (*this)(1,2)*=tMul; (*this)(1,3)*=tMul;
(*this)(2,1)*=tMul; (*this)(2,2)*=tMul; (*this)(2,3)*=tMul;
(*this)(3,1)*=tMul; (*this)(3,2)*=tMul; (*this)(3,3)*=tMul;
return *this;
}
// multiply DOUBLE 3x3 with scalar
__forceinline Matrix<DOUBLE,3,3> &Matrix<DOUBLE,3,3>::operator*=(const DOUBLE tMul)
{
(*this)(1,1)*=tMul; (*this)(1,2)*=tMul; (*this)(1,3)*=tMul;
(*this)(2,1)*=tMul; (*this)(2,2)*=tMul; (*this)(2,3)*=tMul;
(*this)(3,1)*=tMul; (*this)(3,2)*=tMul; (*this)(3,3)*=tMul;
return *this;
}
// multiply matrix with scalar
template<class Type, int iRows, int iColumns>
__forceinline Matrix<Type, iRows, iColumns> &Matrix<Type, iRows, iColumns>::operator*=(const Type tMul)
{
// multiply member by member
ASSERT( iRows!=3 && iColumns!=3); // 3 is optimized special case
for(int iRow=1; iRow<=iRows; iRow++) {
for(int iColumn=1; iColumn<=iColumns; iColumn++) {
(*this)(iRow, iColumn) *= tMul;
}
}
return *this;
}
// multiply matrix with scalar
template<class Type, int iRows, int iColumns>
__forceinline Matrix<Type, iRows, iColumns> Matrix<Type, iRows, iColumns>::operator*(const Type tMul) const
{
return Matrix<Type, iRows, iColumns>(*this)*=tMul;
}
// divide matrix with scalar
template<class Type, int iRows, int iColumns>
__forceinline Matrix<Type, iRows, iColumns> &Matrix<Type, iRows, iColumns>::operator/=(const Type tDiv)
{
// multiply with reciprocal
(*this)*=(1/tDiv);
return *this;
}
// divide matrix with scalar
template<class Type, int iRows, int iColumns>
__forceinline Matrix<Type, iRows, iColumns> Matrix<Type, iRows, iColumns>::operator/(const Type tDiv) const
{
// multiply with reciprocal
return Matrix<Type, iRows, iColumns>(*this)*=(1/tDiv);
}
/*
* Set matrix main diagonal.
*/
template<class Type, int iRows, int iColumns>
void Matrix<Type, iRows, iColumns>::Diagonal(Type x)
{
// check that the matrix is symetric
ASSERT(iRows==iColumns);
// clear whole matrix to zeroes
for(int iRow=1; iRow<=iRows; iRow++) {
for(int iColumn=1; iColumn<=iColumns; iColumn++) {
(*this)(iRow, iColumn) = Type(0);
}
}
// set the main diagonal
{for(int iRow=0; iRow<iRows; iRow++) {
matrix[iRow][iRow] = x;
}}
}
template<class Type, int iRows, int iColumns>
void Matrix<Type, iRows, iColumns>::Diagonal(const Vector<Type, iRows> &v)
{
// check that the matrix is symetric
ASSERT(iRows==iColumns);
// clear whole matrix to zeroes
for(int iRow=1; iRow<=iRows; iRow++) {
for(int iColumn=1; iColumn<=iColumns; iColumn++) {
(*this)(iRow, iColumn) = Type(0);
}
}
// set the main diagonal
{for(int iRow=1; iRow<=iRows; iRow++) {
operator()(iRow, iRow) = v(iRow);
}}
}
// get main vectors of matrix
template<class Type, int iRows, int iColumns>
Vector<Type, iColumns> Matrix<Type, iRows, iColumns>::GetRow(Type iRow) const
{
Vector<Type, iColumns> v;
for(int iColumn=1; iColumn<=iColumns; iColumn++) {
v(iColumn) = (*this)(iRow, iColumn);
}
return v;
}
template<class Type, int iRows, int iColumns>
Vector<Type, iRows> Matrix<Type, iRows, iColumns>::GetColumn(Type iColumn) const
{
Vector<Type, iRows> v;
for(int iRow=1; iRow<=iRows; iRow++) {
v(iRow) = (*this)(iRow, iColumn);
}
return v;
}
// helper functions for converting between FLOAT and DOUBLE matrices
__forceinline DOUBLEmatrix3D FLOATtoDOUBLE(const FLOATmatrix3D &mf)
{
DOUBLEmatrix3D m;
m(1,1) = FLOATtoDOUBLE(mf(1,1)); m(1,2) = FLOATtoDOUBLE(mf(1,2)); m(1,3) = FLOATtoDOUBLE(mf(1,3));
m(2,1) = FLOATtoDOUBLE(mf(2,1)); m(2,2) = FLOATtoDOUBLE(mf(2,2)); m(2,3) = FLOATtoDOUBLE(mf(2,3));
m(3,1) = FLOATtoDOUBLE(mf(3,1)); m(3,2) = FLOATtoDOUBLE(mf(3,2)); m(3,3) = FLOATtoDOUBLE(mf(3,3));
return m;
}
__forceinline FLOATmatrix3D DOUBLEtoFLOAT(const DOUBLEmatrix3D &md) {
FLOATmatrix3D m;
m(1,1) = DOUBLEtoFLOAT(md(1,1)); m(1,2) = DOUBLEtoFLOAT(md(1,2)); m(1,3) = DOUBLEtoFLOAT(md(1,3));
m(2,1) = DOUBLEtoFLOAT(md(2,1)); m(2,2) = DOUBLEtoFLOAT(md(2,2)); m(2,3) = DOUBLEtoFLOAT(md(2,3));
m(3,1) = DOUBLEtoFLOAT(md(3,1)); m(3,2) = DOUBLEtoFLOAT(md(3,2)); m(3,3) = DOUBLEtoFLOAT(md(3,3));
return m;
}
#endif /* include-once check. */