public class Matrix3D {
  private double data[][] = new double[4][4];

  Matrix3D() {
    zero();
  }

  public void zero() {
    for (int i=0 ; i<4 ; i++)
      for (int j=0 ; j<4 ; j++)
        data[i][j] = 0.0;
  }

  public void identity() {
    zero();
    data[0][0] = 1.0;
    data[1][1] = 1.0;
    data[2][2] = 1.0;
    data[3][3] = 1.0;
  }

  public void translationMatrix(double a, double b, double c) {
    identity();
    data[0][3] = a;
    data[1][3] = b;
    data[2][3] = c;
  }

  public void xRotationMatrix(double theta) {
    double cos = Math.cos(theta);
    double sin = Math.sin(theta);
    identity();
    data[1][1] =  cos;
    data[2][2] =  cos;
    data[2][1] =  sin;
    data[1][2] = -sin;
  }

  public void yRotationMatrix(double theta) {
    double cos = Math.cos(theta);
    double sin = Math.sin(theta);
    identity();
    data[0][0] =  cos;
    data[2][2] =  cos;
    data[0][2] =  sin;
    data[2][0] = -sin;
  }

  public void zRotationMatrix(double theta) {
    double cos = Math.cos(theta);
    double sin = Math.sin(theta);
    identity();
    data[0][0] =  cos;
    data[1][1] =  cos;
    data[0][1] =  sin;
    data[1][0] = -sin;
  }

  public void scaleMatrix(double x, double y, double z) {
    identity();
    data[0][0] = x;
    data[1][1] = y;
    data[2][2] = z;
  }

  public void set(int i, int j, double value) { data[i][j] = value; }
  public double get(int i, int j) { return data[i][j]; }

  public Vector4 getRowVector(int row) {
    return new Vector4(data[row][0], data[row][1], data[row][2], data[row][3]);
  }

  public Vector4 getColumnVector(int col) {
    return new Vector4(data[0][col], data[1][col], data[2][col], data[3][col]);
  }

  public void copy(Matrix3D src) {
    for (int i=0 ; i<4 ; i++)
      for (int j=0 ; j<4 ; j++)
        set(i,j, src.get(i,j));
  }

  public void multiply(Matrix3D m) {
    Matrix3D result = new Matrix3D();
    for (int i=0 ; i<4 ; i++)
      for (int j=0 ; j<4 ; j++)
        result.set(i,j, this.getRowVector(i).dotProduct(m.getColumnVector(j)));

    copy(result);
  }


  public void translate(double a, double b, double c) {
    Matrix3D t = new Matrix3D();
    t.translationMatrix(a, b, c);
    multiply(t);
  }

  public void rotateX(double theta) {
    Matrix3D r = new Matrix3D();
    r.xRotationMatrix(theta);
    multiply(r);
  }

  public void rotateY(double theta) {
    Matrix3D r = new Matrix3D();
    r.yRotationMatrix(theta);
    multiply(r);
  }

  public void rotateZ(double theta) {
    Matrix3D r = new Matrix3D();
    r.zRotationMatrix(theta);
    multiply(r);
  }

  public void scale(double x, double y, double z) {
    Matrix3D s = new Matrix3D();
    s.scaleMatrix(x, y, z);
    multiply(s);
  }

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

  /*
  public String toString() {
    StringBuilder sb = new StringBuilder();
    for (int i=0 ; i<4 ; i++)
      sb.append( String.format("%f   %f   %f   %f\n",
             data[i][0], data[i][1], data[i][2], data[i][3])
      );

    return sb.toString();
  }
  */
}