source: orxonox.OLD/orxonox/branches/trackManager/src/matrix.cc @ 3434

/* 
   orxonox - the future of 3D-vertical-scrollers

   Copyright (C) 2004 orx

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2, or (at your option)
   any later version.

   ### File Specific:
   main-programmer: Benjamin Grauer
   co-programmer: ...

   \todo Null-Parent => center of the coord system - singleton
   \todo Smooth-Parent: delay, speed
   \todo destroy the stuff again, delete...
*/

#include "matrix.h"

Matrix::Matrix (size_t row, size_t col)
{
  _m = new base_mat( row, col, 0);
}

// copy constructor
Matrix::Matrix (const Matrix& m)
{
    _m = m._m;
    _m->Refcnt++;
}

// Internal copy constructor
void Matrix::clone ()
{
    _m->Refcnt--;
    _m = new base_mat( _m->Row, _m->Col, _m->Val);
}

// destructor
Matrix::~Matrix ()
{
   if (--_m->Refcnt == 0) delete _m;
}

// assignment operator
Matrix& Matrix::operator = (const Matrix& m) 
{
    m._m->Refcnt++;
    if (--_m->Refcnt == 0) delete _m;
    _m = m._m;
    return *this;
}

//  reallocation method
void Matrix::realloc (size_t row, size_t col)
{
   if (row == _m->RowSiz && col == _m->ColSiz)
   {
      _m->Row = _m->RowSiz;
      _m->Col = _m->ColSiz;
      return;
   }

   base_mat *m1 = new base_mat( row, col, NULL);
   size_t colSize = min(_m->Col,col) * sizeof(float);
   size_t minRow = min(_m->Row,row);

   for (size_t i=0; i < minRow; i++)
      memcpy( m1->Val[i], _m->Val[i], colSize);

   if (--_m->Refcnt == 0) 
       delete _m;
   _m = m1;

   return;
}

// public method for resizing Matrix
void Matrix::SetSize (size_t row, size_t col) 
{
   size_t i,j;
   size_t oldRow = _m->Row;
   size_t oldCol = _m->Col;

   if (row != _m->RowSiz || col != _m->ColSiz)
      realloc( row, col);

   for (i=oldRow; i < row; i++)
      for (j=0; j < col; j++)
         _m->Val[i][j] = float(0);

   for (i=0; i < row; i++)                      
      for (j=oldCol; j < col; j++)
         _m->Val[i][j] = float(0);

   return;
}

// subscript operator to get/set individual elements
float& Matrix::operator () (size_t row, size_t col) 
{
   if (row >= _m->Row || col >= _m->Col)
      printf( "Matrix::operator(): Index out of range!\n");
   if (_m->Refcnt > 1) clone();
   return _m->Val[row][col];
}

// subscript operator to get/set individual elements
float Matrix::operator () (size_t row, size_t col) const 
{
   if (row >= _m->Row || col >= _m->Col)
      printf( "Matrix::operator(): Index out of range!\n");
   return _m->Val[row][col];
}

// input stream function
istream& operator >> (istream& istrm, Matrix& m)
{
   for (size_t i=0; i < m.RowNo(); i++)
      for (size_t j=0; j < m.ColNo(); j++)
      {
         float x;
         istrm >> x;
         m(i,j) = x;
      }
   return istrm;
}

// output stream function
ostream& operator << (ostream& ostrm, const Matrix& m)
{
   for (size_t i=0; i < m.RowNo(); i++)
   {
      for (size_t j=0; j < m.ColNo(); j++)
      {
         float x = m(i,j);
         ostrm << x << '\t';
      }
      ostrm << endl;
   }
   return ostrm;
}


// logical equal-to operator
bool operator == (const Matrix& m1, const Matrix& m2) 
{
   if (m1.RowNo() != m2.RowNo() || m1.ColNo() != m2.ColNo())
      return false;

   for (size_t i=0; i < m1.RowNo(); i++)
      for (size_t j=0; j < m1.ColNo(); j++)
              if (m1(i,j) != m2(i,j))
                 return false;

   return true;
}

// logical no-equal-to operator
bool operator != (const Matrix& m1, const Matrix& m2) 
{
    return (m1 == m2) ? false : true;
}

// combined addition and assignment operator
Matrix& Matrix::operator += (const Matrix& m) 
{
   if (_m->Row != m._m->Row || _m->Col != m._m->Col)
     printf("Matrix::operator+= : Inconsistent Matrix sizes in addition!\n");
   if (_m->Refcnt > 1) clone();
   for (size_t i=0; i < m._m->Row; i++)
      for (size_t j=0; j < m._m->Col; j++)
         _m->Val[i][j] += m._m->Val[i][j];
   return *this;
}

// combined subtraction and assignment operator
Matrix& Matrix::operator -= (const Matrix& m) 
{
   if (_m->Row != m._m->Row || _m->Col != m._m->Col)
      printf( "Matrix::operator-= : Inconsistent Matrix sizes in subtraction!\n");
   if (_m->Refcnt > 1) clone();
   for (size_t i=0; i < m._m->Row; i++)
      for (size_t j=0; j < m._m->Col; j++)
         _m->Val[i][j] -= m._m->Val[i][j];
   return *this;
}

// combined scalar multiplication and assignment operator
 Matrix&
Matrix::operator *= (const float& c) 
{
    if (_m->Refcnt > 1) clone();
    for (size_t i=0; i < _m->Row; i++)
        for (size_t j=0; j < _m->Col; j++)
            _m->Val[i][j] *= c;
    return *this;
}

// combined Matrix multiplication and assignment operator
 Matrix&
Matrix::operator *= (const Matrix& m) 
{
   if (_m->Col != m._m->Row)
      printf( "Matrix::operator*= : Inconsistent Matrix sizes in multiplication!\n");

   Matrix temp(_m->Row,m._m->Col);

   for (size_t i=0; i < _m->Row; i++)
      for (size_t j=0; j < m._m->Col; j++)
      {
         temp._m->Val[i][j] = float(0);
         for (size_t k=0; k < _m->Col; k++)
            temp._m->Val[i][j] += _m->Val[i][k] * m._m->Val[k][j];
      }
   *this = temp;

   return *this;
}

// combined scalar division and assignment operator
 Matrix&
Matrix::operator /= (const float& c) 
{
    if (_m->Refcnt > 1) clone();
    for (size_t i=0; i < _m->Row; i++)
        for (size_t j=0; j < _m->Col; j++)
            _m->Val[i][j] /= c;

    return *this;
}

// combined power and assignment operator
 Matrix&
Matrix::operator ^= (const size_t& pow) 
{
  Matrix temp(*this);

  for (size_t i=2; i <= pow; i++)
    *this *=  temp; // changed from *this = *this * temp;

  return *this;
}

// unary negation operator
 Matrix
Matrix::operator - () 
{
   Matrix temp(_m->Row,_m->Col);

   for (size_t i=0; i < _m->Row; i++)
      for (size_t j=0; j < _m->Col; j++)
         temp._m->Val[i][j] = - _m->Val[i][j];

   return temp;
}

// binary addition operator
 Matrix
operator + (const Matrix& m1, const Matrix& m2) 
{
   Matrix temp = m1;
   temp += m2;
   return temp;
}

// binary subtraction operator
 Matrix
operator - (const Matrix& m1, const Matrix& m2) 
{
   Matrix temp = m1;
   temp -= m2;
   return temp;
}

// binary scalar multiplication operator
 Matrix
operator * (const Matrix& m, const float& no) 
{
   Matrix temp = m;
   temp *= no;
   return temp;
}


// binary scalar multiplication operator
 Matrix
operator * (const float& no, const Matrix& m) 
{
   return (m * no);
}

// binary Matrix multiplication operator
 Matrix
operator * (const Matrix& m1, const Matrix& m2) 
{
   Matrix temp = m1;
   temp *= m2;
   return temp;
}

// binary scalar division operator
 Matrix
operator / (const Matrix& m, const float& no) 
{
    return (m * (float(1) / no));
}


// binary scalar division operator
 Matrix
operator / (const float& no, const Matrix& m) 
{
    return (!m * no);
}

// binary Matrix division operator
 Matrix
operator / (const Matrix& m1, const Matrix& m2) 
{
    return (m1 * !m2);
}

// binary power operator
 Matrix
operator ^ (const Matrix& m, const size_t& pow) 
{
   Matrix temp = m;
   temp ^= pow;
   return temp;
}

// unary transpose operator
 Matrix
operator ~ (const Matrix& m) 
{
   Matrix temp(m.ColNo(),m.RowNo());

   for (size_t i=0; i < m.RowNo(); i++)
      for (size_t j=0; j < m.ColNo(); j++)
      {
         float x = m(i,j);
              temp(j,i) = x;
      }
   return temp;
}

// unary inversion operator
 Matrix
operator ! (const Matrix m) 
{
   Matrix temp = m;
   return temp.Inv();
}

// inversion function
 Matrix
Matrix::Inv () 
{
   size_t i,j,k;
   float a1,a2,*rowptr;

   if (_m->Row != _m->Col)
      printf( "Matrix::operator!: Inversion of a non-square Matrix\n");

   Matrix temp(_m->Row,_m->Col);
   if (_m->Refcnt > 1) clone();


   temp.Unit();
   for (k=0; k < _m->Row; k++)
   {
      int indx = pivot(k);
      if (indx == -1)
              printf( "Matrix::operator!: Inversion of a singular Matrix\n");

      if (indx != 0)
      {
              rowptr = temp._m->Val[k];
              temp._m->Val[k] = temp._m->Val[indx];
              temp._m->Val[indx] = rowptr;
      }
      a1 = _m->Val[k][k];
      for (j=0; j < _m->Row; j++)
      {
              _m->Val[k][j] /= a1;
              temp._m->Val[k][j] /= a1;
      }
      for (i=0; i < _m->Row; i++)
           if (i != k)
           {
              a2 = _m->Val[i][k];
              for (j=0; j < _m->Row; j++)
              {
                 _m->Val[i][j] -= a2 * _m->Val[k][j];
                 temp._m->Val[i][j] -= a2 * temp._m->Val[k][j];
              }
           }
   }
   return temp;
}

// solve simultaneous equation
 Matrix
Matrix::Solve (const Matrix& v) const 
{
   size_t i,j,k;
   float a1;

   if (!(_m->Row == _m->Col && _m->Col == v._m->Row))
      printf( "Matrix::Solve():Inconsistent matrices!\n");

   Matrix temp(_m->Row,_m->Col+v._m->Col);
   for (i=0; i < _m->Row; i++)
   {
      for (j=0; j < _m->Col; j++)
         temp._m->Val[i][j] = _m->Val[i][j];
      for (k=0; k < v._m->Col; k++)
         temp._m->Val[i][_m->Col+k] = v._m->Val[i][k];
   }
   for (k=0; k < _m->Row; k++)
   {
      int indx = temp.pivot(k);
      if (indx == -1)
         printf( "Matrix::Solve(): Singular Matrix!\n");

      a1 = temp._m->Val[k][k];
      for (j=k; j < temp._m->Col; j++)
         temp._m->Val[k][j] /= a1;

      for (i=k+1; i < _m->Row; i++)
      {
         a1 = temp._m->Val[i][k];
         for (j=k; j < temp._m->Col; j++)
           temp._m->Val[i][j] -= a1 * temp._m->Val[k][j];
      }
   }
   Matrix s(v._m->Row,v._m->Col);
   for (k=0; k < v._m->Col; k++)
      for (int m=int(_m->Row)-1; m >= 0; m--)
      {
         s._m->Val[m][k] = temp._m->Val[m][_m->Col+k];
         for (j=m+1; j < _m->Col; j++)
            s._m->Val[m][k] -= temp._m->Val[m][j] * s._m->Val[j][k];
      }
   return s;
}

// set zero to all elements of this Matrix
 void
Matrix::Null (const size_t& row, const size_t& col) 
{
    if (row != _m->Row || col != _m->Col)
        realloc( row,col);

    if (_m->Refcnt > 1) 
        clone();

    for (size_t i=0; i < _m->Row; i++)
        for (size_t j=0; j < _m->Col; j++)
            _m->Val[i][j] = float(0);
    return;
}

// set zero to all elements of this Matrix
 void
Matrix::Null() 
{
    if (_m->Refcnt > 1) clone();   
    for (size_t i=0; i < _m->Row; i++)
        for (size_t j=0; j < _m->Col; j++)
                _m->Val[i][j] = float(0);
    return;
}

// set this Matrix to unity
 void
Matrix::Unit (const size_t& row) 
{
    if (row != _m->Row || row != _m->Col)
        realloc( row, row);
        
    if (_m->Refcnt > 1) 
        clone();

    for (size_t i=0; i < _m->Row; i++)
        for (size_t j=0; j < _m->Col; j++)
            _m->Val[i][j] = i == j ? float(1) : float(0);
    return;
}

// set this Matrix to unity
 void
Matrix::Unit () 
{
    if (_m->Refcnt > 1) clone();   
    size_t row = min(_m->Row,_m->Col);
    _m->Row = _m->Col = row;

    for (size_t i=0; i < _m->Row; i++)
        for (size_t j=0; j < _m->Col; j++)
            _m->Val[i][j] = i == j ? float(1) : float(0);
    return;
}

// private partial pivoting method
 int
Matrix::pivot (size_t row)
{
  int k = int(row);
  double amax,temp;

  amax = -1;
  for (size_t i=row; i < _m->Row; i++)
    if ( (temp = abs( _m->Val[i][row])) > amax && temp != 0.0)
     {
       amax = temp;
       k = i;
     }
  if (_m->Val[k][row] == float(0))
     return -1;
  if (k != int(row))
  {
     float* rowptr = _m->Val[k];
     _m->Val[k] = _m->Val[row];
     _m->Val[row] = rowptr;
     return k;
  }
  return 0;
}

// calculate the determinant of a Matrix
 float
Matrix::Det () const 
{
   size_t i,j,k;
   float piv,detVal = float(1);

   if (_m->Row != _m->Col)
      printf( "Matrix::Det(): Determinant a non-squareMatrix!\n");
   
   Matrix temp(*this);
   if (temp._m->Refcnt > 1) temp.clone();

   for (k=0; k < _m->Row; k++)
   {
      int indx = temp.pivot(k);
      if (indx == -1)
         return 0;
      if (indx != 0)
         detVal = - detVal;
      detVal = detVal * temp._m->Val[k][k];
      for (i=k+1; i < _m->Row; i++)
      {
         piv = temp._m->Val[i][k] / temp._m->Val[k][k];
         for (j=k+1; j < _m->Row; j++)
            temp._m->Val[i][j] -= piv * temp._m->Val[k][j];
      }
   }
   return detVal;
}

// calculate the norm of a Matrix
 float
Matrix::Norm () 
{
   float retVal = float(0);

   for (size_t i=0; i < _m->Row; i++)
      for (size_t j=0; j < _m->Col; j++)
         retVal += _m->Val[i][j] * _m->Val[i][j];
   retVal = sqrt( retVal);

   return retVal;
}

// calculate the condition number of a Matrix
 float
Matrix::Cond () 
{
   Matrix inv = ! (*this);
   return (Norm() * inv.Norm());
}

// calculate the cofactor of a Matrix for a given element
 float
Matrix::Cofact (size_t row, size_t col) 
{
   size_t i,i1,j,j1;

   if (_m->Row != _m->Col)
      printf( "Matrix::Cofact(): Cofactor of a non-square Matrix!\n");

   if (row > _m->Row || col > _m->Col)
      printf( "Matrix::Cofact(): Index out of range!\n");

   Matrix temp (_m->Row-1,_m->Col-1);

   for (i=i1=0; i < _m->Row; i++)
   {
      if (i == row)
        continue;
      for (j=j1=0; j < _m->Col; j++)
      {
         if (j == col)
            continue;
         temp._m->Val[i1][j1] = _m->Val[i][j];
         j1++;
      }
      i1++;
   }
   float  cof = temp.Det();
   if ((row+col)%2 == 1)
      cof = -cof;

   return cof;
}


// calculate adjoin of a Matrix
 Matrix
Matrix::Adj () 
{
   if (_m->Row != _m->Col)
      printf( "Matrix::Adj(): Adjoin of a non-square Matrix.\n");

   Matrix temp(_m->Row,_m->Col);

   for (size_t i=0; i < _m->Row; i++)
      for (size_t j=0; j < _m->Col; j++)
          temp._m->Val[j][i] = Cofact(i,j);
   return temp;
}

// Determine if the Matrix is singular
 bool
Matrix::IsSingular () 
{
   if (_m->Row != _m->Col)
      return false;
   return (Det() == float(0));
}

// Determine if the Matrix is diagonal
 bool
Matrix::IsDiagonal () 
{
   if (_m->Row != _m->Col)
      return false;
   for (size_t i=0; i < _m->Row; i++)
     for (size_t j=0; j < _m->Col; j++)
        if (i != j && _m->Val[i][j] != float(0))
          return false;
   return true;
}

// Determine if the Matrix is scalar
 bool
Matrix::IsScalar () 
{
   if (!IsDiagonal())
     return false;
   float v = _m->Val[0][0];
   for (size_t i=1; i < _m->Row; i++)
     if (_m->Val[i][i] != v)
        return false;
   return true;
}

// Determine if the Matrix is a unit Matrix
 bool
Matrix::IsUnit () 
{
   if (IsScalar() && _m->Val[0][0] == float(1))
     return true;
   return false;
}

// Determine if this is a null Matrix
 bool
Matrix::IsNull () 
{
   for (size_t i=0; i < _m->Row; i++)
      for (size_t j=0; j < _m->Col; j++)
         if (_m->Val[i][j] != float(0))
            return false;
   return true;
}

// Determine if the Matrix is symmetric
 bool
Matrix::IsSymmetric () 
{
   if (_m->Row != _m->Col)
      return false;
   for (size_t i=0; i < _m->Row; i++)
      for (size_t j=0; j < _m->Col; j++)
         if (_m->Val[i][j] != _m->Val[j][i])
            return false;
   return true;
}
           
// Determine if the Matrix is skew-symmetric
 bool
Matrix::IsSkewSymmetric () 
{
   if (_m->Row != _m->Col)
      return false;
   for (size_t i=0; i < _m->Row; i++)
      for (size_t j=0; j < _m->Col; j++)
         if (_m->Val[i][j] != -_m->Val[j][i])
            return false;
   return true;
}
   
// Determine if the Matrix is upper triangular
 bool
Matrix::IsUpperTriangular () 
{
   if (_m->Row != _m->Col)
      return false;
   for (size_t i=1; i < _m->Row; i++)
      for (size_t j=0; j < i-1; j++)
         if (_m->Val[i][j] != float(0))
            return false;
   return true;
}

// Determine if the Matrix is lower triangular
 bool
Matrix::IsLowerTriangular () 
{
   if (_m->Row != _m->Col)
      return false;

   for (size_t j=1; j < _m->Col; j++)
      for (size_t i=0; i < j-1; i++)
         if (_m->Val[i][j] != float(0))
            return false;

   return true;
}