/***************************************************************************
 *   Copyright (C) 2007 by A.J. Tavakoli                                   *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   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.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

#ifndef MATRIX4_H
#define MATRIX4_H

#include <iostream>
#include "Point3.h"
#include "Vector3.h"
#include "numeric.h"


namespace Math3D {


template<class Real>
class Matrix4 {
public:
   inline Matrix4();
   inline Matrix4(const Real *_a);
   inline Matrix4(Real _m11, Real _m12, Real _m13, Real _m14,
                  Real _m21, Real _m22, Real _m23, Real _m24,
                  Real _m31, Real _m32, Real _m33, Real _m34,
                  Real _m41, Real _m42, Real _m43, Real _m44);

   inline void set(Real _m11, Real _m12, Real _m13, Real _m14,
                   Real _m21, Real _m22, Real _m23, Real _m24,
                   Real _m31, Real _m32, Real _m33, Real _m34,
                   Real _m41, Real _m42, Real _m43, Real _m44);

   inline Real& operator() (int i, int j) { return m[i][j]; }
   inline Real  operator() (int i, int j) const { return m[i][j]; }

   // matrix operations
   //inline Real    det() const;
   //inline Matrix4 inv() const;
   //inline void    inv(Matrix4 &result) const;
   inline Matrix4 transpose() const;
   inline void    transpose(Matrix4 &r) const;

   inline Matrix4 operator * (const Matrix4 &m2) const; // matrix multiplication
   inline Matrix4 operator + (const Matrix4 &m2) const; // matrix addition
   inline Matrix4 operator - (const Matrix4 &m2) const; // matrix subtraction
   inline Matrix4 operator * (Real s) const;           // multiplication by a scalar

   inline const Matrix4 & operator *= (const Matrix4 &m2); // matrix multiplication-assignment
   inline const Matrix4 & operator += (const Matrix4 &m2); // matrix addition-assignment
   inline const Matrix4 & operator -= (const Matrix4 &m2); // matrix subtraction-assignment
   inline const Matrix4 & operator *= (Real s);           // multiplication by scalar-assignment

   // matrix-vector operations
   inline Vector3<Real> transform(const Vector3<Real> &v) const;

   // matrix-point operations
   inline Point3<Real> transform(const Point3<Real> &p) const;

   // rotation transformations
   inline void rot(const Vector3<Real> &axis, Real angle); // rotation about arbitrary axis
   inline void rotx(Real angle); // rotation about x axis
   inline void roty(Real angle); // rotation about y axis
   inline void rotz(Real angle); // rotation about z axis

   // scale transformation
   inline void scale(Real sx, Real sy, Real sz);

   // translation transformation
   inline void translate(Real dx, Real dy, Real dz);

private:
  inline Real dotRC(const Matrix4 &m2, int r, int c) const;

  union {
    Real m[4][4];
    Real a[16];
  };
};


template<class Real>
inline Matrix4<Real>::Matrix4() {
  // set to identity matrix
  set(Real(1.0), Real(0.0), Real(0.0), Real(0.0),
      Real(0.0), Real(1.0), Real(0.0), Real(0.0),
      Real(0.0), Real(0.0), Real(1.0), Real(0.0),
      Real(0.0), Real(0.0), Real(0.0), Real(1.0));
}


// get matrix data from array
template<class Real>
inline Matrix4<Real>::Matrix4(const Real *_a) {
   for (int i=0; i < 16; i++)
      a[i] = _a[i];
}


// pass in each element separately
template<class Real>
inline Matrix4<Real>::Matrix4(Real _m11, Real _m12, Real _m13, Real _m14,
                              Real _m21, Real _m22, Real _m23, Real _m24,
                              Real _m31, Real _m32, Real _m33, Real _m34,
                              Real _m41, Real _m42, Real _m43, Real _m44) {
  set(_m11, _m12, _m13, _m14,
      _m21, _m22, _m23, _m24,
      _m31, _m32, _m33, _m34,
      _m41, _m42, _m43, _m44);
}


template<class Real>
inline void Matrix4<Real>::set(Real _m11, Real _m12, Real _m13, Real _m14,
                               Real _m21, Real _m22, Real _m23, Real _m24,
                               Real _m31, Real _m32, Real _m33, Real _m34,
                               Real _m41, Real _m42, Real _m43, Real _m44) {
  a[0]  = _m11; a[1]  = _m12; a[2]  = _m13; a[3]  = _m14;
  a[4]  = _m21; a[5]  = _m22; a[6]  = _m23; a[7]  = _m24;
  a[8]  = _m31; a[9]  = _m32; a[10] = _m33; a[11] = _m34;
  a[12] = _m41; a[13] = _m42; a[14] = _m43; a[15] = _m44;
}


// internal helper method for matrix multiplication methods
// (dot product of row from this matrix and column from m2)
template<class Real>
inline Real Matrix4<Real>::dotRC(const Matrix4 &m2, int r, int c) const { 
   return m[r][0]*m2.m[0][c] +
          m[r][1]*m2.m[1][c] +
          m[r][2]*m2.m[2][c] +
          m[r][3]*m2.m[3][c];
}


// computes and returns transpose of matrix
template<class Real>
inline Matrix4<Real> Matrix4<Real>::transpose() const {
   Matrix4<Real> result;

   for (int i=0; i < 4; i++)
      for (int j=0; j < 4; j++)
         result.m[j][i] = m[i][j];

   return result;
}


// computes transpose and stores result in r
template<class Real>
inline void Matrix4<Real>::transpose(Matrix4<Real> &r) const {
   for (int i=0; i < 4; i++)
      for (int j=0; j < 4; j++)
         r.m[j][i] = m[i][j];
}


// multiplies Matrix4 object by m2 and returns resulting Matrix4
template<class Real>
inline Matrix4<Real> Matrix4<Real>::operator *(const Matrix4<Real> &m2) const {
   Matrix4<Real> result;

   for (int i=0; i < 4; i++)
      for (int j=0; j < 4; j++)
         result(i, j) = dotRC(m2, i, j);

   return result;
}


// adds Matrix4 object to m2 and returns resulting Matrix4
template<class Real>
inline Matrix4<Real> Matrix4<Real>::operator +(const Matrix4<Real> &m2) const {

   return Matrix4<Real>(m[0][0] + m2.m[0][0], m[0][1] + m2.m[0][1], m[0][2] + m2.m[0][2], m[0][3] + m2.m[0][3],
                        m[1][0] + m2.m[1][0], m[1][1] + m2.m[1][1], m[1][2] + m2.m[1][2], m[1][3] + m2.m[1][3],
                        m[2][0] + m2.m[2][0], m[2][1] + m2.m[2][1], m[2][2] + m2.m[2][2], m[2][3] + m2.m[2][3],
                        m[3][0] + m2.m[3][0], m[3][1] + m2.m[3][1], m[3][2] + m2.m[3][2], m[3][3] + m2.m[3][3]);
}


// subtracts m2 from Matrix4 object and returns resulting Matrix4
template<class Real>
inline Matrix4<Real> Matrix4<Real>::operator -(const Matrix4<Real> &m2) const {

return Matrix4<Real>(m[0][0] - m2.m[0][0], m[0][1] - m2.m[0][1], m[0][2] - m2.m[0][2], m[0][3] - m2.m[0][3],
                     m[1][0] - m2.m[1][0], m[1][1] - m2.m[1][1], m[1][2] - m2.m[1][2], m[1][3] - m2.m[1][3],
                     m[2][0] - m2.m[2][0], m[2][1] - m2.m[2][1], m[2][2] - m2.m[2][2], m[2][3] - m2.m[2][3],
                     m[3][0] - m2.m[3][0], m[3][1] - m2.m[3][1], m[3][2] - m2.m[3][2], m[3][3] - m2.m[3][3]);
}


// multiplies Matrix4 object by scalar and returns resulting Matrix4
template<class Real>
inline Matrix4<Real> Matrix4<Real>::operator *(Real s) const {
  
  return Matrix4<Real>(s*m[0][0], s*m[0][1], s*m[0][2], s*m[0][3],
                       s*m[1][0], s*m[1][1], s*m[1][2], s*m[1][3],
                       s*m[2][0], s*m[2][1], s*m[2][2], s*m[2][3],
                       s*m[3][0], s*m[3][1], s*m[3][2], s*m[3][3]);
}


// multiplies Matrix4 object by m2 and stores result in self
template<class Real>
inline const Matrix4<Real> & Matrix4<Real>::operator *=(const Matrix4<Real> &m2) {
   Matrix4<Real> temp; // temporary storage for result

   for (int i=0; i < 4; i++)
      for (int j=0; j < 4; j++)
         temp.m[i][j] = dotRC(m2, i, j);

   // copy back result
   for (int i=0; i < 16; i++)
      a[i] = temp.a[i];

   return (*this);
}


// adds m2 to self, and stores result in self
template<class Real>
inline const Matrix4<Real> & Matrix4<Real>::operator += (const Matrix4<Real> &m2) {
   for (int i=0; i < 16; i++)
      a[i] += m2.a[i];

   return (*this);
}


// subtracts m2 from self, and stores result in self
template<class Real>
inline const Matrix4<Real> & Matrix4<Real>::operator -= (const Matrix4<Real> &m2) {
   for (int i=0; i < 16; i++)
      a[i] -= m2.a[i];

   return (*this);
}


// multiplies Matrix4 object by scalar, and stores result in self
template<class Real>
inline const Matrix4<Real> & Matrix4<Real>::operator *= (Real s) {
   for (int i=0; i < 16; i++)
      a[i] *= s;

   return (*this);
}


// multiplies Vector3 by Matrix4, and returns resulting Vector3
// (converts Vector3 to a 4-dimensional vector internally, <v.x, v.y, v.z, 1>,
// and then divides by w (4th component) after transforming the 4-dimensional
// representation.
template<class Real>
inline Vector3<Real> Matrix4<Real>::transform(const Vector3<Real> &v) const {
  Vector3<Real> result(v[0]*m[0][0] + v[1]*m[0][1] + v[2]*m[0][2] + m[0][3],
                       v[0]*m[1][0] + v[1]*m[1][1] + v[2]*m[1][2] + m[1][3],
                       v[0]*m[2][0] + v[1]*m[2][1] + v[2]*m[2][2] + m[2][3]);
  // calculate temporary 4th component
  Real w = v[0]*m[3][0] + v[1]*m[3][1] + v[2]*m[3][2] + m[3][3];

  // divide by w
  result *= Real(1.0)/w;

  return result;
}


// multiplies Point3 by Matrix4, and returns resulting Point3
// (converts Point3 to a 4-dimensional point internally, <p.x, p.y, p.z, 1>,
// and then divides by w (4th component) after transforming the 4-dimensional
// representation.
template<class Real>
inline Point3<Real> Matrix4<Real>::transform(const Point3<Real> &p) const {
  Point3<Real> result(p[0]*m[0][0] + p[1]*m[0][1] + p[2]*m[0][2] + m[0][3],
                      p[0]*m[1][0] + p[1]*m[1][1] + p[2]*m[1][2] + m[1][3],
                      p[0]*m[2][0] + p[1]*m[2][1] + p[2]*m[2][2] + m[2][3]);
  // calculate temporary 4th component
  Real w = p[0]*m[3][0] + p[1]*m[3][1] + p[2]*m[3][2] + m[3][3];

  // divide by w
  result *= Real(1.0)/w;

  return result;
}


// creates a matrix to perform a rotation about the x-axis
template<class Real>
inline void Matrix4<Real>::rotx(Real angle) {
   Real sinTheta = rsin(angle);
   Real cosTheta = rcos(angle);

   set( Real(1.0), 0.0,       0.0,      0.0,
             0.0,  cosTheta, -sinTheta, 0.0,
             0.0,  sinTheta,  cosTheta, 0.0,
             0.0,  0.0,       0.0,      Real(1.0) );
}


// creates a matrix to perform a rotation about the y-axis
template<class Real>
inline void Matrix4<Real>::roty(Real angle) {
   Real sinTheta = rsin(angle);
   Real cosTheta = rcos(angle);

   set( cosTheta,      0.0,  sinTheta, 0.0,
        0.0,      Real(1.0), 0.0,      0.0,
       -sinTheta,      0.0,  cosTheta, 0.0,
        0.0,           0.0,  0.0,      Real(1.0) );
}


// creates a matrix to perform a rotation about the z-axis
template<class Real>
inline void Matrix4<Real>::rotz(Real angle) {
   Real sinTheta = rsin(angle);
   Real cosTheta = rcos(angle);

   set( cosTheta, -sinTheta, 0.0,       0.0,
        sinTheta, cosTheta,  0.0,       0.0,
        0.0,      0.0,       Real(1.0), 0.0,
        0.0,      0.0,       0.0,       Real(1.0) );
}


// creates a matrix to perform a rotation about an arbitrary axis
// (angle is assumed to be in radians, and axis is assumed to be normalized!)
template<class Real>
inline void Matrix4<Real>::rot(const Vector3<Real> &axis, Real angle) {
   Real sinTheta = rsin(angle);
   Real cosTheta = rcos(angle);

   m[0][0] = axis[0]*axis[0] + (Real(1.0) - axis[0]*axis[0])*cosTheta;
   m[0][1] = axis[0]*axis[1] * (Real(1.0) - cosTheta) - axis[2]*sinTheta;
   m[0][2] = axis[0]*axis[2] * (Real(1.0) - cosTheta) + axis[1]*sinTheta;

   m[1][0] = axis[0]*axis[1] * (Real(1.0) - cosTheta) + axis[2]*sinTheta;
   m[1][1] = axis[1]*axis[1] + (Real(1.0) - axis[1]*axis[1])*cosTheta;
   m[1][2] = axis[1]*axis[2] * (Real(1.0) - cosTheta) - axis[0]*sinTheta;
   
   m[2][0] = axis[0]*axis[2] * (Real(1.0) - cosTheta) - axis[1]*sinTheta;
   m[2][1] = axis[1]*axis[2] * (Real(1.0) - cosTheta) + axis[0]*sinTheta;
   m[2][2] = axis[2]*axis[2] + (Real(1.0) - axis[2]*axis[2])*cosTheta;
 
   m[0][3] = m[1][3] = m[2][3] = m[3][0] = m[3][1] = m[3][2] = 0.0;
   m[3][3] = Real(1.0);
}


// creates a scale matrix
template<class Real>
inline void Matrix4<Real>::scale(Real sx, Real sy, Real sz) {
  set( sx,  0.0, 0.0, 0.0,
       0.0, sy,  0.0, 0.0,
       0.0, 0.0, sz,  0.0,
       0.0, 0.0, 0.0, Real(1.0) );
}


// creates a translation matrix
template<class Real>
inline void Matrix4<Real>::translate(Real dx, Real dy, Real dz) {
  set( Real(1.0), 0.0,       0.0,       dx,
       0.0,       Real(1.0), 0.0,       dy,
       0.0,       0.0,       Real(1.0), dz,
       0.0,       0.0,       0.0,       Real(1.0) );
}


// overloaded stream insertion operator for Matrix4
template<class Real>
std::ostream& operator << (std::ostream &out, const Matrix4<Real> &m) {
  for ( int i=0; i < 4; i++ ) {
    for ( int j=0; j < 4; j++ )
      out << m(i, j) << " ";
    out << std::endl;
  }

  return out;
}


} // namespace Math3D


#endif
