Matrix4x4.h

/*
Copyright 2007-2025. Algoryx Simulation AB.
 
All AGX source code, intellectual property, documentation, sample code,
tutorials, scene files and technical white papers, are copyrighted, proprietary
and confidential material of Algoryx Simulation AB. You may not download, read,
store, distribute, publish, copy or otherwise disseminate, use or expose this
material without having a written signed agreement with Algoryx Simulation AB.
 
Algoryx Simulation AB disclaims all responsibilities for loss or damage caused
from using this software, unless otherwise stated in written agreements with
Algoryx Simulation AB.
*/
#ifndef MOMENTUM_MATRIX4X4_H
#define MOMENTUM_MATRIX4X4_H
 
#include "momentum_export.h"
#include <momentum_namespace.h>
 
#include "macros.h"
#include <math.h>
#include "Quat.h"
#include "Vec3.h"
#include "Vec4.h"
 
#include <agx/Prefetch.h>
 
namespace MOMENTUM_NAMESPACE
{
  class EulerAngles;
 
#define MOMENTUM_MATRIX4X4_SET_ROW(row, v1, v2, v3, v4 )    \
  m_data[(row)][0] = (v1); \
  m_data[(row)][1] = (v2); \
  m_data[(row)][2] = (v3); \
  m_data[(row)][3] = (v4);
 
#define MOMENTUM_MATRIX4X4_INNER_PRODUCT(a,b,r,c) \
   ((a).m_data[r][0] * (b).m_data[0][c]) \
  +((a).m_data[r][1] * (b).m_data[1][c]) \
  +((a).m_data[r][2] * (b).m_data[2][c]) \
  +((a).m_data[r][3] * (b).m_data[3][c])
 
  class MOMENTUM_EXPORT Matrix4x4
  {
    public:
 
    /*=====================================================
                        Constructors
    =======================================================*/
 
    Matrix4x4();
 
    Matrix4x4( const Matrix4x4& mat );
 
    Matrix4x4( double const * const ptr );
 
 
    Matrix4x4(const Quat& rotation, const Vec3& translation = Vec3());
 
    Matrix4x4( const EulerAngles& rotation, const Vec3& translation = Vec3());
 
 
    Matrix4x4( double a00, double a01, double a02, double a03,
                     double a10, double a11, double a12, double a13,
                     double a20, double a21, double a22, double a23,
                     double a30, double a31, double a32, double a33 );
 
 
    ~Matrix4x4();
 
    /*=====================================================
                          Accessors
    =======================================================*/
 
    bool valid() const;
 
    bool isNaN() const;
 
    bool isFinite() const;
 
    bool isIdentity() const;
 
    bool isRigidTransformation() const;
 
    Matrix4x4 inverse() const;
 
    Matrix4x4 transpose() const;
 
    /*=====================================================
                          Mutators
    =======================================================*/
 
     Matrix4x4 set(double const * const ptr );
     Matrix4x4 set(double a00, double a01, double a02, double a03,
                      double a10, double a11, double a12, double a13,
                      double a20, double a21, double a22, double a23,
                      double a30, double a31, double a32, double a33 );
 
    Matrix4x4 set( const Quat& q );
    Matrix4x4 set( const EulerAngles& e );
 
 
    void setRow(size_t row, const Vec4& vec ) {
      m_data[row][0] = vec[0];
      m_data[row][1] = vec[1];
      m_data[row][2] = vec[2];
      m_data[row][3] = vec[3];
    }
 
    void setCol(size_t col, const Vec4& vec ) {
      m_data[0][col] = vec[0];
      m_data[1][col] = vec[1];
      m_data[2][col] = vec[2];
      m_data[3][col] = vec[3];
    }
 
    Vec4 getRow(size_t row) const
    {
      return Vec4(
        m_data[row][0],
        m_data[row][1],
        m_data[row][2],
        m_data[row][3]
      );
    }
 
    Vec4 getCol(size_t col) const
    {
      return Vec4(
        m_data[0][col],
        m_data[1][col],
        m_data[2][col],
        m_data[3][col]
      );
    }
 
 
    Matrix4x4 setRotate(const Quat& q);
    Matrix4x4 setRotate( const EulerAngles& euler );
    Matrix4x4 setRotate( const Vec3& from, const Vec3& to );
    Matrix4x4 setRotate( double angle, const Vec3& axis );
    Matrix4x4 setRotate( double angle, double x, double y, double z );
    Matrix4x4 setRotate( double angle1, const Vec3& axis1,
                                double angle2, const Vec3& axis2,
                                double angle3, const Vec3& axis3 );
 
    Matrix4x4 setTranslate( const Vec3& t );
    Matrix4x4 setTranslate(double tx, double ty, double tz);
 
    Matrix4x4 addTranslate( const Vec3& t );
    Matrix4x4 addTranslate(double tx, double ty, double tz);
 
    Matrix4x4 setIdentity();
 
    /*=====================================================
                          Accessors
    =======================================================*/
    Vec3 getTranslate() const;
 
    Quat getRotate() const;
 
    Quat getAsQuat() const;
    EulerAngles getAsEulerAngles() const;
 
 
    /* Get the internal row-major buffer */
    double *ptr();
    const double *ptr() const;
 
    /*=====================================================
                        Static methods
    =======================================================*/
 
    static Matrix4x4 crossMatrix(const Vec3& vec);
    static Matrix4x4 translate( const Vec3& dv );
    static Matrix4x4 translate( double x, double y, double z );
    static Matrix4x4 rotate( const Vec3& from, const Vec3& to );
    static Matrix4x4 rotate( double angle, double x, double y, double z );
    static Matrix4x4 rotate( double angle, const Vec3& axis );
    static Matrix4x4 rotate( double angle1, const Vec3& axis1,
                                          double angle2, const Vec3& axis2,
                                          double angle3, const Vec3& axis3 );
 
    /*=====================================================
                        Operators
    =======================================================*/
    Vec3 operator* ( const Vec3& v ) const;
    Vec4 operator* ( const Vec4& v ) const;
 
    void operator*= ( const Matrix4x4& other );
    Matrix4x4 operator* ( const Matrix4x4 &m ) const;
    bool operator== ( const Matrix4x4& m ) const;
    bool operator!= ( const Matrix4x4& m ) const;
#ifndef SWIG
    double& operator()(size_t row, size_t col);
    Matrix4x4 operator= ( const Matrix4x4& rhs );
#endif
    double operator()(size_t row, size_t col) const;
 
    Vec3 transform3x3(const Vec3& vIn) const;
 
    Vec3 transform3x3Inv( const Vec3& vIn) const;
 
    Vec3 preMult(const Vec3& v) const;
 
    Vec3 postMult(const Vec3& v) const;
 
    Vec4 preMult(const Vec4& v) const;
 
    Vec4 postMult(const Vec4& v) const;
 
    void mult( const Matrix4x4&, const Matrix4x4& );
    void preMult( const Matrix4x4& );
    void postMult( const Matrix4x4& );
    Matrix4x4 preMultTranslate( const Vec3& v );
    Matrix4x4 postMultTranslate( const Vec3& v );
 
    std::string __str__() const;
 
  protected:
 
    bool invert( const Matrix4x4& rhs );
 
 
    double m_data[4][4];
 
  };
}
 
 
#ifndef SWIG
/* ****************************************** */
/* *           IMPLEMENTATION              ** */
/* ****************************************** */
#include "EulerAngles.h"
 
namespace MOMENTUM_NAMESPACE
{
 
  /*=====================================================
                      Constructors
  =======================================================*/
 
  inline Matrix4x4::Matrix4x4()
  {
    this->setIdentity();
  }
 
  inline Matrix4x4::Matrix4x4( const Matrix4x4& mat )
  {
    m_data[0][0] = mat(0, 0);
    m_data[0][1] = mat(0, 1);
    m_data[0][2] = mat(0, 2);
    m_data[0][3] = mat(0, 3);
 
    m_data[1][0] = mat(1, 0);
    m_data[1][1] = mat(1, 1);
    m_data[1][2] = mat(1, 2);
    m_data[1][3] = mat(1, 3);
 
    m_data[2][0] = mat(2, 0);
    m_data[2][1] = mat(2, 1);
    m_data[2][2] = mat(2, 2);
    m_data[2][3] = mat(2, 3);
 
    m_data[3][0] = mat(3, 0);
    m_data[3][1] = mat(3, 1);
    m_data[3][2] = mat(3, 2);
    m_data[3][3] = mat(3, 3);
  }
 
  inline Matrix4x4::Matrix4x4( double a00, double a01, double a02, double a03,
                                           double a10, double a11, double a12, double a13,
                                           double a20, double a21, double a22, double a23,
                                           double a30, double a31, double a32, double a33 )
  {
    m_data[0][0] = a00;
    m_data[0][1] = a01;
    m_data[0][2] = a02;
    m_data[0][3] = a03;
 
    m_data[1][0] = a10;
    m_data[1][1] = a11;
    m_data[1][2] = a12;
    m_data[1][3] = a13;
 
    m_data[2][0] = a20;
    m_data[2][1] = a21;
    m_data[2][2] = a22;
    m_data[2][3] = a23;
 
    m_data[3][0] = a30;
    m_data[3][1] = a31;
    m_data[3][2] = a32;
    m_data[3][3] = a33;
 
  }
 
  inline Matrix4x4::Matrix4x4( double const * const ptr )
  {
    set( ptr );
  }
 
 
  inline Matrix4x4::Matrix4x4(const Quat& quat, const Vec3& translation)
  {
    this->setIdentity();
    this->set( quat );
    this->setTranslate(translation);
  }
 
  inline Matrix4x4::Matrix4x4(const EulerAngles& rotation, const Vec3& translation)
  {
    this->setIdentity();
    this->set(rotation);
    this->setTranslate(translation);
  }
 
  inline Matrix4x4::~Matrix4x4()
  {}
 
 
  /*=====================================================
                        Queries
  =======================================================*/
 
  inline bool Matrix4x4::valid() const
  {
    return !isNaN();
  }
 
 
 
  inline bool Matrix4x4::isNaN() const
  {
    return std::isnan( m_data[0][0] ) || std::isnan( m_data[0][1] )
      || std::isnan( m_data[0][2] ) || std::isnan( m_data[0][3] )
      || std::isnan( m_data[1][0] ) || std::isnan( m_data[1][1] )
      || std::isnan( m_data[1][2] ) || std::isnan( m_data[1][3] )
      || std::isnan( m_data[2][0] ) || std::isnan( m_data[2][1] )
      || std::isnan( m_data[2][2] ) || std::isnan( m_data[2][3] )
      || std::isnan( m_data[3][0] ) || std::isnan( m_data[3][1] )
      || std::isnan( m_data[3][2] ) || std::isnan( m_data[3][3] );
  }
 
 
 
  inline bool Matrix4x4::isFinite() const
  {
    return std::isfinite( m_data[0][0] ) && std::isfinite( m_data[0][1] )
      && std::isfinite( m_data[0][2] ) && std::isfinite( m_data[0][3] )
      && std::isfinite( m_data[1][0] ) && std::isfinite( m_data[1][1] )
      && std::isfinite( m_data[1][2] ) && std::isfinite( m_data[1][3] )
      && std::isfinite( m_data[2][0] ) && std::isfinite( m_data[2][1] )
      && std::isfinite( m_data[2][2] ) && std::isfinite( m_data[2][3] )
      && std::isfinite( m_data[3][0] ) && std::isfinite( m_data[3][1] )
      && std::isfinite( m_data[3][2] ) && std::isfinite( m_data[3][3] );
  }
 
 
  inline bool Matrix4x4::isIdentity() const
  {
    return *this == Matrix4x4();
  }
 
 
  inline bool Matrix4x4::isRigidTransformation() const
  {
    bool isRigid = true;
    // rows have norm 1
    isRigid &= equivalent(m_data[0][0]*m_data[0][0] + m_data[0][1]*m_data[0][1] + m_data[0][2]*m_data[0][2], double(1), double(1.0e-3));
    isRigid &= equivalent(m_data[1][0]*m_data[1][0] + m_data[1][1]*m_data[1][1] + m_data[1][2]*m_data[1][2], double(1), double(1.0e-3));
    isRigid &= equivalent(m_data[2][0]*m_data[2][0] + m_data[2][1]*m_data[2][1] + m_data[2][2]*m_data[2][2], double(1), double(1.0e-3));
    // columns have norm 1
    isRigid &= equivalent(m_data[0][0]*m_data[0][0] + m_data[1][0]*m_data[1][0] + m_data[2][0]*m_data[2][0], double(1), double(1.0e-3));
    isRigid &= equivalent(m_data[0][1]*m_data[0][1] + m_data[1][1]*m_data[1][1] + m_data[2][1]*m_data[2][1], double(1), double(1.0e-3));
    isRigid &= equivalent(m_data[0][2]*m_data[0][2] + m_data[1][2]*m_data[1][2] + m_data[2][2]*m_data[2][2], double(1), double(1.0e-3));
    // determinant 1
    isRigid &= equivalent(m_data[0][0]*m_data[1][1]*m_data[2][2] + m_data[0][1]*m_data[1][2]*m_data[2][0] + m_data[0][2]*m_data[1][0]*m_data[2][1]
    -  m_data[2][0]*m_data[1][1]*m_data[0][2] - m_data[1][0]*m_data[0][1]*m_data[2][2] - m_data[0][0]*m_data[2][1]*m_data[1][2],
      double(1), double(1.0e-1));
    // no row scaling
    isRigid &= (m_data[0][3] == double(0));
    isRigid &= (m_data[1][3] == double(0));
    isRigid &= (m_data[2][3] == double(0));
    // no transform scaling
    isRigid &= (m_data[3][3] == double(1));
    return isRigid;
  }
 
 
 
  /*=====================================================
                        Setters
  =======================================================*/
 
  inline Matrix4x4 Matrix4x4::set( double const * const ptr )
  {
    memcpy(m_data, ptr, sizeof(double)*16);
    return *this;
  }
 
 
 
  inline Matrix4x4 Matrix4x4::set( double a00, double a01, double a02, double a03,
                                          double a10, double a11, double a12, double a13,
                                          double a20, double a21, double a22, double a23,
                                          double a30, double a31, double a32, double a33 )
  {
   m_data[0][0] = a00;
   m_data[0][1] = a01;
   m_data[0][2] = a02;
   m_data[0][3] = a03;
 
   m_data[1][0] = a10;
   m_data[1][1] = a11;
   m_data[1][2] = a12;
   m_data[1][3] = a13;
 
   m_data[2][0] = a20;
   m_data[2][1] = a21;
   m_data[2][2] = a22;
   m_data[2][3] = a23;
 
   m_data[3][0] = a30;
   m_data[3][1] = a31;
   m_data[3][2] = a32;
   m_data[3][3] = a33;
 
   return *this;
  }
 
 
  inline Matrix4x4 Matrix4x4::set(const Quat& q)
  {
    this->setRotate(q);
    this->setTranslate(0.0, 0.0, 0.0);
 
    return *this;
  }
 
 
  inline Matrix4x4 Matrix4x4::setRotate( const Vec3& from, const Vec3& to )
  {
    Quat quat;
    quat.setRotate(from,to);
    this->setRotate(quat);
    return *this;
  }
 
 
  inline Matrix4x4 Matrix4x4::setRotate( double angle, const Vec3& axis )
  {
    Quat quat;
    quat.setRotate( angle, axis);
    this->setRotate(quat);
    return *this;
  }
 
 
  inline Matrix4x4 Matrix4x4::setRotate( double angle, double x, double y, double z )
  {
    Quat quat;
    quat.setRotate( angle, x, y, z);
    this->setRotate(quat);
    return *this;
  }
 
 
  inline Matrix4x4 Matrix4x4::setRotate( double angle1, const Vec3& axis1,
                              double angle2, const Vec3& axis2,
                              double angle3, const Vec3& axis3 )
  {
    Quat quat;
    quat.setRotate(angle1, axis1,
                   angle2, axis2,
                   angle3, axis3);
    this->setRotate(quat);
    return *this;
  }
 
 
  inline Matrix4x4 Matrix4x4::setTranslate( const Vec3& v )
  {
    m_data[3][0] = v[0];
    m_data[3][1] = v[1];
    m_data[3][2] = v[2];
 
    return *this;
  }
 
 
  inline Matrix4x4 Matrix4x4::setTranslate(double tx, double ty, double tz)
  {
    m_data[3][0] = tx;
    m_data[3][1] = ty;
    m_data[3][2] = tz;
 
    return *this;
  }
 
 
  inline Matrix4x4 Matrix4x4::addTranslate( const Vec3& v )
  {
    m_data[3][0] += v[0];
    m_data[3][1] += v[1];
    m_data[3][2] += v[2];
 
    return *this;
  }
 
 
  inline Matrix4x4 Matrix4x4::addTranslate(double tx, double ty, double tz)
  {
    m_data[3][0] += tx;
    m_data[3][1] += ty;
    m_data[3][2] += tz;
 
    return *this;
  }
 
 
 
  inline Matrix4x4 Matrix4x4::setIdentity()
  {
    this->set(1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1);
    return *this;
  }
 
  /*=====================================================
                        Getters
  =======================================================*/
 
 
  inline Vec3 Matrix4x4::getTranslate() const
  {
    return Vec3( m_data[3][0], m_data[3][1], m_data[3][2] );
  }
 
 
 
  inline Quat Matrix4x4::getRotate() const
  {
    Quat q;
    q = getAsQuat();
    return q;
  }
 
  inline double *Matrix4x4::ptr()
  {
    return ( double* )m_data;
  }
 
 
  inline const double *Matrix4x4::ptr() const
  {
    return ( const double * )m_data;
  }
 
  /*=====================================================
                      Serialization methods
  =======================================================*/
 
  /*=====================================================
                      Static methods
  =======================================================*/
 
 
  inline Matrix4x4 Matrix4x4::crossMatrix(const Vec3& v)
  {
    return Matrix4x4( 0, -v.z(), v.y(), 0,
                            v.z(), 0, -v.x(), 0,
                            -v.y(), v.x(), 0, 0,
                            0, 0, 0, 1 );
  }
 
 
 
  inline Matrix4x4 Matrix4x4::translate( const Vec3& dv )
  {
    Matrix4x4 m;
    m.setTranslate(dv);
    return m;
  }
 
 
  inline Matrix4x4 Matrix4x4::translate( double x, double y, double z )
  {
    Matrix4x4 m;
    m.setTranslate(x, y, z);
    return m;
  }
 
 
  inline Matrix4x4 Matrix4x4::rotate( const Vec3& from, const Vec3& to )
  {
    Matrix4x4 m;
    m.setRotate(from, to);
    return m;
  }
 
 
  inline Matrix4x4 Matrix4x4::rotate( double angle, double x, double y, double z )
  {
    Matrix4x4 m;
    m.setRotate(angle, x, y, z);
    return m;
  }
 
 
  inline Matrix4x4 Matrix4x4::rotate( double angle, const Vec3& axis )
  {
    Matrix4x4 m;
    m.setRotate(angle, axis);
    return m;
  }
 
 
  inline Matrix4x4 Matrix4x4::rotate( double angle1, const Vec3& axis1,
                                        double angle2, const Vec3& axis2,
                                        double angle3, const Vec3& axis3 )
  {
    Matrix4x4 m;
    m.setRotate(angle1, axis1, angle2, axis2, angle3, axis3);
    return m;
  }
 
 
  /*=====================================================
                      Operators
  =======================================================*/
 
 
  inline void Matrix4x4::mult( const Matrix4x4& lhs, const Matrix4x4& rhs )
  {
    agx::prefetch<agx::L1>( lhs.m_data );
    agx::prefetch<agx::L1>( rhs.m_data );
 
    if (&lhs==this)
    {
      postMult(rhs);
      return;
    }
    if (&rhs==this)
    {
      preMult(lhs);
      return;
    }
 
    m_data[0][0] = MOMENTUM_MATRIX4X4_INNER_PRODUCT(lhs, rhs, 0, 0);
    m_data[0][1] = MOMENTUM_MATRIX4X4_INNER_PRODUCT(lhs, rhs, 0, 1);
    m_data[0][2] = MOMENTUM_MATRIX4X4_INNER_PRODUCT(lhs, rhs, 0, 2);
    m_data[0][3] = MOMENTUM_MATRIX4X4_INNER_PRODUCT(lhs, rhs, 0, 3);
 
    m_data[1][0] = MOMENTUM_MATRIX4X4_INNER_PRODUCT(lhs, rhs, 1, 0);
    m_data[1][1] = MOMENTUM_MATRIX4X4_INNER_PRODUCT(lhs, rhs, 1, 1);
    m_data[1][2] = MOMENTUM_MATRIX4X4_INNER_PRODUCT(lhs, rhs, 1, 2);
    m_data[1][3] = MOMENTUM_MATRIX4X4_INNER_PRODUCT(lhs, rhs, 1, 3);
 
    m_data[2][0] = MOMENTUM_MATRIX4X4_INNER_PRODUCT(lhs, rhs, 2, 0);
    m_data[2][1] = MOMENTUM_MATRIX4X4_INNER_PRODUCT(lhs, rhs, 2, 1);
    m_data[2][2] = MOMENTUM_MATRIX4X4_INNER_PRODUCT(lhs, rhs, 2, 2);
    m_data[2][3] = MOMENTUM_MATRIX4X4_INNER_PRODUCT(lhs, rhs, 2, 3);
 
    m_data[3][0] = MOMENTUM_MATRIX4X4_INNER_PRODUCT(lhs, rhs, 3, 0);
    m_data[3][1] = MOMENTUM_MATRIX4X4_INNER_PRODUCT(lhs, rhs, 3, 1);
    m_data[3][2] = MOMENTUM_MATRIX4X4_INNER_PRODUCT(lhs, rhs, 3, 2);
    m_data[3][3] = MOMENTUM_MATRIX4X4_INNER_PRODUCT(lhs, rhs, 3, 3);
 
  }
 
 
  inline void Matrix4x4::preMult( const Matrix4x4& other )
  {
 
    // more efficient method just use a T[4] for temporary storage.
    double t[4];
    for(size_t col=0; col<4; ++col) {
      t[0] = MOMENTUM_MATRIX4X4_INNER_PRODUCT( other, *this, 0, col );
      t[1] = MOMENTUM_MATRIX4X4_INNER_PRODUCT( other, *this, 1, col );
      t[2] = MOMENTUM_MATRIX4X4_INNER_PRODUCT( other, *this, 2, col );
      t[3] = MOMENTUM_MATRIX4X4_INNER_PRODUCT( other, *this, 3, col );
      m_data[0][col] = t[0];
      m_data[1][col] = t[1];
      m_data[2][col] = t[2];
      m_data[3][col] = t[3];
    }
  }
 
 
  inline void Matrix4x4::postMult( const Matrix4x4& other )
  {
 
    // more efficient method just use a T[3] for temporary storage.
    double t[4];
    for(size_t row=0; row<4; ++row)
    {
      t[0] = MOMENTUM_MATRIX4X4_INNER_PRODUCT( *this, other, row, 0 );
      t[1] = MOMENTUM_MATRIX4X4_INNER_PRODUCT( *this, other, row, 1 );
      t[2] = MOMENTUM_MATRIX4X4_INNER_PRODUCT( *this, other, row, 2 );
      t[3] = MOMENTUM_MATRIX4X4_INNER_PRODUCT( *this, other, row, 3 );
 
      MOMENTUM_MATRIX4X4_SET_ROW(row, t[0], t[1], t[2], t[3] )
    }
  }
 
 
  inline Matrix4x4 Matrix4x4::preMultTranslate( const Vec3& v )
  {
    for (unsigned i = 0; i < 3; ++i)
    {
      m_data[3][0] += v[i]*m_data[i][0];
      m_data[3][1] += v[i]*m_data[i][1];
      m_data[3][2] += v[i]*m_data[i][2];
      m_data[3][3] += v[i]*m_data[i][3];
    }
    return *this;
  }
 
 
  inline Matrix4x4 Matrix4x4::postMultTranslate( const Vec3& v )
  {
    for (unsigned i = 0; i < 3; ++i)
    {
      m_data[0][i] += v[i]*m_data[0][3];
      m_data[1][i] += v[i]*m_data[1][3];
      m_data[2][i] += v[i]*m_data[2][3];
      m_data[3][i] += v[i]*m_data[3][3];
    }
    return *this;
  }
 
 
  inline bool Matrix4x4::operator== ( const Matrix4x4& m ) const
  {
    const int bitCompare = memcmp(m_data, m.m_data, sizeof(m_data));
    return (bitCompare == 0);
  }
 
 
  inline bool Matrix4x4::operator!= ( const Matrix4x4& m ) const
  {
    const int bitCompare = memcmp(m_data, m.m_data, sizeof(m_data));
    return (bitCompare != 0);
  }
 
#ifndef SWIG
  inline double& Matrix4x4::operator()( size_t row, size_t col )
  {
    return m_data[row][col];
  }
#endif
 
 
  inline double Matrix4x4::operator()( size_t row, size_t col ) const
  {
    return m_data[row][col];
  }
 
 
  inline Matrix4x4 Matrix4x4::operator= ( const Matrix4x4& rhs )
  {
    if ( this != &rhs )
      this->set( rhs.ptr() );
 
    return *this;
  }
 
 
  inline void Matrix4x4::operator*= ( const Matrix4x4& other )
  {
    if ( this == &other )
    {
      Matrix4x4 temp( other );
      postMult( temp );
    }
    else postMult( other );
  }
 
 
  inline Matrix4x4 Matrix4x4::operator* ( const Matrix4x4 &m ) const
  {
    Matrix4x4 r;
    r.mult( *this, m );
    return  r;
  }
 
 
  inline Vec3 Matrix4x4::operator* ( const Vec3& v ) const
  {
    return postMult( v );
  }
 
 
  inline Vec4 Matrix4x4::operator* ( const Vec4& v ) const
  {
    return postMult( v );
  }
 
 
  inline Vec3 operator* ( const Vec3& v, const Matrix4x4& m )
  {
    return m.preMult( v );
  }
 
  inline Vec4 operator* ( const Vec4& v, const Matrix4x4& m )
  {
    return m.preMult( v );
  }
 
  std::ostream& operator<< (std::ostream& os, const Matrix4x4& m);
 
  /*=====================================================
                      Internal
  =======================================================*/
 
  inline Vec3 Matrix4x4::transform3x3( const Vec3& vIn ) const
  {
    Vec3 vOut;
    vOut.set(
      (m_data[0][0] * vIn.x() + m_data[1][0] * vIn.y() + m_data[2][0] * vIn.z()),
      (m_data[0][1] * vIn.x() + m_data[1][1] * vIn.y() + m_data[2][1] * vIn.z()),
      (m_data[0][2] * vIn.x() + m_data[1][2] * vIn.y() + m_data[2][2] * vIn.z()));
    return vOut;
  }
 
 
  inline Vec3 Matrix4x4::transform3x3Inv( const Vec3& vIn ) const
  {
    Vec3 vOut;
    vOut.set(
      (m_data[0][0] * vIn.x() + m_data[0][1] * vIn.y() + m_data[0][2] * vIn.z()),
      (m_data[1][0] * vIn.x() + m_data[1][1] * vIn.y() + m_data[1][2] * vIn.z()),
      (m_data[2][0] * vIn.x() + m_data[2][1] * vIn.y() + m_data[2][2] * vIn.z()));
    return vOut;
  }
 
 
  inline Vec3 Matrix4x4::postMult( const Vec3& v ) const
  {
    double d = double(1.0) / ( m_data[3][0] * v.x() + m_data[3][1] * v.y() + m_data[3][2] * v.z() + double(1.0) ) ;
    return Vec3( ( m_data[0][0]*v.x() + m_data[0][1]*v.y() + m_data[0][2]*v.z() ) *d,
                 ( m_data[1][0]*v.x() + m_data[1][1]*v.y() + m_data[1][2]*v.z() ) *d,
                 ( m_data[2][0]*v.x() + m_data[2][1]*v.y() + m_data[2][2]*v.z() ) *d ) ;
  }
 
 
  inline Vec3 Matrix4x4::preMult( const Vec3& v ) const
  {
    return Vec3(
             ( m_data[0][0]*v.x() + m_data[1][0]*v.y() + m_data[2][0]*v.z() + m_data[3][0] ),
             ( m_data[0][1]*v.x() + m_data[1][1]*v.y() + m_data[2][1]*v.z() + m_data[3][1] ),
             ( m_data[0][2]*v.x() + m_data[1][2]*v.y() + m_data[2][2]*v.z() + m_data[3][2] ) );
  }
 
 
  inline Vec4 Matrix4x4::postMult( const Vec4& v ) const
  {
    return Vec4(
             ( m_data[0][0]*v.x() + m_data[0][1]*v.y() + m_data[0][2]*v.z() + m_data[0][3]*v.w() ),
             ( m_data[1][0]*v.x() + m_data[1][1]*v.y() + m_data[1][2]*v.z() + m_data[1][3]*v.w() ),
             ( m_data[2][0]*v.x() + m_data[2][1]*v.y() + m_data[2][2]*v.z() + m_data[2][3]*v.w() ),
             ( m_data[3][0]*v.x() + m_data[3][1]*v.y() + m_data[3][2]*v.z() + m_data[3][3]*v.w() ) ) ;
  }
 
 
  inline Vec4 Matrix4x4::preMult( const Vec4& v ) const
  {
    return Vec4(
             ( m_data[0][0]*v.x() + m_data[1][0]*v.y() + m_data[2][0]*v.z() + m_data[3][0]*v.w() ),
             ( m_data[0][1]*v.x() + m_data[1][1]*v.y() + m_data[2][1]*v.z() + m_data[3][1]*v.w() ),
             ( m_data[0][2]*v.x() + m_data[1][2]*v.y() + m_data[2][2]*v.z() + m_data[3][2]*v.w() ),
             ( m_data[0][3]*v.x() + m_data[1][3]*v.y() + m_data[2][3]*v.z() + m_data[3][3]*v.w() ) );
  }
 
 
 
  inline Matrix4x4 Matrix4x4::transpose() const
  {
    return Matrix4x4( m_data[0][0], m_data[1][0], m_data[2][0], m_data[3][0],
                            m_data[0][1], m_data[1][1], m_data[2][1], m_data[3][1],
                            m_data[0][2], m_data[1][2], m_data[2][2], m_data[3][2],
                            m_data[0][3], m_data[1][3], m_data[2][3], m_data[3][3]);
  }
 
 
 
  inline Matrix4x4 Matrix4x4::inverse() const
  {
    Matrix4x4 m;
    m.invert( *this );
    return m;
  }
 
 
  inline bool equivalent( const Matrix4x4& a, const Matrix4x4& b, double epsilon = 1e-6 )
  {
    return
      agx::equivalent(a(0, 0), b(0, 0), epsilon) &&
      agx::equivalent(a(0, 1), b(0, 1), epsilon) &&
      agx::equivalent(a(0, 2), b(0, 2), epsilon) &&
      agx::equivalent(a(0, 3), b(0, 3), epsilon) &&
      agx::equivalent(a(1, 0), b(1, 0), epsilon) &&
      agx::equivalent(a(1, 1), b(1, 1), epsilon) &&
      agx::equivalent(a(1, 2), b(1, 2), epsilon) &&
      agx::equivalent(a(1, 3), b(1, 3), epsilon) &&
      agx::equivalent(a(2, 0), b(2, 0), epsilon) &&
      agx::equivalent(a(2, 1), b(2, 1), epsilon) &&
      agx::equivalent(a(2, 2), b(2, 2), epsilon) &&
      agx::equivalent(a(2, 3), b(2, 3), epsilon) &&
      agx::equivalent(a(3, 0), b(3, 0), epsilon) &&
      agx::equivalent(a(3, 1), b(3, 1), epsilon) &&
      agx::equivalent(a(3, 2), b(3, 2), epsilon) &&
      agx::equivalent(a(3, 3), b(3, 3), epsilon);
  }
 
}
 
#endif //"#ifndef SWIG"
 
#endif