MeVis/Foundation/Sources/MLLinearAlgebra/mlMatrix3.h

Go to the documentation of this file.

// **InsertLicense** code 
//=====================================================================================

//=====================================================================================

#ifndef __mlMatrix3_H
#define __mlMatrix3_H

// Include system independency file and project settings.
#ifndef __mlLinearAlgebraSystem_H
#include "mlLinearAlgebraSystem.h"
#endif
#ifndef __mlLinearAlgebraDefs_H
#include "mlLinearAlgebraDefs.h"
#endif
#ifndef __mlLinearAlgebraTools_H
#include "mlLinearAlgebraTools.h"
#endif

#ifndef __mlFloatingPointMatrix_H
#include "mlFloatingPointMatrix.h"
#endif

#ifndef __mlMatrix2_H
#include "mlMatrix2.h"
#endif
#ifndef __mlVector3_H
#include "mlVector3.h"
#endif

// All declarations of this header will be in the ML_LA_NAMESPACE namespace.
ML_LA_START_NAMESPACE

//--------------------------------------------------------------------
//--------------------------------------------------------------------
template <class DT>
class Tmat3 : public FloatingPointMatrix<Tvec3<DT>, 3>
{
  
public:
  
  typedef DT ComponentType;
  
  // Build 3x3 matrix from 9 zero elements.
  Tmat3();
  
  // Build matrix that has the argument at the diagonal values, zero otherwise
  Tmat3(const DT diagValue);

  // Build matrix of the three row vectors row0, row1, row2.
  Tmat3(const Tvec3<DT> &row0, const Tvec3<DT> &row1, const Tvec3<DT> &row2);
  
  // Copy constructor from the Tmat3 mat.
  Tmat3(const Tmat3<DT> &mat);
  
  // Constructor from 9 floating point values in an array given by mat, row by row.
  Tmat3(const float mat[9]);
  
  // Constructor from 9 double values in an array given by mat, row by row.
  Tmat3(const double mat[9]);
  
  // Initialize all matrix elements explicitly with scalars,
  // filling it row by row.
  Tmat3(const double in00, const double in01, const double in02,
        const double in10, const double in11, const double in12,
        const double in20, const double in21, const double in22);
  
  // Copy contents from float array mat into *this, row by row.
  void setValues(const float mat[9]);
  
  // Copy contents of *this into float array mat, row by row.
  // Note that range and precision of the float values may not be
  // sufficient for the double matrix contents.
  void getValues(float mat[9]) const;
  
  // Copy contents from double array mat into *this, row by row.
  void setValues(const double mat[9]);
  
  // Copy contents of *this into double array mat, row by row.
  void getValues(double mat[9]) const;
  
  // Set diagonal matrix with scale on diagonal.
  void setScaleMatrix(const DT scale);
  
  // Return a matrix filled with values val.
  static Tmat3<DT> getMat(const double val);
  
  // Set all values to val.
  void set(DT val);
  
  bool operator<(const Tmat3<DT> &) const { return false; }
  
  // Assignment from a Tmat3
  const Tmat3<DT> &operator=(const Tmat3<DT> &m);
  
  // Incrementation by a Tmat3.
  const Tmat3<DT> &operator+=(const Tmat3<DT> &m);
  
  // Decrement by a Tmat3
  const Tmat3<DT> &operator-=(const Tmat3<DT> &m);
  
  // Multiplication by a scalar constant
  const Tmat3<DT> &operator*=(const DT d);
  
  // Division by a scalar constant. 
  // Division by zero is not handled and must be avoided by caller.
  const Tmat3<DT> &operator/=(const DT d);
  
  //-------------------------------------------------
  // Special functions
  //-------------------------------------------------
  // Determinant.
  DT det() const;
  
  // Transpose.
  Tmat3 transpose() const;
  
  // Return identity matrix.
  static Tmat3 getIdentity();
  
  // Returns the inverse. Gauss-Jordan elimination with partial pivoting.
  // If a non-NULL Boolean pointer is passed to isInvertible 
  // then true is returned in *isInvertible in the case of a 
  // successful inversion or false if the inversion is not possible
  // (function return is the identity then).
  // If a NULL pointer is passed as isInvertible the matrix must
  // be invertible, otherwise errors will occur.
  Tmat3 inverse(bool* isInvertible=NULL) const;
  
  // Apply the function fct to each component.
  const Tmat3<DT> &apply(MLDblFuncPtr fct);
  
  // Calculate Jacobi-Decomposition of 3x3 matrix.
  Tmat3 jacobi(Tvec3<DT> &eVal, int &rots) const;
  
}; // end of class *Tmat3*



//---------------------------------------------------------------------

//---------------------------------------------------------------------
template <class DT>
inline Tmat3<DT>::Tmat3()
{
  ML_TRACE_IN_TIME_CRITICAL("Tmat3<DT>::Tmat3())");
  
  this->v[0] = this->v[1] = this->v[2] = Tvec3<DT>(0);
}

// Construct matrix that has the argument at the diagonal values, zero otherwise
template <class DT>
inline Tmat3<DT>::Tmat3(const DT diagValue)
{
  ML_TRACE_IN_TIME_CRITICAL("Tmat3<DT>::Tmat3(diagValue)");
  
  this->v[0][0] = this->v[1][1] = this->v[2][2] = diagValue;
  this->v[1][0] = this->v[2][0] = this->v[2][1] = 0;
  this->v[0][1] = this->v[0][2] = this->v[1][2] = 0;
}

template <class DT>
inline Tmat3<DT>::Tmat3(const Tvec3<DT> &row0, const Tvec3<DT> &row1, const Tvec3<DT> &row2)
{
  ML_TRACE_IN_TIME_CRITICAL("Tmat3<DT>::Tmat3(const Tvec3<DT> &row0, const Tvec3<DT> &row1, const Tvec3<DT> &row2)");
  
  this->v[0] = row0; 
  this->v[1] = row1; 
  this->v[2] = row2;
}

template <class DT>
inline Tmat3<DT>::Tmat3(const Tmat3<DT> &mat)
{
  ML_TRACE_IN_TIME_CRITICAL("Tmat3<DT>::Tmat3(const Tmat3<DT> &mat)");
  
  this->v[0] = mat.v[0]; 
  this->v[1] = mat.v[1]; 
  this->v[2] = mat.v[2];
}

template <class DT>
inline Tmat3<DT>::Tmat3(const float mat[9])
{
  ML_TRACE_IN_TIME_CRITICAL("Tmat3<DT>::Tmat3(const float mat[9]))");
  
  setValues(mat);
}

template <class DT>
inline Tmat3<DT>::Tmat3(const double mat[9])
{
  ML_TRACE_IN_TIME_CRITICAL("Tmat3<DT>::Tmat3(const double mat[9])");
  
  setValues(mat);
}

template <class DT>
inline Tmat3<DT> Tmat3<DT>::getMat(const double val)
{
  ML_TRACE_IN_TIME_CRITICAL("Tmat3<DT>::getMat(const double val)");
  
  return Tmat3<DT>(val, val, val,
                   val, val, val,
                   val, val, val);
}

template <class DT>
inline void Tmat3<DT>::set(DT val)
{
  ML_TRACE_IN_TIME_CRITICAL("Tmat3<DT>::set(DT val)");
  
  this->v[0] = this->v[1] = this->v[2] = Tvec3<DT>(val);
}

template <class DT>
inline const Tmat3<DT>  &Tmat3<DT>::operator=(const Tmat3<DT> &m)
{
  ML_TRACE_IN_TIME_CRITICAL("Tmat3<DT>::operator=(const Tmat3<DT> &m)");
  
  if (&m != this){   
    this->v[0] = m.v[0]; 
    this->v[1] = m.v[1]; 
    this->v[2] = m.v[2];
  }
  
  return *this;
}

template <class DT>
inline const Tmat3<DT>  &Tmat3<DT>::operator+=(const Tmat3<DT> &m)
{
  ML_TRACE_IN_TIME_CRITICAL("Tmat3<DT>::operator+=(const Tmat3<DT> &m)");
  
  this->v[0] += m.v[0]; 
  this->v[1] += m.v[1]; 
  this->v[2] += m.v[2];
  
  return *this;
}

template <class DT>
inline const Tmat3<DT>  &Tmat3<DT>::operator-=(const Tmat3<DT> &m)
{
  ML_TRACE_IN_TIME_CRITICAL("Tmat3<DT>::operator-=(const Tmat3<DT> &m)");
  
  this->v[0] -= m.v[0]; 
  this->v[1] -= m.v[1]; 
  this->v[2] -= m.v[2];
  
  return *this;
}

template <class DT>
inline const Tmat3<DT>  &Tmat3<DT>::operator*=(const DT d)
{
  ML_TRACE_IN_TIME_CRITICAL("Tmat3<DT>::operator*=(const DT d)");
  
  this->v[0] *= d; 
  this->v[1] *= d; 
  this->v[2] *= d;
  
  return *this;
}

template <class DT>
inline const Tmat3<DT>  &Tmat3<DT>::operator/=(const DT d)
{
  ML_TRACE_IN("Tmat3<DT>::operator/=(const DT d)");
  ML_TRY
  {
    this->v[0] /= d; 
    this->v[1] /= d; 
    this->v[2] /= d;
  }
  ML_CATCH_RETHROW;
  return *this;
}


//====================================================================

//====================================================================
template <class DT>
inline Tmat3<DT> operator-(const Tmat3<DT> &a)
{
  ML_TRACE_IN_TIME_CRITICAL("mlMatrix3.h: operator-(const Tmat3<DT> &a)");
  
  return Tmat3<DT>(a) *= static_cast<DT>(-1.0);
}

template <class DT>
inline Tmat3<DT> operator+(const Tmat3<DT> &a, const Tmat3<DT> &b)
{
  ML_TRACE_IN_TIME_CRITICAL("mlMatrix3.h: operator+(const Tmat3<DT> &a, const Tmat3<DT> &b)");
  
  return Tmat3<DT>(a) += b;
}

template <class DT>
inline Tmat3<DT> operator-(const Tmat3<DT> &a, const Tmat3<DT> &b)
{
  ML_TRACE_IN_TIME_CRITICAL("mlMatrix3.h: operator-(const Tmat3<DT> &a, const Tmat3<DT> &b)");
  
  return Tmat3<DT>(a) -= b;
}

template <class DT>
inline Tmat3<DT> operator*(const Tmat3<DT> &a, const DT d)
{
  ML_TRACE_IN_TIME_CRITICAL("mlMatrix3.h: operator*(const Tmat3<DT> &a, const DT d)");
  
  return Tmat3<DT>(a) *= d;
}

template <class DT>
inline Tmat3<DT> operator*(const DT d, const Tmat3<DT> &a)
{
  ML_TRACE_IN_TIME_CRITICAL("mlMatrix3.h: operator*(const DT d, const Tmat3<DT> &a)");
  
  return Tmat3<DT>(a) *= d;
}

template <class DT>
inline Tmat3<DT> operator/(const Tmat3<DT>  &a, const DT d)
{
  ML_TRACE_IN_TIME_CRITICAL("mlMatrix3.h: operator/(const Tmat3<DT>  &a, const DT d)");
  
  Tmat3<DT> retVal(a);
  ML_TRY
  {
    retVal /= d;
  }
  ML_CATCH_RETHROW;
  return retVal;
}


//-------------------------------------------------------------------
//

//
//-------------------------------------------------------------------

//-------------------------------------------------------------------
//-------------------------------------------------------------------
template <class DT>
inline Tmat3<DT>::Tmat3(const double in00, const double in01, const double in02,
                        const double in10, const double in11, const double in12,
                        const double in20, const double in21, const double in22)
{
  ML_TRACE_IN("Tmat3<DT>::Tmat3( )");
  
  this->v[0][0]=static_cast<DT>(in00); this->v[0][1]=static_cast<DT>(in01); this->v[0][2]=static_cast<DT>(in02);
  this->v[1][0]=static_cast<DT>(in10); this->v[1][1]=static_cast<DT>(in11); this->v[1][2]=static_cast<DT>(in12);
  this->v[2][0]=static_cast<DT>(in20); this->v[2][1]=static_cast<DT>(in21); this->v[2][2]=static_cast<DT>(in22);
}


//--------------------------------------------------------------------
//--------------------------------------------------------------------
template <class DT>
inline void Tmat3<DT>::setValues(const float mat[9])
{
  ML_TRACE_IN("Tmat3<DT>::setValues( )");
  ML_TRY
  {
    this->v[0][0] = static_cast<DT>(mat[0]); this->v[0][1] = static_cast<DT>(mat[1]); this->v[0][2] = static_cast<DT>(mat[2]);
    this->v[1][0] = static_cast<DT>(mat[3]); this->v[1][1] = static_cast<DT>(mat[4]); this->v[1][2] = static_cast<DT>(mat[5]);
    this->v[2][0] = static_cast<DT>(mat[6]); this->v[2][1] = static_cast<DT>(mat[7]); this->v[2][2] = static_cast<DT>(mat[8]);
  }
  ML_CATCH_RETHROW;
}

//--------------------------------------------------------------------
//--------------------------------------------------------------------
template <class DT>
inline void Tmat3<DT>::getValues(float mat[9]) const
{
  ML_TRACE_IN("Tmat3<DT>::setValues( )");
  ML_TRY
  {
    mat[0] = static_cast<float>(this->v[0][0]); mat[1] = static_cast<float>(this->v[0][1]); mat[2] = static_cast<float>(this->v[0][2]);
    mat[3] = static_cast<float>(this->v[1][0]); mat[4] = static_cast<float>(this->v[1][1]); mat[5] = static_cast<float>(this->v[1][2]);
    mat[6] = static_cast<float>(this->v[2][0]); mat[7] = static_cast<float>(this->v[2][1]); mat[8] = static_cast<float>(this->v[2][2]);
  }
  ML_CATCH_RETHROW;
}

//--------------------------------------------------------------------
//--------------------------------------------------------------------
template <class DT>
inline void Tmat3<DT>::setValues(const double mat[9])
{
  ML_TRACE_IN("Tmat3<DT>::setValues( )");
  ML_TRY
  {
    this->v[0][0] = static_cast<DT>(mat[0]); this->v[0][1] = static_cast<DT>(mat[1]); this->v[0][2] = static_cast<DT>(mat[2]);
    this->v[1][0] = static_cast<DT>(mat[3]); this->v[1][1] = static_cast<DT>(mat[4]); this->v[1][2] = static_cast<DT>(mat[5]);
    this->v[2][0] = static_cast<DT>(mat[6]); this->v[2][1] = static_cast<DT>(mat[7]); this->v[2][2] = static_cast<DT>(mat[8]);
  }
  ML_CATCH_RETHROW;
}

//--------------------------------------------------------------------
//--------------------------------------------------------------------
template <class DT>
inline void Tmat3<DT>::getValues(double mat[9]) const
{
  ML_TRACE_IN("Tmat3<DT>::setValues( )");
  ML_TRY
  {
    mat[0] = static_cast<double>(this->v[0][0]); mat[1] = static_cast<double>(this->v[0][1]); mat[2] = static_cast<double>(this->v[0][2]);
    mat[3] = static_cast<double>(this->v[1][0]); mat[4] = static_cast<double>(this->v[1][1]); mat[5] = static_cast<double>(this->v[1][2]);
    mat[6] = static_cast<double>(this->v[2][0]); mat[7] = static_cast<double>(this->v[2][1]); mat[8] = static_cast<double>(this->v[2][2]);
  }
  ML_CATCH_RETHROW;
}


//-------------------------------------------------------------------
//-------------------------------------------------------------------
template <class DT>
inline void Tmat3<DT>::setScaleMatrix(const DT scale)
{
  ML_TRACE_IN("Tmat3<DT>::setScaleMatrix( )");
  
  this->v[0][0]=scale; this->v[0][1]=0;     this->v[0][2]=0;
  this->v[1][0]=0;     this->v[1][1]=scale; this->v[1][2]=0;
  this->v[2][0]=0;     this->v[2][1]=0;     this->v[2][2]=scale;
}


//-------------------------------------------------------------------

//-------------------------------------------------------------------

#define DET3(A,B,C,D,E,F,G,H,I) ((A*E*I + B*F*G + C*D*H) - (A*F*H + B*D*I + C*E*G))
//-------------------------------------------------------------------
//-------------------------------------------------------------------
template <class DT>
DT Tmat3<DT>::det() const
{
  ML_TRACE_IN("Tmat3<DT>::det( )");
  
  return DET3(this->v[0][0], this->v[0][1], this->v[0][2],
              this->v[1][0], this->v[1][1], this->v[1][2],
              this->v[2][0], this->v[2][1], this->v[2][2]);
}
#undef DET3

//-------------------------------------------------------------------
//-------------------------------------------------------------------
template <class DT>
inline Tmat3<DT> Tmat3<DT>::transpose() const
{
  ML_TRACE_IN_TIME_CRITICAL("Tmat3<DT>::transpose()");
  
  return Tmat3<DT>(Tvec3<DT>(this->v[0][0], this->v[1][0], this->v[2][0]),
                   Tvec3<DT>(this->v[0][1], this->v[1][1], this->v[2][1]),
                   Tvec3<DT>(this->v[0][2], this->v[1][2], this->v[2][2]));
}

//-------------------------------------------------------------------
//-------------------------------------------------------------------
template <class DT>
inline Tmat3<DT> Tmat3<DT>::getIdentity()
{
  ML_TRACE_IN_TIME_CRITICAL("Tmat3<DT>::getIdentity()");
  
  return Tmat3<DT>(Tvec3<DT>(1,0,0),
                   Tvec3<DT>(0,1,0),
                   Tvec3<DT>(0,0,1));
}

//-------------------------------------------------------------------
//-------------------------------------------------------------------
template <class DT>
inline const Tmat3<DT>  &Tmat3<DT>::apply(MLDblFuncPtr fct)
{
  ML_TRACE_IN_TIME_CRITICAL("Tmat3<DT>::apply(MLDblFuncPtr fct)");
  ML_TRY
  {
    this->v[0].apply(fct);
    this->v[1].apply(fct);
    this->v[2].apply(fct);
  }
  ML_CATCH_RETHROW;
  return *this;
}

#define _ML_MAT3_RC(i, j) a[i][0]*b[0][j] + a[i][1]*b[1][j] + a[i][2]*b[2][j]
//-------------------------------------------------------------------
//-------------------------------------------------------------------
template <class DT>
inline Tmat3<DT> operator*(const Tmat3<DT> &a, const Tmat3<DT> &b)
{
  ML_TRACE_IN_TIME_CRITICAL("mlMatrix3.h: operator*(const Tmat3<DT> &a, const Tmat3<DT> &b)");
  
  return Tmat3<DT>(Tvec3<DT>(_ML_MAT3_RC(0,0), _ML_MAT3_RC(0,1), _ML_MAT3_RC(0,2)),
                   Tvec3<DT>(_ML_MAT3_RC(1,0), _ML_MAT3_RC(1,1), _ML_MAT3_RC(1,2)),
                   Tvec3<DT>(_ML_MAT3_RC(2,0), _ML_MAT3_RC(2,1), _ML_MAT3_RC(2,2)));
  
}
#undef _ML_MAT3_RC

//-------------------------------------------------------------------
//-------------------------------------------------------------------
template <class DT>
inline bool operator==(const Tmat3<DT> &a, const Tmat3<DT> &b)
{
  ML_TRACE_IN_TIME_CRITICAL("mlMatrix3.h: operator==(const Tmat3<DT> &a, const Tmat3<DT> &b)");
  
  return (a[0] == b[0]) &&
  (a[1] == b[1]) &&
  (a[2] == b[2]);
}

//-------------------------------------------------------------------
//-------------------------------------------------------------------
template <class DT>
inline bool operator!=(const Tmat3<DT> &a, const Tmat3<DT> &b)
{
  ML_TRACE_IN_TIME_CRITICAL("mlMatrix3.h: operator!=(const Tmat3<DT> &a, const Tmat3<DT> &b)");
  
  return !(a == b);
}

//-------------------------------------------------------------------
//
//       Special global 2D functions and 3D functions
//
//-------------------------------------------------------------------

//--------------------------------------------------------------------
//--------------------------------------------------------------------
template <class DT>
Tmat3<DT> identity2D()
{
  ML_TRACE_IN_TIME_CRITICAL("mlMatrix3.h: identity2D()");
  
  return Tmat3<DT>::getIdentity();
}

//-------------------------------------------------------------------
//-------------------------------------------------------------------
template <class DT>
inline Tmat3<DT> translation2D(const Tvec2<DT> &v)
{
  ML_TRACE_IN("mlMatrix3.h: translation2D(const Tvec2<DT> &v)");
  
  return Tmat3<DT>(Tvec3<DT>(1.0, 0.0, v[0]),
                   Tvec3<DT>(0.0, 1.0, v[1]),
                   Tvec3<DT>(0.0, 0.0, 1.0));
}

//-------------------------------------------------------------------
//-------------------------------------------------------------------
template <class DT>
Tmat3<DT> rotation2D(const Tvec2<DT> &Center, const DT angleDeg)
{
  ML_TRACE_IN("mlMatrix3.h: rotation2D(const Tvec2<DT> &Center, const DT angleDeg)");
  
  DT  angleRad = angleDeg * M_PI / 180.0;
  DT  c = cos(angleRad);
  DT  s = sin(angleRad);
  
  return Tmat3<DT>(Tvec3<DT>(c, -s, Center[0] * (1.0-c) + Center[1] * s),
                   Tvec3<DT>(s,  c, Center[1] * (1.0-c) - Center[0] * s),
                   Tvec3<DT>(0.0, 0.0, 1.0));
}

//-------------------------------------------------------------------
//-------------------------------------------------------------------
template <class DT>
inline Tmat3<DT> scaling2D(const Tvec2<DT> &scaleVector)
{
  ML_TRACE_IN("mlMatrix3.h: scaling2D(const Tvec2<DT> &scaleVector)");
  
  return Tmat3<DT>(Tvec3<DT>(scaleVector[0], 0.0, 0.0),
                   Tvec3<DT>(0.0, scaleVector[1], 0.0),
                   Tvec3<DT>(0.0, 0.0, 1.0));
}



//-------------------------------------------------------------------
//-------------------------------------------------------------------
template <class DT>
Tmat3<DT> Tmat3<DT>::jacobi(Tvec3<DT> &evalues, int &rots) const
{
  ML_TRACE_IN("Tmat3<DT>::jacobi( )");
  
  Tmat3<DT> evectors (getIdentity());
  ML_TRY
  {
    // TODO: Further documentation of the algorithm required!
    
    DT  sm = 0;               // smallest entry
    DT  theta = 0;            // angle for Jacobi rotation
    DT  c = 0, s = 0, t = 0;  // cosine, sine, tangent of theta
    DT  tau = 0;              // sine / (1 + cos)
    DT  h = 0, g = 0;         // two scrap values
    DT  thresh = 0;           // threshold below which no rotation done
    
    int   p = 0, q = 0, i = 0, j = 0;
    
    // Initialization
    Tvec3<DT> b (this->v[0][0], this->v[1][1], this->v[2][2]);      // diagonal elements
    Tvec3<DT> z (0, 0, 0);
    evalues = b;
    
    Tmat3<DT> a (*this);
    
    rots = 0;
    for (i = 0; i < 50; i++){           // typical matrices require 6 to 10 sweeps to achieve convergence - hence 50 gives some safety margin
      sm = 0.0;
      for (p = 0; p < 2; p++){
        for (q = p+1; q < 3; q++){
          sm += ML_ABS(a.v[p][q]);         // sum off-diagonal elements
        }
      }
      
      if (sm == 0.0){
        return evectors;
      }
      
      thresh = (i < 3 ? (.2 * sm / 9) : 0.0);
      
      for (p = 0; p < 2; p++) {
        for (q = p+1; q < 3; q++) {    // compute on sweep, i.e. one rotation for each of-diagonal element of the matrix
          
          g = 100.0 * ML_ABS(a.v[p][q]);
          
          if ((i > 3) && 
              (ML_ABS(evalues[p]) + g == ML_ABS(evalues[p])) &&
              (ML_ABS(evalues[q]) + g == ML_ABS(evalues[q]))){
            a.v[p][q] = 0.0;          // after three sweeps, skip rotation of small off diagonal elements
          }
          else if (ML_ABS(a.v[p][q]) > thresh) {
            h = evalues[q] - evalues[p];
            if (ML_ABS(h) + g == ML_ABS(h)){
              t = a.v[p][q] / h;
            }
            else {
              theta = .5 * h / a.v[p][q];
              t = 1.0 / (ML_ABS(theta) + sqrt(1 + theta * theta));
              if (theta < 0.0){  
                t = -t;
              }
            }
            // End of computing tangent of rotation angle
            
            c = 1.0 / sqrt(1.0 + t*t);
            s = t * c;
            tau = s / (1.0 + c);
            h = t * a.v[p][q];
            z[p]    -= h;
            z[q]    += h;
            evalues[p] -= h;
            evalues[q] += h;
            a.v[p][q] = 0.0;
            
            for (j = 0; j < p; j++) {
              g = a.v[j][p];
              h = a.v[j][q];
              a.v[j][p] = g - s * (h + g * tau);
              a.v[j][q] = h + s * (g - h * tau);
            }
            
            for (j = p+1; j < q; j++) {
              g = a.v[p][j];
              h = a.v[j][q];
              a.v[p][j] = g - s * (h + g * tau);
              a.v[j][q] = h + s * (g - h * tau);
            }
            
            for (j = q+1; j < 3; j++) {
              g = a.v[p][j];
              h = a.v[q][j];
              a.v[p][j] = g - s * (h + g * tau);
              a.v[q][j] = h + s * (g - h * tau);
            }
            
            for (j = 0; j < 3; j++) {
              g = evectors.v[j][p];
              h = evectors.v[j][q];
              evectors.v[j][p] = g - s * (h + g * tau);
              evectors.v[j][q] = h + s * (g - h * tau);
            }
          } // else if (ML_ABS(a.v[p][q]) > thresh)
          
          rots++;
          
        } // for (q = p+1; q < 3; q++) 
      } // for (p = 0; p < 2; p++) 
      
      for (p = 0; p < 3; p++) {
        evalues[p] = b[p] += z[p];
        z[p] = 0;
      }
      
    } // for (i = 0; i < 50; i++)
    ML_PRINT_ERROR("Tmat3 Tmat3<DT>::jacobi(Tvec3<DT> &evalues, int &rots) const, matrix not complete decomposed", 
                   ML_BAD_PARAMETER, 
                   "Returning incomplete decomposition");
  }
  ML_CATCH_RETHROW;
  return evectors;
}

//-------------------------------------------------------------------
//-------------------------------------------------------------------
template <class DT>
Tmat3<DT> Tmat3<DT>::inverse(bool* isInvertible) const
{
  ML_TRACE_IN("Tmat3<DT>::inverse( )");
  
  Tmat3<DT> retVal;
  ML_TRY
  {
    // Epsilon for comparison with 0 in inversion process.
    static const DT Epsilon = static_cast<DT>(0.);
    
    // Use helper function from tools to invert the matrix.
    retVal = MLInverseMatHelper(*this,
                                isInvertible, 
                                Epsilon,
                                "Tmat3<DT> Tmat3<DT>::inverse(bool* isInvertible) const, matrix not invertable",
                                getIdentity(),
                                3);
  }
  ML_CATCH_RETHROW;
  return retVal;
}

//-----------------------------------------------------------------------------------

//-----------------------------------------------------------------------------------

typedef Tmat3<MLfloat>   Matrix3f;
typedef Tmat3<MLdouble>  Matrix3d;
typedef Tmat3<MLldouble> Matrix3ld;
typedef Tmat3<MLdouble>  Matrix3;


#ifdef ML_DEPRECATED



ML_DEPRECATED typedef Tmat3<MLfloat>   matf3;
ML_DEPRECATED typedef Tmat3<MLdouble>  matd3;
ML_DEPRECATED typedef Tmat3<MLldouble> matld3;
ML_DEPRECATED typedef Tmat3<MLdouble> mat3;


#endif // ML_DEPRECATED



ML_LA_END_NAMESPACE

namespace std
{
  //-------------------------------------------------------------------
  //-------------------------------------------------------------------
  template <class DT>
  inline std::ostream &operator<<(std::ostream &os, const ML_LA_NAMESPACE::Tmat3<DT> &m)
  {
    return os << m[0] << '\n' << m[1] << '\n' << m[2];
  }
  
  //-------------------------------------------------------------------
  //-------------------------------------------------------------------
  template <class DT>
  inline std::istream &operator>>(std::istream &is, ML_LA_NAMESPACE::Tmat3<DT> &m)
  {
    ML_LA_NAMESPACE::Tmat3<DT> m_tmp;
    
    is >> m_tmp[0] >> m_tmp[1] >> m_tmp[2];
    if (is){ m = m_tmp; }
    return is;
  }
}


#endif // __mlMatrix3_H