Matrix4x4T.hh

/*===========================================================================*\
 *                                                                           *
 *                              OpenFlipper                                  *
 *           Copyright (c) 2001-2015, RWTH-Aachen University                 *
 *           Department of Computer Graphics and Multimedia                  *
 *                          All rights reserved.                             *
 *                            www.openflipper.org                            *
 *                                                                           *
 *---------------------------------------------------------------------------*
 * This file is part of OpenFlipper.                                         *
 *---------------------------------------------------------------------------*
 *                                                                           *
 * Redistribution and use in source and binary forms, with or without        *
 * modification, are permitted provided that the following conditions        *
 * are met:                                                                  *
 *                                                                           *
 * 1. Redistributions of source code must retain the above copyright notice, *
 *    this list of conditions and the following disclaimer.                  *
 *                                                                           *
 * 2. Redistributions in binary form must reproduce the above copyright      *
 *    notice, this list of conditions and the following disclaimer in the    *
 *    documentation and/or other materials provided with the distribution.   *
 *                                                                           *
 * 3. Neither the name of the copyright holder nor the names of its          *
 *    contributors may be used to endorse or promote products derived from   *
 *    this software without specific prior written permission.               *
 *                                                                           *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS       *
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED *
 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A           *
 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER *
 * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,  *
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,       *
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR        *
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF    *
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING      *
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS        *
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.              *
 *                                                                           *
\*===========================================================================*/

/*===========================================================================*\
 *                                                                           *
 *   $Revision$                                                       *
 *   $Author$                                                      *
 *   $Date$                   *
 *                                                                           *
\*===========================================================================*/



//=============================================================================
//
//  CLASS Matrix4x4T
//
//=============================================================================


#ifndef ACG_MATRIX4X4_HH
#define ACG_MATRIX4X4_HH


//== INCLUDES =================================================================

#include "VectorT.hh"
#include <cmath>
#include "../Config/ACGDefines.hh"
#include <iostream>
#include <algorithm>
#include <functional>


//== NAMESPACES  ==============================================================


namespace ACG {


//== MACROS / HELPERS =========================================================

template<typename Scalar> inline bool checkEpsilon(Scalar x) {
  return fabs(x) < (1e-6);
}

template<> inline bool checkEpsilon(float x) {
  return fabs(x) < (1e-4);
}



//== CLASS DEFINITION =========================================================


template <class Scalar>
class Matrix4x4T
{
public:

  Matrix4x4T() {}
 
  template <class OtherScalar>
  inline Matrix4x4T(const Matrix4x4T<OtherScalar>& _rhs) {
    operator=(_rhs);
  }
  
  inline Matrix4x4T(const Scalar _array[16]) {
    std::copy(_array,_array+16, mat_);
  }

  ~Matrix4x4T() {}


  template<typename otherScalar>
  inline Matrix4x4T<Scalar>& operator=(const Matrix4x4T<otherScalar>& _rhs) {
    // produces warning C4244 on msvc (implicit cast double to float)
//    std::copy(_rhs.data(),_rhs.data()+16,mat_);
    for (int i = 0; i < 16; ++i)
      mat_[i] = Scalar(_rhs.data()[i]);

    return *this;
  }



  inline Scalar& operator()(unsigned int row, unsigned int col) {
    return  mat_[(row)+((col)<<2)];
  }

  inline const Scalar& operator()(unsigned int row, unsigned int col) const {
    return mat_[(row)+((col)<<2)];
  }


  inline bool operator== (const Matrix4x4T<Scalar>& _rhs) const {
    int i;
    const Scalar *a = mat_;
    const Scalar *b = _rhs.mat_;
    for(i=0;i< 16;i++,a++,b++)
      if(! checkEpsilon( *a - *b ))
        return false;
    return true;
  }

  inline bool operator!= (const Matrix4x4T<Scalar>& _rhs) const {
    return !( operator==(_rhs) );
  }


  inline Matrix4x4T operator+ (Matrix4x4T<Scalar> _rhs) const {
    std::transform(mat_,mat_+16, _rhs.mat_, _rhs.mat_, std::plus<Scalar>());
    return _rhs;
  }

  inline Matrix4x4T operator- (Matrix4x4T<Scalar> _rhs) const {
    std::transform(mat_,mat_+16, _rhs.mat_, _rhs.mat_, std::minus<Scalar>());
    return _rhs;
  }

  Matrix4x4T operator*(const Matrix4x4T<Scalar>& inst) const;

  Matrix4x4T operator*(const Scalar& scalar);


  inline Matrix4x4T& operator+= ( const Matrix4x4T<Scalar>& _rhs) {
    std::transform(mat_,mat_+16,_rhs.mat_, mat_, std::plus<Scalar>());
    return *this;
  }

  inline Matrix4x4T& operator-= ( const Matrix4x4T<Scalar>& _rhs) {
    std::transform(mat_,mat_+16, _rhs.mat_, mat_, std::minus<Scalar>());
    return *this;
  }

  Matrix4x4T& operator*= (const Matrix4x4T<Scalar>& _rhs);

  Matrix4x4T& leftMult(const Matrix4x4T<Scalar>& _rhs);


  template <typename T>
  inline VectorT<T,4> operator*(const VectorT<T,4>& _v) const;

  template <typename T>
  inline VectorT<T,3> transform_point(const VectorT<T,3>& _v) const;

  template <typename T>
  inline VectorT<T,3> transform_vector(const VectorT<T,3>& _v) const;

  inline void clear();

  inline void identity();


  inline bool is_identity() const {
    int i;
    const Scalar *a = mat_;
    Scalar b = 0.0;
    for(i=0;i< 16;i++,a++,b++) {
      if ( ( i == 0) || ( i == 5 ) || ( i == 10 ) || ( i == 15 ) )
         b = 1.0;
      else
         b = 0.0;
      if(! checkEpsilon( *a - b ))
        return false;
    }
    return true;
  }


  inline void transpose();

  
  bool invert();

  Scalar determinant() const {
      return  mat_[12] * mat_[9] * mat_[6] * mat_[3] - mat_[8] * mat_[13] * mat_[6] * mat_[3] -
              mat_[12] * mat_[5] * mat_[10] * mat_[3] + mat_[4] * mat_[13] * mat_[10] * mat_[3] +
              mat_[8] * mat_[5] * mat_[14] * mat_[3] - mat_[4] * mat_[9] * mat_[14] * mat_[3] -
              mat_[12] * mat_[9] * mat_[2] * mat_[7] + mat_[8] * mat_[13] * mat_[2] * mat_[7] +
              mat_[12] * mat_[1] * mat_[10] * mat_[7] - mat_[0] * mat_[13] * mat_[10] * mat_[7] -
              mat_[8] * mat_[1] * mat_[14] * mat_[7] + mat_[0] * mat_[9] * mat_[14] * mat_[7] +
              mat_[12] * mat_[5] * mat_[2] * mat_[11] - mat_[4] * mat_[13] * mat_[2] * mat_[11] -
              mat_[12] * mat_[1] * mat_[6] * mat_[11] + mat_[0] * mat_[13] * mat_[6] * mat_[11] +
              mat_[4] * mat_[1] * mat_[14] * mat_[11] - mat_[0] * mat_[5] * mat_[14] * mat_[11] -
              mat_[8] * mat_[5] * mat_[2] * mat_[15] + mat_[4] * mat_[9] * mat_[2] * mat_[15] +
              mat_[8] * mat_[1] * mat_[6] * mat_[15] - mat_[0] * mat_[9] * mat_[6] * mat_[15] -
              mat_[4] * mat_[1] * mat_[10] * mat_[15] + mat_[0] * mat_[5] * mat_[10] * mat_[15];
  }


  inline const Scalar* get_raw_data() const { return mat_; }
  inline const Scalar* raw() const { return mat_; }
  inline const Scalar* data() const { return mat_; }

protected:

    Scalar mat_[16];
};


typedef Matrix4x4T<float>  Matrix4x4f;
typedef Matrix4x4T<double> Matrix4x4d;




//== IO to/from streams =======================================================


template<typename Scalar>
inline std::ostream& 
operator<<(std::ostream& os, const Matrix4x4T<Scalar>& m)
{
  for(int i=0; i<4; i++)
  {
    for(int j=0; j<4; j++)
      os << m(i,j) << " ";
    os << "\n";
  }
  return os;
}
 
 
template<typename Scalar>
inline std::istream& 
operator>>(std::istream& is, Matrix4x4T<Scalar>& m) 
{
  for(int i=0; i<4; i++)
    for(int j=0; j<4; j++)
      is >> m(i,j);
  return is;
}

//=============================================================================
} // namespace ACG
//=============================================================================
#if defined(INCLUDE_TEMPLATES) && !defined(ACG_MATRIX4X4_C)
#define ACG_MATRIX4X4_TEMPLATES
#include "Matrix4x4T.cc"
#endif
//=============================================================================
#endif // ACG_MATRIX4X4_HH defined
//=============================================================================