Tools/Math/Matrix_nxn.h

Go to the documentation of this file.

/**
* @file Matrix_nxn.h
* Contains class Matrix_nxn
*
* @author <a href="mailto:stefanuhrig@gmx.net">Stefan Uhrig</a>
*/
//------------------------------------------------------------------------------
#ifndef MATRIX_NXN_H_INCLUDED
#define MATRIX_NXN_H_INCLUDED
//------------------------------------------------------------------------------
#define MATRIX_NXN_HEAP
//------------------------------------------------------------------------------
#include "MVTools.h"
#include "Vector_n.h"
//------------------------------------------------------------------------------
// Ensure that memory version (use stack or heap) ist defined
#if !defined(MATRIX_NXN_STACK) && !defined(MATRIX_NXN_HEAP)
#error Define MATRIX_NXN_STACK if you'd like Matrix_nxn to use stack memory or \
MATRIX_NXN_HEAP if you'd like Matrix_nxn to use heap memory!
#endif
//------------------------------------------------------------------------------
// Forward-Deklarationen
//------------------------------------------------------------------------------
template <class T, size_t N>
class Matrix_nxn;
template<class T, size_t N>
Matrix_nxn<T, N> operator*(const Matrix_nxn<T, N>& m1,
                           const Matrix_nxn<T, N>& m2);
template<class T, size_t N>
Matrix_nxn<T, N> invert(const Matrix_nxn<T, N>& m);
//------------------------------------------------------------------------------
/**
* @class Matrix_nxn
* Represents a nxn matrix of type T
*
* Matrix_nxn represents a nxn matrix of type T. The class supplies
* standard arithmetic operations.
*/
template <class T, size_t N>
class Matrix_nxn
{
public:
  //----------------------------------------------------------------------------
  /**
  * Standard constructor
  *
  * Initializes the matrix with values supplied by the standard constructor
  * of T.
  *
  * Complexity: n^2
  */
  Matrix_nxn()
  {
    #ifdef MATRIX_NXN_HEAP
      content = new Vector_n<T, N>[N];
    #endif

    Vector_n<T, N>* init = content;
    Vector_n<T, N> zero = Vector_n<T, N>();
    for (size_t i = 0; i < N; ++i, ++init)
      *init = zero;
  }
  //----------------------------------------------------------------------------
  /**
  * Constructor that initializes the matrix with the values in the passed array
  * @param v Array with initialization values
  *
  * Complexity: n^2
  */
  Matrix_nxn(const T* v)
  {
    #ifdef MATRIX_NXN_HEAP
      content = new Vector_n<T, N>[N];
    #endif

    Vector_n<T, N>* init = content;
    for (size_t i = 0; i < N; ++i, ++init, v+=N)
      *init = Vector_n<T, N>(v);
  }
  //----------------------------------------------------------------------------
  /** 
  * Copy constructor
  * @param m Matrix to copy
  *
  * Complexity: n^2
  */
  Matrix_nxn(const Matrix_nxn<T, N>& m)
  {
    #ifdef MATRIX_NXN_HEAP
      content = new Vector_n<T, N>[N];
    #endif

    *this = m;
  }
  //----------------------------------------------------------------------------
  /**
  * Destructor
  *
  * Complexity: n^2
  */
  ~Matrix_nxn()
  {
    #ifdef MATRIX_NXN_HEAP
      delete [] content;
    #endif
  }
  //----------------------------------------------------------------------------
  /**
  * Copy operator
  * @param m Matrix to copy
  *
  * Complexity: n^2
  */
  Matrix_nxn<T, N>& operator=(const Matrix_nxn<T, N>& m)
  {
    for (size_t i = 0; i < N; ++i)
      content[i] = m.content[i];
    return *this;
  }
  //----------------------------------------------------------------------------
  /**
  * Copy operator
  * @param m Array with values to assign to this matrix
  *
  * Complexity: n^2
  */
  Matrix_nxn<T, N>& operator=(const T* m)
  {
    for (size_t i = 0; i < N; ++i)
      for (size_t j = 0; j <N; ++j, ++m)
        content[i][j] = *m;
    return *this;
  }
  //----------------------------------------------------------------------------
  /**
  * Copies contents of matrix to passed array
  * @param m Array the values are stored to
  *
  * Complexity: n^2
  */
  void copyTo(T* m) const
  {
    const Vector_n<T, N>* con = content;
    for (size_t i = 0; i < N; ++i, m+=N, ++con)
      con->copyTo(m);
  }
  //----------------------------------------------------------------------------
  /**
  * Constant row vector access operator
  * @param i Index of row vector to access (first row vector has index 0)
  * @return Constant reference to row vector
  *
  * Complexity: 1
  */
  const Vector_n<T, N>& operator[](size_t i) const
  {
    return content[i];
  }
  //----------------------------------------------------------------------------
  /**
  * Row vector access operator
  * @param i Index of row vector to access (first row vector has index 0)
  * @return Reference to row vector
  *
  * Complexity: 1
  */
  Vector_n<T, N>& operator[](size_t i)
  {
    return content[i];
  }
  //----------------------------------------------------------------------------
  /**
  * Operator +=
  * @param m Matrix to add
  *
  * Complexity: n^2
  */
  Matrix_nxn<T, N>& operator+=(const Matrix_nxn<T, N>& m)
  {
    for (size_t i = 0; i < N; ++i)
      content[i] += m.content[i];
    return *this;
  }
  //----------------------------------------------------------------------------
  /**
  * Operator -=
  * @param m Matrix to subtract
  *
  * Complexity: n^2
  */
  Matrix_nxn<T, N>& operator-=(const Matrix_nxn<T, N>& m)
  {
    for (size_t i = 0; i < N; ++i)
      content[i] -= m.content[i];
    return *this;
  }
  //----------------------------------------------------------------------------
  /**
  * Operator *=
  * @param m Matrix this matrix is to multiplied with
  *
  * Complexity: n^3
  */
  Matrix_nxn<T, N>& operator*=(const Matrix_nxn<T, N>& m)
  {
    *this = *this * m;
    return *this;
  }
  //----------------------------------------------------------------------------
  /**
  * Operator /=
  * @param m Matrix this matrix is to divided by
  *
  * Complexity: n^3
  */
  Matrix_nxn<T, N>& operator/=(const Matrix_nxn<T, N>& m)
  {
    *this = *this / m;
    return *this;
  }
  //----------------------------------------------------------------------------
  /**
  * Transposes this matrix
  *
  * Complexity: n^2
  */
  Matrix_nxn<T, N>& transpose()
  {
    *this = transpose(*this);
    return *this;
  }
  //----------------------------------------------------------------------------
  /**
  * Inverts this matrix
  *
  * Complexity: n^3
  */
  Matrix_nxn<T, N>& invert()
  {
    *this = invert(*this);
    return *this;
  }
  //----------------------------------------------------------------------------
  /**
  * Solves the system A*x=b where A is the actual matrix
  * @param b Vector b
  * @return Solution x
  *
  * Complexity: n^3
  *
  */
  Vector_n<T, N> solve(Vector_n<T, N> b) const
  {
    // create copy of actual matrix
    Matrix_nxn<T, N> m(*this);
    
    // initialize ranking vector
    Vector_n<int, N> ranking;
    size_t i;
    for (i = 0; i < N; ++i)
      ranking[i] = i;

    T z = T();
    int c;
    int r;
    for (c = 0; c < (int)N-1; ++c)
    {
      // find row containing highest value
      int maxRow = c;
      T maxValue = m[ranking[maxRow]][c];
      if (maxValue < z)
        maxValue = -maxValue;
      for (r = c+1; r < (int)N; ++r)
      {
        T value = m[ranking[r]][c];
        if (value < z)
          value = -value;
        if (value > maxValue)
        {
          maxRow = r;
          maxValue = value;
        }
      }

      // if maximum value zero --> matrix is singular
      if (MVTools::isNearZero(maxValue))
      {
        if (MVTools::isNearNegZero(maxValue))
          throw MVException(MVException::DivByNegZero);
        else
          throw MVException(MVException::DivByPosZero);
      }
      /*
      if (maxValue == z)
        return Vector_n<T, N>();
      */

      // swap rows in ranking
      int temp = ranking[c];
      ranking[c] = ranking[maxRow];
      ranking[maxRow] = temp;

      // process all following rows
      for (r = c+1; r < (int)N; ++r)
      {
        // calc factor for subtracting
        T factor = m[ranking[r]][c] / m[ranking[c]][c];
        if (MVTools::isNearInf(factor))
        {
          if (MVTools::isNearPosInf(factor))
            throw MVException(MVException::PosInfValue);
          else
            throw MVException(MVException::NegInfValue);
        }

        T sub;
        sub = factor*b[ranking[c]];
        if (MVTools::isNearInf(sub))
        {
          if (MVTools::isNearPosInf(sub))
            throw MVException(MVException::PosInfValue);
          else
            throw MVException(MVException::NegInfValue);
        }

        // change vector b
        b[ranking[r]] -= sub;

        // change matrix
        m[ranking[r]][c] = T();
        for (int c2 = c+1; c2 < (int)N; ++c2)
        {
          sub = factor*m[ranking[c]][c2];
          if (MVTools::isNearInf(sub))
          {
            if (MVTools::isNearPosInf(sub))
              throw MVException(MVException::PosInfValue);
            else
              throw MVException(MVException::NegInfValue);
          }
          m[ranking[r]][c2] -= sub;
        }
      }
    }

    // if last entry of matrix zero --> matrix is singular
    if (MVTools::isNearZero(m[ranking[N-1]][N-1]))
    {
      if (MVTools::isNearNegZero(m[ranking[N-1]][N-1]))
        throw MVException(MVException::DivByNegZero);
      else
        throw MVException(MVException::DivByPosZero);
    }
    /*
    if (m[ranking[N-1]][N-1] == z)
      return Vector_n<T, N>();
    */

    // matrix has triangle form
    // calculate solutions
    b[ranking[N-1]] /= m[ranking[N-1]][N-1];
    for (r = N-2; r >= 0; --r)
    {
      T sum = T();
      for (c = r+1; c < (int)N; ++c)
        sum += m[ranking[r]][c] * b[ranking[c]];
      if (MVTools::isNearInf(sum))
      {
        if (MVTools::isNearPosInf(sum))
          throw MVException(MVException::PosInfValue);
        else
          throw MVException(MVException::NegInfValue);
      }
      
      if (MVTools::isNearZero(m[ranking[r]][r]))
      {
        if (MVTools::isNearNegZero(m[ranking[r]][r]))
          throw MVException(MVException::DivByNegZero);
        else
          throw MVException(MVException::DivByPosZero);
      }
      b[ranking[r]] = (b[ranking[r]] - sum) / m[ranking[r]][r];

      if (MVTools::isNearInf(b[ranking[r]]))
      {
        if (MVTools::isNearPosInf(b[ranking[r]]))
          throw MVException(MVException::PosInfValue);
        else
          throw MVException(MVException::NegInfValue);
      }
    }

    // create vector with correct order
    Vector_n<T, N> x;
    for (r = 0; r < (int)N; ++r)
      x[r] = b[ranking[r]];

    return x;
  }
  //----------------------------------------------------------------------------
  /**
  * Returns determinant of this matrix
  * @return determinant
  *
  * Complexity: n^3
  */
  T det() const
  {
    // create copy of actual matrix
    Matrix_nxn<T, N> m(*this);
    
    // initialize ranking vector
    Vector_n<int, N> ranking;
    size_t i;
    for (i = 0; i < N; ++i)
      ranking[i] = i;

    T z = T();
    bool bPositive = true;
    int c;
    int r;
    for (c = 0; c < (int)(N-1); ++c)
    {
      // find row containing highest value
      int maxRow = c;
      T maxValue = m[ranking[maxRow]][c];
      if (maxValue < z)
        maxValue = -maxValue;
      for (r = c+1; r < (int)N; ++r)
      {
        T value = m[ranking[r]][c];
        if (value < z)
          value = -value;
        if (value > maxValue)
        {
          maxRow = r;
          maxValue = value;
        }
      }

      // if maximum value zero --> determinant is zero
      if (MVTools::isNearZero(maxValue))
        return z;
      /*if (maxValue == z)
        return z;*/

      // swap rows in ranking
      if (c != maxRow)
      {
        int temp = ranking[c];
        ranking[c] = ranking[maxRow];
        ranking[maxRow] = temp;
        bPositive = !bPositive;
      }

      // process all following rows
      for (r = c+1; r < (int)N; ++r)
      {
        // calc factor for subtracting
        T factor = m[ranking[r]][c] / m[ranking[c]][c];
        if (MVTools::isNearInf(factor))
        {
          if (MVTools::isNearPosInf(factor))
            throw MVException(MVException::PosInfValue);
          else
            throw MVException(MVException::NegInfValue);
        }

        // change matrix
        m[ranking[r]][c] = T();
        for (int c2 = c+1; c2 < (int)N; ++c2)
        {
          m[ranking[r]][c2] -= factor*m[ranking[c]][c2];
          T sub;
          sub = factor*m[ranking[c]][c2];
          if (MVTools::isNearInf(sub))
          {
            if (MVTools::isNearPosInf(sub))
              throw MVException(MVException::PosInfValue);
            else
              throw MVException(MVException::NegInfValue);
          }
        }
      }
    }

    // if last entry of matrix zero --> determinant is zero
    if (MVTools::isNearZero(m[ranking[N-1]][N-1]))
      return z;
    /*if (m[ranking[N-1]][N-1] == z)
      return z;*/

    // matrix has triangle form
    // calculate determinant
    T res = m[ranking[0]][0];
    for (r = 1; r < (int)N; ++r)
    {
      res *= m[ranking[r]][r];
      if (MVTools::isNearInf(res))
      {
        if (MVTools::isNearPosInf(res))
          throw MVException(MVException::PosInfValue);
        else
          throw MVException(MVException::NegInfValue);
      }
    }    

    if (!bPositive)
      res = -res;
    return res;
  }
  //----------------------------------------------------------------------------
  /**
  * Returns the identity matrix with same dimensions like this matrix
  * @return nxn identity matrix
  *
  * Complexity: n^2
  */
  static Matrix_nxn<T, N> eye()
  {
    Matrix_nxn<T, N> res;
    for (size_t i = 0; i < N; ++i)
      res[i][i] = (T)1;
    return res;
  }
  //----------------------------------------------------------------------------
private:
  /** The vectors holding the contents of the matrix */
  #if defined(MATRIX_NXN_HEAP)
    Vector_n<T, N>* content;
  #elif defined(MATRIX_NXN_STACK)
    Vector_n<T, N> content[N];
  #endif
};
//------------------------------------------------------------------------------
/**
* Operator + for matrix additions
* @param m1 First matrix
* @param m2 Second matrix
* @return m1+m2
*
* Complexity: n^2
*/
template<class T, size_t N>
Matrix_nxn<T, N> operator+(const Matrix_nxn<T, N>& m1,
                           const Matrix_nxn<T, N>& m2)
{
  Matrix_nxn<T, N> res(m1);
  res += m2;
  return res;
}
//------------------------------------------------------------------------------
/**
* Operator - for matrix subtractions
* @param m1 First matrix
* @param m2 Second matrix
* @return m1-m2
*
* Complexity: n^2
*/
template<class T, size_t N>
Matrix_nxn<T, N> operator-(const Matrix_nxn<T, N>& m1,
                           const Matrix_nxn<T, N>& m2)
{
  Matrix_nxn<T, N> res(m1);
  res -= m2;
  return res;
}
//------------------------------------------------------------------------------
/**
* Operator * for matrix-matrix multiplication
* @param m1 First matrix
* @param m2 Second matrix
* @return m1*m2
*
* Complexity: n^3
*/
template<class T, size_t N>
Matrix_nxn<T, N> operator*(const Matrix_nxn<T, N>& m1,
                           const Matrix_nxn<T, N>& m2)
{
  Matrix_nxn<T, N> res;
  size_t i, j, k;
  for (i = 0; i < N; ++i)
  {
    for (j = 0; j < N; ++j)
    {
      for (k = 0; k < N; ++k)
      {
        res[i][j] += m1[i][k] * m2[k][j];
      }
      if (MVTools::isNearInf(res[i][j]))
      {
        if (MVTools::isNearPosInf(res[i][j]))
          throw MVException(MVException::PosInfValue);
        else
          throw MVException(MVException::NegInfValue);
      }
    }
  }
  return res;
}
//------------------------------------------------------------------------------
/**
* Operator * for matrix-vector multiplication
* @param m Matrix
* @param v Vector
* @return m*v
*
* Complexity: n^2
*/
template<class T, size_t N>
Vector_n<T, N> operator*(const Matrix_nxn<T, N>& m,
                         const Vector_n<T, N>& v)
{
  Vector_n<T, N> res;
  for (size_t i = 0; i < N; ++i)
  {
    for (size_t j = 0; j < N; ++j)
    {
      res[i] += m[i][j]*v[j];
    }
    if (MVTools::isNearInf(res[i]))
    {
      if (MVTools::isNearPosInf(res[i]))
        throw MVException(MVException::PosInfValue);
      else
        throw MVException(MVException::NegInfValue);
    }
  }
  return res;
}
//------------------------------------------------------------------------------
/**
* Transposes a matrix
* @param m Matrix to be transposed
* @return Transposed matrix
*
* Complexity: n^2
*/
template<class T, size_t N>
Matrix_nxn<T, N> transpose(const Matrix_nxn<T, N>& m)
{
  Matrix_nxn<T, N> res;
  for (size_t i = 0; i < N; ++i)
    for (size_t j = 0; j < N; ++j)
      res[i][j] = m[j][i];
  return res;
}
//------------------------------------------------------------------------------
/**
* Creates an identity matrix
* @return nxn identity matrix
*
* Complexity: n^2
*/
template<class T, size_t N>
Matrix_nxn<T, N> eye_nxn()
{
  Matrix_nxn<T, N> res;
  for (size_t i = 0; i < N; ++i)
    res[i][i] = (T)1;
  return res;
}
//------------------------------------------------------------------------------
/**
* Inverts the passed matrix. Matrix must not be singular
* @param m Matrix to be inverted
* @return Inverted matrix
*
* Complexity: n^3
*/
template<class T, size_t N>
Matrix_nxn<T, N> invert(const Matrix_nxn<T, N>& m)
{
  Matrix_nxn<T, N> left(m);
  Matrix_nxn<T, N> right(eye_nxn<T, N>());
  Vector_n<int, N> ranking;
  int i;
  for (i = 0; i < (int)N; ++i)
    ranking[i] = i;

  T zero = T();
  int r, r2, maxrow;
  for (r = 0; r < (int)(N-1); ++r)
  {
    // find highest value
    T maxval = left[ranking[r]][r];
    maxrow = r;
    if (maxval < zero)
      maxval = -maxval;
    for (r2 = r+1; r2 < (int)N; ++r2)
    {
      T val = left[ranking[r2]][r];
      if (val < zero)
        val = -val;
      if (val > maxval)
      {
        maxval = val;
        maxrow = r2;
      }
    }

    // swap rows
    int temp = ranking[r];
    ranking[r] = ranking[maxrow];
    ranking[maxrow] = temp;

    if (MVTools::isNearZero(left[ranking[r]][r]))
    {
      if (MVTools::isNearNegZero(left[ranking[r]][r]))
        throw MVException(MVException::DivByNegZero);
      else
        throw MVException(MVException::DivByPosZero);
    }

    for (r2 = r+1; r2 < (int)N; ++r2)
    {
      // calc factor for subtracting
      T factor = left[ranking[r2]][r] / left[ranking[r]][r];
      if (MVTools::isNearInf(factor))
      {
        if (MVTools::isNearPosInf(factor))
          throw MVException(MVException::PosInfValue);
        else
          throw MVException(MVException::NegInfValue);
      }

      // change left matrix
      left[ranking[r2]] -= factor*left[ranking[r]];
      
      // change right matrix
      right[ranking[r2]] -= factor*right[ranking[r]];
    }
  }

  // matrix has triangle form
  // bring to diagonal form
  for (r = (int)(N-1); r > 0; --r)
  {
    if (MVTools::isNearZero(left[ranking[r]][r]))
    {
      if (MVTools::isNearNegZero(left[ranking[r]][r]))
        throw MVException(MVException::DivByNegZero);
      else
        throw MVException(MVException::DivByPosZero);
    }
    for (r2 = r-1; r2 >= 0; --r2)
    {
      T factor = left[ranking[r2]][r] / left[ranking[r]][r];
      if (MVTools::isNearInf(factor))
      {
        if (MVTools::isNearPosInf(factor))
          throw MVException(MVException::PosInfValue);
        else
          throw MVException(MVException::NegInfValue);
      }

      // change left matrix
      left[ranking[r2]] -= factor*left[ranking[r]];
      
      // change right matrix
      right[ranking[r2]] -= factor*right[ranking[r]];
    }
  }

  // matrix has diagonal form
  // set entries of left matrix to 1 and apply multiplication to right
  Matrix_nxn<T, N> res;
  for (r = 0; r < (int)N; ++r)
  {
    res[r] = right[ranking[r]];

    if (MVTools::isNearZero(left[ranking[r]][r]))
    {
      if (MVTools::isNearNegZero(left[ranking[r]][r]))
        throw MVException(MVException::DivByNegZero);
      else
        throw MVException(MVException::DivByPosZero);
    }
    res[r] /= left[ranking[r]][r];
  }

  return res;
}
//------------------------------------------------------------------------------
/**
* Inverts the passed matrix. Matrix must not be singular
* @param m Matrix to be inverted
* @return Inverted matrix
*
* Complexity: n^3
*/
template<class T, size_t N>
Matrix_nxn<T, N> invert2(const Matrix_nxn<T, N>& m)
{
  Matrix_nxn<T, N> res;
  for (size_t i = 0; i < N; ++i)
  {
    Vector_n<T, N> e;
    e[i] = (T)1;
    res[i] = m.solve(e);
  }
  return transpose(res);
}
//------------------------------------------------------------------------------
/**
* Divides first matrix by second matrix (multiplicates with inverse of
* second matrix).
* @param m1 First matrix
* @param m2 Second matrix
* @return m1 & m2
*
* Complexity: n^3
*/
template<class T, size_t N>
Matrix_nxn<T, N> operator/(const Matrix_nxn<T, N>& m1,
                           const Matrix_nxn<T, N>& m2)
{
  return m1*invert(m2);
}
//------------------------------------------------------------------------------
/**
* Returns determinant of the passed matrix
* @param m Matrix the determinant is to be calculated of
* @return det(m)
*
* Complexity: n^3
*/
template<class T, size_t N>
T det(const Matrix_nxn<T, N>& m)
{
  return m.det();
}
//------------------------------------------------------------------------------
#endif
//------------------------------------------------------------------------------

/*
 * Change log :
 * $Log: Matrix_nxn.h,v $
 * Revision 1.1.1.1  2004/05/22 17:37:12  cvsadm
 * created new repository GT2004_WM
 *
 * Revision 1.4  2004/03/27 12:27:25  risler
 * changed to heap implementation since stack seems to produce compile problems on some few machines
 *
 * Revision 1.3  2004/03/18 14:50:50  uhrig
 * Corrected function solve for gnu compiler
 *
 * Revision 1.2  2004/03/15 12:28:52  risler
 * change log added
 *
 *
 */

---