#include <math.h>


void InitMatrix(float Matrix[3][3]);
void MultMatrixes(float Matrix1[3][3], float Matrix2[3][3], int type);
void xRotateMatrix(float x, float Matrix[3][3], int type);
void yRotateMatrix(float y, float Matrix[3][3], int type);
void zRotateMatrix(float z, float Matrix[3][3], int type);


void InitMatrix(float Matrix[3][3])
{
  Matrix[0][0]=1.0;
  Matrix[0][1]=0.0;
  Matrix[0][2]=0.0;
  Matrix[1][0]=0.0;
  Matrix[1][1]=1.0;
  Matrix[1][2]=0.0;
  Matrix[2][0]=0.0;
  Matrix[2][1]=0.0;
  Matrix[2][2]=1.0;
}


void MultMatrixes(float Matrix1[3][3], float Matrix2[3][3], int type)
{
  float NewMatrix[3][3];
  int i, j;

  NewMatrix[0][0]=Matrix1[0][0]*Matrix2[0][0]+Matrix1[0][1]*Matrix2[1][0]+
                  Matrix1[0][2]*Matrix2[2][0];
  NewMatrix[0][1]=Matrix1[0][0]*Matrix2[0][1]+Matrix1[0][1]*Matrix2[1][1]+
                  Matrix1[0][2]*Matrix2[2][1];
  NewMatrix[0][2]=Matrix1[0][0]*Matrix2[0][2]+Matrix1[0][1]*Matrix2[1][2]+
                  Matrix1[0][2]*Matrix2[2][2];
  NewMatrix[1][0]=Matrix1[1][0]*Matrix2[0][0]+Matrix1[1][1]*Matrix2[1][0]+
                  Matrix1[1][2]*Matrix2[2][0];
  NewMatrix[1][1]=Matrix1[1][0]*Matrix2[0][1]+Matrix1[1][1]*Matrix2[1][1]+
                  Matrix1[1][2]*Matrix2[2][1];
  NewMatrix[1][2]=Matrix1[1][0]*Matrix2[0][2]+Matrix1[1][1]*Matrix2[1][2]+
                  Matrix1[1][2]*Matrix2[2][2];
  NewMatrix[2][0]=Matrix1[2][0]*Matrix2[0][0]+Matrix1[2][1]*Matrix2[1][0]+
                  Matrix1[2][2]*Matrix2[2][0];
  NewMatrix[2][1]=Matrix1[2][0]*Matrix2[0][1]+Matrix1[2][1]*Matrix2[1][1]+
                  Matrix1[2][2]*Matrix2[2][1];
  NewMatrix[2][2]=Matrix1[2][0]*Matrix2[0][2]+Matrix1[2][1]*Matrix2[1][2]+
                  Matrix1[2][2]*Matrix2[2][2];

  for (i=0; i<3; i++) {
    for (j=0; j<3; j++) {
      if (type==1) {
	Matrix1[i][j]=NewMatrix[i][j];
      } else {
	Matrix2[i][j]=NewMatrix[i][j];
      }
    }
  }
}


void xRotateMatrix(float x, float Matrix[3][3], int type)
{
  float RotationMatrix[3][3];
  float sine, cosine;

  sine=sin(x*(M_PI/180.0));
  cosine=cos(x*(M_PI/180.0));

  RotationMatrix[0][0]=1.0;
  RotationMatrix[0][1]=0.0;
  RotationMatrix[0][2]=0.0;
  RotationMatrix[1][0]=0.0;
  RotationMatrix[1][1]=cosine;
  RotationMatrix[1][2]=sine;
  RotationMatrix[2][0]=0.0;
  RotationMatrix[2][1]=-sine;
  RotationMatrix[2][2]=cosine;

  if (type==1) {
    MultMatrixes(Matrix, RotationMatrix, 1);
  } else if (type==2) {
    MultMatrixes(RotationMatrix, Matrix, 2);
  }
}


void yRotateMatrix(float y, float Matrix[3][3], int type)
{
  float RotationMatrix[3][3];
  float sine, cosine;

  sine=sin(y*(M_PI/180.0));
  cosine=cos(y*(M_PI/180.0));

  RotationMatrix[0][0]=cosine;
  RotationMatrix[0][1]=0.0;
  RotationMatrix[0][2]=sine;
  RotationMatrix[1][0]=0.0;
  RotationMatrix[1][1]=1.0;
  RotationMatrix[1][2]=0.0;
  RotationMatrix[2][0]=-sine;
  RotationMatrix[2][1]=0.0;
  RotationMatrix[2][2]=cosine;

  if (type==1) {
    MultMatrixes(Matrix, RotationMatrix, 1);
  } else if (type==2) {
    MultMatrixes(RotationMatrix, Matrix, 2);
  }
}


void zRotateMatrix(float z, float Matrix[3][3], int type)
{
  float RotationMatrix[3][3];
  float sine, cosine;

  sine=sin(z*(M_PI/180.0));
  cosine=cos(z*(M_PI/180.0));

  RotationMatrix[0][0]=cosine;
  RotationMatrix[0][1]=sine;
  RotationMatrix[0][2]=0.0;
  RotationMatrix[1][0]=-sine;
  RotationMatrix[1][1]=cosine;
  RotationMatrix[1][2]=0.0;
  RotationMatrix[2][0]=0.0;
  RotationMatrix[2][1]=0.0;
  RotationMatrix[2][2]=1.0;

  if (type==1) {
    MultMatrixes(Matrix, RotationMatrix, 1);
  } else if (type==2) {
    MultMatrixes(RotationMatrix, Matrix, 2);
  }
}
