/* ps4, 11/23/98
	Created by: Prof. McMillan
	Modified by: Andrew Galland
	Mod History:
		AJG001	changed transforms to work
	*/

import Vertex3D;

public class Matrix3D {
    private float m[];

    public Matrix3D()      // null constructor allows for extension
    {
        m = new float[16];
        loadIdentity();
    }

    public Matrix3D(ZRaster r)
    {
        m = new float[16];
        float w = r.width / 2;
        float h = r.height / 2;
        float d = ZRaster.MAXZ / 2;
        m[ 0] = w;  m[ 1] = 0;  m[ 2] = 0;  m[ 3] = w;
        m[ 4] = 0;  m[ 5] = h;  m[ 6] = 0;  m[ 7] = h;
        m[ 8] = 0;  m[ 9] = 0;  m[10] = d;  m[11] = d;
        m[12] = 0;  m[13] = 0;  m[14] = 0;  m[15] = 1;
    }

    public Matrix3D(Matrix3D copy)    // makes a copy of the matrix
    {
        m = new float[16];
        System.arraycopy(copy, 0, m, 0, 16);
    }


    /*
        ... Methods for setting and getting matrix elements ...
    */
    public void set(int j, int i, float val)
    {
        m[4*j+i] = val;
    }

    public float get(int j, int i)
    {
        return m[4*j+i];
    }

    protected void set(int i, float val)
    {
        m[i] = val;
    }
    
    protected float get(int i)
    {
        return m[i];
    }


    public final void copy(Matrix3D src)
    {
        System.arraycopy(src, 0, m, 0, 16);
    }

    public void transform(Vertex3D in[], Vertex3D out[], int vertices)
    {
		/* AJG001+ */
        for (int i = 0; i < vertices; i++) {
			float x,y,z,w;
			x = in[i].x;
			y = in[i].y;
			z = in[i].z;
			w = m[12]*x+
				m[13]*y+
				m[14]*z+
				m[15];
			out[i].x = (m[0]*x+
				m[1]*y+
				m[2]*z+
				m[3])/w;
			out[i].y = (m[4]*x+
				m[5]*y+
				m[6]*z+
				m[7])/w;
			out[i].z = (m[8]*x+
				m[9]*y+
				m[10]*z+
				m[11])/w;

			if (false) {
				//this implements transforming normal.
				//i put it in an if (false) because it
				//would only slow down my already slow
				//implementation of everything and
				//isn't needed for my implementation

				//I did this last.  I don't think it is quite right.
				//matlab does all my matrix inverses for me.  :-)

				//this is gauss-jordan elimation from the 1.00 lecture notes
				//for this year.
				int neqn = 3; 
				float pivot, m;
				int j,k; 
				float[][] work = new float[neqn][neqn]; // working matrix 
				for(i=0; i<neqn-1; i++) { 
					for(j=0; j< neqn; j++)  // copy a into working matrix 
						work[i][j] = get(i, j); 
				} 

				// triangularize the matrix of coefficients 
				for(i=0; i<neqn-1; i++) { 
					pivot = work[i][i]; 
					for(j=i+1; j<neqn; j++) { 
						m = work[j][i]/pivot; 
						for(k=i+1; k<=neqn; k++) 
							work[j][k]= work[j][k] - work[i][k]*m; 
					}
				}

				// at this point we have the inverse of the matrix
				// i think.  I haven't done gauss-jordan in a while.
				// any way, assuming we do, now we transpose and 
				// multiply

				out[i].nx = work[0][0] * in[i].nx +
							work[1][0] * in[i].ny +
							work[2][0] * in[i].nz;
				out[i].ny = work[0][1] * in[i].nx +
							work[1][1] * in[i].ny +
							work[2][1] * in[i].nz;
				out[i].nz = work[0][2] * in[i].nx +
							work[1][2] * in[i].ny +
							work[2][2] * in[i].nz;
			}
        }
		/* AJG001- */
    }

    public Vertex3D transform(Vertex3D v)
    {
		/* AJG001+ */
		Vertex3D result = new Vertex3D();
		float x,y,z,w;
		x = v.x;
		y = v.y;
		z = v.z;
		w = m[12]*x+
			m[13]*y+
			m[14]*z+
			m[15];
		result.x = (m[0]*x+
			m[1]*y+
			m[2]*z+
			m[3])/w;
		result.y = (m[4]*x+
			m[5]*y+
			m[6]*z+
			m[7])/w;
		result.z = (m[8]*x+
			m[9]*y+
			m[10]*z+
			m[11])/w;
		/* AJG001- */
        
        return result;
    }

    public final void compose(Matrix3D s)
    {
        float t0, t1, t2, t3;
        for (int i = 0; i < 16; i += 4) {
            t0 = m[i  ];
            t1 = m[i+1];
            t2 = m[i+2];
            t3 = m[i+3];
            m[i  ] = t0*s.get(0) + t1*s.get(4) + t2*s.get( 8) + t3*s.get(12);
            m[i+1] = t0*s.get(1) + t1*s.get(5) + t2*s.get( 9) + t3*s.get(13);
            m[i+2] = t0*s.get(2) + t1*s.get(6) + t2*s.get(10) + t3*s.get(14);
            m[i+3] = t0*s.get(3) + t1*s.get(7) + t2*s.get(11) + t3*s.get(15);
        }
    }

    public void loadIdentity()
    {
        for (int i = 0; i < 16; i++)
            if ((i >> 2) == (i & 3))
                m[i] = 1;
            else
                m[i] = 0;
    }

    public void translate(float tx, float ty, float tz)
    {
        m[ 3] += m[ 0]*tx + m[ 1]*ty + m[ 2]*tz;
        m[ 7] += m[ 4]*tx + m[ 5]*ty + m[ 6]*tz;
        m[11] += m[ 8]*tx + m[ 9]*ty + m[10]*tz;
        m[15] += m[12]*tx + m[13]*ty + m[14]*tz;
    }

    public void scale(float sx, float sy, float sz)
    {
        m[ 0] *= sx; m[ 1] *= sy; m[ 2] *= sz;
        m[ 4] *= sx; m[ 5] *= sy; m[ 6] *= sz;
        m[ 8] *= sx; m[ 9] *= sy; m[10] *= sz;
        m[12] *= sx; m[13] *= sy; m[14] *= sz;
    }

    public void rotate(float ax, float ay, float az, float angle)
    {
        float t0, t1, t2;

        if (angle == 0) return;          // return with m unmodified

        t0 = ax*ax + ay*ay + az*az;
        if (t0 == 0) return;

        float cosx = (float) Math.cos(angle);
        float sinx = (float) Math.sin(angle);
        t0 = 1f / ((float) Math.sqrt(t0));
        ax *= t0;
        ay *= t0;
        az *= t0;
        t0 = 1f - cosx;

        float r11 = ax*ax*t0 + cosx;
        float r22 = ay*ay*t0 + cosx;
        float r33 = az*az*t0 + cosx;

        t1 = ax*ay*t0;
        t2 = az*sinx;
        float r12 = t1 - t2;
        float r21 = t1 + t2;

        t1 = ax*az*t0;
        t2 = ay*sinx;
        float r13 = t1 + t2;
        float r31 = t1 - t2;

        t1 = ay*az*t0;
        t2 = ax*sinx;
        float r23 = t1 - t2;
        float r32 = t1 + t2;

        for (int i = 0; i < 16; i += 4) {
            t0 = m[i];
            t1 = m[i+1];
            t2 = m[i+2];
            m[i  ] = t0*r11 + t1*r21 + t2*r31;
            m[i+1] = t0*r12 + t1*r22 + t2*r32;
            m[i+2] = t0*r13 + t1*r23 + t2*r33;
        }
    }

    public void lookAt(float eyex, float eyey, float eyez,
                       float atx,  float aty,  float atz,
                       float upx,  float upy,  float upz)
    {
        float t0, t1, t2;

        /*
            .... a unit vector along the line of sight ....
        */
        atx -= eyex;
        aty -= eyey;
        atz -= eyez;

        t0 = atx*atx + aty*aty + atz*atz;
        if (t0 == 0) return;                // at and eye at same point
        t0 = (float) (1 / Math.sqrt(t0));
        atx *= t0;
        aty *= t0;
        atz *= t0;

        /*
            .... a unit vector to the right ....
        */
        float rightx, righty, rightz;
        rightx = aty*upz - atz*upy;
        righty = atz*upx - atx*upz;
        rightz = atx*upy - aty*upx;
        t0 = rightx*rightx + righty*righty + rightz*rightz;
        if (t0 == 0) return;                // up is the same as at
        t0 = (float) (1 / Math.sqrt(t0));
        rightx *= t0;
        righty *= t0;
        rightz *= t0;


        /*
            .... a unit up vector ....
        */
        upx = righty*atz - rightz*aty;
        upy = rightz*atx - rightx*atz;
        upz = rightx*aty - righty*atx;


        /*
            .... find camera translation ....
        */
        float tx, ty, tz;
        tx = rightx*eyex + righty*eyey + rightz*eyez;
        ty = upx*eyex + upy*eyey + upz*eyez;
        tz = atx*eyex + aty*eyey + atz*eyez;

        /*
            .... do transform ....
        */
        for (int i = 0; i < 16; i += 4) {
            t0 = m[i];
            t1 = m[i+1];
            t2 = m[i+2];
            m[i  ] = t0*rightx + t1*upx - t2*atx;
            m[i+1] = t0*righty + t1*upy - t2*aty;
            m[i+2] = t0*rightz + t1*upz - t2*atz;
            m[i+3] -= t0*tx + t1*ty - t2*tz;
        }
    }

    public void perspective(float left, float right,
                            float bottom, float top,
                            float near, float far)
    {
        float t0, t1, t2, t3;

        t0 = 1f / (right - left);
        t1 = 1f / (bottom - top);
        t2 = 1f / (far - near);

        float m13 = -t0*(right + left);
        float m23 = -t1*(bottom + top);
        float m33 = t2*(far + near);

        near *= 2;
        float m11 = t0*near;
        float m22 = t1*near;
        float m34 = -t2*far*near;

        for (int i = 0; i < 16; i += 4) {
            t0 = m[i];
            t1 = m[i+1];
            t2 = m[i+2];
            m[i  ] = t0*m11;
            m[i+1] = t1*m22;
            m[i+2] = t0*m13 + t1*m23 + t2*m33 + m[i+3];
            m[i+3] = t2*m34;
        }
    }
    
    public void orthographic(float left, float right,
                             float bottom, float top,
                             float near, float far)
    {
        float t0, t1, t2, t3;

        t0 = 1f / (right - left);
        t1 = 1f / (bottom - top);
        t2 = 1f / (far - near);

        float m11 = 2*t0;
        float m22 = 2*t1;
        float m33 = 2*t2;
        float m14 = -t0*(right + left);
        float m24 = -t1*(bottom + top);
        float m34 = -t2*(far + near);

        for (int i = 0; i < 16; i += 4) {
            t0 = m[i];
            t1 = m[i+1];
            t2 = m[i+2];
            m[i  ] = t0*m11;
            m[i+1] = t1*m22;
            m[i+2] = t2*m33;
            m[i+3] = t0*m14 + t1*m24 + t2*m34 + m[i+3];
        }
    }

    public String toString()
    {
        return ("[ ["+m[ 0]+", "+m[ 1]+", "+m[ 2]+", "+m[ 3]+" ], ["+
                      m[ 4]+", "+m[ 5]+", "+m[ 6]+", "+m[ 7]+" ], ["+
                      m[ 8]+", "+m[ 9]+", "+m[10]+", "+m[11]+" ], ["+
                      m[12]+", "+m[13]+", "+m[14]+", "+m[15]+" ] ]");
    }
}