Matrix3x3.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_MATRIX3X3_H
#define MOMENTUM_MATRIX3X3_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>
 
#ifndef SWIG
 
#include <agx/Matrix3x3.h>
 
#endif
 
namespace MOMENTUM_NAMESPACE
{
#define MOMENTUM_MATRIX3X3_SET_ROW(row, v1, v2, v3)    \
  m_data[(row)][0] = (v1); \
  m_data[(row)][1] = (v2); \
  m_data[(row)][2] = (v3);
 
#define MOMENTUM_MATRIX3X3_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])
 
#define MOMENTUM_MATRIX3X3_DET2(a, b, c, d) ((a)*(d) - (b)*(c))
 
  class MOMENTUM_EXPORT Matrix3x3
  {
    public:
 
    /*=====================================================
                        Constructors
    =======================================================*/
 
#ifndef SWIG
    operator agx::Matrix3x3() const
    {
      agx::Matrix3x3 mat;
      mat.set(
        this->m_data[0][0], this->m_data[0][1], this->m_data[0][2],
        this->m_data[1][0], this->m_data[1][1], this->m_data[1][2],
        this->m_data[2][0], this->m_data[2][1], this->m_data[2][2]
      );
      return mat;
    }
    Matrix3x3(agx::Matrix3x3 mat)
    {
      for (auto i = 0; i < 3; i++)
        for (auto j = 0; j < 3; j++)
          this->m_data[i][j] = mat(i, j);
    }
#endif
    Matrix3x3();
 
    Matrix3x3( const Matrix3x3& mat );
 
    Matrix3x3( double const * const ptr );
 
 
    Matrix3x3(const Quat& rotation);
 
 
    Matrix3x3( double a00, double a01, double a02,
               double a10, double a11, double a12,
               double a20, double a21, double a22);
 
 
    ~Matrix3x3();
 
    /*=====================================================
                          Accessors
    =======================================================*/
 
    bool valid() const;
 
    bool isNaN() const;
 
    bool isFinite() const;
 
    bool isIdentity() const;
 
    bool isRigidTransformation() const;
 
    Matrix3x3 inverse() const;
 
    Matrix3x3 transpose() const;
 
    /*=====================================================
                          Mutators
    =======================================================*/
 
     Matrix3x3 set(double const * const ptr );
     Matrix3x3 set(double a00, double a01, double a02,
                   double a10, double a11, double a12,
                   double a20, double a21, double a22);
 
 
    void setRow(size_t row, const Vec3& vec ) {
      m_data[row][0] = vec[0];
      m_data[row][1] = vec[1];
      m_data[row][2] = vec[2];
    }
 
    void setCol(size_t col, const Vec3& vec ) {
      m_data[0][col] = vec[0];
      m_data[1][col] = vec[1];
      m_data[2][col] = vec[2];
    }
 
    Vec3 getRow(size_t row) const
    {
      return Vec3(
        m_data[row][0],
        m_data[row][1],
        m_data[row][2]
      );
    }
 
    Vec3 getCol(size_t col) const
    {
      return Vec3(
        m_data[0][col],
        m_data[1][col],
        m_data[2][col]
      );
    }
 
    double at(size_t row, size_t col) const
    {
      return m_data[row][col];
    }
 
 
    Matrix3x3 setRotate(const Quat& q);
    Matrix3x3 setRotate( const Vec3& from, const Vec3& to );
    Matrix3x3 setRotate( double angle, const Vec3& axis );
    Matrix3x3 setRotate( double angle, double x, double y, double z );
    Matrix3x3 setRotate( double angle1, const Vec3& axis1,
                         double angle2, const Vec3& axis2,
                         double angle3, const Vec3& axis3 );
 
    Matrix3x3 setIdentity();
 
    /*=====================================================
                          Accessors
    =======================================================*/
 
    Quat getRotate() const;
 
    Quat getAsQuat() const;
 
 
    /* Get the internal row-major buffer */
    double *ptr();
    const double *ptr() const;
 
    /*=====================================================
                        Static methods
    =======================================================*/
 
    static Matrix3x3 crossMatrix(const Vec3& vec);
    static Matrix3x3 rotate( const Vec3& from, const Vec3& to );
    static Matrix3x3 rotate( double angle, double x, double y, double z );
    static Matrix3x3 rotate( double angle, const Vec3& axis );
    static Matrix3x3 rotate( double angle1, const Vec3& axis1,
                             double angle2, const Vec3& axis2,
                             double angle3, const Vec3& axis3 );
 
    /*=====================================================
                        Operators
    =======================================================*/
    Vec3 operator* ( const Vec3& v ) const;
 
    void operator*= ( const Matrix3x3& other );
    Matrix3x3 operator* ( const Matrix3x3 &m ) const;
    bool operator== ( const Matrix3x3& m ) const;
    bool operator!= ( const Matrix3x3& m ) const;
#ifndef SWIG
    double& operator()(size_t row, size_t col);
    Matrix3x3 operator= ( const Matrix3x3& rhs );
#endif
    double operator()(size_t row, size_t col) const;
 
    Vec3 mult(const Vec3& v) const;
 
    void mult( const Matrix3x3&, const Matrix3x3& );
    void preMult( const Matrix3x3& );
    void postMult( const Matrix3x3& );
 
    std::string __str__() const;
 
  protected:
 
    bool invert( const Matrix3x3& rhs );
 
    Matrix3x3 fastInverse(double det) const;
    bool isDiagonal() const;
    double determinant() const;
 
 
    double m_data[3][3];
 
  };
}
 
 
#ifndef SWIG
/* ****************************************** */
/* *           IMPLEMENTATION              ** */
/* ****************************************** */
 
namespace MOMENTUM_NAMESPACE
{
 
  /*=====================================================
                      Constructors
  =======================================================*/
 
  inline Matrix3x3::Matrix3x3()
  {
    this->setIdentity();
  }
 
  inline Matrix3x3::Matrix3x3( const Matrix3x3& mat )
  {
    m_data[0][0] = mat(0, 0);
    m_data[0][1] = mat(0, 1);
    m_data[0][2] = mat(0, 2);
 
    m_data[1][0] = mat(1, 0);
    m_data[1][1] = mat(1, 1);
    m_data[1][2] = mat(1, 2);
 
    m_data[2][0] = mat(2, 0);
    m_data[2][1] = mat(2, 1);
    m_data[2][2] = mat(2, 2);
  }
 
  inline Matrix3x3::Matrix3x3( double a00, double a01, double a02,
                               double a10, double a11, double a12,
                               double a20, double a21, double a22)
  {
    m_data[0][0] = a00;
    m_data[0][1] = a01;
    m_data[0][2] = a02;
 
    m_data[1][0] = a10;
    m_data[1][1] = a11;
    m_data[1][2] = a12;
 
    m_data[2][0] = a20;
    m_data[2][1] = a21;
    m_data[2][2] = a22;
 
  }
 
  inline Matrix3x3::Matrix3x3( double const * const ptr )
  {
    set( ptr );
  }
 
 
  inline Matrix3x3::Matrix3x3(const Quat& quat)
  {
    this->setIdentity();
    this->setRotate( quat );
  }
 
  inline Matrix3x3::~Matrix3x3()
  {}
 
 
  /*=====================================================
                        Queries
  =======================================================*/
 
  inline bool Matrix3x3::valid() const
  {
    return !isNaN();
  }
 
 
 
  inline bool Matrix3x3::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[1][0] ) || std::isnan( m_data[1][1] )
      || std::isnan( m_data[1][2] )
      || std::isnan( m_data[2][0] ) || std::isnan( m_data[2][1] )
      || std::isnan( m_data[2][2] );
  }
 
 
 
  inline bool Matrix3x3::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[1][0] ) && std::isfinite( m_data[1][1] )
      && std::isfinite( m_data[1][2] )
      && std::isfinite( m_data[2][0] ) && std::isfinite( m_data[2][1] )
      && std::isfinite( m_data[2][2] );
  }
 
 
  inline bool Matrix3x3::isIdentity() const
  {
    return *this == Matrix3x3();
  }
 
 
  inline bool Matrix3x3::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));
    return isRigid;
  }
 
 
 
  /*=====================================================
                        Setters
  =======================================================*/
 
  inline Matrix3x3 Matrix3x3::set( double const * const ptr )
  {
    memcpy(m_data, ptr, sizeof(double)*9);
    return *this;
  }
 
 
 
  inline Matrix3x3 Matrix3x3::set( double a00, double a01, double a02,
                                   double a10, double a11, double a12,
                                   double a20, double a21, double a22)
  {
   m_data[0][0] = a00;
   m_data[0][1] = a01;
   m_data[0][2] = a02;
 
   m_data[1][0] = a10;
   m_data[1][1] = a11;
   m_data[1][2] = a12;
 
   m_data[2][0] = a20;
   m_data[2][1] = a21;
   m_data[2][2] = a22;
 
   return *this;
  }
 
 
  inline Matrix3x3 Matrix3x3::setRotate( const Vec3& from, const Vec3& to )
  {
    Quat quat;
    quat.setRotate(from,to);
    this->setRotate(quat);
    return *this;
  }
 
 
  inline Matrix3x3 Matrix3x3::setRotate( double angle, const Vec3& axis )
  {
    Quat quat;
    quat.setRotate( angle, axis);
    this->setRotate(quat);
    return *this;
  }
 
 
  inline Matrix3x3 Matrix3x3::setRotate( double angle, double x, double y, double z )
  {
    Quat quat;
    quat.setRotate( angle, x, y, z);
    this->setRotate(quat);
    return *this;
  }
 
 
  inline Matrix3x3 Matrix3x3::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 Matrix3x3 Matrix3x3::setRotate(const Quat& q_in)
  {
    Quat q(q_in);
    double length2 = q.length2();
    if (length2 != 1.0 && length2 != 0.0)
    {
      // normalize quat if required.
      q /= agx::Quat::Type(std::sqrt(length2));
    }
 
    // Source: Gamasutra, Rotating Objects Using Quaternions
    //
    //http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm
 
    double wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
 
    // calculate coefficients
    x2 = q[0] + q[0];
    y2 = q[1] + q[1];
    z2 = q[2] + q[2];
 
    xx = q[0] * x2;
    xy = q[0] * y2;
    xz = q[0] * z2;
 
    yy = q[1] * y2;
    yz = q[1] * z2;
    zz = q[2] * z2;
 
    wx = q[3] * x2;
    wy = q[3] * y2;
    wz = q[3] * z2;
 
    // Note.  Gamasutra gets the matrix assignments inverted, resulting
    // in left-handed rotations, which is contrary to OpenGL and OSG's
    // methodology.  The matrix assignment has been altered in the next
    // few lines of code to do the right thing.
    // Don Burns - Oct 13, 2001
    m_data[0][0] = (1.0 - (yy + zz));
    m_data[1][0] = (xy - wz);
    m_data[2][0] = (xz + wy);
 
 
    m_data[0][1] = (xy + wz);
    m_data[1][1] = (1.0 - (xx + zz));
    m_data[2][1] = (yz - wx);
 
 
    m_data[0][2] = (xz - wy);
    m_data[1][2] = (yz + wx);
    m_data[2][2] = (1.0 - (xx + yy));
 
    return *this;
  }
 
 
  inline Matrix3x3 Matrix3x3::setIdentity()
  {
    this->set(1,0,0, 0,1,0, 0,0,1);
    return *this;
  }
 
  /*=====================================================
                        Getters
  =======================================================*/
 
  inline Quat Matrix3x3::getRotate() const
  {
    Quat q;
    q = getAsQuat();
    return q;
  }
 
  inline double *Matrix3x3::ptr()
  {
    return ( double* )m_data;
  }
 
 
  inline const double *Matrix3x3::ptr() const
  {
    return ( const double * )m_data;
  }
 
  /*=====================================================
                      Serialization methods
  =======================================================*/
 
  /*=====================================================
                      Static methods
  =======================================================*/
 
 
  inline Matrix3x3 Matrix3x3::crossMatrix(const Vec3& v)
  {
    return Matrix3x3( 0, -v.z(), v.y(),
                            v.z(), 0, -v.x(),
                            -v.y(), v.x(), 0);
  }
 
  inline Matrix3x3 Matrix3x3::rotate( const Vec3& from, const Vec3& to )
  {
    Matrix3x3 m;
    m.setRotate(from, to);
    return m;
  }
 
 
  inline Matrix3x3 Matrix3x3::rotate( double angle, double x, double y, double z )
  {
    Matrix3x3 m;
    m.setRotate(angle, x, y, z);
    return m;
  }
 
 
  inline Matrix3x3 Matrix3x3::rotate( double angle, const Vec3& axis )
  {
    Matrix3x3 m;
    m.setRotate(angle, axis);
    return m;
  }
 
 
  inline Matrix3x3 Matrix3x3::rotate( double angle1, const Vec3& axis1,
                                        double angle2, const Vec3& axis2,
                                        double angle3, const Vec3& axis3 )
  {
    Matrix3x3 m;
    m.setRotate(angle1, axis1, angle2, axis2, angle3, axis3);
    return m;
  }
 
 
  /*=====================================================
                      Operators
  =======================================================*/
 
 
  inline void Matrix3x3::mult( const Matrix3x3& lhs, const Matrix3x3& 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_MATRIX3X3_INNER_PRODUCT(lhs, rhs, 0, 0);
    m_data[0][1] = MOMENTUM_MATRIX3X3_INNER_PRODUCT(lhs, rhs, 0, 1);
    m_data[0][2] = MOMENTUM_MATRIX3X3_INNER_PRODUCT(lhs, rhs, 0, 2);
 
    m_data[1][0] = MOMENTUM_MATRIX3X3_INNER_PRODUCT(lhs, rhs, 1, 0);
    m_data[1][1] = MOMENTUM_MATRIX3X3_INNER_PRODUCT(lhs, rhs, 1, 1);
    m_data[1][2] = MOMENTUM_MATRIX3X3_INNER_PRODUCT(lhs, rhs, 1, 2);
 
    m_data[2][0] = MOMENTUM_MATRIX3X3_INNER_PRODUCT(lhs, rhs, 2, 0);
    m_data[2][1] = MOMENTUM_MATRIX3X3_INNER_PRODUCT(lhs, rhs, 2, 1);
    m_data[2][2] = MOMENTUM_MATRIX3X3_INNER_PRODUCT(lhs, rhs, 2, 2);
 
  }
 
 
  inline void Matrix3x3::preMult( const Matrix3x3& other )
  {
 
    // more efficient method just use a T[3] for temporary storage.
    double t[3];
    for(size_t col=0; col<3; ++col) {
      t[0] = MOMENTUM_MATRIX3X3_INNER_PRODUCT( other, *this, 0, col );
      t[1] = MOMENTUM_MATRIX3X3_INNER_PRODUCT( other, *this, 1, col );
      t[2] = MOMENTUM_MATRIX3X3_INNER_PRODUCT( other, *this, 2, col );
      m_data[0][col] = t[0];
      m_data[1][col] = t[1];
      m_data[2][col] = t[2];
    }
  }
 
 
  inline void Matrix3x3::postMult( const Matrix3x3& other )
  {
 
    // more efficient method just use a T[3] for temporary storage.
    double t[3];
    for(size_t row=0; row<3; ++row)
    {
      t[0] = MOMENTUM_MATRIX3X3_INNER_PRODUCT( *this, other, row, 0 );
      t[1] = MOMENTUM_MATRIX3X3_INNER_PRODUCT( *this, other, row, 1 );
      t[2] = MOMENTUM_MATRIX3X3_INNER_PRODUCT( *this, other, row, 2 );
 
      MOMENTUM_MATRIX3X3_SET_ROW(row, t[0], t[1], t[2])
    }
  }
 
  inline bool Matrix3x3::operator== ( const Matrix3x3& m ) const
  {
    const int bitCompare = memcmp(m_data, m.m_data, sizeof(m_data));
    return (bitCompare == 0);
  }
 
 
  inline bool Matrix3x3::operator!= ( const Matrix3x3& m ) const
  {
    const int bitCompare = memcmp(m_data, m.m_data, sizeof(m_data));
    return (bitCompare != 0);
  }
 
#ifndef SWIG
 
  inline double& Matrix3x3::operator()( size_t row, size_t col )
  {
    return m_data[row][col];
  }
#endif
 
 
  inline double Matrix3x3::operator()( size_t row, size_t col ) const
  {
    return m_data[row][col];
  }
 
 
  inline Matrix3x3 Matrix3x3::operator= ( const Matrix3x3& rhs )
  {
    if ( this != &rhs )
      this->set( rhs.ptr() );
 
    return *this;
  }
 
 
  inline void Matrix3x3::operator*= ( const Matrix3x3& other )
  {
    if ( this == &other )
    {
      Matrix3x3 temp( other );
      postMult( temp );
    }
    else postMult( other );
  }
 
 
  inline Matrix3x3 Matrix3x3::operator* ( const Matrix3x3 &m ) const
  {
    Matrix3x3 r;
    r.mult( *this, m );
    return  r;
  }
 
 
  inline Vec3 Matrix3x3::operator* ( const Vec3& v ) const
  {
    return mult( v );
  }
 
  inline Vec3 operator* ( const Vec3& v, const Matrix3x3& m )
  {
    return m.mult( v );
  }
 
  std::ostream& operator<< (std::ostream& os, const Matrix3x3& m);
 
  /*=====================================================
                      Internal
  =======================================================*/
 
  inline Vec3 Matrix3x3::mult( const Vec3& v ) const
  {
    return Vec3( ( m_data[0][0]*v.x() + m_data[0][1]*v.y() + m_data[0][2]*v.z() ) ,
                 ( m_data[1][0]*v.x() + m_data[1][1]*v.y() + m_data[1][2]*v.z() ) ,
                 ( m_data[2][0]*v.x() + m_data[2][1]*v.y() + m_data[2][2]*v.z() ) ) ;
  }
 
 
 
  inline Matrix3x3 Matrix3x3::transpose() const
  {
    return Matrix3x3( m_data[0][0], m_data[1][0], m_data[2][0],
                            m_data[0][1], m_data[1][1], m_data[2][1],
                            m_data[0][2], m_data[1][2], m_data[2][2]);
  }
 
  inline double Matrix3x3::determinant() const
  {
    return m_data[0][0] * m_data[1][1] * m_data[2][2] - m_data[0][0] * m_data[1][2] * m_data[2][1] -
      m_data[0][1] * m_data[1][0] * 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[0][2] * m_data[1][1] * m_data[2][0];
  }
 
  inline bool Matrix3x3::isDiagonal() const
  {
    return m_data[0][1] == 0.0 && m_data[0][2] == 0.0 &&
      m_data[1][0] == 0.0 && m_data[1][2] == 0.0 &&
      m_data[2][0] == 0.0 && m_data[2][1] == 0.0;
  }
 
  inline Matrix3x3 Matrix3x3::fastInverse(double det) const
  {
    Matrix3x3 dest;
 
    double k = 1.0 / det;
 
    dest(0, 0) = k * MOMENTUM_MATRIX3X3_DET2(m_data[1][1], m_data[1][2], m_data[2][1], m_data[2][2]);
    dest(0, 1) = k * MOMENTUM_MATRIX3X3_DET2(m_data[0][2], m_data[0][1], m_data[2][2], m_data[2][1]);
    dest(0, 2) = k * MOMENTUM_MATRIX3X3_DET2(m_data[0][1], m_data[0][2], m_data[1][1], m_data[1][2]);
 
    dest(1, 0) = k * MOMENTUM_MATRIX3X3_DET2(m_data[1][2], m_data[1][0], m_data[2][2], m_data[2][0]);
    dest(1, 1) = k * MOMENTUM_MATRIX3X3_DET2(m_data[0][0], m_data[0][2], m_data[2][0], m_data[2][2]);
    dest(1, 2) = k * MOMENTUM_MATRIX3X3_DET2(m_data[0][2], m_data[0][0], m_data[1][2], m_data[1][0]);
 
    dest(2, 0) = k * MOMENTUM_MATRIX3X3_DET2(m_data[1][0], m_data[1][1], m_data[2][0], m_data[2][1]);
    dest(2, 1) = k * MOMENTUM_MATRIX3X3_DET2(m_data[0][1], m_data[0][0], m_data[2][1], m_data[2][0]);
    dest(2, 2) = k * MOMENTUM_MATRIX3X3_DET2(m_data[0][0], m_data[0][1], m_data[1][0], m_data[1][1]);
 
    return dest;
  }
 
  namespace {
    inline void momentum_findMaxElement(const Matrix3x3& mat, bool skipFlags[3], int coord[2])
    {
      double max = 0;
      // Will return {the first non-skipped row},0 in case of an all-NaN or all-inf matrix.
      coord[0] = (skipFlags[0] ? (skipFlags[1] ? 2 : 1) : 0);
      coord[1] = 1;
 
      for (int i = 0; i < 3; i++)
      {
        if (skipFlags[i])
          continue;
 
        for (int j = 0; j < 3; j++)
        {
          if (std::abs(mat(i, j)) > max)
          {
            coord[0] = i;
            coord[1] = j;
            max = std::abs(mat(i, j));
          }
        }
      }
 
      agxAssert(coord[0] >= 0 && coord[1] >= 0);
    }
  }
 
  inline Matrix3x3 Matrix3x3::inverse() const
  {
    /* Check if diagonal matrix */
    if (this->isDiagonal())
      return Matrix3x3(1.0 / m_data[0][0], 0, 0, 0, 1.0 / m_data[1][1], 0, 0, 0, 1.0 / m_data[2][2]);
 
    /* Check if determinant is large enough to do fast inverse */
    double determinant = this->determinant();
 
    if (determinant > agx::RealEpsilon)
      return this->fastInverse(determinant);
 
    /* Else do robust, but slow inversion */
    const int next[3] = { 1, 2, 0 };
    Matrix3x3 result;
    Matrix3x3 tmp(*this);
 
    bool done[3] = { false, false, false };
    int pivotColumns[3] = { -1, -1, -1 };
 
    for (int i = 0; i < 3; i++)
    {
      // Select pivot row
      int coord[2];
      momentum_findMaxElement(tmp, done, coord);
      pivotColumns[(int)coord[0]] = (int)coord[1];
 
      // Gauss-Jordan pivoting
      double *row = &tmp(coord[0], 0);
      double *resultRow = &result(coord[0], 0);
 
      // Scale target row
      double scale = 1.0 / row[coord[1]];
      for (int j = 0; j < 3; j++)
      {
        row[j] *= scale;
        resultRow[j] *= scale;
      }
 
 
      // Column operations on other rows
      for (int r = next[coord[0]]; r != coord[0]; r = next[r])
      {
        double *ra = &tmp(r, 0);
        double *rb = &result(r, 0);
        double f = ra[coord[1]];
 
        for (int j = 0; j < 3; j++)
        {
          ra[j] -= f*row[j];
          rb[j] -= f*resultRow[j];
        }
      }
 
      done[coord[0]] = true;
    }
 
    // Do final row permutations
    for (int i = 0; i < 3; i++)
    {
      double *ra = &tmp(pivotColumns[i], 0);
      double *rb = &result(i, 0);
      memcpy(ra, rb, sizeof(double) * 3);
    }
 
    return tmp;
  }
 
 
  inline bool equivalent( const Matrix3x3& a, const Matrix3x3& 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(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(2, 0), b(2, 0), epsilon) &&
      agx::equivalent(a(2, 1), b(2, 1), epsilon) &&
      agx::equivalent(a(2, 2), b(2, 2), epsilon);
  }
 
}
 
#endif //"#ifndef SWIG"
 
#endif