/*
    I think my matrices are backward or inverted.
                                 GW
                                     */
public class Matrix3D {
    double data_[][] = new double[4][4];
    static private Matrix3D multiplyTemp_ = new Matrix3D(),
        constantTemp_ = new Matrix3D();
    static private Vector3D transformTemp_ = new Vector3D();

    public Matrix3D() {}
    public Matrix3D(
            double a1, double a2, double a3, double a4,
            double b1, double b2, double b3, double b4,
            double c1, double c2, double c3, double c4,
            double d1, double d2, double d3, double d4) {
        data_[0][0] = a1; data_[0][1] = a2; data_[0][2] = a3; data_[0][3] = a4;
        data_[1][0] = b1; data_[1][1] = b2; data_[1][2] = b3; data_[1][3] = b4;
        data_[2][0] = c1; data_[2][1] = c2; data_[2][2] = c3; data_[2][3] = c4;
        data_[3][0] = d1; data_[3][1] = d2; data_[3][2] = d3; data_[3][3] = d4;
    }
    public void set(int i, int j, double val) {
        data_[i][j] = val;
    }
    public double get(int i, int j) { 
        return data_[i][j];
    }
    public Matrix3D copy(Matrix3D src) {
        for(int i = 0; i<4; ++i) 
            for(int j = 0; j<4; ++j)
                data_[i][j] = src.data_[i][j];
        return this;
    }
    public Matrix3D multiply(Matrix3D M, Matrix3D out) {
        for(int i = 0; i<4; ++i)
            for(int j = 0; j<4; ++j) {
                /* a tiny premature optimization over
                double sum = .0;
                for(int k = 0; k<4; ++k)
                    sum += data_[i][k]*M.data_[k][j]; */
                /* seems to be backward
                out.data_[i][j] = data_[i][0]*M.data_[0][j]
                                + data_[i][1]*M.data_[1][j]
                                + data_[i][2]*M.data_[2][j]
                                + data_[i][3]*M.data_[3][j];
                */
                out.data_[i][j] = data_[0][j]*M.data_[i][0]
                                + data_[1][j]*M.data_[i][1]
                                + data_[2][j]*M.data_[i][2]
                                + data_[3][j]*M.data_[i][3];                
            }
        return out;
    }
    public Matrix3D multiply(Matrix3D M) {
        return multiplyTemp_.copy(this).multiply(M,this);
    }
    // nouns
    public void zero() {
        for(int i = 0; i<4; ++i) 
            for(int j = 0; j<4; ++j)
                data_[i][j] = .0;
    }
    public Matrix3D identity() {
        zero();
        data_[0][0] = 
            data_[1][1] = 
                data_[2][2] = 
                    data_[3][3] = 1.;
        return this;
    }
    public Matrix3D translation(double x, double y, double z) {
        identity();
        data_[0][3] = x;
        data_[1][3] = y;
        data_[2][3] = z;
        return this;
    }
    public Matrix3D translation(Vector3D v) {
        return translation(v.x(),v.y(),v.z());
    }
    public Matrix3D xRotation(double theta) {
        identity(); 
        data_[1][1] = Math.cos(theta);
        data_[1][2] = -Math.sin(theta);
        data_[2][1] = Math.sin(theta);
        data_[2][2] = Math.cos(theta);
        return this;
    }
    public Matrix3D yRotation(double theta) {
        identity();
        data_[0][0] = Math.cos(theta);
        data_[0][2] = Math.sin(theta);
        data_[2][0] = -Math.sin(theta);
        data_[2][2] = Math.cos(theta);
        return this;
    }
    public Matrix3D zRotation(double theta) {
        identity();
        data_[0][0] = Math.cos(theta);
        data_[0][1] = -Math.sin(theta);
        data_[1][0] = Math.sin(theta);
        data_[1][1] = Math.cos(theta);
        return this;
    }
    public Matrix3D scaler(double x, double y, double z) {
        zero();
        data_[0][0] = x;
        data_[1][1] = y;
        data_[2][2] = z;
        data_[3][3] = 1.;
        return this;
    }
    public Matrix3D scaler(Vector3D v) {
        return scaler(v.x(),v.y(),v.z());
    }
    public Matrix3D perspectivator(double f) {
        zero();
        // backward once again
        data_[0][0] = data_[1][1] = data_[3][2] = f;
        data_[2][3] = 1.;
        return this;
    }
    // verbs
    public Matrix3D translate(double x, double y, double z, Matrix3D out) {
        return multiply(constantTemp_.translation(x,y,z), out);
    }
    public Matrix3D translate(Vector3D v, Matrix3D out) {
        return translate(v.x(),v.y(),v.z(),out);
    }
    public Matrix3D translate(double x, double y, double z) {
        return multiply(constantTemp_.translation(x,y,z));
    }
    public Matrix3D translate(Vector3D v) {
        return translate(v.x(),v.y(),v.z());
    }
    public Matrix3D rotateX(double theta, Matrix3D out) {
        return multiply(constantTemp_.xRotation(theta),out);
    }
    public Matrix3D rotateX(double theta) {
        return multiply(constantTemp_.xRotation(theta));
    }
    public Matrix3D rotateY(double theta, Matrix3D out) {
        return multiply(constantTemp_.yRotation(theta),out);
    }
    public Matrix3D rotateY(double theta) {
        return multiply(constantTemp_.yRotation(theta));
    }
    public Matrix3D rotateZ(double theta, Matrix3D out) {
        return multiply(constantTemp_.zRotation(theta),out);
    }
    public Matrix3D rotateZ(double theta) {
        return multiply(constantTemp_.zRotation(theta));
    }
    public Matrix3D scale(double x, double y, double z, Matrix3D out) {
        return multiply(constantTemp_.scaler(x,y,z),out);
    }
    public Matrix3D scale(Vector3D v, Matrix3D out) {
        return scale(v.x(),v.y(),v.z(),out);
    }
    public Matrix3D scale(double x, double y, double z) {
        return multiply(constantTemp_.scaler(x,y,z));
    }
    public Matrix3D scale(Vector3D v) {
        return scale(v.x(),v.y(),v.z());
    }
    public void transform(double src[], double dst[]) {
        for (int i = 0; i<4; ++i)
            dst[i] = data_[i][0] * src[0] + data_[i][1] * src[1] + data_[i][2] * src[2] + data_[i][3] * src[3];
    }
    public void transform(Vector3D src, Vector3D dst) {
        transform(src.vector(),dst.vector());
    }
    public void transform(Vector3D v) {
        transformTemp_.copy(v);
        transform(transformTemp_,v);
    }
    // (L i think if you're most people)
    public void transformR(double src[], double dst[]) {
        for (int i = 0; i<4; ++i)
            dst[i] = data_[0][i] * src[0] + data_[1][i] * src[1] + data_[2][i] * src[2] + data_[3][i] * src[3];
    }
    public void transformR(Vector3D src, Vector3D dst) {
        transformR(src.vector(),dst.vector());
    }
    public void transformR(Vector3D v) {
        transformTemp_.copy(v);
        transformR(transformTemp_,v);
    }
    public Matrix3D inverse(Matrix3D out) {
        // COMPUTE ADJOINT COFACTOR MATRIX FOR THE ROTATION/SCALE 3x3 SUBMATRIX
        for (int i = 0 ; i < 3 ; i++)
            for (int j = 0 ; j < 3 ; j++) {
                int iu = (i + 1) % 3, iv = (i + 2) % 3;
                int ju = (j + 1) % 3, jv = (j + 2) % 3;
                out.data_[j][i] = data_[iu][ju] * data_[iv][jv] - data_[iu][jv] * data_[iv][ju];
            }

        // RENORMALIZE BY DETERMINANT TO INVERT ROTATION/SCALE SUBMATRIX
        double det = data_[0][0]*out.data_[0][0] + data_[1][0]*out.data_[0][1] + data_[2][0]*out.data_[0][2];
        for (int i = 0 ; i < 3 ; i++)
            for (int j = 0 ; j < 3 ; j++)
                out.data_[i][j] /= det;

        // INVERT TRANSLATION
        for (int i = 0 ; i < 3 ; i++)
            out.data_[i][3] = -out.data_[i][0]*data_[0][3] - out.data_[i][1]*data_[1][3] - out.data_[i][2]*data_[2][3];

        // NO PERSPECTIVE
        for (int i = 0 ; i < 4 ; i++)
            out.data_[3][i] = i < 3 ? 0 : 1;
        return out;
    }
    public Matrix3D transpose(Matrix3D out) {
        for(int i = 0; i<4; ++i)
            for(int j = 0; j<4; ++j) 
                out.data_[i][j] = data_[j][i];
        return out;
    }
        
    public String toString() {
        StringBuffer buf = new StringBuffer(100);
        buf.append("{");
        for(int i = 0; i<4; ++i) {
            buf.append("{");
            for(int j = 0; j<4; ++j) {
                buf.append(data_[i][j]);
                if(j!=3)
                    buf.append(",");
            }
            buf.append("}");
            if(i!=3)
                buf.append(",");
        }
        buf.append("}");
        return buf.toString();
    }
}