/*
 * pMatrix.java		v0.01	May 16th, 1998
 *
 * A general purpose (rotation and translation) matrix.
 * (05/16/1998,05/17/1998,4/2005)
 *
 * Copyright(c) 1998, Alex S, Particle
 */

import java.lang.String;
import java.util.Vector;

/**
 * pMatrix class, to handle all the matrix thingies.
 * the class represents a 4x4 matrix, with some 
 * useful operations defined on it.
 *
 * Note: This thing is Right Handed!; just like OpenGL :-)
 */
public class pMatrix {

    /**
     * the matrix stored as 2D array.
     */
    //protected float[] matrix;
    public float m00,m01,m02,m03,
                    m10,m11,m12,m13,
                    m20,m21,m22,m23,
                    m30,m31,m32,m33;

    /**
     * the stack to hold working matrices.
     */
    protected static Vector stack;

    /**
     * get the stack going...
     */
    static{
        stack = new Vector();
    }

    /**
     * default constructor, initializes the matrix, 
     * and makes sure it's an "intentity" matrix.
     */
    public pMatrix(){
        //matrix = new float[16];
        ident();
    }

    /**
     * create a matrix from a 16 element array
     */
    public pMatrix(float[] m){
        //matrix = new float[16];        
        //System.arraycopy(m,0,matrix, 0, 16);        
        m00=m[0];   m01=m[1];   m02=m[2];   m03=m[3];
        m10=m[4];   m11=m[5];   m12=m[6];   m13=m[7];
        m20=m[8];   m21=m[9];   m22=m[10];  m23=m[11];
        m30=m[12];  m31=m[13];  m32=m[14];  m33=m[15];
    }

    /**
     * a copy constructor.
     *
     * @param m The matrix to copy.
     */
    public pMatrix(pMatrix m){
        //matrix = new float[16];
        //System.arraycopy(m.matrix, 0, matrix, 0, 16);
        m00=m.m00; m01=m.m01; m02=m.m02; m03=m.m03;
        m10=m.m10; m11=m.m11; m12=m.m12; m13=m.m13;
        m20=m.m20; m21=m.m21; m22=m.m22; m23=m.m23;
        m30=m.m30; m31=m.m31; m32=m.m32; m33=m.m33;
    }

    /**
     * this matrix equals another matrix.
     */
    public pMatrix assign(pMatrix m){        
        m00=m.m00; m01=m.m01; m02=m.m02; m03=m.m03;
        m10=m.m10; m11=m.m11; m12=m.m12; m13=m.m13;
        m20=m.m20; m21=m.m21; m22=m.m22; m23=m.m23;
        m30=m.m30; m31=m.m31; m32=m.m32; m33=m.m33;
        return this;
    }
 
    public void copyInto(float[] m){
        //matrix = new float[16];        
        //System.arraycopy(m,0,matrix, 0, 16);        
        m[0]=m00;   m[1]=m01;   m[2]=m02;   m[3]=m03;
        m[4]=m10;   m[5]=m11;   m[6]=m12;   m[7]=m13;
        m[8]=m20;   m[9]=m21;   m[10]=m22;  m[11]=m23;
        m[12]=m30;  m[13]=m31;  m[14]=m32;  m[15]=m33;
    }

    /**
     * pushes the current matrix onto the stack, 
     * (the current matrix is still there though)
     */
    public void push(){
        pMatrix tmp = new pMatrix(this);
        stack.addElement(tmp);
    }

    /**
     * pops the last pushed matrix from the stack,
     * and makes it current. (the previous one is 
     * erased)
     * <p>
     * NOTE: no error checking is performed, you WILL
     * get a NoSuchElementException if you're not careful
     * and try to pop an empty stack.
     *
     * @return The freshly poped matrix.
     */
    public pMatrix pop(){
        assign((pMatrix)stack.lastElement());
        stack.removeElementAt(stack.size()-1);
        return this;
    }

    /**
     * makes this matrix into an identity matrix.
     * (current info of the matrix is erased)
     *
     * @return Current identity matrix.
     */
    public pMatrix ident(){
        /*
        for (int i=0;i<4;i++)
            for (int j=0;j<4;j++)
                matrix[(i<<2)+j] = i == j ? 1:0;
        */        
        m00=1; m01=0; m02=0; m03=0;
        m10=0; m11=1; m12=0; m13=0;
        m20=0; m21=0; m22=1; m23=0;
        m30=0; m31=0; m32=0; m33=1;
        return this;
    }

    /**
     * add another matrix to this one.
     * (changes current matrix)
     *
     * @param m The matrix to add.
     * @return The current changed matrix.
     */
    public pMatrix add(pMatrix m){
        /*
        for (int i=0;i<16;i++)
            matrix[i] += m.matrix[i];
        */
        
        m00+=m.m00; m01+=m.m01; m02+=m.m02; m03+=m.m03;
        m10+=m.m10; m11+=m.m11; m12+=m.m12; m13+=m.m13;
        m20+=m.m20; m21+=m.m21; m22+=m.m22; m23+=m.m23;
        m30+=m.m30; m31+=m.m31; m32+=m.m32; m33+=m.m33;
        return this;
    }

    /**
     * subtract a matrix from this one
     * (changes the current matrix)
     *
     * @param m The matrix to subtract.
     * @return The current changed matrix.
     */
    public pMatrix sub(pMatrix m){
        /*
        for (int i=0;i<16;i++)
            matrix[i] -= m.matrix[i];    
        */        
        m00-=m.m00; m01-=m.m01; m02-=m.m02; m03-=m.m03;
        m10-=m.m10; m11-=m.m11; m12-=m.m12; m13-=m.m13;
        m20-=m.m20; m21-=m.m21; m22-=m.m22; m23-=m.m23;
        m30-=m.m30; m31-=m.m31; m32-=m.m32; m33-=m.m33;
        return this;
    }

    /**
     * get function to the rest of the world.
     *
     * @param i The column.
     * @param j The row.
     * @return The location of that row and column.
     */
    /*
    public float get(int i,int j){
        //return matrix[(i<<2)+j];
    }
    */

    /**
     * set function for the rest of the world.
     *
     * @param i The column.
     * @param j The row.
     * @param v The value of the new location.
     */
    /*
    public void set(int i,int j,float v){
        //matrix[(i<<2)+j] = v;
    }
    */

    /**
     * function to multiply this matrix by another.
     * (the result is a cross multiply of this matrix
     * by the parameter matrix.)
     * (current matrix changes!)
     *
     * @param m The matrix to multiply by.
     * @return The current changed matrix.
     */
    public pMatrix mult(pMatrix m){
        /*
        pMatrix tmp = new pMatrix(this);
        for (int i=0;i<4;i++)
            for (int j=0;j<4;j++) {
                matrix[(i<<2)+j] = 0;
                for (int k=0;k<4;k++)
                    matrix[(i<<2)+j] += tmp.matrix[(i<<2)+k] * m.matrix[(k<<2)+j];
            }
        */
        pMatrix tmp = new pMatrix(this);
        m00=tmp.m00*m.m00+tmp.m01*m.m10+tmp.m02*m.m20+tmp.m03*m.m30;
        m01=tmp.m00*m.m01+tmp.m01*m.m11+tmp.m02*m.m21+tmp.m03*m.m31;
        m02=tmp.m00*m.m02+tmp.m01*m.m12+tmp.m02*m.m22+tmp.m03*m.m32;
        m03=tmp.m00*m.m03+tmp.m01*m.m13+tmp.m02*m.m23+tmp.m03*m.m33;
        m10=tmp.m10*m.m00+tmp.m11*m.m10+tmp.m12*m.m20+tmp.m13*m.m30;
        m11=tmp.m10*m.m01+tmp.m11*m.m11+tmp.m12*m.m21+tmp.m13*m.m31;
        m12=tmp.m10*m.m02+tmp.m11*m.m12+tmp.m12*m.m22+tmp.m13*m.m32;
        m13=tmp.m10*m.m03+tmp.m11*m.m13+tmp.m12*m.m23+tmp.m13*m.m33;
        m20=tmp.m20*m.m00+tmp.m21*m.m10+tmp.m22*m.m20+tmp.m23*m.m30;
        m21=tmp.m20*m.m01+tmp.m21*m.m11+tmp.m22*m.m21+tmp.m23*m.m31;
        m22=tmp.m20*m.m02+tmp.m21*m.m12+tmp.m22*m.m22+tmp.m23*m.m32;
        m23=tmp.m20*m.m03+tmp.m21*m.m13+tmp.m22*m.m23+tmp.m23*m.m33;
        m30=tmp.m30*m.m00+tmp.m31*m.m10+tmp.m32*m.m20+tmp.m33*m.m30;
        m31=tmp.m30*m.m01+tmp.m31*m.m11+tmp.m32*m.m21+tmp.m33*m.m31;
        m32=tmp.m30*m.m02+tmp.m31*m.m12+tmp.m32*m.m22+tmp.m33*m.m32;
        m33=tmp.m30*m.m03+tmp.m31*m.m13+tmp.m32*m.m23+tmp.m33*m.m33;
        return this;
    }

    /**
     * function to transform a vector, 
     * (a vector consists of a 4 element array of floats,
     * the last element of the array is reserved)
     * 
     * @param v The vector to transform.
     * @return The transformed vector.
     */
    public float[] mult(float[] v){
        float tmp0,tmp1,tmp2,tmp3;
        tmp0 = v[0]; tmp1=v[1]; tmp2=v[2]; tmp3=v[3];
        /*        
        for (i=0;i<4;i++) {
            v[i] = 0;
            for (int j=0;j<4;j++)
                v[i] += matrix[(i<<2)+j] * tmp[j];
        }
        */
        v[0] = m00*tmp0+m01*tmp1+m02*tmp2+m03*tmp3;
        v[1] = m10*tmp0+m11*tmp1+m12*tmp2+m13*tmp3;
        v[2] = m20*tmp0+m21*tmp1+m22*tmp2+m23*tmp3;
        v[3] = m30*tmp0+m31*tmp1+m32*tmp2+m33*tmp3;
        return v;
    }

    /**
     * function to transform an array of vectors...
     *
     * @param a The array of arrays of vectors.
     * @return The transformed array...
     */
    public float[][] mult(float[][] a){
        for (int i=0;i<a.length;i++)
            mult(a[i]);
        return a;
    }

    /**
     * rotate this matrix around the X axis.
     * (chances current matrix)
     * Note: change sin term signs to change handedness.
     *
     * @param a The angle to rotate by.
     * @return The new rotated matrix.
     */
    public pMatrix rotatex(float a){
        /*
        pMatrix tmp = new pMatrix();
        float cos = (float)Math.cos(a);
        float sin = (float)Math.sin(a);
        tmp.matrix[(1<<2)+1] = cos;
        tmp.matrix[(1<<2)+2] = sin;
        tmp.matrix[(2<<2)+1] = -sin;
        tmp.matrix[(2<<2)+2] = cos;
        return mult(tmp);
        */
        float t10,t11,t12,t20,t21,t22;
        float cos = (float)Math.cos(a);
        float sin = (float)Math.sin(a);
        t10 = cos * m10 + sin * m20;
        t11 = cos * m11 + sin * m21;
        t12 = cos * m12 + sin * m22;
        t20 = -sin * m10 + cos * m20;
        t21 = -sin * m11 + cos * m21;
        t22 = -sin * m12 + cos * m22;
        m10 = t10; m11 = t11;
        m12 = t12; m20 = t20;
        m21 = t21; m22 = t22;
        return this;
    }

    /**
     * rotate this matrix around the Y axis.
     * (chances current matrix)
     *
     * @param a The angle to rotate by.
     * @return The new rotated matrix.
     */
    public pMatrix rotatey(float a){
        /*
        pMatrix tmp = new pMatrix();
        float cos = (float)Math.cos(a);
        float sin = (float)Math.sin(a);
        tmp.matrix[(0<<2)+0] = cos;
        tmp.matrix[(0<<2)+2] = sin;
        tmp.matrix[(2<<2)+0] = -sin;
        tmp.matrix[(2<<2)+2] = cos;
        return mult(tmp);
        */
        float t00,t01,t02,t20,t21,t22;
        float cos = (float)Math.cos(a);
        float sin = (float)Math.sin(a);
        t00 = cos * m00 + sin * m20;
        t01 = cos * m01 + sin * m21;
        t02 = cos * m02 + sin * m22;
        t20 = -sin * m00 + cos * m20;
        t21 = -sin * m01 + cos * m21;
        t22 = -sin * m02 + cos * m22;
        m00 = t00; m01 = t01;
        m02 = t02; m20 = t20;
        m21 = t21; m22 = t22;
        return this;
    }

    /**
     * rotate this matrix around the Z axis.
     * (chances current matrix)
     *
     * @param a The angle to rotate by.
     * @return The new rotated matrix.
     */
    public pMatrix rotatez(float a){
        /*
        pMatrix tmp = new pMatrix();
        float cos = (float)Math.cos(a);
        float sin = (float)Math.sin(a);
        tmp.matrix[(0<<2)+0] = cos;
        tmp.matrix[(0<<2)+1] = -sin;
        tmp.matrix[(1<<2)+0] = sin;
        tmp.matrix[(1<<2)+1] = cos;
        return mult(tmp);
        */
        float t00,t01,t02,t10,t11,t12;
        float cos = (float)Math.cos(a);
        float sin = (float)Math.sin(a);
        t00 = cos * m00 + -sin * m10;
        t01 = cos * m01 + -sin * m11;
        t02 = cos * m02 + -sin * m12;
        t10 = sin * m00 + cos * m10;
        t11 = sin * m01 + cos * m11;
        t12 = sin * m02 + cos * m12;
        m00 = t00; m01 = t01;
        m02 = t02; m10 = t10;
        m11 = t11; m12 = t12;
        return this;
    }

    /**
     * translate the current matrix by that amount.
     * (changes the current matrix)
     *
     * @param x The x value to translate.
     * @param y The y value to translate.
     * @param z The z value to translate.
     * @return The translated matrix.
     */
    public pMatrix translate(float x,float y,float z){
        pMatrix tmp = new pMatrix();
        tmp.m03 = x;
        tmp.m13 = y;
        tmp.m23 = z;
        return mult(tmp);
    }

    /**
     * translate the current matrix by a 4 element vector.
     * (changes the current matrix)
     *
     * @param v The 4 element vector to translate by.
     * @return The translated matrix.
     */
    public pMatrix translate(float[] v){
        pMatrix tmp = new pMatrix();
        tmp.m03 = v[0];
        tmp.m13 = v[1];
        tmp.m23 = v[2];
        return mult(tmp);
    }

    /**
     * scales the matrix.
     * (changes the current matrix)
     *
     * @param s The scale to apply.
     * @return The scaled matrix.
     */
    public pMatrix scale(float s){
        pMatrix tmp = new pMatrix();
        tmp.m00 = s;
        tmp.m11 = s;
        tmp.m22 = s;
        return mult(tmp);
    }


    /**
     * scales the matrix.
     * (changes the current matrix)
     *
     * @param s The scale to apply.
     * @return The scaled matrix.
     */
    public pMatrix scale(float x,float y,float z){
        pMatrix tmp = new pMatrix();
        tmp.m00 = x;
        tmp.m11 = y;
        tmp.m22 = z;
        return mult(tmp);
    }


    /**
     * scales a matrix in relation to a point
     * (changes the curernt matrix)
     *
     * @param s The scale.
     * @param v The center of point where to scale.
     * @return The scaled matrrix.
     */
    public pMatrix scale(float s,float[] v){
        pMatrix tmp = new pMatrix();
        tmp.m00 = s;
        tmp.m03 = (1 - s) * v[0];
        tmp.m11 = s;
        tmp.m13 = (1 - s) * v[1];
        tmp.m22 = s;
        tmp.m23 = (1 - s) * v[2];
        return mult(tmp);
    }

    /**
     * scales a matrix in relation to a point
     * (changes the curernt matrix)
     *
     * @param s The scales (one for each coord).
     * @param v The center of point where to scale.
     * @return The scaled matrrix.
     */
    public pMatrix scale(float[] s,float[] v){
        pMatrix tmp = new pMatrix();
        tmp.m00 = s[0];
        tmp.m03 = (1 - s[0]) * v[0];
        tmp.m11 = s[1];
        tmp.m13 = (1 - s[1]) * v[1];
        tmp.m22 = s[2];
        tmp.m23 = (1 - s[2]) * v[2];
        return mult(tmp);
    }

    /**
     * reflect in the X axis.
     *
     * @return The refelected matrix.
     */
    public pMatrix reflectx(){
        pMatrix tmp = new pMatrix();
        tmp.m00 = -tmp.m00;
        return mult(tmp);
    }

    /**
     * reflect in the Y axis.
     *
     * @return The refelected matrix.
     */
    public pMatrix reflecty(){
        pMatrix tmp = new pMatrix();
        tmp.m11 = -tmp.m11;
        return mult(tmp);
    }

    /**
     * reflect in the Z axis.
     *
     * also known as conversion from right handled
     * to left handled coord system, or vice versa.
     *
     * @return The refelected matrix.
     */
    public pMatrix reflectz(){
        pMatrix tmp = new pMatrix();
        tmp.m22 = -tmp.m22;
        return mult(tmp);
    }


    /** 
     * create perspective matrix; turns current matrix into 
     * perspective matrix.
     *
     * @param zrpr at what z is the eye?
     * @param zvp at what z is the view plane?
     */
    public pMatrix perspective(float zprp,float zvp){
        float t2,t3;
        float d1 = zprp - zvp;
        float a = -zvp/d1;
        float b = -1/d1;
        float c = zvp*(zprp/d1);
        float d = zvp/d1;
            
        t2 = m02;
        t3 = m03;
        m02 = t2*a + t3*b;
        m03 = t2*c + t3*d;
            
        t2 = m12;
        t3 = m13;
        m12 = t2*a + t3*b;
        m13 = t2*c + t3*d;
            
        t2 = m22;
        t3 = m23;
        m22 = t2*a + t3*b;
        m23 = t2*c + t3*d;
            
        t2 = m32;
        t3 = m33;
        m32 = t2*a + t3*b;
        m33 = t2*c + t3*d;

        return this;
    }

    /**
     * standard toString method to allow for 
     * conherent printing of this object.
     *
     * @return The String object representing this matrix.
     */
    public String toString(){
        String s = new String("pMatrix:\n");
        /*
        for (int i=0;i<4;i++) {
            for (int j=0;j<4;j++) {
                s += matrix[(i<<2)+j]+", ";
            }
            s += "\n";
        }
        */

        s+= m00+","+m01+","+m02+","+m03+"\n"+
            m10+","+m11+","+m12+","+m13+"\n"+
            m20+","+m21+","+m22+","+m23+"\n"+
            m30+","+m31+","+m32+","+m33+"\n";

        return s;
    }

    /**
     * returns the inverse of the current matrix
     */
    public pMatrix inverse(){
        float[] a = new float[16];
        float[] b = new float[16];
        copyInto(b);
        if(m4_inverse(a,b)){
            return new pMatrix(a);
        }
        return new pMatrix();
    }


    /**
     * various matrix operations (inverse). taken from:
     * http://skal.planet-d.net/demo/matrixfaq.htm
     */

    /**
     * compute determinant of a 3x3 matrix
     */
    private static float m3_det(float[] mat){
        float det;
        det = mat[0] * ( mat[4]*mat[8] - mat[7]*mat[5] )
                - mat[1] * ( mat[3]*mat[8] - mat[6]*mat[5] )
                + mat[2] * ( mat[3]*mat[7] - mat[6]*mat[4] );
        return det;
    }

    private static boolean m3_inverse(float[] mr,float[] ma){
        float det = m3_det( ma );
        if ( Math.abs( det ) < (float)0.0005 ) {            
            //m3_identity( mr );
            return false;
        }
        mr[0] =    ma[4]*ma[8] - ma[5]*ma[7]   / det;
        mr[1] = -( ma[1]*ma[8] - ma[7]*ma[2] ) / det;
        mr[2] =    ma[1]*ma[5] - ma[4]*ma[2]   / det;
        mr[3] = -( ma[3]*ma[8] - ma[5]*ma[6] ) / det;
        mr[4] =    ma[0]*ma[8] - ma[6]*ma[2]   / det;
        mr[5] = -( ma[0]*ma[5] - ma[3]*ma[2] ) / det;
        mr[6] =    ma[3]*ma[7] - ma[6]*ma[4]   / det;
        mr[7] = -( ma[0]*ma[7] - ma[6]*ma[1] ) / det;
        mr[8] =    ma[0]*ma[4] - ma[1]*ma[3]   / det;
        return true;
    }

    /**
     * mr is 4x4, mb is 3x4
     */
    private static void m4_submat(float[] mr,float[] mb, int i, int j ) {
        int di, dj, si, sj;
        // loop through 3x3 submatrix
        for( di = 0; di < 3; di ++ ) {
            for( dj = 0; dj < 3; dj ++ ) {
                // map 3x3 element (destination) to 4x4 element (source)
                si = di + ( ( di >= i ) ? 1 : 0 );
                sj = dj + ( ( dj >= j ) ? 1 : 0 );
                // copy element
                mb[di * 3 + dj] = mr[si * 4 + sj];
            }
        }
    }

    private static float m4_det(float[] mr) {
        float det, result = 0, i = 1;
        float[] msub3 = new float[3*3];
        int     n;
        for ( n = 0; n < 4; n++, i *= -1 ){
            m4_submat( mr, msub3, 0, n );
            det     = m3_det( msub3 );
            result += mr[n] * det * i;
        }
        return result;
    }

    private static boolean m4_inverse(float[] mr, float[] ma ){
        float mdet = m4_det( ma );
        float[] mtemp = new float[3*3];
        int     i, j, sign;
        if (Math.abs( mdet ) < (float)0.0005 ){
            // m4_identity( mr );
            return false;
        }
        for ( i = 0; i < 4; i++ )
            for ( j = 0; j < 4; j++ ){
                sign = 1 - ( (i +j) % 2 ) * 2;
                m4_submat( ma, mtemp, i, j );
                mr[i+j*4] = ( m3_det( mtemp ) * sign ) / mdet;
            }
        return true;
    }

}