/*
 * pMatrix.java		v0.02	May 16th, 1998
 *
 * A general purpose matrix, with 3D thingies, 
 * LU decomposition, inverse, linear system solver,
 * 
 * (05/16/1998,05/17/1998,4/2005, 5/2008, 6/2008)
 *
 * Copyright(c) 1998-2008, Alex S, Particle
 */

// import java.text.*;

/*
// things to add: 
median filter
detect edges
draw line
fill polygon

weighted mean
gaussian function
convolve (kernel)
variance
standard deviation
entropy
min
max
quaternions
svd
dct/idct
dwt/idwt
zigzag
non-negative matrix factorization
sort (use 0 location as index)
percentile
median
*/


/**
 * pMatrix class, to handle all the matrix thingies.
 *
 * The 3d operations assume a 4x4 matrix.
 * Note: This thing is Right Handed!; just like OpenGL :-)
 */
public class pMatrix {

    public int rows,cols;
    
    private int pitch;
    private double[] arr;

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

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

    /**
     * create new matrix of size: Rows x Cols
     */
    public pMatrix(int r,int c){
        rows = r <= 0 ? 1 : r;
        cols = c <= 0 ? 1 : c;
        pitch = (int)( Math.ceil(Math.log(cols)/Math.log(2)) );
        arr = new double[rows * (1<<pitch)];
    }

    /**
     * init matrix from array
     *
     * col is whether this array needs to become a column matrix.
     */
    public pMatrix(double[] arr,boolean col){
        this.rows = col ? arr.length : 1;
        this.cols = col ? 1 : arr.length;
        pitch = (int)( Math.ceil(Math.log(cols)/Math.log(2)) );
        arr = new double[rows * (1<<pitch)];
        System.arraycopy(arr,0,this.arr,0,arr.length);        
    }

    /**
     * make a row matrix out of an array.
     */
    public pMatrix(double[] arr){
        this(arr,false);
    }

    /**
     * return matrix of new size; with old data.
     */
    public pMatrix resize(int r,int c){
        pMatrix n = new pMatrix(r,c);
        r = r < rows ? r : rows;
        c = c < cols ? c : cols;
        for(int i=0;i<r;i++)
            for(int j=0;j<c;j++)
                n.set(i,j,get(i,j));
        return n;
    }

    /**
     * pushes the current matrix onto the stack,
     * (the current matrix is still there though)
     */
    public void push(){
        stack.addElement(dup());
    }

    /**
     * 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(){
        pMatrix p = (pMatrix)stack.lastElement();
        stack.removeElement(p);
        rows = p.rows;
        cols = p.cols;
        pitch = p.pitch;
        arr = p.arr;
        return this;
    }

    /**
     * efficient duplication
     */
    public pMatrix dup(){
        pMatrix v = new pMatrix(rows,cols);
        System.arraycopy(arr,0,v.arr,0,arr.length);
        return v;
    }

    /**
     * make identity matrix.
     */
    public static pMatrix I(int rows,int cols){
        pMatrix v = new pMatrix(rows,cols);        
        for(int i=0;i<rows;i++){
            for(int j=0;j<cols;j++){
                v.arr[(i << v.pitch) + j] = i == j ? 1.0 : 0.0;
            }
        }
        return v;
    }

    /**
     * make identity (I) matrix of same size
     */
    public pMatrix I(){
        return I(rows,cols);
    }

    /**
     * set all to a constant
     */
    public void set(double v){
        for(int i=0;i<arr.length;i++)
            arr[i] = v;
    }

    /**
     * set i-row, j-column value.
     */
    public void set(int i,int j,double v){
        arr[(i << pitch) + j] = v;
    }

    /**
     * get i-row, j-column value
     */
    public double get(int i,int j){
        return arr[(i << pitch) + j];
    }

    /**
     * get sub matrix at i-row, j-column 
     */
    public pMatrix get(int i,int j,int r,int c){
        pMatrix v = new pMatrix(r,c);
        for(int y=0;y<r;y++){
            for(int x=0;x<c;x++){
                v.set(y,x,get(i+y,j+x));
            }
        }
        return v;
    }

    /**
     * set matrix at i-row, j-column
     */
    public void set(int i,int j,pMatrix v){
        for(int y=0;y<v.rows;y++){
            for(int x=0;x<v.cols;x++){
                set(i+y,j+x,v.get(y,x));
            }
        }
    }

    /**
     * trade operation; sum of diagonal
     */
    public double trace(){
        double sum = 0;
        for(int i=0;i<rows && i<cols;i++)
            sum += get(i,i);
        return 0;
    }

    /**
     * Frobenius norm, M.N = trace(M^T*N)
     */
    public double frobenius(pMatrix b){
        return T().mult(b).trace();
    }

    /**
     * similarity
     */
    public double alignment(pMatrix b){
        return frobenius(b) / Math.sqrt( frobenius(this) * b.frobenius(b) );
    }

    /**
     * distance squared
     */
    public double distSq(pMatrix b){
        double d = 0;
        for(int i=0;i<rows;i++){
            for(int j=0;j<cols;j++){
                double v = get(i,j) - b.get(i,j);
                d += v*v;
            }
        }
        return d;
    }

    /**
     * euclidean distance
     */
    public double dist(pMatrix b){
        return Math.sqrt(distSq(b));
    }

    /**
     * correlations
     * returns array with: 
     *  pearson, leeandlee, chandra, coxluminiandmaio, manjunath
     */
    public static double[] correlations(pMatrix x,pMatrix y){
        double sumx,sumy,sumxy,sumx2,sumy2;
        sumx = sumy = sumxy = sumx2 = sumy2 = 0.0;
        for(int i=0;i<x.rows;i++){
            for(int j=0;j<x.cols;j++){
                double xv = x.get(i,j);
                double yv = y.get(i,j);                
                sumx  += xv;
                sumy  += yv;
                sumxy += xv*yv;
                sumx2 += xv*xv;    
                sumy2 += yv*yv;
            }
        }
        double n = x.rows * x.cols;
        double pearson = ( (n *sumxy) - (sumx * sumy) ) / 
            Math.sqrt( (n*sumx2 - sumx*sumx) * (n*sumy2 - sumy*sumy) );
        double leeandlee = sumxy / sumx2;
        double chandra = sumxy / (Math.sqrt(sumx2) * Math.sqrt(sumy2));
        double coxluminiandmaio = sumxy / Math.sqrt(sumy2);
        double manjunath = sumxy / sumy2;
        return new double[] { pearson, leeandlee, chandra, coxluminiandmaio, manjunath };
    } 

    /**
     * returns Pearson correlation
     */
    public double pearson(pMatrix b){
        double[] a = correlations(this,b);
        return a[0];
    }


    /**
     * returns Gram matrix; X*X^T, or Gij = xi*xj
     */
    public pMatrix gram(){
        return mult(T());
    }

    /**
     * make a Hessian matrix; Hij = yi*yj*Gij
     */
    public pMatrix hessian(pMatrix y){
        pMatrix yy = y.mult(y.T());
        return gram().smult(yy);
    }

    /**
     * transverse
     */
    public pMatrix T(){
        pMatrix v = new pMatrix(cols,rows);
        if(rows == 1 || cols == 1){
            System.arraycopy(arr,0,v.arr,0,rows * cols);
        }else{
            for(int i=0;i<rows;i++){
                for(int j=0;j<cols;j++){
                    v.arr[(j << v.pitch) + i] = arr[(i << pitch) + j];
                    //v.set(j,i,get(i,j));
                }
            }
        }
        return v;
    }

    /**
     * matrix multiply.
     * c[i][j] = sum_k a[i][k] * b[k][j]
     */
    public pMatrix mult(pMatrix b) {
        pMatrix c = new pMatrix(rows,b.cols);
        if(cols != b.rows)  // incompatible matrices.
            return null;
        for(int i=0;i<rows;i++){
            for(int j=0;j<b.cols;j++){
                double sum = 0;
                for(int k=0;k<cols;k++){
                    sum += arr[(i << pitch) + k] * b.arr[(k << b.pitch) + j];
                    //sum += get(i,k) * b.get(k,j);
                }
                c.arr[(i << c.pitch) + j] = sum;
                //c.set(i,j,sum);
            }
        }
        return c;
    }

    /**
     * matrix * vector
     */
    public double[] mult(double[] v) {
        double[] u = new double[v.length];
        for (int i=0;i<4;i++) {
            u[i] = 0;
            for (int j=0;j<4;j++)
                u[i] += arr[(i<<pitch)+j] * v[j];
        }
        return u;
    }

    /**
     * sum up all the values in the matrix.
     */
    public double sum(){
        double s = 0;
        for(int i=0;i<rows;i++){
            for(int j=0;j<cols;j++){
                s += arr[(i << pitch) + j];
            }
        }
        return s;
    }

    /**
     * avg all values in matrix.
     */
    public double avg(){
        return sum() / (rows * cols);
    }

    /**
     * dot product between two matrices
     */
    public double dot(pMatrix b){
        return smult(b).sum();
    }

    /**
     * Neural Network thing.
     * for every element, apply sigmoid function.
     * f(a) = 1 / (1 + exp(-a)) 
     */
    public pMatrix sigmoid(double s){
        pMatrix out = new pMatrix(rows,cols);
        for(int i=0;i<rows;i++){
            for(int j=0;j<cols;j++){
                out.set(i,j, 1.0/(1.0+Math.exp(-get(i,j)*s)));
            }
        }
        return out;
    }

    /**
     * Neural Network thing.
     * for every element, apply sigmoid function.
     * f(a) = 1 / (1 + exp(-a)) 
     */
    public pMatrix sigmoidEq(double s){
        for(int i=0;i<rows;i++){
            for(int j=0;j<cols;j++){
                set(i,j, 1.0/(1.0+Math.exp(-get(i,j)*s)));
            }
        }
        return this;
    }

    /**
     * Neural Network thing.
     * for every element, apply sigmoid function.
     * f(a) = 1 / (1 + exp(-a)) 
     */
    public pMatrix sigmoid(){
        return sigmoid(1.0);
    }

    /**
     * Neural Network thing.
     * for every element, apply sigmoid function.
     * f(a) = 1 / (1 + exp(-a)) 
     */
    public pMatrix sigmoidEq(){
        return sigmoidEq(1.0);
    }

    /**
     * Neural Network thing
     * for every element, apply tanh (sigmoid with -1 to 1 range).
     * f(a) = tanh(a)
     */
    public pMatrix tanh(){
        pMatrix out = new pMatrix(rows,cols);
        for(int i=0;i<rows;i++){
            for(int j=0;j<cols;j++){
                //double a = get(i,j);
                //double e1 = Math.exp(a);
                //double e2 = Math.exp(-a);
                //out.set(i,j,(e1-e2)*(e1+e2));
                out.set(i,j, 2.0/(1.0+Math.exp(-get(i,j)))-1);
            }
        }
        return out;
    }

    /**
     * Neural Network thing
     * for every element, apply threshold with probability sigmoid.
     * f(a) = 1 with probability 1 / (1 + exp(-a)), else -1.
     */
    public pMatrix heatbath(){
        pMatrix out = new pMatrix(rows,cols);
        for(int i=0;i<rows;i++){
            for(int j=0;j<cols;j++){
                double a = 1.0/(1.0+Math.exp(-get(i,j)));
                out.set(i,j,Math.random() < a ? 1 : -1);
            }
        }
        return out;
    }
    
    /**
     * Neural Network thing.
     * apply threshold funtion
     * f(a) = a gt 0 ? 1 : -1
     */
    public pMatrix threshold(){
        pMatrix out = new pMatrix(rows,cols);
        for(int i=0;i<rows;i++){
            for(int j=0;j<cols;j++){
                double a = get(i,j);
                out.set(i,j, a > 0.0 ? 1.0 : -1.0);
            }
        }
        return out;
    }

    /**
     * magnitude.
     */
    public double mag(){
        return Math.sqrt(dot(this));
    }

    /**
     * normalize the whole thing.
     */
    public pMatrix norm(){
        return smult(1/mag());
    }

    /**
     * normalize rows
     */
    public pMatrix normRows(){
        pMatrix c = new pMatrix(rows,cols);    
        for(int i=0;i<rows;i++){
            double l = 0;
            for(int j=0;j<cols;j++){
                double v = get(i,j);
                l += v*v;
            }
            l = Math.sqrt(l);
            for(int j=0;j<cols;j++){
                c.set(i,j, get(i,j)/l);
            }
        }
        return c;
    }

    /**
     * normalize rows in place.
     */
    public pMatrix normRowsEq(){
        for(int i=0;i<rows;i++){
            double l = 0;
            for(int j=0;j<cols;j++){
                double v = get(i,j);
                l += v*v;
            }
            l = Math.sqrt(l);
            for(int j=0;j<cols;j++){
                set(i,j, get(i,j)/l);
            }
        }
        return this;
    }


    /**
     * scalar product; c = a * b
     */
    public pMatrix smult(pMatrix b){
        pMatrix a = this;
        pMatrix c = new pMatrix(rows,cols);
        for(int i=0;i<arr.length;i++){
            c.arr[i] = arr[i] * b.arr[i];
        }
        return c;
    }

    /**
     * scalar product; this *= b
     */
    public pMatrix smultEq(pMatrix b){
        for(int i=0;i<arr.length;i++){
            arr[i] *= b.arr[i];
        }
        return this;
    }

    /**
     * scalar product; c = a * s
     */
    public pMatrix smult(double s){
        pMatrix c = new pMatrix(rows,cols);
        for(int i=0;i<arr.length;i++){
            c.arr[i] = arr[i] * s;
        }
        return c;
    }

    /**
     * scalar product; this *= s
     */
    public pMatrix smultEq(double s){
        for(int i=0;i<arr.length;i++){
            arr[i] *= s;
        }
        return this;
    }

    /**
     * scalar sum; c = a + b
     */
    public pMatrix add(pMatrix b){
        pMatrix c = new pMatrix(rows,cols);
        for(int i=0;i<arr.length;i++){
            c.arr[i] = arr[i] + b.arr[i];
        }
        return c;
    }

    /**
     * scalar sum; this += b
     */
    public pMatrix addEq(pMatrix b){
        for(int i=0;i<arr.length;i++){
            arr[i] += b.arr[i];
        }
        return this;
    }


    /**
     * scalar sub; c = a - b
     */
    public pMatrix subtract(pMatrix b){
        pMatrix c = new pMatrix(rows,cols);
        for(int i=0;i<arr.length;i++){
            c.arr[i] = arr[i] - b.arr[i];
        }
        return c;
    }

    /**
     * scalar sub; this -= b
     */
    public pMatrix subtractEq(pMatrix b){
        for(int i=0;i<arr.length;i++){
            arr[i] -= b.arr[i];
        }
        return this;
    }

    /**
     * non-negative matrix factorization
     * w.mult(h) = this
     * w is weights matrix
     * h is feature matrix.
     * TODO
     */
    // public pMatrix[] nmf(int pc,int iter){
        // ...
    // }

    /** 
     * LU decomposition
     * algorithm from: Numerical Recipes in C.
     * 
     * m is the matrix.
     * n is size of matrix (n by n).
     * pitch is (int)( Math.ceil(Math.log(n)/Math.log(2)) )
     * indx (out) is the permutation of rows (input matrix).
     *
     * the input matrix m is destroyed.
     * returns false if matrix is singular. true otherwise.
     */
    private static boolean ludcmp(double[] m,int n,int pitch,int[] indx) {
        double[] vv = new double[n];

        int i,j,k,imax=0;
        double big,temp,sum;

        for(i=0;i<n;i++){   // for every row
            big=0;
            for(j=0;j<n;j++){   // for every column
                if((temp = Math.abs(m[(i<<pitch)+j])) > big)
                    big = temp;
            }
            if(Math.abs(big) < 0.000001)    // singular matrix.
                return false;
            vv[i] = 1.0 / big;
        }

        for(j=0;j<n;j++){
            for(i=0;i<j;i++){
                sum = m[(i<<pitch)+j];
                for(k=0;k<i;k++){
                    sum -= m[(i<<pitch)+k] * m[(k<<pitch)+j];
                }
                m[(i<<pitch)+j] = sum;
            }
            big = 0;
            for(i=j;i<n;i++){
                sum = m[(i<<pitch)+j];
                for(k=0;k<j;k++)
                    sum -= m[(i<<pitch)+k] * m[(k<<pitch)+j];
                m[(i<<pitch)+j] = sum;
                if ( (temp=vv[i]*Math.abs(sum)) >= big) {
                    big=temp;
                    imax=i;
                }
            }
            if(j != imax){
                for (k=0;k<n;k++) { 
                    temp = m[(imax<<pitch)+k];
                    m[(imax<<pitch)+k] = m[(j<<pitch)+k]; 
                    m[(j<<pitch)+k] = temp;
                }
                vv[imax]=vv[j];
            }            
            indx[j]=imax;

            if (Math.abs(m[(j<<pitch)+j]) < 0.000001)
                m[(j<<pitch)+j] = 0.000001;

            if (j != n-1) { 
                temp=1.0/m[(j<<pitch)+j];
                for (i=j+1;i<n;i++)
                    m[(i<<pitch)+j] = m[(i<<pitch)+j] * temp;
            }
        }
        return true;
    }


    /** 
     * back substitution
     * algorithm from: Numerical Recipes in C.
     * uses output from ludcmp.
     * 
     * can be used to solve a system of linear equations, 
     * X a = b
     *
     * inputs are same as ludcmp, 
     * b are the input 'b' values, and output 'a' values.
     */
    private static void lubksb(double[] m,int n,int pitch,int[] indx,double[] b) {
        int i,j,ip,ii = -1;
        double sum;
        for (i=0;i<n;i++) { 
            ip=indx[i];
            sum=b[ip];
            b[ip]=b[i];
            if(ii >= 0){
                for (j=ii;j<=i-1;j++)
                    sum -= m[(i<<pitch)+j] * b[j];
            }else if(sum != 0){
                ii=i;
            }
            b[i] = sum;
        }
        for (i=n-1;i>=0;i--){ 
            sum=b[i];
            for (j=i+1;j<n;j++){ 
                sum -= m[(i<<pitch)+j] * b[j];
            }
            b[i] = sum / m[(i<<pitch)+i];
        }
    }

    /** 
     * matrix inverse
     * algorithm from: Numerical Recipes in C.
     * uses LU decomposition.
     * destroys input matrix.
     * output matrix must be the same size.
     */
    private static boolean inv(double[] m,int n,int pitch,double[] out) {

        boolean ret;
        int[] indx = new int[n];
        ret = ludcmp(m,n,pitch,indx);
        if(!ret)    // matrix is singular.
            return ret;        
        int i,j;
        double[] col = new double[n];

        for(j=0;j<n;j++){
            for(i=0;i<n;i++)
                col[i] = 0.0;
            col[j] = 1;            
            lubksb(m,n,pitch,indx,col);
            for(i=0;i<n;i++){
                out[(i<<pitch) + j] = col[i];
            }
        }
        return ret;
    }

    /**
     * get inverse of matrix.
     *
     * returns null if matrix is singular.
     */
    public pMatrix inv(){
        if(rows != cols)
            return null;
        pMatrix tmp = dup();    // we end up destroying a matrix
        pMatrix out = new pMatrix(rows,cols);
        if(inv(tmp.arr,rows,pitch,out.arr))
            return out;
        return null;
    }

    /**
     * solve X * w = y
     * 
     * returns false if matrix is singular
     * w is a row matrix.
     */
    public boolean solv(double[] y){
        if(rows <= cols || cols != y.length)
            return false;
        int n = cols;
        // pick top n * n matrix.
        pMatrix tmp = get(0,0,n,n);        
        boolean ret;
        int[] indx = new int[n];
        ret = ludcmp(tmp.arr,n,pitch,indx);
        if(!ret)    // matrix is singular.
            return ret;
        lubksb(tmp.arr,n,pitch,indx,y);
        return true;
    }


    /**
     * solve X * w = y
     * 
     * y and w are column matrices.
     * returns 'w' matrix.
     */
    public pMatrix solv(pMatrix y){
        if(rows <= cols || cols != y.rows || y.cols != 1)
            return null;
        int n = cols;
        // pick top n * n matrix.
        pMatrix tmp = get(0,0,n,n);
        boolean ret;
        int[] indx = new int[n];
        ret = ludcmp(tmp.arr,n,pitch,indx);
        if(!ret)    // matrix is singular.
            return null;
        pMatrix out = y.dup();
        lubksb(tmp.arr,n,pitch,indx,out.arr);
        return out;
    }


    /**
     * 3D, 4x4 matrix.
     * returns a rotation matrix around X axix.
     * Note: change sin term signs to change handedness.
     *
     * nx = x;
     * ny = x * 0 + y * cos + z * -sin;
     * nz = x * 0 + y * sin + z * cos;
     */
    public static pMatrix rotatex3d(double a){
        pMatrix tmp = I(4,4);
        double cos = (double)Math.cos(a);
        double sin = (double)Math.sin(a);
        tmp.arr[(1<<tmp.pitch)+1] = cos;
        tmp.arr[(1<<tmp.pitch)+2] = -sin;
        tmp.arr[(2<<tmp.pitch)+1] = sin;
        tmp.arr[(2<<tmp.pitch)+2] = cos;
        return tmp;
    }

    /**
     * 3D, 4x4 matrix.
     * returns a rotation matrix around Y axix.
     *
     * nx = x * cos + y * 0 + z * sin;
     * ny = y;
     * nz = x * -sin + y * 0 + z * cos;
     */
    public static pMatrix rotatey3d(double a){
        pMatrix tmp = I(4,4);
        double cos = (double)Math.cos(a);
        double sin = (double)Math.sin(a);
        tmp.arr[(0<<tmp.pitch)+0] = cos;
        tmp.arr[(0<<tmp.pitch)+2] = sin;
        tmp.arr[(2<<tmp.pitch)+0] = -sin;
        tmp.arr[(2<<tmp.pitch)+2] = cos;
        return tmp;
    }

    /**
     * 3D, 4x4 matrix.
     * returns a rotation matrix around Z axix.
     *
     * nx = x * cos + y * -sin + z * 0;
     * ny = x * sin + y * cos + z * 0;
     * nz = z;
     */
    public static pMatrix rotatez3d(double a){
        pMatrix tmp = I(4,4);
        double cos = (double)Math.cos(a);
        double sin = (double)Math.sin(a);
        tmp.arr[(0<<tmp.pitch)+0] = cos;
        tmp.arr[(0<<tmp.pitch)+1] = -sin;
        tmp.arr[(1<<tmp.pitch)+0] = sin;
        tmp.arr[(1<<tmp.pitch)+1] = cos;
        return tmp;
    }

    /**
     * 3D, 4x4 matrix.
     * returns a translation matrix 
     */
    public static pMatrix translate3d(double x,double y,double z){
        pMatrix tmp = I(4,4);
        tmp.arr[(0<<tmp.pitch)+3] = x;
        tmp.arr[(1<<tmp.pitch)+3] = y;
        tmp.arr[(2<<tmp.pitch)+3] = z;
        return tmp;
    }

    /**
     * 3D, 4x4 matrix.
     * returns a translation matrix 
     */
    public static pMatrix translate3d(double[] v){
        pMatrix tmp = I(4,4);
        tmp.arr[(0<<tmp.pitch)+3] = v[0];
        tmp.arr[(1<<tmp.pitch)+3] = v[1];
        tmp.arr[(2<<tmp.pitch)+3] = v[2];
        return tmp;
    }


    /**
     * 3D, 4x4 matrix.
     * scales the matrix.
     */
    public static pMatrix scale3d(double s){
        pMatrix tmp = I(4,4);
        tmp.arr[(0<<tmp.pitch)+0] = s;
        tmp.arr[(1<<tmp.pitch)+1] = s;
        tmp.arr[(2<<tmp.pitch)+2] = s;
        return tmp;
    }


    /**
     * 3D, 4x4 matrix.
     * scales the matrix.
     */
    public static pMatrix scale3d(double x,double y,double z){
        pMatrix tmp = I(4,4);
        tmp.arr[(0<<tmp.pitch)+0] = x;
        tmp.arr[(1<<tmp.pitch)+1] = y;
        tmp.arr[(2<<tmp.pitch)+2] = z;
        return tmp;
    }


    /**
     * 3D, 4x4 matrix.
     * scales a matrix in relation to a point
     *
     * @param s The scale.
     * @param v The center of point where to scale.
     */
    public static pMatrix scale3d(double s,double[] v){
        pMatrix tmp = I(4,4);
        tmp.arr[(0<<tmp.pitch)+0] = s;
        tmp.arr[(0<<tmp.pitch)+3] = (1 - s) * v[0];
        tmp.arr[(1<<tmp.pitch)+1] = s;
        tmp.arr[(1<<tmp.pitch)+3] = (1 - s) * v[1];
        tmp.arr[(2<<tmp.pitch)+2] = s;
        tmp.arr[(2<<tmp.pitch)+3] = (1 - s) * v[2];
        return tmp;
    }

    /**
     * 3D, 4x4 matrix.
     * scales a matrix in relation to a point
     *
     * @param s The scales (one for each coord).
     * @param v The center of point where to scale.
     */
    public static pMatrix scale3d(double[] s,double[] v){
        pMatrix tmp = I(4,4);
        tmp.arr[(0<<tmp.pitch)+0] = s[0];
        tmp.arr[(0<<tmp.pitch)+3] = (1 - s[0]) * v[0];
        tmp.arr[(1<<tmp.pitch)+1] = s[1];
        tmp.arr[(1<<tmp.pitch)+3] = (1 - s[1]) * v[1];
        tmp.arr[(2<<tmp.pitch)+2] = s[2];
        tmp.arr[(2<<tmp.pitch)+3] = (1 - s[2]) * v[2];
        return tmp;
    }


    /**
     * 3D, 4x4 matrix.    
     * reflect in the X axis.
     */
    public static pMatrix reflectx3d(){
        pMatrix tmp = I(4,4);
        tmp.arr[(0<<tmp.pitch)+0] = -tmp.arr[(0<<tmp.pitch)+0];
        return tmp;
    }

    /**
     * 3D, 4x4 matrix.    
     * reflect in the Y axis.
     */
    public static pMatrix reflecty3d(){
        pMatrix tmp = I(4,4);
        tmp.arr[(1<<tmp.pitch)+1] = -tmp.arr[(1<<tmp.pitch)+1];
        return tmp;
    }

    /**
     * 3D, 4x4 matrix.    
     * reflect in the Z axis.
     *
     * also known as conversion from right handled
     * to left handled coord system, or vice versa.
     */
    public static pMatrix reflectz3d(){
        pMatrix tmp = I(4,4);
        tmp.arr[(2<<tmp.pitch)+2] = -tmp.arr[(2<<tmp.pitch)+2];
        return tmp;
    }

    /**
     * 3D, 4x4 matrix.    
     * create perspective matrix; turns current matrix into
     * perspective matrix.
     * 
     * Note: changes current matrix!
     *
     * @param zrpr at what z is the eye?
     * @param zvp at what z is the view plane?
     */
    public pMatrix perspective3d(double zprp,double zvp){
        double t2,t3;
        double d1 = zprp - zvp;
        double a = -zvp/d1;
        double b = -1/d1;
        double c = zvp*(zprp/d1);
        double d = zvp/d1;
        for (int i=0;i<4;i++) {
            t2 = arr[(i<<pitch)+2];
            t3 = arr[(i<<pitch)+3];
            arr[(i<<pitch)+2] = t2*a + t3*b;
            arr[(i<<pitch)+3] = t2*c + t3*d;
        }
        return this;
    }


    /**
     * 3D, 4x4 matrix.    
     * returns the inverse of the current matrix
     */
    public pMatrix inv3d(){
        pMatrix n = I(4,4);
        if(m4_inverse(n.arr,arr)){
            return n;
        }
        return I(4,4);
    }

    /**
     * various matrix operations 
     * (inverse; probably faster than LU decomposition 
     * method above.). taken from:
     * http://skal.planet-d.net/demo/matrixfaq.htm
     */

    /**
     * compute determinant of a 3x3 matrix
     */
    private static double m3_det(double[] mat){
        double 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(double[] mr,double[] ma){
        double det = m3_det( ma );
        if ( Math.abs( det ) < (double)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(double[] mr,double[] 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 double m4_det(double[] mr) {
        double det, result = 0, i = 1;
        double[] msub3 = new double[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(double[] mr, double[] ma ){
        double mdet = m4_det( ma );
        double[] mtemp = new double[3*3];
        int     i, j, sign;
        if (Math.abs( mdet ) < (double)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;
    }


    /**
     * output matrix in csv format
     */
    public String toString(){
        StringBuffer sb = new StringBuffer();
        sb.append("pMatrix["+rows+"x"+cols+"][pitch="+pitch+"][len="+arr.length+"]:\n");
        /*
        NumberFormat nf = NumberFormat.getInstance();
        nf.setGroupingUsed(false);
        nf.setMaximumFractionDigits(0x6);
        nf.setMaximumIntegerDigits(0xFF);
        nf.setMinimumFractionDigits(0);
        nf.setMinimumIntegerDigits(1);
        */
        for(int i=0;i<rows;i++){
            for(int j=0;j<cols;j++){
                // sb.append(nf.format(get(i,j)) + (j+1 < cols ? "," : "\n") );
                sb.append(get(i,j) + (j+1 < cols ? "," : "\n") );
            }
        }
        return sb.toString();
    }
    
}