mlMatrix3.h

Go to the documentation of this file.

/*************************************************************************************
**
** Copyright 2007, MeVis Medical Solutions AG
**
** The user may use this file in accordance with the license agreement provided with
** the Software or, alternatively, in accordance with the terms contained in a
** written agreement between the user and MeVis Medical Solutions AG.
**
** For further information use the contact form at https://www.mevislab.de/contact
**
**************************************************************************************/
 
#ifndef ML_MATRIX3_H
#define ML_MATRIX3_H

 
// Include system independent file and project settings.
#include "mlLinearAlgebraSystem.h"
#include "mlLinearAlgebraDefs.h"
#include "mlLinearAlgebraTools.h"
 
#include "mlFloatingPointMatrix.h"
 
#include "mlMatrix2.h"
#include "mlVector3.h"
 
#include <cstdlib>
// 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;

  Tmat3();

  Tmat3(const DT diagValue);

  Tmat3(const Tvec3<DT> &row0, const Tvec3<DT> &row1, const Tvec3<DT> &row2);

  Tmat3(const Tmat3<DT> &mat);

  Tmat3(const float mat[9]);
  
  Tmat3(const double mat[9]);

  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);

  void setValues(const float mat[9]);

  void getValues(float mat[9]) const;

  void setValues(const double mat[9]);

  void getValues(double mat[9]) const;

  void setScaleMatrix(const DT scale);

  static Tmat3<DT> getMat(const double val);

  void set(DT val);

  bool operator<(const Tmat3<DT> &) const { return false; }

  const Tmat3<DT> &operator=(const Tmat3<DT> &m);

  const Tmat3<DT> &operator+=(const Tmat3<DT> &m);

  const Tmat3<DT> &operator-=(const Tmat3<DT> &m);

  const Tmat3<DT> &operator*=(const DT d);

  const Tmat3<DT> &operator/=(const DT d);
 
  //-------------------------------------------------
  // Special functions
  //-------------------------------------------------
  DT det() const;

  Tmat3 transpose() const;

  static Tmat3 getIdentity();

  Tmat3 inverse(bool* isInvertible=nullptr) const;

  const Tmat3<DT> &apply(MLDblFuncPtr fct);

  Tmat3 jacobi(Tvec3<DT> &eVal, int &rots) const;
 
}; // end of class *Tmat3*
 
 
 
//---------------------------------------------------------------------
//---------------------------------------------------------------------
template <class DT>
inline Tmat3<DT>::Tmat3()
{
  this->v[0] = this->v[1] = this->v[2] = Tvec3<DT>(0);
}

template <class DT>
inline Tmat3<DT>::Tmat3(const DT 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)
{
  this->v[0] = row0;
  this->v[1] = row1;
  this->v[2] = row2;
}

template <class DT>
inline 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])
{
  setValues(mat);
}

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

template <class DT>
inline Tmat3<DT> 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)
{
  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)
{
  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)
{
  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)
{
  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)
{
  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)
{
  this->v[0] /= d;
  this->v[1] /= d;
  this->v[2] /= d;
  return *this;
}

 
 
//====================================================================
//====================================================================
template <class DT>
inline Tmat3<DT> 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)
{
  return Tmat3<DT>(a) += b;
}

template <class DT>
inline Tmat3<DT> 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)
{
  return Tmat3<DT>(a) *= d;
}

template <class DT>
inline Tmat3<DT> 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)
{
  Tmat3<DT> retVal(a);
  retVal /= d;
  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)
{
  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])
{
  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]);
}
 
//--------------------------------------------------------------------
//--------------------------------------------------------------------
template <class DT>
inline void Tmat3<DT>::getValues(float mat[9]) const
{
  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]);
}
 
//--------------------------------------------------------------------
//--------------------------------------------------------------------
template <class DT>
inline void Tmat3<DT>::setValues(const double mat[9])
{
  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]);
}
 
//--------------------------------------------------------------------
//--------------------------------------------------------------------
template <class DT>
inline void Tmat3<DT>::getValues(double mat[9]) const
{
  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]);
}
 
 
//-------------------------------------------------------------------
//-------------------------------------------------------------------
template <class DT>
inline void Tmat3<DT>::setScaleMatrix(const DT scale)
{
  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
{
  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
{
  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()
{
  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)
{
  this->v[0].apply(fct);
  this->v[1].apply(fct);
  this->v[2].apply(fct);
  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)
{
  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)
{
  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)
{
  return !(a == b);
}
 
//-------------------------------------------------------------------
//
//       Special global 2D functions and 3D functions
//
//-------------------------------------------------------------------
 
//--------------------------------------------------------------------
//--------------------------------------------------------------------
template <class DT>
Tmat3<DT> identity2D()
{
  return Tmat3<DT>::getIdentity();
}
 
//-------------------------------------------------------------------
//-------------------------------------------------------------------
template <class DT>
inline Tmat3<DT> 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)
{
  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)
{
  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
{
  Tmat3<DT> evectors (getIdentity());
 
  // 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 += std::abs(a.v[p][q]);         // sum off-diagonal elements
      }
    }
 
    if (sm == 0.0){
      return *(&evectors); // circumvent a code generation bug in Visual Studio 2019 (version 16.5.4)
    }
 
    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 * std::abs(a.v[p][q]);
 
        if ((i > 3) &&
            (std::abs(evalues[p]) + g == std::abs(evalues[p])) &&
            (std::abs(evalues[q]) + g == std::abs(evalues[q]))){
          a.v[p][q] = 0.0;          // after three sweeps, skip rotation of small off diagonal elements
        }
        else if (std::abs(a.v[p][q]) > thresh) {
          h = evalues[q] - evalues[p];
          if (std::abs(h) + g == std::abs(h)){
            t = a.v[p][q] / h;
          }
          else {
            theta = .5 * h / a.v[p][q];
            t = 1.0 / (std::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 (std::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++)
  return evectors;
}
 
//-------------------------------------------------------------------
//-------------------------------------------------------------------
template <class DT>
Tmat3<DT> Tmat3<DT>::inverse(bool* isInvertible) const
{
  // 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.
  return MLInverseMatHelper(*this,
                            isInvertible,
                            Epsilon,
                            "Tmat3<DT> Tmat3<DT>::inverse(bool* isInvertible) const, matrix not invertable",
                            getIdentity(),
                            3);
}
 
//-----------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------

typedef Tmat3<MLfloat>   Matrix3f;
typedef Tmat3<MLdouble>  Matrix3d;
typedef Tmat3<MLldouble> Matrix3ld;
typedef Tmat3<MLdouble>  Matrix3;
 
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