public class Matrix3D {

	private float[][] array;


	// Constructors

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

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


	
	public Matrix3D(ZbufferedRaster r)          // initialize with a mapping from canonical space to screen space
	{
		array = matrix44(	0.5f*(r.width-1),	0,					0,											0.5f*(r.width-1),
							0,					0.5f*(r.height-1),	0,											0.5f*(r.height-1),
							0,					0,					((float)(r.Zfar>>1)-(float)(r.Znear>>1)),	0.5f*(r.Znear+r.Zfar),
							0,					0,					0,											1						);
	};



	//ar is a float[4][4]
	private Matrix3D(float[][] ar)
	{
		array = ar;
	};




	// General interface methods
	
    public void set(int i, int j, float value)          // set element [j][i] to value
	{
		array[i][j] = value;
	};
	
	
	public float get(int i, int j)                      // return element [j][i]
	{
		return array[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 transformNormal(Vector3[] in, Vector3 out[], int length) {
		float[][] m33 = new float[3][3];
		float[][] a = array;
		float[][] det22 = new float[3][3];
		det22[0][0] = a[1][1]*a[2][2] - a[2][1]*a[1][2];
		det22[0][1] = a[1][0]*a[2][2] - a[1][2]*a[2][0];
		det22[0][2] = a[1][0]*a[2][1] - a[2][0]*a[1][1];
		det22[1][0] = a[0][1]*a[2][2] - a[2][1]*a[0][2];
		det22[1][1] = a[0][0]*a[2][2] - a[0][2]*a[2][0];
		det22[1][2] = a[0][0]*a[2][1] - a[2][0]*a[0][1];
		det22[2][0] = a[0][1]*a[1][2] - a[1][1]*a[0][2];
		det22[2][1] = a[0][0]*a[1][2] - a[1][0]*a[0][2];
		det22[2][2] = a[0][0]*a[1][1] - a[1][0]*a[0][1];
		float oneOverDet = 1/(a[0][0]*det22[0][0] - a[0][1]*det22[0][1] + a[0][2]*det22[0][2]);
		float[][] inv = new float[3][3];
		for (int i=0; i<3; i++)
			for (int j=0; j<3; j++)
				inv[i][j] = (((i+j)%2==0)?1:-1) * det22[j][i] * oneOverDet;
		float[][] trans = new float[3][3];
		for (int i=0; i<3; i++)
			for (int j=0; j<3; j++)
				trans[i][j] = inv[j][i];
		for (int i=0; i<length; i++) {
			out[i].x = in[i].x * trans[0][0] + in[i].y * trans[0][1] + in[i].z * trans[0][2];
			out[i].y = in[i].x * trans[1][0] + in[i].y * trans[1][1] + in[i].z * trans[1][2];
			out[i].z = in[i].x * trans[2][0] + in[i].y * trans[2][1] + in[i].z * trans[2][2];
		};
	};


	public void transform(Point3D in[], Point3D out[], int start, int length)
	{
		for (int i=start; i<(start+length); i++)
		{
			matrixMultVector(array, in[i], out[i]);
		};
	};


	public void transform(Vertex3D in[], Vertex3D out[], int start, int length)
	{
		for (int i=start; i<(start+length); i++)
		{
			matrixMultVector(array, in[i], out[i]);
		};
	};



	public void transform(Vertex3D in[], Vertex3D out[], int length)
	{
		transform(in,out,0,length);
	};



	public Point3D transform(Point3D in)
	{
		return matrixMultVector(array, in);
	};



	public Vertex3D transform(Vertex3D in)
	{
		return matrixMultVector(array, in);
	};



	public Vector3 transform(Vector3 in)
	{
		return matrixMultVector(array, in);
	};



	public final void compose(Matrix3D src)                        // this = this * src
	{
		float[][] temp = matrixMult44(array, src.array);
		array = temp;
	};


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

	
	public void translate(float tx, float ty, float tz)            // this = this * t
	{
		float[][] temp = matrix44(	1, 0, 0, tx,
									0, 1, 0, ty,
									0, 0, 1, tz,
									0, 0, 0, 1);
		float[][] temp2 = matrixMult44(temp, array);
		array = temp2;
	};



	public static Matrix3D translationMatrix(float tx, float ty, float tz)
	{
		float[][] temp = matrix44(	1, 0, 0, tx,
									0, 1, 0, ty,
									0, 0, 1, tz,
									0, 0, 0, 1);

		return new Matrix3D(temp);
	};



	public void scale(float sx, float sy, float sz)                // this = this * scale
	{
		float[][] temp = matrix44(	sx,  0,  0, 0,
									 0, sy,  0, 0,
									 0,  0, sz, 0,
									 0,  0,  0, 1);
		float[][] temp2 = matrixMult44(temp, array);
		array = temp2;
	};



	public static Matrix3D scalingMatrix(float sx, float sy, float sz)
	{
		float[][] temp = matrix44(	sx,  0,  0, 0,
									 0, sy,  0, 0,
									 0,  0, sz, 0,
									 0,  0,  0, 1);
		return new Matrix3D(temp);
	};

	

	public void skew(float kxy, float kxz, float kyz)              // this = this * skew
	{
		float[][] temp = matrix44(	1, kxy, kxz, 0,
									0,   1, kyz, 0,
									0,   0,   1, 0,
									0,   0,   0, 1);
		float[][] temp2 = matrixMult44(temp, array);
		array = temp2;
	};




	public static Matrix3D skewMatrix(float kxy, float kxz, float kyz)
	{
		float[][] temp = matrix44(	1, kxy, kxz, 0,
									0,   1, kyz, 0,
									0,   0,   1, 0,
									0,   0,   0, 1);
		return new Matrix3D(temp);
	};




	public void rotate(float ax, float ay, float az, float angle)  // this = this * rotate 
	{
		float scale = (float)(1/Math.sqrt(ax*ax+ay*ay+az*az));
		ax *= scale;
		ay *= scale;
		az *= scale;
		float v = (float) Math.sin(angle);	// v = sin(theta)
		float w = (float) Math.cos(angle);	// w = cos(theta)
		float u = 1-w;						// u = 1-cos(theta)
		float xy_u = ax * ay * u;
		float yz_u = ay * az * u;
		float xz_u = ax * az * u;
		float x_sina = ax * v;
		float y_sina = ay * v;
		float z_sina = az * v; 
		float [][] temp = matrix44(	ax*ax*u + w,	xy_u - z_sina,	xz_u + y_sina,	0,
									xy_u + z_sina,	ay*ay*u + w,	yz_u - x_sina,	0,
									xz_u - y_sina,	yz_u + x_sina,	az*az*u + w,	0,
									0,				0,				0,				1);
		float[][] temp2 = matrixMult44(temp, array);
		array = temp2;
	};




	public static Matrix3D rotationMatrix(float ax, float ay, float az, float angle)
	{
		float scale = (float)(1/Math.sqrt(ax*ax+ay*ay+az*az));
		ax *= scale;
		ay *= scale;
		az *= scale;
		float v = (float) Math.sin(angle);	// v = sin(theta)
		float w = (float) Math.cos(angle);	// w = cos(theta)
		float u = 1-w;						// u = 1-cos(theta)
		float xy_u = ax * ay * u;
		float yz_u = ay * az * u;
		float xz_u = ax * az * u;
		float x_sina = ax * v;
		float y_sina = ay * v;
		float z_sina = az * v; 
		float [][] temp = matrix44(	ax*ax*u + w,	xy_u - z_sina,	xz_u + y_sina,	0,
									xy_u + z_sina,	ay*ay*u + w,	yz_u - x_sina,	0,
									xz_u - y_sina,	yz_u + x_sina,	az*az*u + w,	0,
									0,				0,				0,				1);
		return new Matrix3D(temp);
	};



	public void lookAt(float eyex, float eyey, float eyez,
                           float atx,  float aty,  float atz,
                           float upx,  float upy,  float upz)          // this = this * lookat
	{
		Vector3 up = new Vector3 (upx,upy,upz);
		Vector3 eye = new Vector3 (eyex,eyey,eyez);

		Vector3 l = new Vector3 (atx-eyex , aty-eyey , atz-eyez);
		Vector3 r = l.cross(up);
		Vector3 u = r.cross(l);
		l.normalize();
		r.normalize();
		u.normalize();

		float[][] temp = matrix44(	r.x,	r.y,	r.z,	-r.dot(eye),
									u.x,	u.y,	u.z,	-u.dot(eye),
									-l.x,	-l.y,	-l.z,	l.dot(eye),
									0,		0,		0,		1 );
		float[][] temp2 = matrixMult44(temp, array);
		array = temp2;
	};



	public static Matrix3D lookAtMatrix(Vector3 eye, Vector3 at, Vector3 up)
	{
		Vector3 l = at.minus(eye);
		Vector3 r = l.cross(up);
		Vector3 u = r.cross(l);
		l.normalize();
		r.normalize();
		u.normalize();

		float[][] temp = matrix44(	r.x,	r.y,	r.z,	-r.dot(eye),
									u.x,	u.y,	u.z,	-u.dot(eye),
									-l.x,	-l.y,	-l.z,	l.dot(eye),
									0,		0,		0,		1 );
		return new Matrix3D(temp);
	};



	// 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)
	{
		float[][] temp = matrix44(	2*near/(right-left),	0,						-(right+left)/(right-left),	0,
									0,						2*near/(bottom-top),	-(bottom+top)/(bottom-top),	0,
									0,						0,						(far+near)/(far-near),		-2*far*near/(far-near),
									0,						0,						1,							0	);
		float[][] temp2 = matrixMult44(temp, array);
		array = temp2;
	};



	//same as perspective
	public void frustum(float left, float right,                // this = this * persp
                            float bottom, float top,
                            float near, float far)
	{
		float[][] temp = matrix44(	2*near/(right-left),	0,						-(right+left)/(right-left),	0,
									0,						2*near/(bottom-top),	-(bottom+top)/(bottom-top),	0,
									0,						0,						(far+near)/(far-near),		-2*far*near/(far-near),
									0,						0,						1,							0	);
		float[][] temp2 = matrixMult44(temp, array);
		array = temp2;
	};




	public static Matrix3D perspectiveMatrix(float left, float right,                // this = this * persp
                            float bottom, float top,
                            float near, float far)
	{
		float[][] temp = matrix44(	2*near/(right-left),	0,						-(right+left)/(right-left),	0,
									0,						2*near/(bottom-top),	-(bottom+top)/(bottom-top),	0,
									0,						0,						(far+near)/(far-near),		-2*far*near/(far-near),
									0,						0,						1,							0	);
		return new Matrix3D(temp);
	};




	public void orthographic(float left, float right,               // this = this * ortho
                             float bottom, float top,
                             float near, float far)
	{
		float[][] temp = matrix44(	2/(right-left),	0,				0,				-(right+left)/(right-left),
									0,				2/(bottom-top),	0,				-(bottom+top)/(bottom-top),
									0,				0,				2/(far-near),	-(far+near)/(far-near),
									0,				0,				0,				1);
		float[][] temp2 = matrixMult44(temp, array);
		array = temp2;
	};




	public static Matrix3D orthographicMatrix(float left, float right,               // this = this * ortho
                             float bottom, float top,
                             float near, float far)
	{
		float[][] temp = matrix44(	2/(right-left),	0,				0,				-(right+left)/(right-left),
									0,				2/(bottom-top),	0,				-(bottom+top)/(bottom-top),
									0,				0,				2/(far-near),	-(far+near)/(far-near),
									0,				0,				0,				1);
		return new Matrix3D(temp);
	};

	

	public String toString()
	{
		String s = "[\t"+array[0][0]+"\t"+array[0][1]+"\t"+array[0][2]+"\t"+array[0][3];
		s = s + "\n\t"+array[1][0]+"\t"+array[1][1]+"\t"+array[1][2]+"\t"+array[1][3];
		s = s + "\n\t"+array[2][0]+"\t"+array[2][1]+"\t"+array[2][2]+"\t"+array[2][3];
		s = s + "\n\t"+array[3][0]+"\t"+array[3][1]+"\t"+array[3][2]+"\t"+array[3][3]+"\t]\n";
		return s;
	};

	

	//Private Methods//////////////////////////////////////////////////
	
	
	private void matrixMultVector(float[][] m, Point3D in, Point3D out)
	{
		out.x = in.x * m[0][0] + in.y * m[0][1] + in.z * m[0][2] + m[0][3];
		out.y = in.x * m[1][0] + in.y * m[1][1] + in.z * m[1][2] + m[1][3];
		out.z = in.x * m[2][0] + in.y * m[2][1] + in.z * m[2][2] + m[2][3];
		float one_over_w = 1/(in.x * m[3][0] + in.y * m[3][1] + in.z * m[3][2] + m[3][3]);
		out.x *= one_over_w;
		out.y *= one_over_w;
		out.z *= one_over_w;
		out.wgt0 = one_over_w>0;
	};



	private static void matrixMultVector(float[][] m, Vertex3D in, Vertex3D out)
	{
		out.x = in.x * m[0][0] + in.y * m[0][1] + in.z * m[0][2] + m[0][3];
		out.y = in.x * m[1][0] + in.y * m[1][1] + in.z * m[1][2] + m[1][3];
		out.z = in.x * m[2][0] + in.y * m[2][1] + in.z * m[2][2] + m[2][3];
		float one_over_w = 1/(in.x * m[3][0] + in.y * m[3][1] + in.z * m[3][2] + m[3][3]);
		out.x *= one_over_w;
		out.y *= one_over_w;
		out.z *= one_over_w;
		out.wgt0 = one_over_w>0;
	};



	private static Point3D matrixMultVector(float[][] m, Point3D in)
	{
		Point3D out = new Point3D();
		out.x = in.x * m[0][0] + in.y * m[0][1] + in.z * m[0][2] + m[0][3];
		out.y = in.x * m[1][0] + in.y * m[1][1] + in.z * m[1][2] + m[1][3];
		out.z = in.x * m[2][0] + in.y * m[2][1] + in.z * m[2][2] + m[2][3];
		float one_over_w = 1/(in.x * m[3][0] + in.y * m[3][1] + in.z * m[3][2] + m[3][3]);
		out.x *= one_over_w;
		out.y *= one_over_w;
		out.z *= one_over_w;
		out.wgt0 = one_over_w>0;
		return out;
	};


	private Vertex3D matrixMultVector(float[][] m, Vertex3D in)
	{
		Vertex3D out = new Vertex3D(in);
		out.x = in.x * m[0][0] + in.y * m[0][1] + in.z * m[0][2] + m[0][3];
		out.y = in.x * m[1][0] + in.y * m[1][1] + in.z * m[1][2] + m[1][3];
		out.z = in.x * m[2][0] + in.y * m[2][1] + in.z * m[2][2] + m[2][3];
		float one_over_w = 1/(in.x * m[3][0] + in.y * m[3][1] + in.z * m[3][2] + m[3][3]);
		out.x *= one_over_w;
		out.y *= one_over_w;
		out.z *= one_over_w;
		out.wgt0 = one_over_w>0;
		return out;
	};



	private static Vector3 matrixMultVector(float[][] m, Vector3 in)
	{
		Vector3 out = new Vector3();
		out.x = in.x * m[0][0] + in.y * m[0][1] + in.z * m[0][2] + m[0][3];
		out.y = in.x * m[1][0] + in.y * m[1][1] + in.z * m[1][2] + m[1][3];
		out.z = in.x * m[2][0] + in.y * m[2][1] + in.z * m[2][2] + m[2][3];
		float one_over_w = 1/(in.x * m[3][0] + in.y * m[3][1] + in.z * m[3][2] + m[3][3]);
		out.x *= one_over_w;
		out.y *= one_over_w;
		out.z *= one_over_w;
		return out;
	};



	private static float[][] matrixMult44(float[][] a, float[][] b)
	{
		float[][] temp = new float[4][4];
		for (int i=0; i<4; i++)
			for (int j=0; j<4; j++)
				temp[i][j] = b[i][0] * a[0][j] + b[i][1] * a[1][j] + b[i][2] * a[2][j] + b[i][3] * a[3][j];
		return temp;
	};



	private static float[][] matrix44(	float a00, float a01, float a02, float a03,
										float a10, float a11, float a12, float a13,
										float a20, float a21, float a22, float a23,
										float a30, float a31, float a32, float a33)
	{
		float[][] temp = new float[4][4];
		temp[0][0] = a00; temp[0][1] = a01; temp[0][2] = a02; temp[0][3] = a03;
		temp[1][0] = a10; temp[1][1] = a11; temp[1][2] = a12; temp[1][3] = a13;
		temp[2][0] = a20; temp[2][1] = a21; temp[2][2] = a22; temp[2][3] = a23;
		temp[3][0] = a30; temp[3][1] = a31; temp[3][2] = a32; temp[3][3] = a33;
		return temp;
	};


}