343 lines
11 KiB
C++
Executable File
343 lines
11 KiB
C++
Executable File
#ifndef __DTSMATRIX_H
|
|
#define __DTSMATRIX_H
|
|
|
|
#include <iomanip>
|
|
|
|
#include <vector>
|
|
#include <string>
|
|
#include <ostream>
|
|
#include <cmath>
|
|
#include <cassert>
|
|
|
|
#include "DTSVector.h"
|
|
#include "DTSPoint.h"
|
|
|
|
namespace DTS
|
|
{
|
|
//! Defines matrices
|
|
|
|
template <int rows=4, int cols=4, class type=float>
|
|
class Matrix : public Vector<Vector<type,cols>, rows>
|
|
{
|
|
public:
|
|
|
|
typedef Vector<type,cols> Row ;
|
|
typedef type Cell ;
|
|
|
|
Matrix() { /* No initialization */ }
|
|
|
|
Vector<type,rows> col (int n) {
|
|
assert (n >= 0 && n < rows) ;
|
|
|
|
Vector<type,rows> result ;
|
|
for (int r = 0 ; r < rows ; r++)
|
|
result[r] = (*this)[r][n] ;
|
|
return result ;
|
|
}
|
|
|
|
void setCol (int n,Vector<type,rows> col) {
|
|
assert (n >= 0 && n < rows) ;
|
|
|
|
for (int r = 0 ; r < rows ; r++)
|
|
(*this)[r][n] = col[r];
|
|
}
|
|
|
|
Matrix(const type *p) {
|
|
for (int r = 0 ; r < rows ; r++)
|
|
for (int c = 0 ; c < cols ; c++)
|
|
(*this)[r][c] = *p++ ;
|
|
}
|
|
|
|
inline static Matrix<rows,cols,type> identity() {
|
|
Matrix <rows,cols,type> result ;
|
|
for (int r = 0 ; r < rows ; r++)
|
|
for (int c = 0 ; c < cols ; c++)
|
|
result[r][c] = type(r == c ? 1 : 0) ;
|
|
return result ;
|
|
}
|
|
|
|
Matrix <rows, cols, type> transpose() const
|
|
{
|
|
Matrix <rows,cols,type> result ;
|
|
for (int r = 0 ; r < rows ; r++)
|
|
for (int c = 0 ; c < cols ; c++)
|
|
result[c][r] = (*this)[r][c] ;
|
|
return result ;
|
|
}
|
|
|
|
type determinant() const ;
|
|
|
|
Matrix <rows, cols, type> inverse() const ;
|
|
|
|
//!{ Matrix operators
|
|
|
|
Matrix<rows,cols,type> & operator = (const Matrix<rows,cols,type> &a) {
|
|
for (int r = 0 ; r < rows ; r++)
|
|
for (int c = 0 ; c < cols ; c++)
|
|
(*this)[r][c] = a[r][c] ;
|
|
return *this ;
|
|
}
|
|
|
|
template <int rows2, int cols2>
|
|
Matrix<rows,cols2,type>
|
|
operator * (const Matrix<rows2,cols2,type> &b)
|
|
{
|
|
Matrix<rows,cols2,type> result ;
|
|
|
|
assert (rows2 == cols) ;
|
|
for (int r = 0 ; r < rows ; r++)
|
|
{
|
|
for (int c = 0 ; c < cols2 ; c++)
|
|
{
|
|
type cell = (*this)[r][0] * b[0][c] ;
|
|
for (int e = 1 ; e < cols ; e++)
|
|
cell += (*this)[r][e] * b[e][c] ;
|
|
result[r][c] = cell ;
|
|
}
|
|
}
|
|
return result ;
|
|
}
|
|
|
|
Vector<type,rows> operator * (const Vector<type,rows> &v)
|
|
{
|
|
Vector<type,rows> result ;
|
|
for (int r = 0 ; r < rows ; r++)
|
|
{
|
|
type cell = (*this)[r][0] * v[0] ;
|
|
for (int e = 1 ; e < cols ; e++)
|
|
cell += (*this)[r][e] * v[e] ;
|
|
result[r] = cell ;
|
|
}
|
|
return result ;
|
|
}
|
|
|
|
// Not needed -- and doesn't work with vc7
|
|
//template <int rows2, int cols2>
|
|
//operator *= (const Matrix<rows2,cols2,type> &a) { (*this) = (*this) * a ; }
|
|
|
|
Matrix<rows,cols,type> operator + (const Matrix<rows,cols,type> &b)
|
|
{
|
|
Matrix<rows,cols2,type> result ;
|
|
for (int r = 0 ; r < rows ; r++)
|
|
for (int c = 0 ; c < cols2 ; c++)
|
|
result[r][c] = (*this)[r][c] + b[r][c] ;
|
|
return result ;
|
|
}
|
|
|
|
Matrix<rows,cols,type> operator += (const Matrix<rows,cols,type> &b)
|
|
{
|
|
Matrix<rows,cols2,type> result ;
|
|
for (int r = 0 ; r < rows ; r++)
|
|
for (int c = 0 ; c < cols2 ; c++)
|
|
(*this)[r][c] += b[r][c] ;
|
|
return result ;
|
|
}
|
|
|
|
Matrix<rows,cols,type> operator - (const Matrix<rows,cols,type> &b)
|
|
{
|
|
Matrix<rows,cols2,type> result ;
|
|
for (int r = 0 ; r < rows ; r++)
|
|
for (int c = 0 ; c < cols2 ; c++)
|
|
result[r][c] = (*this)[r][c] - b[r][c] ;
|
|
return result ;
|
|
}
|
|
|
|
Matrix<rows,cols,type> operator -= (const Matrix<rows,cols,type> &b)
|
|
{
|
|
Matrix<rows,cols2,type> result ;
|
|
for (int r = 0 ; r < rows ; r++)
|
|
for (int c = 0 ; c < cols2 ; c++)
|
|
(*this)[r][c] -= b[r][c] ;
|
|
return result ;
|
|
}
|
|
|
|
//!}
|
|
|
|
inline static Matrix<4,4> rotationX(float angle)
|
|
{
|
|
float cells[] = {
|
|
1, 0, 0, 0,
|
|
0, cosf(angle), -sinf(angle), 0,
|
|
0, sinf(angle), cosf(angle), 0,
|
|
0, 0, 0, 1
|
|
} ;
|
|
return Matrix<4,4> (cells) ;
|
|
}
|
|
|
|
inline static Matrix<4,4> rotationZ(float angle)
|
|
{
|
|
float cells[] = {
|
|
cosf(angle), -sinf(angle), 0, 0,
|
|
sinf(angle), cosf(angle), 0, 0,
|
|
0, 0, 1, 0,
|
|
0, 0, 0, 1
|
|
} ;
|
|
return Matrix<4,4> (cells) ;
|
|
}
|
|
|
|
inline static Matrix<4,4> rotationY(float angle)
|
|
{
|
|
float cells[] = {
|
|
cosf(angle), 0, -sinf(angle), 0,
|
|
0, 1, 0, 0,
|
|
sinf(angle), 0, cosf(angle), 0,
|
|
0, 0, 0, 1
|
|
} ;
|
|
return Matrix<4,4> (cells) ;
|
|
}
|
|
};
|
|
|
|
template <int rows, int cols, class type>
|
|
type Matrix<rows,cols,type>::determinant() const
|
|
{
|
|
int r, c, f ;
|
|
type result = 0 ;
|
|
const Matrix<rows,cols,type> &m = *this ;
|
|
|
|
assert (rows == cols) ;
|
|
switch (rows)
|
|
{
|
|
case 1:
|
|
return m[0][0] ;
|
|
case 2:
|
|
return m[0][0]*m[1][1] - m[0][1]*m[1][0] ;
|
|
case 3:
|
|
return m[0][0] * ( m[1][1]*m[2][2] - m[1][2]*m[2][1] )
|
|
- m[0][1] * ( m[1][0]*m[2][2] - m[1][2]*m[2][0] )
|
|
+ m[0][2] * ( m[1][0]*m[2][1] - m[1][1]*m[2][0] ) ;
|
|
|
|
#define GREATER_ORDER(o) \
|
|
case o: \
|
|
for (f = 0 ; f < o ; f++) \
|
|
{ \
|
|
type data[(o-1)*(o-1)], *ptr = data ; \
|
|
for (r = 0 ; r < o ; r++) \
|
|
{ \
|
|
if (r == f) continue ; \
|
|
for (c = 1 ; c < o ; c++) \
|
|
*ptr++ = m[r][c] ; \
|
|
} \
|
|
result += m[f][0] * Matrix<o-1,o-1,type>(data) \
|
|
.determinant() * ((f&1) ? -1:1) ; \
|
|
} \
|
|
return result ;
|
|
|
|
GREATER_ORDER(4)
|
|
GREATER_ORDER(5)
|
|
GREATER_ORDER(6)
|
|
GREATER_ORDER(7)
|
|
GREATER_ORDER(8)
|
|
GREATER_ORDER(9)
|
|
|
|
#undef GREATER_ORDER
|
|
|
|
default:
|
|
return 0 ;
|
|
}
|
|
}
|
|
|
|
|
|
template <int rows, int cols, class type>
|
|
Matrix<rows,cols,type> Matrix<rows,cols,type>::inverse() const
|
|
{
|
|
int r, c, r2, c2 ;
|
|
type det = determinant() ;
|
|
type p[rows][cols] ;
|
|
|
|
const Matrix<rows,cols,type> &m = *this ;
|
|
|
|
assert (rows == cols) ;
|
|
assert (det != 0) ;
|
|
|
|
switch (rows)
|
|
{
|
|
case 1:
|
|
p[0][0] = m[0][0]/det ;
|
|
break ;
|
|
case 2:
|
|
p[0][0] = m[1][1] / det ;
|
|
p[0][1] = -m[0][1] / det ;
|
|
p[1][0] = -m[1][0] / det ;
|
|
p[1][1] = m[0][0] / det ;
|
|
break ;
|
|
case 3:
|
|
p[0][0] = ( m[1][1]*m[2][2] - m[1][2]*m[2][1] ) / det ;
|
|
p[1][0] = -( m[1][0]*m[2][2] - m[1][2]*m[2][0] ) / det ;
|
|
p[2][0] = ( m[1][0]*m[2][1] - m[1][1]*m[2][0] ) / det ;
|
|
p[0][1] = -( m[0][1]*m[2][2] - m[0][2]*m[2][1] ) / det ;
|
|
p[1][1] = ( m[0][0]*m[2][2] - m[0][2]*m[2][0] ) / det ;
|
|
p[2][1] = -( m[0][0]*m[2][1] - m[0][1]*m[2][0] ) / det ;
|
|
p[0][2] = ( m[0][1]*m[1][2] - m[0][2]*m[1][1] ) / det ;
|
|
p[1][2] = -( m[0][0]*m[1][2] - m[0][2]*m[1][0] ) / det ;
|
|
p[2][2] = ( m[0][0]*m[1][1] - m[0][1]*m[1][0] ) / det ;
|
|
break ;
|
|
|
|
#define GREATER_ORDER(o) \
|
|
case o: \
|
|
for (r = 0 ; r < o ; r++) \
|
|
{ \
|
|
for (c = 0 ; c < o ; c++) \
|
|
{ \
|
|
type data[o*o], *ptr = data ; \
|
|
for (r2 = 0 ; r2 < o ; r2++) \
|
|
{ \
|
|
if (r2 == r) continue ; \
|
|
for (c2 = 0 ; c2 < o ; c2++) \
|
|
{ \
|
|
if (c2 == c) continue ; \
|
|
*ptr++ = m[r2][c2] ; \
|
|
} \
|
|
} \
|
|
type sign = 1-((r+c)%2)*2 ; \
|
|
p[c][r] = Matrix<o-1,o-1,type>(data) \
|
|
.determinant() * sign / det ; \
|
|
} \
|
|
} \
|
|
break ;
|
|
|
|
GREATER_ORDER(4)
|
|
GREATER_ORDER(5)
|
|
GREATER_ORDER(6)
|
|
GREATER_ORDER(7)
|
|
GREATER_ORDER(8)
|
|
GREATER_ORDER(9)
|
|
|
|
#undef GREATER_ORDER
|
|
|
|
default:
|
|
return 0 ;
|
|
}
|
|
|
|
return Matrix<rows,cols,type> (&p[0][0]) ;
|
|
}
|
|
|
|
template <int rows, int cols, class type>
|
|
std::ostream & operator << (std::ostream &out, const Matrix<rows,cols,type> &m)
|
|
{
|
|
out << std::setprecision(2) << std::setiosflags(std::ios::fixed) ;
|
|
for (int r = 0 ; r < rows ; r++)
|
|
{
|
|
out << "| " ;
|
|
for (int c = 0 ; c < cols ; c++)
|
|
out << std::setw(7) << m[r][c] << ' ' ;
|
|
out << "|\n" ;
|
|
}
|
|
return out ;
|
|
}
|
|
|
|
inline Point operator * (const Point &p, const Matrix<> &m)
|
|
{
|
|
Point result ;
|
|
result.x( p.x()*m[0][0] + p.y()*m[0][1] + p.z()*m[0][2] + m[0][3] ) ;
|
|
result.y( p.x()*m[1][0] + p.y()*m[1][1] + p.z()*m[1][2] + m[1][3] ) ;
|
|
result.z( p.x()*m[2][0] + p.y()*m[2][1] + p.z()*m[2][2] + m[2][3] ) ;
|
|
return result ;
|
|
}
|
|
|
|
inline Point operator * (const Matrix<> &m, const Point &p)
|
|
{
|
|
return p * m ;
|
|
}
|
|
}
|
|
|
|
#endif |