/***************************************************************************
 *   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 MATRIX3_H
#define MATRIX3_H

#include "Point3.h"
#include "Vector3.h"
#include "numeric.h"


namespace Math3D {


template<class Real>
class Matrix3 {
public:
   inline Matrix3();
   inline Matrix3(const Real *_a);
   inline Matrix3(Real _m11, Real _m12, Real _m13,
                  Real _m21, Real _m22, Real _m23,
                  Real _m31, Real _m32, Real _m33);

   inline void set(Real _m11, Real _m12, Real _m13,
                   Real _m21, Real _m22, Real _m23,
                   Real _m31, Real _m32, Real _m33);

   inline void setRowVectors(const Vector3<Real> &r1,
                            const Vector3<Real> &r2,
                            const Vector3<Real> &r3);

   inline void setColumnVectors(const Vector3<Real> &c1,
                               const Vector3<Real> &c2,
                               const Vector3<Real> &c3);

   inline Vector3<Real> getRowVector(int i) const;

   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 Matrix3 inv() const;
   inline void    inv(Matrix3 &result) const;
   inline Matrix3 transpose() const;
   inline void    transpose(Matrix3 &r) const;

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

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

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

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

   // rotation transformations
   inline void rot(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);

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

  union {
    Real m[3][3];
    Real a[9];
  };
};


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


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


// pass in each element separately
template<class Real>
inline Matrix3<Real>::Matrix3(Real _m11, Real _m12, Real _m13,
                             Real _m21, Real _m22, Real _m23,
                             Real _m31, Real _m32, Real _m33) {
  set(_m11, _m12, _m13,
      _m21, _m22, _m23,
      _m31, _m32, _m33);
   
}


template<class Real>
inline void Matrix3<Real>::set(Real _m11, Real _m12, Real _m13,
                              Real _m21, Real _m22, Real _m23,
                              Real _m31, Real _m32, Real _m33) {
  m[0][0] = _m11; m[0][1] = _m12; m[0][2] = _m13;
  m[1][0] = _m21; m[1][1] = _m22; m[1][2] = _m23;
  m[2][0] = _m31; m[2][1] = _m32; m[2][2] = _m33;
}


// sets r1, r2, and r3 as rows of matrix
template<class Real>
inline void Matrix3<Real>::setRowVectors(const Vector3<Real> &r1,
                                        const Vector3<Real> &r2,
                                        const Vector3<Real> &r3) {
  for ( int c=0; c < 3; c++ ) {
    m[0][c] = r1[c];
    m[1][c] = r2[c];
    m[2][c] = r3[c];
  }
}


// sets c1, c2, c3 as columns of matrix
template<class Real>
inline void Matrix3<Real>::setColumnVectors(const Vector3<Real> &c1,
                                           const Vector3<Real> &c2,
                                           const Vector3<Real> &c3) {
  for ( int r=0; r < 3; r++ ) {
    m[r][0] = c1[r];
    m[r][1] = c2[r];
    m[r][2] = c3[r];
  }
}


// returns ith row vector
template<class Real>
inline Vector3<Real> Matrix3<Real>::getRowVector(int i) const {
  return Vector3<Real>(m[i][0], m[i][1], m[i][2]);
}


// internal helper method for matrix multiplication methods
template<class Real>
inline Real Matrix3<Real>::dotRC(const Matrix3 &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];
}


// computes and returns determinant of Matrix3 object
template<class Real>
inline Real Matrix3<Real>::det() const {
   return m[0][0]*m[1][1]*m[2][2] + m[0][1]*m[1][2]*m[2][0] + m[0][2]*m[1][0]*m[2][1] -
         m[0][2]*m[1][1]*m[2][0] - m[0][1]*m[1][0]*m[2][2] - m[0][0]*m[1][2]*m[2][1];
}


// computes and returns inverse of the matrix
template<class Real>
inline Matrix3<Real> Matrix3<Real>::inv() const {
   // compute 1/|det|
   Real absInvDet = Real(1.0)/( fabsf( m[0][0]*m[1][1]*m[2][2] + m[0][1]*m[1][2]*m[2][0] + m[0][2]*m[1][0]*m[2][1] -
                                       m[0][2]*m[1][1]*m[2][0] - m[0][1]*m[1][0]*m[2][2] - m[0][0]*m[1][2]*m[2][1]) );

   // compute elements of adjoint matrix
   Matrix3<Real> result( m[1][1]*m[2][2] - m[1][2]*m[2][1], m[0][2]*m[2][1] - m[0][1]*m[2][2], m[0][1]*m[1][2] - m[0][2]*m[1][1],
                         m[1][2]*m[2][0] - m[1][0]*m[2][2], m[0][0]*m[2][2] - m[0][2]*m[2][0], m[0][2]*m[2][1] - m[0][0]*m[1][2],
                         m[1][0]*m[2][1] - m[1][1]*m[2][1], m[0][1]*m[2][0] - m[0][0]*m[2][1], m[0][0]*m[0][1] - m[0][1]*m[1][0] );

   // multiply adjoint by 1/|det|
   for (int i=0; i < 9; i++)
      result.a[i] *= absInvDet;

   return result;
}


// computes inverse of matrix and stores it in result
template<class Real>
inline void Matrix3<Real>::inv(Matrix3<Real> &result) const {
   // compute 1/|det|
   Real absInvDet = Real(1.0)/( fabsf( m[0][0]*m[1][1]*m[2][2] + m[0][1]*m[1][2]*m[2][0] + m[0][2]*m[1][0]*m[2][1] -
                                       m[0][2]*m[1][1]*m[2][0] - m[0][1]*m[1][0]*m[2][2] - m[0][0]*m[1][2]*m[2][1]) );

   // compute elements of adjoint matrix
   result = Matrix3<Real>( m[1][1]*m[2][2] - m[1][2]*m[2][1], m[0][2]*m[2][1] - m[0][1]*m[2][2], m[0][1]*m[1][2] - m[0][2]*m[1][1],
                           m[1][2]*m[2][0] - m[1][0]*m[2][2], m[0][0]*m[2][2] - m[0][2]*m[2][0], m[0][2]*m[2][1] - m[0][0]*m[1][2],
                           m[1][0]*m[2][1] - m[1][1]*m[2][1], m[0][1]*m[2][0] - m[0][0]*m[2][1], m[0][0]*m[0][1] - m[0][1]*m[1][0] );

   // multiply adjoint by 1/|det|
   for (int i=0; i < 9; i++)
      result.a[i] *= absInvDet;
}


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

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

   return result;
}


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


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

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

   return result;
}

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

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


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

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

}


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


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

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

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

   return (*this);
}


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

   return (*this);
}


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

   return (*this);
}


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

   return (*this);
}


// multiplies Vector3 by Matrix3, and returns resulting Vector3
template<class Real>
inline Vector3<Real> Matrix3<Real>::operator *(const Vector3<Real> &v) const {

   return Vector3<Real>( v[0]*m[0][0] + v[1]*m[0][1] + v[2]*m[0][2],
                        v[0]*m[1][0] + v[1]*m[1][1] + v[2]*m[1][2],
                        v[0]*m[2][0] + v[1]*m[2][1] + v[2]*m[2][2] );
}


// multiplies Point3 by Matrix3, and returns resulting Point3
template<class Real>
inline Point3<Real> Matrix3<Real>::operator *(const Point3<Real> &p) const {
   return Point3<Real>( p[0]*m[0][0] + p[1]*m[0][1] + p[2]*m[0][2],
                       p[0]*m[1][0] + p[1]*m[1][1] + p[2]*m[1][2],
                       p[0]*m[2][0] + p[1]*m[2][1] + p[2]*m[2][2] );
}


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


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

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


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

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


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

   set( cosTheta, -sinTheta,     0.0,
        sinTheta, cosTheta,      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 Matrix3<Real>::rot(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;
}


} // namespace Math3D


#endif
