//----- SIMPLE CLASS TO HANDLE BASIC 3D MATRIX OPERATIONS -----

public class Matrix
{
   public static void identity(double matrix[][]) {
      for (int i = 0 ; i < 4 ; i++)
      for (int j = 0 ; j < 4 ; j++)
         matrix[i][j] = (i == j ? 1 : 0);
   }

   public static double[][] tmp = new double[4][4];

   public static void copy(double src[][], double dst[][]) {
      for (int i = 0 ; i < 4 ; i++)
      for (int j = 0 ; j < 4 ; j++)
         dst[i][j] = src[i][j];
   }

   public static void preMultiply(double dst[][], double b[][]) {
      copy(dst, tmp);
      for (int i = 0 ; i < 4 ; i++)
      for (int j = 0 ; j < 4 ; j++) {
         dst[i][j] = 0;
         for (int k = 0 ; k < 4 ; k++)
            dst[i][j] += tmp[i][k] * b[k][j];
      }
   }

   public static void postMultiply(double dst[][], double b[][]) {
      copy(dst, tmp);
      for (int i = 0 ; i < 4 ; i++)
      for (int j = 0 ; j < 4 ; j++) {
         dst[i][j] = 0;
         for (int k = 0 ; k < 4 ; k++)
            dst[i][j] += b[i][k] * tmp[k][j];
      }
   }

//----- ROUTINES TO ROTATE AND TRANSLATE MATRICES -----

   private static double[][] mat = new double[4][4];

   public static void translate(double m[][], double x, double y, double z) {
      makeTranslationMatrix(mat, x,y,z);
      preMultiply(m, mat);
   }
   public static void rotateX(double m[][], double theta) {
      makeRotationMatrix(mat, 1,2, theta);
      preMultiply(m, mat);
   }
   public static void rotateY(double m[][], double theta) {
      makeRotationMatrix(mat, 2,0, theta);
      preMultiply(m, mat);
   }
   public static void rotateZ(double m[][], double theta) {
      makeRotationMatrix(mat, 0,1, theta);
      preMultiply(m, mat);
   }
   public static void scale(double m[][], double x, double y, double z) {
      makeScaleMatrix(mat, x,y,z);
      preMultiply(m, mat);
   }

//----- INVERTING A 4x4 THAT WAS CREATED BY TRANSLATIONS+ROTATIONS+SCALES

   public static void invert(double src[][], double dst[][]) {

      // COMPUTE ADJOINT COFACTOR MATRIX FOR THE ROTATION+SCALE 3x3

      for (int i = 0 ; i < 3 ; i++)
      for (int j = 0 ; j < 3 ; j++) {
         int i0 = (i+1) % 3;
         int i1 = (i+2) % 3;
         int j0 = (j+1) % 3;
         int j1 = (j+2) % 3;
         dst[j][i] = src[i0][j0] * src[i1][j1] - src[i0][j1] * src[i1][j0];
      }

      // RENORMALIZE BY DETERMINANT TO GET ROTATION+SCALE 3x3 INVERSE

      double determinant = src[0][0] * dst[0][0]
                         + src[1][0] * dst[0][1]
                         + src[2][0] * dst[0][2] ;
      for (int i = 0 ; i < 3 ; i++)
      for (int j = 0 ; j < 3 ; j++)
         dst[i][j] /= determinant;

      // COMPUTE INVERSE TRANSLATION

      for (int i = 0 ; i < 3 ; i++)
         dst[i][3]= - dst[i][0] * src[0][3]
                    - dst[i][1] * src[1][3]
                    - dst[i][2] * src[2][3] ;
   }

//----- FOR DEBUGGING -----

   public static String toString(double m[][]) {
      String s = "{";
      for (int i = 0 ; i < 4 ; i++) {
          s += "{";
          for (int j = 0 ; j < 4 ; j++) {
             int n = (int)(100*m[i][j]);
             s += (n/100.) + (j==3 ? "" : ",");
          }
          s += "}" + (i==3 ? "" : ",");
      }
      return s + "}";
   }

//----- ROUTINES TO GENERATE TRANSFORMATION MATRICES -----

   static void makeTranslationMatrix(double m[][],
         double x, double y, double z) {
      identity(m);
      m[0][3] = x;
      m[1][3] = y;
      m[2][3] = z;
   }
   static void makeRotationMatrix(double m[][],
	 int i,int j,double theta) {
      identity(m);
      m[i][i] = m[j][j] = Math.cos(theta);
      m[i][j] = -Math.sin(theta);
      m[j][i] = -m[i][j];
   }
   static void makeScaleMatrix(double m[][],
	 double x,double y,double z) {
      identity(m);
      m[0][0] *= x;
      m[1][1] *= y;
      m[2][2] *= z;
   }

//----- ROUTINE TO TRANSFORM A POINT -----

   public static void transform(double m[][], double src[], double dst[]){
      for (int j = 0 ; j < 3 ; j++)
         dst[j] = m[j][0]*src[0] + m[j][1]*src[1] + m[j][2]*src[2] + m[j][3];
   }
}