/***************************************************************************************
*****                       CLASS MATRIX4D IMPLEMENTATION                          *****
****************************************************************************************
** Author: Peter Stuart
** Email:  chester@selway.umt.edu
** Date:   3/7/98
** Description: This class implements a 4x4 matrix and all of the operation you could
**    ever want with one (well, maybe not ALL). Operations include: matrix cross product,
**    minor, determinant, cofactor, transpose, inverse matrix, and loads of operators
**    for multiplying matrices by other base types. Also part of the class are methods
**    for creating transformation matrices, such as translation, rotation, scaling, and
**    3D shearing.
**
** NOTE: The matrices, vectors, and points use homogeneous coordinates
**	
***************************************************************************************/

#include "Matrix4D.hpp"

/* Constructors ************
** Default values for private variables:
** matrix[i][j] = 0.0
*/

Matrix4D::Matrix4D() 
{
   matrix[0][0] = 0.0;
   matrix[0][1] = 0.0;
   matrix[0][2] = 0.0;
   matrix[0][3] = 0.0;

   matrix[1][0] = 0.0;
   matrix[1][1] = 0.0;
   matrix[1][2] = 0.0;
   matrix[1][3] = 0.0;

   matrix[2][0] = 0.0;
   matrix[2][1] = 0.0;
   matrix[2][2] = 0.0;
   matrix[2][3] = 0.0;

   matrix[3][0] = 0.0;
   matrix[3][1] = 0.0;
   matrix[3][2] = 0.0;
   matrix[3][3] = 0.0;
}

Matrix4D::Matrix4D(const double mat[4][4]) 
{
   matrix[0][0] = mat[0][0];
   matrix[0][1] = mat[0][1];
   matrix[0][2] = mat[0][2];
   matrix[0][3] = mat[0][3];

   matrix[1][0] = mat[1][0];
   matrix[1][1] = mat[1][1];
   matrix[1][2] = mat[1][2];
   matrix[1][3] = mat[1][3];

   matrix[2][0] = mat[2][0];
   matrix[2][1] = mat[2][1];
   matrix[2][2] = mat[2][2];
   matrix[2][3] = mat[2][3];

   matrix[3][0] = mat[3][0];
   matrix[3][1] = mat[3][1];
   matrix[3][2] = mat[3][2];
   matrix[3][3] = mat[3][3];
}

// Destructor
Matrix4D::~Matrix4D (void)
{
   ;
}

// Copy Constructor
Matrix4D::Matrix4D(const Matrix4D& mat)
{
   matrix[0][0] = mat.matrix[0][0];
   matrix[0][1] = mat.matrix[0][1];
   matrix[0][2] = mat.matrix[0][2];
   matrix[0][3] = mat.matrix[0][3];

   matrix[1][0] = mat.matrix[1][0];
   matrix[1][1] = mat.matrix[1][1];
   matrix[1][2] = mat.matrix[1][2];
   matrix[1][3] = mat.matrix[1][3];

   matrix[2][0] = mat.matrix[2][0];
   matrix[2][1] = mat.matrix[2][1];
   matrix[2][2] = mat.matrix[2][2];
   matrix[2][3] = mat.matrix[2][3];

   matrix[3][0] = mat.matrix[3][0];
   matrix[3][1] = mat.matrix[3][1];
   matrix[3][2] = mat.matrix[3][2];
   matrix[3][3] = mat.matrix[3][3];
}

// BASIC MATRIX OPERATIONS //////////////////////////////////////

// Matrix Cross product
Matrix4D  Matrix4D::cross(const Matrix4D& mat) const 
{
   Matrix4D temp;		// returning matrix

   temp.matrix[0][0] = (matrix[0][0] * mat.matrix[0][0]) 
                     + (matrix[0][1] * mat.matrix[1][0])
                     + (matrix[0][2] * mat.matrix[2][0])
                     + (matrix[0][3] * mat.matrix[3][0]);

   temp.matrix[0][1] = (matrix[0][0] * mat.matrix[0][1]) 
                     + (matrix[0][1] * mat.matrix[1][1])
                     + (matrix[0][2] * mat.matrix[2][1])
                     + (matrix[0][3] * mat.matrix[3][1]);

   temp.matrix[0][2] = (matrix[0][0] * mat.matrix[0][2]) 
                     + (matrix[0][1] * mat.matrix[1][2])
                     + (matrix[0][2] * mat.matrix[2][2])
                     + (matrix[0][3] * mat.matrix[3][2]);
		
   temp.matrix[0][3] = (matrix[0][0] * mat.matrix[0][3]) 
                     + (matrix[0][1] * mat.matrix[1][3])
                     + (matrix[0][2] * mat.matrix[2][3])
                     + (matrix[0][3] * mat.matrix[3][3]);



   temp.matrix[1][0] = (matrix[1][0] * mat.matrix[0][0]) 
                     + (matrix[1][1] * mat.matrix[1][0])
                     + (matrix[1][2] * mat.matrix[2][0])
                     + (matrix[1][3] * mat.matrix[3][0]);

   temp.matrix[1][1] = (matrix[1][0] * mat.matrix[0][1]) 
                     + (matrix[1][1] * mat.matrix[1][1])
                     + (matrix[1][2] * mat.matrix[2][1])
                     + (matrix[1][3] * mat.matrix[3][1]);

   temp.matrix[1][2] = (matrix[1][0] * mat.matrix[0][2]) 
                     + (matrix[1][1] * mat.matrix[1][2])
                     + (matrix[1][2] * mat.matrix[2][2])
                     + (matrix[1][3] * mat.matrix[3][2]);

   temp.matrix[1][3] = (matrix[1][0] * mat.matrix[0][3]) 
                     + (matrix[1][1] * mat.matrix[1][3])
                     + (matrix[1][2] * mat.matrix[2][3])
                     + (matrix[1][3] * mat.matrix[3][3]);



   temp.matrix[2][0] = (matrix[2][0] * mat.matrix[0][0])
                     + (matrix[2][1] * mat.matrix[1][0])
                     + (matrix[2][2] * mat.matrix[2][0])
                     + (matrix[2][3] * mat.matrix[3][0]);

   temp.matrix[2][1] = (matrix[2][0] * mat.matrix[0][1])
                     + (matrix[2][1] * mat.matrix[1][1])
                     + (matrix[2][2] * mat.matrix[2][1])
                     + (matrix[2][3] * mat.matrix[3][1]);

   temp.matrix[2][2] = (matrix[2][0] * mat.matrix[0][2])
                     + (matrix[2][1] * mat.matrix[1][2])
                     + (matrix[2][2] * mat.matrix[2][2])
                     + (matrix[2][3] * mat.matrix[3][2]);

   temp.matrix[2][3] = (matrix[2][0] * mat.matrix[0][3])
                     + (matrix[2][1] * mat.matrix[1][3])
                     + (matrix[2][2] * mat.matrix[2][3])
                     + (matrix[2][3] * mat.matrix[3][3]);



   temp.matrix[3][0] = (matrix[3][0] * mat.matrix[0][0])
                     + (matrix[3][1] * mat.matrix[1][0])
                     + (matrix[3][2] * mat.matrix[2][0])
                     + (matrix[3][3] * mat.matrix[3][0]);

   temp.matrix[3][1] = (matrix[3][0] * mat.matrix[0][1])
                     + (matrix[3][1] * mat.matrix[1][1])
                     + (matrix[3][2] * mat.matrix[2][1])
                     + (matrix[3][3] * mat.matrix[3][1]);

   temp.matrix[3][2] = (matrix[3][0] * mat.matrix[0][2])
                     + (matrix[3][1] * mat.matrix[1][2])
                     + (matrix[3][2] * mat.matrix[2][2])
                     + (matrix[3][3] * mat.matrix[3][2]);

   temp.matrix[3][3] = (matrix[3][0] * mat.matrix[0][3])
                     + (matrix[3][1] * mat.matrix[1][3])
                     + (matrix[3][2] * mat.matrix[2][3])
                     + (matrix[3][3] * mat.matrix[3][3]);

   return temp;
}

// Minor of row, col
double	 Matrix4D::minor(const int row, const int col) const 
{
   double mat3x3[3][3];    // 3x3 matrix of elements in the determinant
   int rowPos=0, colPos=0; // present row and column
   int i, j;               // index variables
 
   // create 3x3 matrix
   for (i=0; i<4; i++) 
   {
      for (j=0; j<4; j++) 
      {
         if ((i!=row) && (j!=col)) 
         {
            mat3x3[rowPos][colPos] = matrix[i][j];
            colPos++;
            if (colPos > 2) 
            {
               colPos = 0;
               rowPos++;
	    }
         }
      }
   }
   return det3x3(mat3x3);
}

// Determinant of 4x4 matrix
double Matrix4D::determinant() 
{
  double val = 0.0;             // returning double
  
  // use Laplace development to find determinant
  val += matrix[0][0] * minor(0, 0);
  val -= matrix[0][1] * minor(0, 1);
  val += matrix[0][2] * minor(0, 2);
  val -= matrix[0][3] * minor(0, 3);

  return val;
}

// Cofactor matrix - C[i][j] = signed minor of matrix[i][j]
Matrix4D Matrix4D::cofactor() 
{
   Matrix4D temp;       // returning matrix

   temp.matrix[0][0] =  minor(0, 0);
   temp.matrix[0][1] = -minor(0, 1);
   temp.matrix[0][2] =  minor(0, 2);
   temp.matrix[0][3] = -minor(0, 3);

   temp.matrix[1][0] = -minor(1, 0);
   temp.matrix[1][1] =  minor(1, 1);
   temp.matrix[1][2] = -minor(1, 2);
   temp.matrix[1][3] =  minor(1, 3);

   temp.matrix[2][0] =  minor(2, 0);
   temp.matrix[2][1] = -minor(2, 1);
   temp.matrix[2][2] =  minor(2, 2);
   temp.matrix[2][3] = -minor(2, 3);

   temp.matrix[3][0] = -minor(3, 0);
   temp.matrix[3][1] =  minor(3, 1);
   temp.matrix[3][2] = -minor(3, 2);
   temp.matrix[3][3] =  minor(3, 3);

   return temp;
}

// Transpose of 4x4 matrix - T[i][j] = matrix[j][i]
Matrix4D Matrix4D::transpose() 
{
   Matrix4D temp;       // returning matrix

   temp.matrix[0][0] = matrix[0][0];
   temp.matrix[0][1] = matrix[1][0];
   temp.matrix[0][2] = matrix[2][0];
   temp.matrix[0][3] = matrix[3][0];

   temp.matrix[1][0] = matrix[0][1];
   temp.matrix[1][1] = matrix[1][1];
   temp.matrix[1][2] = matrix[2][1];
   temp.matrix[1][3] = matrix[3][1];

   temp.matrix[2][0] = matrix[0][2];
   temp.matrix[2][1] = matrix[1][2];
   temp.matrix[2][2] = matrix[2][2];
   temp.matrix[2][3] = matrix[3][2];

   temp.matrix[3][0] = matrix[0][3];
   temp.matrix[3][1] = matrix[1][3];
   temp.matrix[3][2] = matrix[2][3];
   temp.matrix[3][3] = matrix[3][3];

   return temp;
}

// Inverse matrix - using cofactors
Matrix4D Matrix4D::inverse() 
{
   return cofactor().transpose() / determinant();
}

// MATRIX TRANSFORMATIONS /////////////////////////////////////////////////

// Translation matrix

// |1 0 0 0|
// |0 1 0 0|
// |0 0 1 0|
// |x y z 1|

Matrix4D  translate (const double x, const double y, const double z)
{
   Matrix4D temp;     // returning matrix

   temp = temp.identity();
   temp.matrix[3][0] = x;
   temp.matrix[3][1] = y;
   temp.matrix[3][2] = z;

   return temp;
}

// Rotation matrix

// z-axis :
// | cosz  sinz 0 0 |
// | -sinz cosz 0 0 |
// | 0     0    1 0 |
// | 0     0    0 1 |

// x-axis:
// | 1 0     0    0 |
// | 0 cosx  sinx 0 |
// | 0 -sinx cosx 0 |
// | 0 0     0    1 |

// y-axis:
// | cosy 0 -siny 0 |
// | 0    1 0     0 |
// | siny 0 cosy  0 |
// | 0    0 0     1 |

// The other rotation matrices are just combinations of these three
Matrix4D  rotate (const double x, const double y, const double z)
{
   Matrix4D temp;     // returning matrix

   temp = temp.identity();

   if (y == 0.0 && z == 0.0)
   {
      // rotation around x-axis
      temp.matrix[1][1] =  cos(x);
      temp.matrix[1][2] =  sin(x);
      temp.matrix[2][1] = -sin(x);
      temp.matrix[2][2] =  cos(x);
   }
   else if (z == 0.0 && x == 0.0)
   {
      // rotation around y-axis
      temp.matrix[0][0] =  cos(y);
      temp.matrix[2][0] = -sin(y);
      temp.matrix[0][2] =  sin(y);
      temp.matrix[2][2] =  cos(y);
   }
   else if (x == 0.0 && y == 0.0)
   {
      // rotation around z-axis
      temp.matrix[0][0] =  cos(z);
      temp.matrix[1][0] = -sin(z);
      temp.matrix[0][1] =  sin(z);
      temp.matrix[1][1] =  cos(z);
   }
   else if (z == 0.0)
   {
      // rotation around x and y axes
      temp.matrix[0][0] =  cos(y);
      temp.matrix[0][2] = -sin(y);
      temp.matrix[1][0] =  sin(x)*sin(y);
      temp.matrix[1][1] =  cos(x);
      temp.matrix[1][2] =  sin(x)*cos(y);
      temp.matrix[2][0] =  cos(x)*sin(y);
      temp.matrix[2][1] = -sin(x);
      temp.matrix[2][2] =  cos(x)*cos(y);
   }
   else if (x == 0.0)
   {
      // rotation around y and z axes
      temp.matrix[0][0] =  cos(y)*cos(z);
      temp.matrix[0][1] =  cos(y)*sin(z);
      temp.matrix[0][2] = -sin(y);
      temp.matrix[1][0] = -sin(z);
      temp.matrix[1][1] =  cos(z);
      temp.matrix[2][0] =  sin(y)*cos(z);
      temp.matrix[2][1] =  sin(y)*sin(z);
      temp.matrix[2][2] =  cos(z);
   }
   else if (y == 0.0)
   {
      // rotation around x and z axes
      temp.matrix[0][0] =  cos(z);
      temp.matrix[0][1] =  sin(z);
      temp.matrix[1][0] = -cos(x)*sin(z);
      temp.matrix[1][1] =  cos(x)*cos(z);
      temp.matrix[1][2] =  sin(x);
      temp.matrix[2][0] =  sin(x)*sin(z);
      temp.matrix[2][1] = -sin(x)*cos(z);
      temp.matrix[2][2] =  cos(x);
   }
   else
   {
      // rotation around all axes
      temp.matrix[0][0] =  cos(y)*cos(z);
      temp.matrix[0][1] =  cos(y)*sin(z);
      temp.matrix[0][2] = -sin(y);
      temp.matrix[1][0] = -cos(x)*sin(z) + sin(x)*sin(y)*cos(z);
      temp.matrix[1][1] =  cos(x)*cos(z) + sin(x)*sin(y)*sin(z);
      temp.matrix[1][2] =  sin(x)*cos(y);
      temp.matrix[2][0] =  sin(x)*sin(z) + cos(x)*sin(y)*cos(z);
      temp.matrix[2][1] = -sin(x)*cos(z) + cos(x)*sin(y)*sin(z);
      temp.matrix[2][2] =  cos(x)*cos(y);
   }

   return temp;
}

// Scaling matrix

// | x 0 0 0 |
// | 0 y 0 0 |
// | 0 0 z 0 |
// | 0 0 0 1 |

Matrix4D  scale (const double x, const double y, const double z)
{
   Matrix4D temp;      // returning matrix

   temp = temp.identity();
   temp.matrix[0][0] = x;
   temp.matrix[1][1] = y;
   temp.matrix[2][2] = z;

   return temp;
}

// Shear in the XY plane

// | 1 0 0 0 |
// | 0 1 0 0 |
// | x y 1 0 |
// | 0 0 0 1 |

Matrix4D  shearXY(const double shx, const double shy)
{
   Matrix4D temp;       // returning matrix

   temp = temp.identity();
   temp.matrix[2][0] = shx;
   temp.matrix[2][1] = shy;

   return temp;
}

// Shear in the XZ plane

// | 1 0 0 0 |
// | x 1 y 0 |
// | 0 0 1 0 |
// | 0 0 0 1 |

Matrix4D  shearXZ(const double shx, const double shz)
{
   Matrix4D temp;        // returning matrix

   temp = temp.identity();
   temp.matrix[1][0] = shx;
   temp.matrix[1][2] = shz;

   return temp;
}

// Shear in the YZ plane

// | 1 y z 0 |
// | 0 1 0 0 |
// | 0 0 1 0 |
// | 0 0 0 1 |

Matrix4D  shearYZ(const double shy, const double shz)
{
   Matrix4D temp;      // returning matrix

   temp = temp.identity();
   temp.matrix[0][1] = shy;
   temp.matrix[0][2] = shz;

   return temp;
}

// ELEMENTARY OPERATORS

// Assignment operator
Matrix4D& Matrix4D::operator = (const Matrix4D& mat)
{
   if (this == &mat)
      return *this;

   matrix[0][0] = mat.matrix[0][0];
   matrix[0][1] = mat.matrix[0][1];
   matrix[0][2] = mat.matrix[0][2];
   matrix[0][3] = mat.matrix[0][3];

   matrix[1][0] = mat.matrix[1][0];
   matrix[1][1] = mat.matrix[1][1];
   matrix[1][2] = mat.matrix[1][2];
   matrix[1][3] = mat.matrix[1][3];

   matrix[2][0] = mat.matrix[2][0];
   matrix[2][1] = mat.matrix[2][1];
   matrix[2][2] = mat.matrix[2][2];
   matrix[2][3] = mat.matrix[2][3];

   matrix[3][0] = mat.matrix[3][0];
   matrix[3][1] = mat.matrix[3][1];
   matrix[3][2] = mat.matrix[3][2];
   matrix[3][3] = mat.matrix[3][3];

   return (*this);
}

// add two matrices
Matrix4D  operator + (const Matrix4D& mat1, const Matrix4D& mat2) 
{
   Matrix4D temp;

   temp.matrix[0][0] = mat1.matrix[0][0] + mat2.matrix[0][0];
   temp.matrix[0][1] = mat1.matrix[0][1] + mat2.matrix[0][1];
   temp.matrix[0][2] = mat1.matrix[0][2] + mat2.matrix[0][2];
   temp.matrix[0][3] = mat1.matrix[0][3] + mat2.matrix[0][3];

   temp.matrix[1][0] = mat1.matrix[1][0] + mat2.matrix[1][0];
   temp.matrix[1][1] = mat1.matrix[1][1] + mat2.matrix[1][1];
   temp.matrix[1][2] = mat1.matrix[1][2] + mat2.matrix[1][2];
   temp.matrix[1][3] = mat1.matrix[1][3] + mat2.matrix[1][3];

   temp.matrix[2][0] = mat1.matrix[2][0] + mat2.matrix[2][0];
   temp.matrix[2][1] = mat1.matrix[2][1] + mat2.matrix[2][1];
   temp.matrix[2][2] = mat1.matrix[2][2] + mat2.matrix[2][2];
   temp.matrix[2][3] = mat1.matrix[2][3] + mat2.matrix[2][3];

   temp.matrix[3][0] = mat1.matrix[3][0] + mat2.matrix[3][0];
   temp.matrix[3][1] = mat1.matrix[3][1] + mat2.matrix[3][1];
   temp.matrix[3][2] = mat1.matrix[3][2] + mat2.matrix[3][2];
   temp.matrix[3][3] = mat1.matrix[3][3] + mat2.matrix[3][3];

   return temp;	
}

// subtract two matrices
Matrix4D  operator - (const Matrix4D& mat1, const Matrix4D& mat2) 
{
   Matrix4D temp;

   temp.matrix[0][0] = mat1.matrix[0][0] - mat2.matrix[0][0];
   temp.matrix[0][1] = mat1.matrix[0][1] - mat2.matrix[0][1];
   temp.matrix[0][2] = mat1.matrix[0][2] - mat2.matrix[0][2];
   temp.matrix[0][3] = mat1.matrix[0][3] - mat2.matrix[0][3];

   temp.matrix[1][0] = mat1.matrix[1][0] - mat2.matrix[1][0];
   temp.matrix[1][1] = mat1.matrix[1][1] - mat2.matrix[1][1];
   temp.matrix[1][2] = mat1.matrix[1][2] - mat2.matrix[1][2];
   temp.matrix[1][3] = mat1.matrix[1][3] - mat2.matrix[1][3];

   temp.matrix[2][0] = mat1.matrix[2][0] - mat2.matrix[2][0];
   temp.matrix[2][1] = mat1.matrix[2][1] - mat2.matrix[2][1];
   temp.matrix[2][2] = mat1.matrix[2][2] - mat2.matrix[2][2];
   temp.matrix[2][3] = mat1.matrix[2][3] - mat2.matrix[2][3];

   temp.matrix[3][0] = mat1.matrix[3][0] - mat2.matrix[3][0];
   temp.matrix[3][1] = mat1.matrix[3][1] - mat2.matrix[3][1];
   temp.matrix[3][2] = mat1.matrix[3][2] - mat2.matrix[3][2];
   temp.matrix[3][3] = mat1.matrix[3][3] - mat2.matrix[3][3];

   return temp;
}

// cross product of two matrices
Matrix4D  operator * (const Matrix4D& mat1, const Matrix4D& mat2) 
{
   Matrix4D temp;

   temp.matrix[0][0] = (mat1.matrix[0][0] * mat2.matrix[0][0]) 
                     + (mat1.matrix[0][1] * mat2.matrix[1][0])
                     + (mat1.matrix[0][2] * mat2.matrix[2][0])
                     + (mat1.matrix[0][3] * mat2.matrix[3][0]);

   temp.matrix[0][1] = (mat1.matrix[0][0] * mat2.matrix[0][1]) 
                     + (mat1.matrix[0][1] * mat2.matrix[1][1])
                     + (mat1.matrix[0][2] * mat2.matrix[2][1])
                     + (mat1.matrix[0][3] * mat2.matrix[3][1]);

   temp.matrix[0][2] = (mat1.matrix[0][0] * mat2.matrix[0][2]) 
                     + (mat1.matrix[0][1] * mat2.matrix[1][2])
                     + (mat1.matrix[0][2] * mat2.matrix[2][2])
                     + (mat1.matrix[0][3] * mat2.matrix[3][2]);
		
   temp.matrix[0][3] = (mat1.matrix[0][0] * mat2.matrix[0][3]) 
                     + (mat1.matrix[0][1] * mat2.matrix[1][3])
                     + (mat1.matrix[0][2] * mat2.matrix[2][3])
                     + (mat1.matrix[0][3] * mat2.matrix[3][3]);



   temp.matrix[1][0] = (mat1.matrix[1][0] * mat2.matrix[0][0]) 
                     + (mat1.matrix[1][1] * mat2.matrix[1][0])
                     + (mat1.matrix[1][2] * mat2.matrix[2][0])
                     + (mat1.matrix[1][3] * mat2.matrix[3][0]);

   temp.matrix[1][1] = (mat1.matrix[1][0] * mat2.matrix[0][1]) 
                     + (mat1.matrix[1][1] * mat2.matrix[1][1])
                     + (mat1.matrix[1][2] * mat2.matrix[2][1])
                     + (mat1.matrix[1][3] * mat2.matrix[3][1]);

   temp.matrix[1][2] = (mat1.matrix[1][0] * mat2.matrix[0][2]) 
                     + (mat1.matrix[1][1] * mat2.matrix[1][2])
                     + (mat1.matrix[1][2] * mat2.matrix[2][2])
                     + (mat1.matrix[1][3] * mat2.matrix[3][2]);

   temp.matrix[1][3] = (mat1.matrix[1][0] * mat2.matrix[0][3]) 
                     + (mat1.matrix[1][1] * mat2.matrix[1][3])
                     + (mat1.matrix[1][2] * mat2.matrix[2][3])
                     + (mat1.matrix[1][3] * mat2.matrix[3][3]);



   temp.matrix[2][0] = (mat1.matrix[2][0] * mat2.matrix[0][0])
                     + (mat1.matrix[2][1] * mat2.matrix[1][0])
                     + (mat1.matrix[2][2] * mat2.matrix[2][0])
                     + (mat1.matrix[2][3] * mat2.matrix[3][0]);

   temp.matrix[2][1] = (mat1.matrix[2][0] * mat2.matrix[0][1])
                     + (mat1.matrix[2][1] * mat2.matrix[1][1])
                     + (mat1.matrix[2][2] * mat2.matrix[2][1])
                     + (mat1.matrix[2][3] * mat2.matrix[3][1]);

   temp.matrix[2][2] = (mat1.matrix[2][0] * mat2.matrix[0][2])
                     + (mat1.matrix[2][1] * mat2.matrix[1][2])
                     + (mat1.matrix[2][2] * mat2.matrix[2][2])
                     + (mat1.matrix[2][3] * mat2.matrix[3][2]);

   temp.matrix[2][3] = (mat1.matrix[2][0] * mat2.matrix[0][3])
                     + (mat1.matrix[2][1] * mat2.matrix[1][3])
                     + (mat1.matrix[2][2] * mat2.matrix[2][3])
                     + (mat1.matrix[2][3] * mat2.matrix[3][3]);



   temp.matrix[3][0] = (mat1.matrix[3][0] * mat2.matrix[0][0])
                     + (mat1.matrix[3][1] * mat2.matrix[1][0])
                     + (mat1.matrix[3][2] * mat2.matrix[2][0])
                     + (mat1.matrix[3][3] * mat2.matrix[3][0]);

   temp.matrix[3][1] = (mat1.matrix[3][0] * mat2.matrix[0][1])
                     + (mat1.matrix[3][1] * mat2.matrix[1][1])
                     + (mat1.matrix[3][2] * mat2.matrix[2][1])
                     + (mat1.matrix[3][3] * mat2.matrix[3][1]);

   temp.matrix[3][2] = (mat1.matrix[3][0] * mat2.matrix[0][2])
                     + (mat1.matrix[3][1] * mat2.matrix[1][2])
                     + (mat1.matrix[3][2] * mat2.matrix[2][2])
                     + (mat1.matrix[3][3] * mat2.matrix[3][2]);

   temp.matrix[3][3] = (mat1.matrix[3][0] * mat2.matrix[0][3])
                     + (mat1.matrix[3][1] * mat2.matrix[1][3])
                     + (mat1.matrix[3][2] * mat2.matrix[2][3])
                     + (mat1.matrix[3][3] * mat2.matrix[3][3]);

   return temp;
}

// multiply matrix by scalar value
Matrix4D  operator * (const Matrix4D& mat, const double val) 
{
   Matrix4D temp;

   temp.matrix[0][0] = mat.matrix[0][0] * val;
   temp.matrix[0][1] = mat.matrix[0][1] * val;
   temp.matrix[0][2] = mat.matrix[0][2] * val;
   temp.matrix[0][3] = mat.matrix[0][3] * val;

   temp.matrix[1][0] = mat.matrix[1][0] * val;
   temp.matrix[1][1] = mat.matrix[1][1] * val;
   temp.matrix[1][2] = mat.matrix[1][2] * val;
   temp.matrix[1][3] = mat.matrix[1][3] * val;

   temp.matrix[2][0] = mat.matrix[2][0] * val;
   temp.matrix[2][1] = mat.matrix[2][1] * val;
   temp.matrix[2][2] = mat.matrix[2][2] * val;
   temp.matrix[2][3] = mat.matrix[2][3] * val;

   temp.matrix[3][0] = mat.matrix[3][0] * val;
   temp.matrix[3][1] = mat.matrix[3][1] * val;
   temp.matrix[3][2] = mat.matrix[3][2] * val;
   temp.matrix[3][3] = mat.matrix[3][3] * val;
	
   return temp;
}

// multiply matrix by scalar value
Matrix4D  operator * (const double val, const Matrix4D& mat) 
{
   Matrix4D temp;

   temp.matrix[0][0] = mat.matrix[0][0] * val;
   temp.matrix[0][1] = mat.matrix[0][1] * val;
   temp.matrix[0][2] = mat.matrix[0][2] * val;
   temp.matrix[0][3] = mat.matrix[0][3] * val;

   temp.matrix[1][0] = mat.matrix[1][0] * val;
   temp.matrix[1][1] = mat.matrix[1][1] * val;
   temp.matrix[1][2] = mat.matrix[1][2] * val;
   temp.matrix[1][3] = mat.matrix[1][3] * val;

   temp.matrix[2][0] = mat.matrix[2][0] * val;
   temp.matrix[2][1] = mat.matrix[2][1] * val;
   temp.matrix[2][2] = mat.matrix[2][2] * val;
   temp.matrix[2][3] = mat.matrix[2][3] * val;

   temp.matrix[3][0] = mat.matrix[3][0] * val;
   temp.matrix[3][1] = mat.matrix[3][1] * val;
   temp.matrix[3][2] = mat.matrix[3][2] * val;
   temp.matrix[3][3] = mat.matrix[3][3] * val;
	
   return temp;
}

// cross product of vector and matrix
Vector	operator * (const Matrix4D& mat, const Vector& vec)
{
   double x, y, z, w;

   x = mat.matrix[0][0]*vec.x() + mat.matrix[1][0]*vec.y() 
     + mat.matrix[2][0]*vec.z() + mat.matrix[3][0]*vec.w();
   y = mat.matrix[0][1]*vec.x() + mat.matrix[1][1]*vec.y() 
     + mat.matrix[2][1]*vec.z() + mat.matrix[3][1]*vec.w();
   z = mat.matrix[0][2]*vec.x() + mat.matrix[1][2]*vec.y() 
     + mat.matrix[2][2]*vec.z() + mat.matrix[3][2]*vec.w();
   w = mat.matrix[0][3]*vec.x() + mat.matrix[1][3]*vec.y() 
     + mat.matrix[2][3]*vec.z() + mat.matrix[3][3]*vec.w();

   // normalize w component
   return Vector(x/w, y/w, z/w, 1.0);
}

// cross product of vector and matrix
Vector	operator * (const Vector& vec, const Matrix4D& mat)
{
   double x, y, z, w;

   x = mat.matrix[0][0]*vec.x() + mat.matrix[1][0]*vec.y() 
     + mat.matrix[2][0]*vec.z() + mat.matrix[3][0]*vec.w();
   y = mat.matrix[0][1]*vec.x() + mat.matrix[1][1]*vec.y() 
     + mat.matrix[2][1]*vec.z() + mat.matrix[3][1]*vec.w();
   z = mat.matrix[0][2]*vec.x() + mat.matrix[1][2]*vec.y() 
     + mat.matrix[2][2]*vec.z() + mat.matrix[3][2]*vec.w();
   w = mat.matrix[0][3]*vec.x() + mat.matrix[1][3]*vec.y() 
     + mat.matrix[2][3]*vec.z() + mat.matrix[3][3]*vec.w();

   // normalize w component
   return Vector(x/w, y/w, z/w, 1.0);
}

// cross product of point and matrix
Point   operator * (const Matrix4D& mat, const Point& pt)
{
   double x, y, z, w;

   x = mat.matrix[0][0]*pt.x() + mat.matrix[1][0]*pt.y() 
     + mat.matrix[2][0]*pt.z() + mat.matrix[3][0];
   y = mat.matrix[0][1]*pt.x() + mat.matrix[1][1]*pt.y() 
     + mat.matrix[2][1]*pt.z() + mat.matrix[3][1];
   z = mat.matrix[0][2]*pt.x() + mat.matrix[1][2]*pt.y() 
     + mat.matrix[2][2]*pt.z() + mat.matrix[3][2];
   w = mat.matrix[0][3]*pt.x() + mat.matrix[1][3]*pt.y()
     + mat.matrix[2][3]*pt.z() + mat.matrix[3][3];

   // normalize w component
   return Point(x/w, y/w, z/w);
}

// cross product of point and matrix
Point   operator * (const Point& pt, const Matrix4D& mat)
{
   double x, y, z, w;

   x = mat.matrix[0][0]*pt.x() + mat.matrix[1][0]*pt.y() 
     + mat.matrix[2][0]*pt.z() + mat.matrix[3][0];
   y = mat.matrix[0][1]*pt.x() + mat.matrix[1][1]*pt.y() 
     + mat.matrix[2][1]*pt.z() + mat.matrix[3][1];
   z = mat.matrix[0][2]*pt.x() + mat.matrix[1][2]*pt.y() 
     + mat.matrix[2][2]*pt.z() + mat.matrix[3][2];
   w = mat.matrix[0][3]*pt.x() + mat.matrix[1][3]*pt.y()
     + mat.matrix[2][3]*pt.z() + mat.matrix[3][3];

   // normalize w component
   return Point(x/w, y/w, z/w);
}

// divide matrix by scalar value
Matrix4D operator / (const Matrix4D& mat, const double value) 
{
   double val = value;

   // avoid divide by zero
   if (val == 0.0) 
      val = 0.00000001;

   Matrix4D temp;      // returning matrix

   temp.matrix[0][0] = mat.matrix[0][0] / val;
   temp.matrix[0][1] = mat.matrix[0][1] / val;
   temp.matrix[0][2] = mat.matrix[0][2] / val;
   temp.matrix[0][3] = mat.matrix[0][3] / val;

   temp.matrix[1][0] = mat.matrix[1][0] / val;
   temp.matrix[1][1] = mat.matrix[1][1] / val;
   temp.matrix[1][2] = mat.matrix[1][2] / val;
   temp.matrix[1][3] = mat.matrix[1][3] / val;

   temp.matrix[2][0] = mat.matrix[2][0] / val;
   temp.matrix[2][1] = mat.matrix[2][1] / val;
   temp.matrix[2][2] = mat.matrix[2][2] / val;
   temp.matrix[2][3] = mat.matrix[2][3] / val;

   temp.matrix[3][0] = mat.matrix[3][0] / val;
   temp.matrix[3][1] = mat.matrix[3][1] / val;
   temp.matrix[3][2] = mat.matrix[3][2] / val;
   temp.matrix[3][3] = mat.matrix[3][3] / val;
	
   return temp;
}

// assignment of addition of matrix to itself
Matrix4D& Matrix4D::operator += (const Matrix4D& mat)
{
   matrix[0][0] += mat.matrix[0][0];
   matrix[0][1] += mat.matrix[0][1];
   matrix[0][2] += mat.matrix[0][2];
   matrix[0][3] += mat.matrix[0][3];

   matrix[1][0] += mat.matrix[1][0];
   matrix[1][1] += mat.matrix[1][1];
   matrix[1][2] += mat.matrix[1][2];
   matrix[1][3] += mat.matrix[1][3];

   matrix[2][0] += mat.matrix[2][0];
   matrix[2][1] += mat.matrix[2][1];
   matrix[2][2] += mat.matrix[2][2];
   matrix[2][3] += mat.matrix[2][3];

   matrix[3][0] += mat.matrix[3][0];
   matrix[3][1] += mat.matrix[3][1];
   matrix[3][2] += mat.matrix[3][2];
   matrix[3][3] += mat.matrix[3][3];

   return *this;
}

// assignment of subtraction of matrix from itself
Matrix4D& Matrix4D::operator -= (const Matrix4D& mat)
{
   matrix[0][0] -= mat.matrix[0][0];
   matrix[0][1] -= mat.matrix[0][1];
   matrix[0][2] -= mat.matrix[0][2];
   matrix[0][3] -= mat.matrix[0][3];

   matrix[1][0] -= mat.matrix[1][0];
   matrix[1][1] -= mat.matrix[1][1];
   matrix[1][2] -= mat.matrix[1][2];
   matrix[1][3] -= mat.matrix[1][3];

   matrix[2][0] -= mat.matrix[2][0];
   matrix[2][1] -= mat.matrix[2][1];
   matrix[2][2] -= mat.matrix[2][2];
   matrix[2][3] -= mat.matrix[2][3];

   matrix[3][0] -= mat.matrix[3][0];
   matrix[3][1] -= mat.matrix[3][1];
   matrix[3][2] -= mat.matrix[3][2];
   matrix[3][3] -= mat.matrix[3][3];

   return *this;
}

// assignment of matrix multiplied by scalar
Matrix4D& Matrix4D::operator *= (const double val)
{
   matrix[0][0] *= val;
   matrix[0][1] *= val;
   matrix[0][2] *= val;
   matrix[0][3] *= val;

   matrix[1][0] *= val;
   matrix[1][1] *= val;
   matrix[1][2] *= val;
   matrix[1][3] *= val;

   matrix[2][0] *= val;
   matrix[2][1] *= val;
   matrix[2][2] *= val;
   matrix[2][3] *= val;

   matrix[3][0] *= val;
   matrix[3][1] *= val;
   matrix[3][2] *= val;
   matrix[3][3] *= val;

   return *this;
}

// assignment of matrix divided by scalar
Matrix4D& Matrix4D::operator /= (const double val)
{
   matrix[0][0] /= val;
   matrix[0][1] /= val;
   matrix[0][2] /= val;
   matrix[0][3] /= val;

   matrix[1][0] /= val;
   matrix[1][1] /= val;
   matrix[1][2] /= val;
   matrix[1][3] /= val;

   matrix[2][0] /= val;
   matrix[2][1] /= val;
   matrix[2][2] /= val;
   matrix[2][3] /= val;

   matrix[3][0] /= val;
   matrix[3][1] /= val;
   matrix[3][2] /= val;
   matrix[3][3] /= val;

   return *this;
}


// Relational operators
int  operator == (const Matrix4D& mat1, const Matrix4D& mat2) 
{
   int i, j;
   for (i=0; i<4; i++) 
   {
      for (j=0; j<4; j++) 
      {
         if (mat1.matrix[i][j] != mat2.matrix[i][j])
            return FALSE;
      }
   }

   return TRUE;
}

int  operator != (const Matrix4D& mat1, const Matrix4D& mat2) 
{
   int i, j;
   for (i=0; i<4; i++) 
   {
      for (j=0; j<4; j++) 
      {
         if (mat1.matrix[i][j] != mat2.matrix[i][j])
            return TRUE;
      }
   }

   return FALSE;
}

// output stream
ostream&  operator << (ostream& co, const Matrix4D& mat) 
{
   int i, j;
   for (i=0; i<4; i++) 
   {
      co << '|';
      for (j=0; j<4; j++) 
      {
         co << mat.matrix[i][j] << ' ';
      }
      co << '|' << '\n';
   }
   return co;
}

    Source: geocities.com/collegepark/4206

               ( geocities.com/collegepark)