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;
 
  // Builds a 3x3 matrix from 9 zero elements.
  Tmat3();
 
  // Builds a matrix that has the argument at the diagonal values, zero otherwise
  Tmat3(const DT diagValue);
 
  // Builds a 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]);
 
  // Initializes 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);
 
  // Copies the contents from float array mat into *this, row by row.
  void setValues(const float mat[9]);
 
  // Copies the 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;
 
  // Copies the contents from double array mat into *this, row by row.
  void setValues(const double mat[9]);
 
  // Copies the contents of *this into double array mat, row by row.
  void getValues(double mat[9]) const;
 
  // Sets the diagonal matrix with scale on diagonal.
  void setScaleMatrix(const DT scale);
 
  // Returns a matrix filled with values val.
  static Tmat3<DT> getMat(const double val);
 
  // Sets all values to val.
  void set(DT val);
 
  bool operator<(const Tmat3<DT> &) const { return false; }
 
  // Assigns from a Tmat3
  const Tmat3<DT> &operator=(const Tmat3<DT> &m);
 
  // Increments by a Tmat3.
  const Tmat3<DT> &operator+=(const Tmat3<DT> &m);
 
  // Decrements by a Tmat3
  const Tmat3<DT> &operator-=(const Tmat3<DT> &m);
 
  // Multiplies by a scalar constant
  const Tmat3<DT> &operator*=(const DT d);
 
  // Divides 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
  //-------------------------------------------------
  // Returns the determinant of this matrix.
  DT det() const;
 
  // Returns the transpose of this matrix.
  Tmat3 transpose() const;
 
  // Returns the 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=nullptr) const;
 
  // Applies the function fct to each component.
  const Tmat3<DT> &apply(MLDblFuncPtr fct);
 
  // Calculates the Jacobi-Decomposition of 3x3 matrix.
  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);
}
 
// Constructs a matrix that has the argument at the diagonal values, zero otherwise
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