Xentax forum attachments archive/xentax.7z/4819/rage.7z/slMatrix.h

C/C++ Source or Header | 2011-09-19 | 13.0 KB | 551 lines

#ifndef __SL_MATRIX_H
#define __SL_MATRIX_H

#include<iostream>
#include<cstring>
#include<cmath>

#include "slMathTraits.h"
#include "slVector3D.h"

namespace sl {

template<class T>
class matrix4x4 {
 private :
  T m[16];
 public :
  static const unsigned int M11 =  0;
  static const unsigned int M12 =  1;
  static const unsigned int M13 =  2;
  static const unsigned int M14 =  3;
  static const unsigned int M21 =  4;
  static const unsigned int M22 =  5;
  static const unsigned int M23 =  6;
  static const unsigned int M24 =  7;
  static const unsigned int M31 =  8;
  static const unsigned int M32 =  9;
  static const unsigned int M33 = 10;
  static const unsigned int M34 = 11;
  static const unsigned int M41 = 12;
  static const unsigned int M42 = 13;
  static const unsigned int M43 = 14;
  static const unsigned int M44 = 15;
 public :
  T& operator [](int index);
  const T& operator [](int index)const;
  T& operator ()(int r, int c);
  const T& operator ()(int r, int c)const;
 public :
  matrix4x4<T>& operator +=(const matrix4x4<T>& M);
  matrix4x4<T>& operator -=(const matrix4x4<T>& M);
  matrix4x4<T>& operator *=(const matrix4x4<T>& M);
  matrix4x4<T>& operator *=(T scalar);
  matrix4x4<T>& operator /=(T scalar);
 public :
  matrix4x4<T>& operator =(const matrix4x4<T>& M);
 public :
  const T* c_ptr(void)const;
 public :
  void load_identity(void);
  void load_translation(T x, T y, T z);
  void load_translation(const vector3D<T>& v);
  void load_rotation_x(T radians);
  void load_rotation_y(T radians);
  void load_rotation_z(T radians);
  void load_rotation(T radians, T x, T y, T z);
  void load_euler(T H, T P, T B);
  void load_euler_lw(T H, T P, T B);
 public :
  void translate(T x, T y, T z);
  void translate(const vector3D<T>& v);
  void rotate_x(T radians);
  void rotate_y(T radians);
  void rotate_z(T radians);
  void rotate(T radians, T x, T y, T z);
 public :
  void to_euler(T& H, T& P, T& B);
  void multiply_by(const vector3D<T>& V, vector3D<T>& R);
 public :
  matrix4x4();
  matrix4x4(T* data);
  matrix4x4(const matrix4x4<T>& M);
 ~matrix4x4();
};

template<class T>
inline matrix4x4<T>::matrix4x4()
{
}

template<class T>
inline matrix4x4<T>::matrix4x4(T* data)
{
 std::memmove(m, data, 16*sizeof(T));
}

template<class T>
inline matrix4x4<T>::matrix4x4(const matrix4x4<T>& M)
{
 std::memmove(m, M.m, 16*sizeof(T));
}

template<class T>
inline matrix4x4<T>::~matrix4x4()
{
}

template<class T>
inline matrix4x4<T>& matrix4x4<T>::operator =(const matrix4x4<T>& M)
{
 if(this == &M) return *this;
 std::memmove(m, M.m, 16*sizeof(T));
 return *this;
}

template<class T>
T& matrix4x4<T>::operator [](int index)
{
 return m[index];
}

template<class T>
const T& matrix4x4<T>::operator [](int index)const
{
 return m[index];
}

template<class T>
inline T& matrix4x4<T>::operator ()(int r, int c)
{
 return m[4*r + c];
}

template<class T>
inline const T& matrix4x4<T>::operator ()(int r, int c)const
{
 return m[4*r + c];
}

template<class T>
inline const T* matrix4x4<T>::c_ptr(void)const
{
 return &m[0];
}

template<class T>
inline void matrix4x4<T>::load_identity(void)
{
 T v0 = math_traits<T>::zero();
 T v1 = math_traits<T>::one();
 m[ 0] = v1; m[ 1] = v0; m[ 2] = v0; m[ 3] = v0;
 m[ 4] = v0; m[ 5] = v1; m[ 6] = v0; m[ 7] = v0;
 m[ 8] = v0; m[ 9] = v0; m[10] = v1; m[11] = v0;
 m[12] = v0; m[13] = v0; m[14] = v0; m[15] = v1;
}

template<class T>
inline void matrix4x4<T>::load_translation(T x, T y, T z)
{
 T v0 = math_traits<T>::zero();
 T v1 = math_traits<T>::one();
 m[ 0] = v1; m[ 1] = v0; m[ 2] = v0; m[ 3] = x;
 m[ 4] = v0; m[ 5] = v1; m[ 6] = v0; m[ 7] = y;
 m[ 8] = v0; m[ 9] = v0; m[10] = v1; m[11] = z;
 m[12] = v0; m[13] = v0; m[14] = v0; m[15] = v1;
}

template<class T>
inline void matrix4x4<T>::load_translation(const vector3D<T>& v)
{
 T v0 = math_traits<T>::zero();
 T v1 = math_traits<T>::one();
 m[ 0] = v1; m[ 1] = v0; m[ 2] = v0; m[ 3] = v.data[0];
 m[ 4] = v0; m[ 5] = v1; m[ 6] = v0; m[ 7] = v.data[1];
 m[ 8] = v0; m[ 9] = v0; m[10] = v1; m[11] = v.data[2];
 m[12] = v0; m[13] = v0; m[14] = v0; m[15] = v1;
}

template<class T>
inline void matrix4x4<T>::load_rotation_x(T radians)
{
 T v0 = math_traits<T>::zero();
 T v1 = math_traits<T>::one();
 T pc = std::cos(radians);
 T ps = std::sin(radians);
 T ns = -ps;
 m[ 0] = v1; m[ 1] = v0; m[ 2] = v0; m[ 3] = v0;
 m[ 4] = v0; m[ 5] = pc; m[ 6] = ns; m[ 7] = v0;
 m[ 8] = v0; m[ 9] = ps; m[10] = pc; m[11] = v0;
 m[12] = v0; m[13] = v0; m[14] = v0; m[15] = v1;
}

template<class T>
inline void matrix4x4<T>::load_rotation_y(T radians)
{
 T v0 = math_traits<T>::zero();
 T v1 = math_traits<T>::one();
 T pc = std::cos(radians);
 T ps = std::sin(radians);
 T ns = -ps;
 m[ 0] = pc; m[ 1] = v0; m[ 2] = ps; m[ 3] = v0;
 m[ 4] = v0; m[ 5] = v1; m[ 6] = v0; m[ 7] = v0;
 m[ 8] = ns; m[ 9] = v0; m[10] = pc; m[11] = v0;
 m[12] = v0; m[13] = v0; m[14] = v0; m[15] = v1;
}

template<class T>
inline void matrix4x4<T>::load_rotation_z(T radians)
{
 T v0 = math_traits<T>::zero();
 T v1 = math_traits<T>::one();
 T pc = std::cos(radians);
 T ps = std::sin(radians);
 T ns = -ps;
 m[ 0] = pc; m[ 1] = ns; m[ 2] = v0; m[ 3] = v0;
 m[ 4] = ps; m[ 5] = pc; m[ 6] = v0; m[ 7] = v0;
 m[ 8] = v0; m[ 9] = v0; m[10] = v1; m[11] = v0;
 m[12] = v0; m[13] = v0; m[14] = v0; m[15] = v1;
}

template<class T>
inline void matrix4x4<T>::load_rotation(T radians, T x, T y, T z)
{
 // pre-calculated values
 T v0 = math_traits<T>::zero();
 T v1 = math_traits<T>::one();
 T pc = std::cos(radians);
 T ps = std::sin(radians);
 T psx = ps*x;
 T psy = ps*y;
 T psz = ps*z;
 T one_minus_pc = v1 - pc;
 T one_minus_pc_xy = one_minus_pc*x*y;
 T one_minus_pc_xz = one_minus_pc*x*z;
 T one_minus_pc_yz = one_minus_pc*y*z;

 // r1
 m[M11] = pc + one_minus_pc*x*x;
 m[M12] = one_minus_pc_xy - psz;
 m[M13] = one_minus_pc_xz + psy;
 m[M14] = v0;

 // r2
 m[M21] = one_minus_pc_xy + psz;
 m[M22] = pc + one_minus_pc*y*y;
 m[M23] = one_minus_pc_yz - psx;
 m[M24] = v0;

 // r3
 m[M31] = one_minus_pc_xz - psy;
 m[M32] = one_minus_pc_yz + psx;
 m[M33] = pc + one_minus_pc*z*z;
 m[M34] = v0;

 // r4
 m[M41] = v0;
 m[M42] = v0;
 m[M43] = v0;
 m[M44] = v1;
}

template<class T>
inline void matrix4x4<T>::load_euler(T H, T P, T B)
{
 // pre-calculated values
 T v0 = math_traits<T>::zero();
 T v1 = math_traits<T>::one();
 T cH = std::cos(H); T sH = std::sin(H);
 T cP = std::cos(P); T sP = std::sin(P);
 T cB = std::cos(B); T sB = std::sin(B);
 T sPsH = sP*sH;
 T sPcH = sP*cH;

 // r1
 m[M11] = cB*cH - sB*sPsH;
 m[M12] = -sB*cP;
 m[M13] = cB*sH + sB*sPcH;
 m[M14] = v0;

 // r2
 m[M21] = sB*cH + cB*sPsH;
 m[M22] = cB*cP;
 m[M23] = sB*sH - cB*sPcH;
 m[M24] = v0;

 // r3
 m[M31] = -sH*cP;
 m[M32] = sP;
 m[M33] = cP*cH;
 m[M34] = v0;

 // r4
 m[M41] = v0;
 m[M42] = v0;
 m[M43] = v0;
 m[M44] = v1;
}

template<class T>
inline void matrix4x4<T>::load_euler_lw(T H, T P, T B)
{
 // pre-calculated values
 T v0 = math_traits<T>::zero();
 T v1 = math_traits<T>::one();
 T cH = std::cos(H); T sH = std::sin(H);
 T cP = std::cos(P); T sP = std::sin(P);
 T cB = std::cos(B); T sB = std::sin(B);

 // r1
 m[M11] =  cH*cB;
 m[M12] = -cH*sB;
 m[M13] =  sH;
 m[M14] =  v0;

 // r2
 m[M21] =  cP*sB + sH*sP*cB;
 m[M22] =  cP*cB - sH*sP*sB;
 m[M23] = -cH*sP;
 m[M24] =  v0;

 // r3
 m[M31] = sP*sB - sH*cP*cB;
 m[M32] = sP*cB + sH*cP*sB;
 m[M33] = cH*cP;
 m[M34] = v0;

 // r4
 m[M41] = v0;
 m[M42] = v0;
 m[M43] = v0;
 m[M44] = v1;
}

template<class T>
inline void matrix4x4<T>::translate(T x, T y, T z)
{
 m[M14] += m[M11]*x + m[M12]*y + m[M13]*z;
 m[M24] += m[M21]*x + m[M22]*y + m[M23]*z;
 m[M34] += m[M31]*x + m[M32]*y + m[M33]*z;
 m[M44] += m[M41]*x + m[M42]*y + m[M43]*z;
}

template<class T>
inline void matrix4x4<T>::translate(const vector3D<T>& v)
{
 m[M14] += m[M11]*v.data[0] + m[M12]*v.data[1] + m[M13]*v.data[2];
 m[M24] += m[M21]*v.data[0] + m[M22]*v.data[1] + m[M23]*v.data[2];
 m[M34] += m[M31]*v.data[0] + m[M32]*v.data[1] + m[M33]*v.data[2];
 m[M44] += m[M41]*v.data[0] + m[M42]*v.data[1] + m[M43]*v.data[2];
}

template<class T>
inline void matrix4x4<T>::rotate_x(T radians)
{
 // TODO
}

template<class T>
inline void matrix4x4<T>::rotate_y(T radians)
{
 // TODO
}

template<class T>
inline void matrix4x4<T>::rotate_z(T radians)
{
 // TODO
}

template<class T>
inline void matrix4x4<T>::rotate(T radians, T x, T y, T z)
{
 // TODO
}

template<class T>
inline void matrix4x4<T>::to_euler(T& H, T& P, T& B)
{
 // sin(P) = +1, cos(P) = 0
 if(m[M32] == math_traits<T>::one())
   {
    H = math_traits<T>::zero();
    P = math_traits<T>::pi_over_2();
    B = std::atan2(-m[M21], -m[M22]);
   }
 // sin(P) = -1, cos(P) = 0
 else if(m[M31] == math_traits<T>::neg_one())
   {
    H = math_traits<T>::zero();
    P = math_traits<T>::neg_pi_over_2();
    B = std::atan2(m[M21], m[M22]);
   }
 // otherwise
 else
   {
    P = asin(m[M32]);
    T temp = cos(P);
    H = atan2(-m[M31]/temp, m[M33]/temp);
    B = atan2(-m[M12]/temp, m[M22]/temp);
   }
}

template<class T>
inline void matrix4x4<T>::multiply_by(const vector3D<T>& V, vector3D<T>& R)
{
 R[0] = m[M11]*V[0] + m[M12]*V[1] + m[M13]*V[2];
 R[1] = m[M21]*V[0] + m[M22]*V[1] + m[M23]*V[2];
 R[2] = m[M31]*V[0] + m[M32]*V[1] + m[M33]*V[2];
}

template<class T>
inline matrix4x4<T>& matrix4x4<T>::operator +=(const matrix4x4<T>& M)
{
 // r1
 m[M11] += M.m[M11];
 m[M12] += M.m[M12];
 m[M13] += M.m[M13];
 m[M14] += M.m[M14];

 // r2
 m[M21] += M.m[M21];
 m[M22] += M.m[M22];
 m[M23] += M.m[M23];
 m[M24] += M.m[M24];

 // r3
 m[M31] += M.m[M31];
 m[M32] += M.m[M32];
 m[M33] += M.m[M33];
 m[M34] += M.m[M34];

 // r4
 m[M41] += M.m[M41];
 m[M42] += M.m[M42];
 m[M43] += M.m[M43];
 m[M44] += M.m[M44];

 return *this;
}

template<class T>
inline matrix4x4<T>& matrix4x4<T>::operator -=(const matrix4x4<T>& M)
{
 // r1
 m[M11] -= M.m[M11];
 m[M12] -= M.m[M12];
 m[M13] -= M.m[M13];
 m[M14] -= M.m[M14];

 // r2
 m[M21] -= M.m[M21];
 m[M22] -= M.m[M22];
 m[M23] -= M.m[M23];
 m[M24] -= M.m[M24];

 // r3
 m[M31] -= M.m[M31];
 m[M32] -= M.m[M32];
 m[M33] -= M.m[M33];
 m[M34] -= M.m[M34];

 // r4
 m[M41] -= M.m[M41];
 m[M42] -= M.m[M42];
 m[M43] -= M.m[M43];
 m[M44] -= M.m[M44];

 return *this;
}

template<class T>
inline matrix4x4<T>& matrix4x4<T>::operator *=(const matrix4x4<T>& M)
{
 // copy this matrix
 T A[16] = {
  m[M11], m[M12], m[M13], m[M14],
  m[M21], m[M22], m[M23], m[M24],
  m[M31], m[M32], m[M33], m[M34],
  m[M41], m[M42], m[M43], m[M44]
 };

 // r1
 m[M11] = A[M11]*M.m[M11] + A[M12]*M.m[M21] + A[M13]*M.m[M31] + A[M14]*M.m[M41];
 m[M12] = A[M11]*M.m[M12] + A[M12]*M.m[M22] + A[M13]*M.m[M32] + A[M14]*M.m[M42];
 m[M13] = A[M11]*M.m[M13] + A[M12]*M.m[M23] + A[M13]*M.m[M33] + A[M14]*M.m[M43];
 m[M14] = A[M11]*M.m[M14] + A[M12]*M.m[M24] + A[M13]*M.m[M34] + A[M14]*M.m[M44];

 // r2
 m[M21] = A[M21]*M.m[M11] + A[M22]*M.m[M21] + A[M23]*M.m[M31] + A[M24]*M.m[M41];
 m[M22] = A[M21]*M.m[M12] + A[M22]*M.m[M22] + A[M23]*M.m[M32] + A[M24]*M.m[M42];
 m[M23] = A[M21]*M.m[M13] + A[M22]*M.m[M23] + A[M23]*M.m[M33] + A[M24]*M.m[M43];
 m[M24] = A[M21]*M.m[M14] + A[M22]*M.m[M24] + A[M23]*M.m[M34] + A[M24]*M.m[M44];

 // r3
 m[M31] = A[M31]*M.m[M11] + A[M32]*M.m[M21] + A[M33]*M.m[M31] + A[M34]*M.m[M41];
 m[M32] = A[M31]*M.m[M12] + A[M32]*M.m[M22] + A[M33]*M.m[M32] + A[M34]*M.m[M42];
 m[M33] = A[M31]*M.m[M13] + A[M32]*M.m[M23] + A[M33]*M.m[M33] + A[M34]*M.m[M43];
 m[M34] = A[M31]*M.m[M14] + A[M32]*M.m[M24] + A[M33]*M.m[M34] + A[M34]*M.m[M44];

 // r4
 m[M41] = A[M41]*M.m[M11] + A[M42]*M.m[M21] + A[M43]*M.m[M31] + A[M44]*M.m[M41];
 m[M42] = A[M41]*M.m[M12] + A[M42]*M.m[M22] + A[M43]*M.m[M32] + A[M44]*M.m[M42];
 m[M43] = A[M41]*M.m[M13] + A[M42]*M.m[M23] + A[M43]*M.m[M33] + A[M44]*M.m[M43];
 m[M44] = A[M41]*M.m[M14] + A[M42]*M.m[M24] + A[M43]*M.m[M34] + A[M44]*M.m[M44];

 return *this;
}

template<class T>
inline matrix4x4<T>& matrix4x4<T>::operator *=(T scalar)
{
 // r1
 m[M11] *= scalar;
 m[M12] *= scalar;
 m[M13] *= scalar;
 m[M14] *= scalar;

 // r2
 m[M21] *= scalar;
 m[M22] *= scalar;
 m[M23] *= scalar;
 m[M24] *= scalar;

 // r3
 m[M31] *= scalar;
 m[M32] *= scalar;
 m[M33] *= scalar;
 m[M34] *= scalar;

 // r4
 m[M41] *= scalar;
 m[M42] *= scalar;
 m[M43] *= scalar;
 m[M44] *= scalar;

 return *this;
}

template<class T>
inline matrix4x4<T>& matrix4x4<T>::operator /=(T scalar)
{
 scalar = math_traits<T>::one()/scalar;
 return ((*this) *= scalar);
}

};

template<class T>
inline ostream& operator <<(ostream& os, const sl::matrix4x4<T>& M)
{
 os << "[" <<
    "[" << M[ 0] << "," << M[ 1] << "," << M[ 2] << "," << M[ 3] << "]" << "," <<
    "[" << M[ 4] << "," << M[ 5] << "," << M[ 6] << "," << M[ 7] << "]" << "," <<
    "[" << M[ 8] << "," << M[ 9] << "," << M[10] << "," << M[11] << "]" << "," <<
    "[" << M[12] << "," << M[13] << "," << M[14] << "," << M[15] << "]" << "]";
 return os;
}

#endif