import Raster;

public class Matrix3D
{
    float matrix[][]; // Current transformation matrix.

    // CONSTRUCTORS
    
    /**
     * Create a Matrix3D with identity transform.
     */
    public Matrix3D()
    {
	matrix = new float[4][4];

	loadIdentity();
    }
    
    /**
     * Create a Matrix3D that is a copy of c.
     */
    public Matrix3D(Matrix3D c)
    {
	matrix = new float[4][4];

	// Copy c into matrix.
	for (int j=0; j<4; j++) {
	    for (int i=0; i<4; i++) {
		matrix[j][i] = c.matrix[j][i];
	    }
	}
    }

    /**
     * Create a Matrix3D that maps from canonical space the screenspace of r.
     */
    public Matrix3D(Raster r)
    {
	matrix = new float[4][4];
	
	// Start with the Identity matrix
	loadIdentity();

	// translate(r.width*1/20, r.height*1/20, 0);

	// MATRIX: Screen space (0..width, height..0, 0..2)

	// Scale to raster size
	scale(r.width/2, r.height/2, 1);
	
	// MATRIX: Bar space (0..2, 2..0, 0..2)
	
	// Translate origin from 0, 0 to -1 -1
	translate(1, 1, 0);

	// MATRIX: Foo space (-1..1, 1..-1, -1..1)
	
	// Flip across x and y axis
	//	scale(-1, -1, 1);

	// MATRIX: Canonical space (-1..1, -1..1, -1..1) 

	matrix[2][2] = 32768;
	matrix[2][3] = 32768;
	
    }

    // METHODS

    /**
     * Sets element [j][i] to value.
     */
    public void set(int i, int j, float value)
    {
	matrix[j][i] = value;
    }

    /**
     * Returns element [j][i].
     */
    public float get(int i, int j)
    {
	return matrix[j][i];
    }
    
    /**
     * Transforms the in array to the out array using the current matrix.
     * 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] = transformPoint3D(in[start+i]);
	    //	    System.out.println(in[start+i]+" --> "+out[start+i]);
	}
    }

    /**
     * Returns the Point3D in transformed by the current matrix.
     */
    private final Point3D transformPoint3D(Point3D in)
    {
	Point3D out = new Point3D();
	float w;

	// in is treated as a vector, and multiplied on the left by matrix.
	out.x = 
	    matrix[0][0] * in.x +
	    matrix[0][1] * in.y +
	    matrix[0][2] * in.z +
	    matrix[0][3] * 1;
	out.y = 
	    matrix[1][0] * in.x +
	    matrix[1][1] * in.y +
	    matrix[1][2] * in.z +
	    matrix[1][3] * 1;
	out.z = 
	    matrix[2][0] * in.x +
	    matrix[2][1] * in.y +
	    matrix[2][2] * in.z +
	    matrix[2][3] * 1;
	w =
	    matrix[3][0] * in.x +
	    matrix[3][1] * in.y +
	    matrix[3][2] * in.z +
	    matrix[3][3] * 1;

	out.x /= w;
	out.y /= w;
	out.z /= w;

	return(out);
    }

    /**
     * this = this * src
     */
    public final void compose(Matrix3D src)
    {
	Matrix3D oldMatrix = new Matrix3D(this);

	// System.out.println("old:");
	// System.out.println(this);

	for (int j=0; j<4; j++) {
	    for (int i=0; i<4; i++) {
		matrix[j][i] = 
		    oldMatrix.matrix[j][0] * src.matrix[0][i] +
		    oldMatrix.matrix[j][1] * src.matrix[1][i] +
		    oldMatrix.matrix[j][2] * src.matrix[2][i] +
		    oldMatrix.matrix[j][3] * src.matrix[3][i];
	    }
	}

	//	System.out.println("new:");
	// System.out.println(this);

    }

    /**
     * this = this + src
     */
    private final void add(Matrix3D src)
    {
	for (int j=0; j<4; j++) {
	    for (int i=0; i<4; i++) {
		matrix[j][i] = matrix[j][i] + src.matrix[j][i];
	    }
	}
    }

    /**
     * this = this * m
     */
    private final void scalarMultiply(float m)
    {
	for (int j=0; j<4; j++) {
	    for (int i=0; i<4; i++) {
		matrix[j][i] = m * matrix[j][i];
	    }
	}
    }
	
    /**
     * this = identity
     */
    public void loadIdentity()
    {
	// Clear the matrix
	for (int j=0; j<4; j++) {
	    for (int i=0; i<4; i++) {
		matrix[j][i] = 0;
	    }
	}

	// Set the identity points
	matrix[0][0] = 1;
	matrix[1][1] = 1;
	matrix[2][2] = 1;
	matrix[3][3] = 1;
    }

    /**
     * Loads the Symmetric matrix of (ax,ay,az).
     */
    private final void loadSymmetric(float ax, float ay, float az)
    {
	// From Lecture 12, Slide 15

	matrix[0][0] = ax*ax; 
	matrix[1][1] = ay*ay; 
	matrix[2][2] = az*az; 
	matrix[0][1] = matrix[1][0] = ax*ay;
	matrix[0][2] = matrix[2][0] = ax*az;
	matrix[1][2] = matrix[2][1] = ay*az;
	matrix[3][0] = 0;
	matrix[3][1] = 0;
	matrix[3][2] = 0;
	matrix[3][3] = 1;
	matrix[2][3] = 0;
	matrix[1][3] = 0;
	matrix[0][3] = 0;
    }

    /**
     * Loads the Skew matrix of (ax,ay,az).
     */
    private final void loadSkew(float ax, float ay, float az)
    {
	// From Lecture 12, Slide 16

	matrix[0][0] =   0; matrix[0][1] = -az; matrix[0][2] =  ay;
	matrix[1][0] =  az; matrix[1][1] =   0; matrix[1][2] = -ax;
	matrix[2][0] = -ay; matrix[2][1] =  ax; matrix[2][2] =  0;

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

    /**
     * this = this * t
     */
    public void translate(float tx, float ty, float tz)
    {
	Matrix3D translation = new Matrix3D();

	// From Lecture 12, Slide 23

	translation.matrix[0][3] = tx;
	translation.matrix[1][3] = ty;
	translation.matrix[2][3] = tz;

	// System.out.println("translating...");
	compose(translation);
    }
    
    /**
     * this = this * scale
     */
    public void scale(float sx, float sy, float sz)
    {
	Matrix3D sm = new Matrix3D();

	// From Lecture 12, Slide 24

	sm.matrix[0][0] = sx;
	sm.matrix[1][1] = sy;
	sm.matrix[2][2] = sz;

	// System.out.println("scaling...");
	compose(sm);
    }

    /**
     * this = this * skew
     */
    public void skew(float kxy, float kxz, float kyz)
    {
	Matrix3D sm = new Matrix3D();
	
	// From Lecture 12, Slide 24

	sm.matrix[0][1] = kxy;
	sm.matrix[0][2] = kxz;
	sm.matrix[1][2] = kyz;

	// System.out.println("skewing...");
	compose(sm);
    }
    
    /** 
     * this = this * rotate
     */
    public void rotate(float ax, float ay, float az, float angle)
    {
	Matrix3D rotation, symmetric, skew;
	float cos = (float)Math.cos(angle);

	// System.out.println("rotating: ["+ax+","+ay+","+az+"], "+angle);

	// From Lecture 12, Slide 12

	// normalize a
	float mag = (float)Math.sqrt(ax*ax + ay*ay + az*az);
	ax /= mag;
	ay /= mag;
	az /= mag;

	// System.out.println("scaled: ["+ax+","+ay+","+az+"], "+angle);

	// From Lecture 12, Slides 13-16...

	symmetric = new Matrix3D();
	symmetric.loadSymmetric(ax, ay, az);
	symmetric.scalarMultiply(1 - cos);

	// System.out.println("symmetric:");
	// System.out.println(symmetric);

	skew = new Matrix3D();
	skew.loadSkew(ax, ay, az);
	skew.scalarMultiply((float)Math.sin(angle));
	
	// System.out.println("skew:");
	// System.out.println(skew);

	rotation = new Matrix3D();
	rotation.scalarMultiply(cos);
	// System.out.println("identity:");
	// System.out.println(rotation);
	rotation.add(skew);
	rotation.add(symmetric);
	rotation.matrix[3][3]=1;

	// System.out.println("rotating...");
	compose(rotation);
    }
    
    /**
     * this = this * lookAt
     */
    public void lookAt(float eyex, float eyey, float eyez,
		       float atx,  float aty,  float atz,
		       float upx,  float upy,  float upz)
    {
	float lx, ly, lz; // vector l
	float rx, ry, rz; // vector r
	float ux, uy, uz; // vector u
	float mag;        // magnitude for normalizing vectors

	// From Lecture 12, Slides 29-36

	// System.out.println("eye: "+eyex+" "+eyey+" "+eyez);
	// System.out.println("at : "+ atx+" "+ aty+" "+ atz);
	// System.out.println("up : "+ upx+" "+ upy+" "+ upz);

	// l = at - eye
	lx = atx - eyex;
	ly = aty - eyey;
	lz = atz - eyez;

	// normalize l
	mag = (float)Math.sqrt(lx*lx + ly*ly + lz*lz);
	lx /= mag;
	ly /= mag;
	lz /= mag;

	// r = l cross up
	rx = ly*upz - lz*upy;
	ry = lz*upx - lx*upz;
	rz = lx*upy - ly*upx;
	// System.out.println("rz="+rz);

	// normalize r
	mag = (float)Math.sqrt(rx*rx + ry*ry + rz*rz);
	rx /= mag;
	ry /= mag;
	rz /= mag;

	// u = r cross l
	ux = ry*lz - rz*ly;
	uy = rz*lx - rx*lz;
	uz = rx*ly - ry*lx;

	// normalize u
	mag = (float)Math.sqrt(ux*ux + uy*uy + uz*uz);
	ux /= mag;
	uy /= mag;
	uz /= mag;

	Matrix3D V = new Matrix3D();

	V.matrix[0][0] = rx;
	V.matrix[0][1] = ry;
	V.matrix[0][2] = rz;
	V.matrix[0][3] = - (rx*eyex + ry*eyey + rz*eyez);

	V.matrix[1][0] = ux;
	V.matrix[1][1] = uy;
	V.matrix[1][2] = uz;
	V.matrix[1][3] = - (ux*eyex + uy*eyey + uz*eyez);

	V.matrix[2][0] = -lx;
	V.matrix[2][1] = -ly;
	V.matrix[2][2] = -lz;
	V.matrix[2][3] = lx*eyex + ly*eyey + lz*eyez;

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

	// System.out.println("lookating with:");
	// System.out.println(V);

	// MATRIX: Eye space (Eye at origin)

	compose(V);

	// MATRIX: World space

    }

    /**
     * Transforms points into the canonical viewing space
     */
    public void perspective(float left, float right,
			    float bottom, float top,
			    float near, float far)
    {
	Matrix3D perspect = new Matrix3D();

	// From Lecture 12, Slide 44

	perspect.matrix[0][0] = (2*near) / (right - left);
	perspect.matrix[0][1] = 0;
	perspect.matrix[0][2] = -(right + left) / (right - left);
	perspect.matrix[0][3] = 0;
	perspect.matrix[1][0] = 0;
	perspect.matrix[1][1] = (2*near) / (bottom - top);
	perspect.matrix[1][2] = -(bottom + top) / (bottom - top);
	perspect.matrix[1][3] = 0;
	perspect.matrix[2][0] = 0;
	perspect.matrix[2][1] = 0;
	perspect.matrix[2][2] = (far + near) / (far - near);
	perspect.matrix[2][3] = (-2*far*near) / (far - near);
	perspect.matrix[3][0] = 0;
	perspect.matrix[3][1] = 0;
	perspect.matrix[3][2] = 1;
	perspect.matrix[3][3] = 0;

	//	System.out.println("perspecting with:");
	//	System.out.println("l="+left+" r="+right+" b="+bottom+
	//                         " t="+top+" n="+near+" f="+far);
	//	System.out.println(perspect);

	// MATRIX: Canonical space (-1..1, -1..1, -1..1) 

	compose(perspect);

	// MATRIX: Eye space (left..right, bottom..top, near..far) 

    }

    /**
     * Transforms points into the canonical viewing space
     */
    public void orthographic(float left, float right,
			     float bottom, float top,
			     float near, float far)
    {
	Matrix3D ortho = new Matrix3D();

	// From Lecture 12, Slide 44

	ortho.matrix[0][0] = 2 / (right - left);
	ortho.matrix[0][1] = 0;
	ortho.matrix[0][2] = 0;
	ortho.matrix[0][3] = -(right + left) / (right - left);
	ortho.matrix[1][0] = 0;
	ortho.matrix[1][1] = 2 / (bottom - top);
	ortho.matrix[1][2] = 0;
	ortho.matrix[1][3] = -(bottom + top) / (bottom - top);
	ortho.matrix[2][0] = 0;
	ortho.matrix[2][1] = 0;
	ortho.matrix[2][2] = 2 / (far - near);
	ortho.matrix[2][3] = -(far + near) / (far - near);
	ortho.matrix[3][0] = 0;
	ortho.matrix[3][1] = 0;
	ortho.matrix[3][2] = 0;
	ortho.matrix[3][3] = 1;

	// System.out.println("orthoging...");
	compose(ortho);
    }
    
    /**
     * Returns a String representation of the Matrix3D.
     */
    public final String toString()
    {
	String output = "";

	for (int j=0; j<4; j++) {
	    output = output + "[";
	    for (int i=0; i<4; i++) {
		output = output + " " + matrix[j][i];
	    }
	    output = output + " ]\n";
	}

	return(output);
    }
}