public class Matrix3D {
  // private vars
  private float[][] m;

  //
  // Constructors
  //
  public Matrix3D() {                 // initialize with identity transform
    m = new float[4][4];
    loadIdentity();
  }

  public Matrix3D(Matrix3D copy) {    // initialize with copy of source
    m = new float[4][4];
    for(int j=0; j<4; j++)
      for(int i=0; i<4; i++)
	m[i][j] = copy.get(i, j);
  }

  public Matrix3D(Raster r) {         // initialize with a mapping from
                                      // canonical space to screen space
    m = new float[4][4];
    loadIdentity();
    m[0][0] = r.width/2;
    m[1][1] = r.height/2;
    m[3][0] = r.width/2;
    m[3][1] = r.height/2;
    m[2][2] = 32768; //??
    m[3][2] = 32768; //??
  }
  
  //
  // General interface methods
  //        
  public void set(int i, int j, float value) {         // set element [j][i] to value
    m[i][j] = value;
  }

  public void set(int i, int j, double value) {         // set element [j][i] to value
    m[i][j] = (float)value;
  }

  public float get(int i, int j) {                     // return element [j][i]
    return m[i][j];
  }

  //
  // Transform points from the in array to the out array
  // using the current matrix. The subset of points transformed
  // begins at the start index and has the specified length
  //
  //    for (i = 0; i < length; i++)
  //        out[start+i] = this * in[start+i]
  //
  public void transform(Point3D in[], Point3D out[], int start, int length) {
    for (int i=0; i<length; i++)
      out[start+i] = compose(in[start+i]);
  }

  public final void compose(Matrix3D src) {                       // this = this * src
    float tmp[][] = new float[4][4];
    for(int j=0; j<4; j++)
      for(int i=0; i<4; i++) {
	float acc = 0;
	for(int k=0; k<4; k++)
	  acc += m[k][j] * src.get(i, k);
	tmp[i][j] = acc;
      }
    m = tmp;
  }

  public Point3D compose(Point3D src) {                           // this = this * src
    Point3D tmp = new Point3D(0,0,0);
    tmp.x = (m[0][0] * src.x) + (m[1][0] * src.y) + (m[2][0] * src.z) + m[3][0];
    tmp.y = (m[0][1] * src.x) + (m[1][1] * src.y) + (m[2][1] * src.z) + m[3][1];
    tmp.z = (m[0][2] * src.x) + (m[1][2] * src.y) + (m[2][2] * src.z) + m[3][2];
    float foo = (m[0][3] * src.x) + (m[1][3] * src.y) + (m[2][3] * src.z) + m[3][3];
    tmp.x /= foo;
    tmp.y /= foo;
    tmp.z /= foo;
    return tmp;
  }

  public void compose(double src) {                             // this = this * src
    for(int j=0; j<4; j++)
      for(int i=0; i<4; i++)
	m[i][j] *= src;
  }

  public void add(Matrix3D src) {                              // this = this + src
    for(int j=0; j<4; j++)
      for(int i=0; i<4; i++)
	m[i][j] += src.get(i, j);
  }

  public void loadIdentity() {                                    // this = identity
    for(int j=0; j<4; j++)
      for(int i=0; i<4; i++)
	if(i==j) m[i][j] = 1;
	else m[i][j] = 0;
  }

  public void translate(float tx, float ty, float tz) {           // this = this * t
    Matrix3D tmp = new Matrix3D();
    tmp.set(3, 0, tx);
    tmp.set(3, 1, ty);
    tmp.set(3, 2, tz);
    compose(tmp);
  }

  public void scale(float sx, float sy, float sz) {               // this = this * scale
    Matrix3D tmp = new Matrix3D();
    tmp.set(0, 0, sx); //FIXME?
    tmp.set(1, 1, sy);
    tmp.set(2, 2, sz);
    compose(tmp);
  }

  public void skew2(float kxy, float kxz, float kyz) {             // this = this * skew
    Matrix3D tmp = new Matrix3D();
    tmp.set(1, 0, kxy);
    tmp.set(2, 0, kxz);
    tmp.set(2, 1, kyz);
    compose(tmp);
  }

  public void skew(float kx, float ky, float kz) {                // this = this * skew
    Matrix3D tmp = new Matrix3D();
    tmp.set(1, 0, -kz);
    tmp.set(2, 0, ky);
    tmp.set(2, 1, -kx);
    tmp.set(0, 0, 0);
    tmp.set(1, 1, 0);
    tmp.set(2, 2, 0);
    tmp.set(0, 1, kz);
    tmp.set(0, 2, -ky);
    tmp.set(1, 2, kx);
    compose(tmp);
  }

  public void symmetric(float ax, float ay, float az) {           // this = this * sym
    Matrix3D tmp = new Matrix3D();
    tmp.set(0, 0, ax*ax);
    tmp.set(1, 0, ax*ay);
    tmp.set(2, 0, ax*az);
    tmp.set(0, 1, ax*ay);
    tmp.set(1, 1, ay*ay);
    tmp.set(2, 1, ay*az);
    tmp.set(0, 2, ax*az);
    tmp.set(1, 2, ay*az);
    tmp.set(2, 2, az*az);
    compose(tmp);
  }

  public void rotate(float ax, float ay, float az, float angle) { // this = this * rotate
    float foo = (float)Math.sqrt(ax*ax+ay*ay+az*az);
    ax /= foo;
    ay /= foo;
    az /= foo;
    double tmp[][] = new double[3][3];
    int i, j;
    double s, a, b, c;
    s = Math.cos(angle/2);
    a = ax*Math.sin(angle/2);
    b = ay*Math.sin(angle/2);
    c = az*Math.sin(angle/2);

    tmp[0][0] = 1-2*b*b-2*c*c;
    tmp[1][0] = 2*a*b-2*s*c;
    tmp[2][0] = 2*a*c+2*s*b;
    tmp[0][1] = 2*a*b+2*s*c;
    tmp[1][1] = 1-2*a*a-2*c*c;
    tmp[2][1] = 2*b*c-2*s*a;
    tmp[0][2] = 2*a*c-2*s*b;
    tmp[1][2] = 2*b*c+2*s*a;
    tmp[2][2] = 1-2*a*a-2*b*b;

    Matrix3D tmp2 = new Matrix3D();
    tmp2.set(0, 0, tmp[0][0]);
    tmp2.set(1, 0, tmp[1][0]);
    tmp2.set(2, 0, tmp[2][0]);
    tmp2.set(0, 1, tmp[0][1]);
    tmp2.set(1, 1, tmp[1][1]);
    tmp2.set(2, 1, tmp[2][1]);
    tmp2.set(0, 2, tmp[0][2]);
    tmp2.set(1, 2, tmp[1][2]);
    tmp2.set(2, 2, tmp[2][2]);
    compose(tmp2);
  }

  public void lookAt(float eyex, float eyey, float eyez,
		     float atx,  float aty,  float atz,
		     float upx,  float upy,  float upz) {         // this = this * lookat
    Point3D l = new Point3D(atx-eyex, aty-eyey, atz-eyez);
    Point3D eye = new Point3D(eyex, eyey, eyez);
    Point3D up = new Point3D(upx, upy, upz);
    Point3D u = new Point3D();
    Point3D r = new Point3D();
    Matrix3D tmp = new Matrix3D();
    l.normalize();
    tmp.skew(l.x, l.y, l.z);
    r = tmp.compose(up);
    r.normalize();
    tmp.loadIdentity();
    tmp.skew(r.x, r.y, r.z);
    u = tmp.compose(l);
    u.normalize();
    tmp.loadIdentity();
    tmp.set(0,0,r.x);
    tmp.set(1,0,r.y);
    tmp.set(2,0,r.z);
    tmp.set(0,1,u.x);
    tmp.set(1,1,u.y);
    tmp.set(2,1,u.z);
    tmp.set(0,2,-l.x);
    tmp.set(1,2,-l.y);
    tmp.set(2,2,-l.z);
    tmp.set(0,3,0);
    tmp.set(1,3,0);
    tmp.set(2,3,0);
    tmp.set(3,0,-r.dot(eye));
    tmp.set(3,1,-u.dot(eye));
    tmp.set(3,2,l.dot(eye));
    tmp.set(3,3,1);
    compose(tmp);
  }

  //
  // Assume the following projection transformations
  // transform points into the canonical viewing space
  //
  public void perspective(float left, float right,                // this = this * persp
			  float bottom, float top,
			  float near, float far) {
    Matrix3D tmp = new Matrix3D();
    tmp.set(0, 0, (2*near)/(right-left));
    tmp.set(1, 1, (2*near)/(bottom-top));
    tmp.set(2, 0, -(right+left)/(right-left));
    tmp.set(2, 1, -(bottom+top)/(bottom-top));
    tmp.set(2, 2, (far+near)/(far-near));
    tmp.set(3, 2, -(2*far*near)/(far-near)); 
    tmp.set(2, 3, 1);
    tmp.set(3, 3, 0);
    compose(tmp);
  }

  public void orthographic(float left, float right,               // this = this * ortho
			   float bottom, float top,
			   float near, float far) {
    Matrix3D tmp = new Matrix3D();
    tmp.set(0, 0, 2/(right-left));
    tmp.set(1, 1, 2/(bottom-top));
    tmp.set(3, 0, -(right+left)/(right-left));
    tmp.set(3, 1, -(bottom+top)/(bottom-top));
    tmp.set(3, 2, -(far+near)/(far-near));
    tmp.set(2, 2, 2/(far-near));
    tmp.set(3, 3, 1);
    compose(tmp);
  }
}