matrix.cpp

class for work with 3d Matrix: rotate, transmit, scale, multiple

////////////////////////////////////////////////////////////////////////
// File Matrix.cpp
////////////////////////////////////////////////////////////////////////

#include <math.h>

Matrix::Matrix( double v )
{
 for( register char i = 0; i < 3; i++ )
   for( register char j = 0; j < 3; j++ )
     x[i][j] = (i==j) ? v:0.0;
}

////////////////////////////////////////////////////////////////////////////
Matrix::Matrix( const Matrix& v )
{
 for( register char i = 0; i < 3; i++ )
  for( register char j = 0; j < 3; j++ )
   x[i][j] = v.x[i][j];
}

////////////////////////////////////////////////////////////////////////////
Matrix::Matrix( double a11, double a12, double a13,
                double a21, double a22, double a23,
                double a31, double a32, double a33 )
{
 x[0][0] = a11; x[0][1] = a12; x[0][2] = a13;
 x[1][0] = a21; x[1][1] = a22; x[1][2] = a23;
 x[2][0] = a31; x[2][1] = a32; x[2][2] = a33;
}

////////////////////////////////////////////////////////////////////////////
void Matrix::Invert()
{
 Matrix Out(1);
 for( register char i = 0; i < 3; i++ )
 {
  register double d = x[i][i];
  if( d != 1.0 )
  {
    for( register char j = 0; j < 3; j++ )
    {
      Out.x[i][j] /= d;
      x[i][j] /= d;
    }
  }
  for( register char j = 0; j < 3; j++ )
  {
    if ( j != i )
    {
      if ( x[j][i] != 0.0 )
      {
        register double mulby = x[j][i];
        for( register char k = 0; k < 3; k++ )
        {
          x[j][k] -= mulby*x[i][k];
          Out.x[j][k] -= mulby*Out.x[i][k];
        }
      }
    }
  }
 }
 *this = Out;
}

////////////////////////////////////////////////////////////////////////////
void Matrix::Transpose ()
{
for( register char i = 0; i < 3; i++ )
  for( register char j = 0; j < 3; j++ )
  if( i != j )
  {
    register double t = x[i][j];
    x[i][j] = x[j][i];
    x[j][i] = t;
  }
}

////////////////////////////////////////////////////////////////////////////
Matrix&  Matrix::operator = (const Matrix& A)
{
 for( register char i = 0; i < 3; i++ )
  for( register char j = 0; j < 3; j++ )
   x[i][j] = A.x[i][j];
 return *this;
}

////////////////////////////////////////////////////////////////////////////
Matrix&  Matrix::operator += (const Matrix& A)
{
 for( register char i = 0; i < 3; i++ )
  for( register char j = 0; j < 3; j++ )
   x[i][j] += A.x [i][j];
 return *this;
}

////////////////////////////////////////////////////////////////////////////
Matrix&  Matrix::operator -= (const Matrix& A)
{
 for( register char i = 0; i < 3; i++ )
  for( register char j = 0; j < 3; j++ )
   x[i][j] -= A.x[i][j];
 return * this;
}

////////////////////////////////////////////////////////////////////////////
Matrix&  Matrix :: operator *= ( double v )
{
 for( register char i = 0; i < 3; i++ )
  for( register char j = 0; j < 3; j++ )
   x[i][j] *= v;
 return * this;
}

////////////////////////////////////////////////////////////////////////////
Matrix&  Matrix::operator *= (const Matrix& A)
{
 Matrix  res ( *this );
 for( register char i = 0; i < 3; i++ )
  for( register char j = 0; j < 3; j++ )
  {
    register double sum = 0;
    for ( register int k = 0; k < 3; k++ )
      sum += res.x[i][k] * A.x[k][j];
    x[i][j] = sum;
  }
 return * this;
}

////////////////////////////////////////////////////////////////////////////
Matrix operator + (const Matrix& A, const Matrix& B)
{
 Matrix  res;
 for( register char i = 0; i < 3; i++ )
  for( register char j = 0; j < 3; j++ )
   res.x[i][j] = A.x[i][j] + B.x[i][j];
 return res;
}

////////////////////////////////////////////////////////////////////////////
Matrix operator - (const Matrix& A, const Matrix& B)
{
 Matrix  res;
 for( register char i = 0; i < 3; i++ )
  for( register char j = 0; j < 3; j++ )
   res.x[i][j] = A.x[i][j] - B.x[i][j];
 return res;
}

////////////////////////////////////////////////////////////////////////////
Matrix operator * (const Matrix& A, const Matrix& B)
{
 Matrix  res;
 for( register char i = 0; i < 3; i++ )
  for( register char j = 0; j < 3; j++ )
  {
   register double sum = 0;
   for( register char k = 0; k < 3; k++ )
     sum += A.x[i][k] * B.x[k][j];
   res.x[i][j] = sum;
  }
 return res;
}

////////////////////////////////////////////////////////////////////////////
Matrix operator * (const Matrix& A, double v)
{
 Matrix  res;
 for( register char i = 0; i < 3; i++ )
  for( register char j = 0; j < 3; j++ )
   res.x[i][j] = A.x[i][j] * v;
 return res;
}

////////////////////////////////////////////////////////////////////////////
Vector operator * (const Matrix& M, const Vector& v)
{
 Vector res;
 res.x = v.x*M.x[0][0] + v.y*M.x[0][1] + v.z*M.x[0][2];
 res.y = v.x*M.x[1][0] + v.y*M.x[1][1] + v.z*M.x[1][2];
 res.z = v.x*M.x[2][0] + v.y*M.x[2][1] + v.z*M.x[2][2];
 return res;
}


////////////////////////// Transposition ///////////////////////////////////
Matrix operator ~ (const Matrix& m)
{
 Matrix Out;
 for( register char i = 0; i < 3; i++ )
  for( register char j = 0; j < 3; j++ )
    Out.x[j][i] = m.x[i][j];
 return Out;
}

////////////////////////// Reversion ///////////////////////////////////////
Matrix operator ! (const Matrix& v)
{
 Matrix Out(v);
 Out.Invert();
 return Out;
}

////////////////////////////////////////////////////////////////////////////
/*Matrix Translate( const Vector& Loc )
{
 Matrix  res;
 memset(res.x,0,sizeof(double)*9);
 res.x [0][3] = Loc.x;
 res.x [1][3] = Loc.y;
 res.x [2][3] = Loc.z;
 return res;
}*/

////////////////////////////////////////////////////////////////////////////
Matrix Scale( const Vector& v )
{
 Matrix  res;
 memset(res.x,0,sizeof(double)*9);
 res.x[0][0] = v.x;
 res.x[1][1] = v.y;
 res.x[2][2] = v.z;
 return res;
}

////////////////////////////////////////////////////////////////////////////
Matrix  RotateX( double Angle )
{
 Matrix  res;
 memset(res.x,0,sizeof(double)*9);
 register double Cosine = cos( Angle );
 register double Sine = sin( Angle );
 res.x [1][1] = Cosine;
 res.x [2][1] = -Sine;        
 res.x [1][2] = Sine;
 res.x [2][2] = Cosine;
 res.x[0][0] = 1.0;
 return res;
}


////////////////////////////////////////////////////////////////////////////
Matrix RotateY( double Angle )
{
 Matrix res;
 memset(res.x,0,sizeof(double)*9);
 register double Cosine = cos ( Angle );
 register double Sine = sin ( Angle );
 res.x[0][0] = Cosine;
 res.x[2][0] = Sine;
 res.x[0][2] = -Sine;
 res.x[2][2] = Cosine;
 res.x[1][1] = 1.0;
 return res;
}


////////////////////////////////////////////////////////////////////////////
Matrix RotateZ( double Angle )
{
 Matrix res;
 memset(res.x,0,sizeof(double)*9);
 register double Cosine = cos ( Angle );
 register double Sine = sin ( Angle );
 res.x[0][0] = Cosine;
 res.x[1][0] = -Sine;
 res.x[0][1] = Sine;
 res.x[1][1] = Cosine;
 res.x[2][2] = 1.0;
 return res;
}


////////////////////////////////////////////////////////////////////////////
Matrix Rotation ( const Vector& axis, double angle )
{
 Matrix res(1.);
 register double Cosine = cos( angle );
 register double Sine = sin( angle );
 res.x[0][0] = axis.x * axis.x + ( 1. - axis.x * axis.x ) * Cosine;
 res.x[0][1] = axis.x * axis.y * ( 1. - Cosine ) + axis.z * Sine;
 res.x[0][2] = axis.x * axis.z * ( 1. - Cosine ) - axis.y * Sine;
 res.x[1][0] = axis.x * axis.y * ( 1. - Cosine ) - axis.z * Sine;
 res.x[1][1] = axis.y * axis.y + ( 1. - axis.y * axis.y ) * Cosine;
 res.x[1][2] = axis.y * axis.z * ( 1. - Cosine ) + axis.x * Sine;
 res.x[2][0] = axis.x * axis.z * ( 1. - Cosine ) + axis.y * Sine;
 res.x[2][1] = axis.y * axis.z * ( 1. - Cosine ) - axis.x * Sine;
 res.x[2][2] = axis.z * axis.z + ( 1. - axis.z * axis.z ) * Cosine;
 return res;
}

////////////////////////////////////////////////////////////////////////////
Matrix MirrorX()
{
  Matrix res(1.);
  res.x[0][0] = -1.;
  return res;
}

////////////////////////////////////////////////////////////////////////////
Matrix MirrorY()
{
  Matrix res(1.);
  res.x[1][1] = -1.;
  return res;
}

////////////////////////////////////////////////////////////////////////////
Matrix MirrorZ()
{
 Matrix res(1.);
 res.x[2][2] = -1.;
 return res;
}

////////////////////////////////////////////////////////////////////////////
Matrix Symmetry( const Vector& v )
{
 Matrix res(0.);
 res.x[0][1] = v.z;
 res.x[1][0] = v.z;
 res.x[0][2] = v.y;
 res.x[2][0] = v.y;
 res.x[1][2] = v.x;
 res.x[2][1] = v.x;
 return res;
}

////////////////////////////////////////////////////////////////////////////
Matrix AskSymmetry( const Vector& v )
{
 Matrix res(0.);
 res.x[0][1] = res.x[1][0] = v.z;
 res.x[0][2] = res.x[2][0] = v.y;
 res.x[1][2] = res.x[2][1] = v.x;
 return res;
}

////////////////////////////////////////////////////////////////////////////
Matrix Rodriga( const Vector& n, double fi )
{
 Matrix r;
 register double Sinuse = sin( fi );
 register double Cosine = 1. - cos( fi );

 r.x[0][0] = 1. - ( n.z * n.z + n.y * n.y ) * Cosine;
 r.x[0][1] = n.x * n.y * Cosine - n.z * Sinuse;
 r.x[0][2] = n.y * Sinuse + n.x * n.z * Cosine;

 r.x[1][0] = n.z * Sinuse + n.y * n.x * Cosine;
 r.x[1][1] = 1. - ( n.z * n.z + n.x * n.x ) * Cosine;
 r.x[1][2] = n.y * n.z * Cosine - n.x * Sinuse;

 r.x[2][0] = n.z * n.x * Cosine - n.y * Sinuse;
 r.x[2][1] = n.z * n.y * Cosine + n.x * Sinuse;
 r.x[2][2] = 1. - ( n.x * n.x + n.y * n.y ) * Cosine;

 return r;
}