/*  PROJECT OVERVIEW:
 *  In this project you will implement a matrix class for transforming
 *  three-dimensional points.  You will reuse this class in your next
 *  project.
 *
 *  You are free to implement the Matrix3D class (define instance
 *  variables and private helper methods) as you wish. Note that
 *  matrices are composed on the right, and points are column vectors.
 *
 *  While you are primarily responsible for testing your class, I will
 *  provide an example test program for your use in the near future
 *  (to appear on this web page). Please use your own test applet t
 *  display the functionality of your class on your project web page.
 *  You may, however, use my test program as a starting point, but
 *  please document any modifications that you made as both comments
 *  in the code and on the project web page. Your test program should
 *  exercise each method in your Matrix3D. I make no guarantee that my
 *  test program will do so.
 */

import java.lang.Math;

public class Matrix3D {

    ///////////////////////////////////////////////////////////////////
    // A Matrix 3D is basically a 4x4 array.
    //            +-               -+
    //            | e00 e01 e02 e03 |
    // Matrix3D = | e10 e11 e12 e13 |
    //            | e20 e21 e22 e23 |
    //            | e30 e31 e32 e33 |
    //            +-               -+
    public float m3d[][] = new float[4][4]; // row=4, col=4
    ///////////////////////////////////////////////////////////////////

    //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    // Constructors
    //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    public Matrix3D() {
        loadIdentity(); // initialize with identity transform
    }

    public Matrix3D(Matrix3D copy) {
        m3d = copy.m3d; // initialize with copy of source
    }

    public Matrix3D(Raster r) {
        ///////////////////////////////////////////////////////////////////
        // Slide 40 of Lecture 12
        // Initialize with a mapping from canonical space to screen space.
        // The parameters left, right, top, bottom, near, and far all describe
        // the viewing frustum.  If we assume that the viewing frustum has been
        // converted to canonical form, then it is just a cube with opposite
        // vertices at (1,1,1) and (-1,-1,-1), so these six parameters have the
        // values of only 1 and -1.
        ///////////////////////////////////////////////////////////////////
        //          this        =                 viewport
        //  +-               -+   +-                                  -+
        //  | e00 e01 e02 e03 |   | W/(R-L)      0       0  -L*W/(R-L) |
        //  | e10 e11 e12 e13 | = |      0  H/(B-T)      0  -T*H/(B-T) |
        //  | e20 e21 e22 e23 |   |      0       0  Z/(F-N) -N*Z/(F-N) |
        //  | e30 e31 e32 e33 |   |      0       0       0          1  |
        //  +-               -+   +-                                  -+
        //  Where:
        //  L = left   = -1       +-------+
        //  R = right  =  1      /       /|   Viewing frustrum in
        //  T = top    =  1     +-------+ |   canonical form -> cube
        //  B = bottom = -1     |       | +   with opposite vertices at
        //  N = near   =  1     |       |/    (1,1,1) and (-1,-1,-1)
        //  F = far    = -1     +-------+
        //  Z = Zmax = 1
        //  W = width of raster
        //  H = height of raster
        float L = (float) -1;
        float R = (float)  1;
        float T = (float) -1;
        float B = (float)  1;
        float N = (float)  1;
        float F = (float) -1;
        float Z = (float)  1;
        float W = r.getWidth();
        float H = r.getHeight();
        Matrix3D viewportM3D = new Matrix3D(); // Initialize with identity matrix
        viewportM3D.set(0, 0, W/(R-L));
        viewportM3D.set(0, 3, -L*W/(R-L));
        viewportM3D.set(1, 1, H/(B-T));
        viewportM3D.set(1, 3, -T*H/(B-T));
        viewportM3D.set(2, 2, Z/(F-N));
        viewportM3D.set(2, 3, -N*Z/(F-N));
        this.m3d = viewportM3D.m3d;
    }

    //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    // General interface methods
    //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    public void set(int row, int col, float value) {
        m3d[row][col] = value; // set element [row][col] to value
    }
    public float get(int row, int col) {
        return m3d[row][col]; // return element [row][col]
    }

    //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    // Transformations
    //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    ///////////////////////////////////////////////////////////////////
    // 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 transform(Point3D in[], Point3D out[], int start, int length) {
        ///////////////////////////////////////////////////////////////
        //   out    =       Matrix3d      *  in
        // +-    -+   +-               -+  +    -+
        // | out0 |   | e00 e01 e02 e03 |  | in0 |
        // | out1 | = | e10 e11 e12 e13 |  | in1 |
        // | out2 |   | e20 e21 e22 e23 |  | in2 |
        // | out3 |   | e30 e31 e32 e33 |  | in3 |
        // +-    -+   +-               -+  +-   -+
        ///////////////////////////////////////////////////////////////
        for (int i = 0; i < length; i++) {
            out[start+i].x = get(0,0) * in[start+i].x +
                             get(0,1) * in[start+i].y +
                             get(0,2) * in[start+i].z +
                             get(0,3) * in[start+i].w;
            out[start+i].y = get(1,0) * in[start+i].x +
                             get(1,1) * in[start+i].y +
                             get(1,2) * in[start+i].z +
                             get(1,3) * in[start+i].w;
            out[start+i].z = get(2,0) * in[start+i].x +
                             get(2,1) * in[start+i].y +
                             get(2,2) * in[start+i].z +
                             get(2,3) * in[start+i].w;
            out[start+i].w = get(3,0) * in[start+i].x +
                             get(3,1) * in[start+i].y +
                             get(3,2) * in[start+i].z +
                             get(3,3) * in[start+i].w;
            out[start+i].normalize();
            if (TestMatrix.DEBUG) {
                String str1[] = new String[6];
                String str2[] = new String[6];
                String str3[] = new String[6];
                str1 = out[start+i].toString2();
                str2 = this.toString2();
                str3 = in[start+i].toString2();
                System.out.println(str1[0]+"   "+str2[0]+" "+str3[0]+"\n"+
                                   str1[1]+"   "+str2[1]+" "+str3[1]+"\n"+
                                   str1[2]+" = "+str2[2]+" "+str3[2]+"\n"+
                                   str1[3]+"   "+str2[3]+" "+str3[3]+"\n"+
                                   str1[4]+"   "+str2[4]+" "+str3[4]+"\n"+
                                   str1[5]+"   "+str2[5]+" "+str3[5]+"\n"); //debug
            }
        }
    }

    public final void compose(Matrix3D src) {
        ///////////////////////////////////////////////////////////////
        //  pg. 424 in Hearn & Baker
        ///////////////////////////////////////////////////////////////
        //  Matrix multiplication is (this * src) since I'm mulitplying
        //  the world coordinates on the right.
        ///////////////////////////////////////////////////////////////
        //          this        =         this        *         src
        //  +-               -+   +-               -+  +-               -+
        //  | e00 e01 e02 e03 |   | e00 e01 e02 e03 |  | e00 e01 e02 e03 |
        //  | e10 e11 e12 e13 | = | e10 e11 e12 e13 |  | e10 e11 e12 e13 |
        //  | e20 e21 e22 e23 |   | e20 e21 e22 e23 |  | e20 e21 e22 e23 |
        //  | e30 e31 e32 e33 |   | e30 e31 e32 e33 |  | e30 e31 e32 e33 |
        //  +-               -+   +-               -+  +-               -+
        ///////////////////////////////////////////////////////////////
        Matrix3D tempM3D = new Matrix3D();
        for (int col = 0; col < 4; col++) {
            for (int row = 0; row < 4; row++) {
                float value = this.get(row,0) * src.get(0,col) +
                              this.get(row,1) * src.get(1,col) +
                              this.get(row,2) * src.get(2,col) +
                              this.get(row,3) * src.get(3,col);
                tempM3D.set(row, col, value);
            }
        }

        if (TestMatrix.DEBUG) {
                String str1[] = new String[6];
                String str2[] = new String[6];
                String str3[] = new String[6];
                str1 = tempM3D.toString2();
                str3 = src.toString2();
                str2 = this.toString2();
                System.out.println(str1[0]+"   "+str2[0]+" "+str3[0]+"\n"+
                                   str1[1]+"   "+str2[1]+" "+str3[1]+"\n"+
                                   str1[2]+" = "+str2[2]+" "+str3[2]+"\n"+
                                   str1[3]+"   "+str2[3]+" "+str3[3]+"\n"+
                                   str1[4]+"   "+str2[4]+" "+str3[4]+"\n"+
                                   str1[5]+"   "+str2[5]+" "+str3[5]+"\n"); //debug
        }

        m3d = tempM3D.m3d;
    }

    public void loadIdentity() {
        ///////////////////////////////////////////////////////////////
        //          this        =   identity
        //  +-               -+   +-       -+
        //  | e00 e01 e02 e03 |   | 1 0 0 0 |
        //  | e10 e11 e12 e13 | = | 0 1 0 0 |
        //  | e20 e21 e22 e23 |   | 0 0 1 0 |
        //  | e30 e31 e32 e33 |   | 0 0 0 1 |
        //  +-               -+   +-       -+
        ///////////////////////////////////////////////////////////////
        for (int row = 0; row < 4; row++) {
          for (int col = 0; col < 4; col++) {
                if (row == col)
                    this.set(row, col, 1.0f);
                else
                    this.set(row, col, 0.0f);
          }
        }
    }

    public void translate(float tx, float ty, float tz) {
        ///////////////////////////////////////////////////////////////
        //  pg. 408 and 424 in Hearn & Baker
        ///////////////////////////////////////////////////////////////
        //          this        =         this        *     t
        //  +-               -+   +-               -+  +-        -+
        //  | e00 e01 e02 e03 |   | e00 e01 e02 e03 |  | 1 0 0 tx |
        //  | e10 e11 e12 e13 | = | e10 e11 e12 e13 |  | 0 1 0 ty |
        //  | e20 e21 e22 e23 |   | e20 e21 e22 e23 |  | 0 0 1 tz |
        //  | e30 e31 e32 e33 |   | e30 e31 e32 e33 |  | 0 0 0  1 |
        //  +-               -+   +-               -+  +-        -+
        ///////////////////////////////////////////////////////////////
        Matrix3D translateM3D = new Matrix3D(); // New 3D identity matrix
        translateM3D.set(0, 3, tx);
        translateM3D.set(1, 3, ty);
        translateM3D.set(2, 3, tz);
        compose(translateM3D); // Multiply translation matrix by this
    }

    public void scale(float sx, float sy, float sz) {
        ///////////////////////////////////////////////////////////////
        //  pg. 420 424 in Hearn & Baker
        //?? Does it matter if the object is also transformed in the
        //?? Process?
        ///////////////////////////////////////////////////////////////
        //          this        =         this        *     scale
        //  +-               -+   +-               -+  +-          -+
        //  | e00 e01 e02 e03 |   | e00 e01 e02 e03 |  | sx  0  0 0 |
        //  | e10 e11 e12 e13 | = | e10 e11 e12 e13 |  | 0  sy  0 0 |
        //  | e20 e21 e22 e23 |   | e20 e21 e22 e23 |  | 0   0 sz 0 |
        //  | e30 e31 e32 e33 |   | e30 e31 e32 e33 |  | 0   0  0 1 |
        //  +-               -+   +-               -+  +-            -+
        ///////////////////////////////////////////////////////////////
        // Normalize ax, ay, and az
        //float l = (float) Math.sqrt(sx*sx + sy*sy + sz*sz);
        //sx = sx / l;
        //sy = sy / l;
        //sz = sz / l;
        
        Matrix3D scaleM3D = new Matrix3D(); // New 3D identity matrix
        scaleM3D.set(0, 0, sx);
        scaleM3D.set(1, 1, sy);
        scaleM3D.set(2, 2, sz);
        compose(scaleM3D); // Multiply scaling matrix by this
        //???
    }

    public void skew(float kxy, float kxz, float kyz) {             // this = this * skew
        ///////////////////////////////////////////////////////////////
        //  I'm assuming this means the same thing as shear
        //  Slide 24 of Lecture 12
        //?? Does it matter if the object is also transformed in the
        //?? Process?
        ///////////////////////////////////////////////////////////////
        //          this        =        skew       *       this
        //  +-               -+   +-             -+  +-               -+
        //  | e00 e01 e02 e03 |   | 1   kxy kxz 0 |  | e00 e01 e02 e03 |
        //  | e10 e11 e12 e13 | = | 0   1   kyz 0 |  | e10 e11 e12 e13 |
        //  | e20 e21 e22 e23 |   | 0   0   1   0 |  | e20 e21 e22 e23 |
        //  | e30 e31 e32 e33 |   | 0   0   0   1 |  | e30 e31 e32 e33 |
        //  +-               -+   +-             -+  +-               -+
        ///////////////////////////////////////////////////////////////
        Matrix3D skewM3D = new Matrix3D(); // New 3D identity matrix
        skewM3D.set(0, 1, kxy);
        skewM3D.set(0, 2, kxz);
        skewM3D.set(1, 2, kyz);
        compose(skewM3D); // Multiply skew matrix by this
        //???
    }
    public void rotate(float ax, float ay, float az, float angle) {
        ///////////////////////////////////////////////////////////////
        //  Slide 13 of Lecture 12
        //?? Does it matter if the object is also transformed in the
        //?? Process?
        ///////////////////////////////////////////////////////////////
        //  this = this * rotate
        //  rotate = Symmetric(a) * (1-cos angle) +
        //                Skew(a) * sin angle +
        //               Identity * cos angle
        //                 +-                   -+
        //                 |  ax^2 ax*ay ax*az 0 |
        //  symmetric(a) = | ax*ay  ay^2 ay*az 0 |
        //                 | ax*az ay*az  az^2 0 |
        //                 |     0     0     0 1 |
        //                 +-                   -+
        //                 +-              -+
        //                 |   0  -az  ay 0 |
        //       skew(a) = |  az    0 -ax 0 |
        //                 | -ay   ax   0 0 |
        //                 |   0    0   0 1 |
        //                 +-              -+
        //                 +-       -+
        //                 | 1 0 0 0 |
        //      Identity = | 0 1 0 0 |
        //                 | 0 0 1 0 |
        //                 | 0 0 0 1 |
        //                 +-       -+
        ///////////////////////////////////////////////////////////////
        // Normalize ax, ay, and az
        float l = (float) Math.sqrt(ax*ax + ay*ay + az*az);
        ax = ax / l;
        ay = ay / l;
        az = az / l;

        float scale1 = (float) (1 - java.lang.Math.cos((double) angle));
        float scale2 = (float) java.lang.Math.sin((double) angle);
        float scale3 = 1 - scale1; // cos angle

        Matrix3D rotateM3D = new Matrix3D();
        rotateM3D.set(0, 0, scale1*ax*ax + 0.0f         +  scale3);
        rotateM3D.set(0, 1, scale1*ax*ay + scale2*(-az) + 0.0f);
        rotateM3D.set(0, 2, scale1*ax*az + scale2*(ay)  + 0.0f);
        rotateM3D.set(1, 0, scale1*ay*ax + scale2*(az)  + 0.0f);
        rotateM3D.set(1, 1, scale1*ay*ay + 0.0f         + scale3);
        rotateM3D.set(1, 2, scale1*ay*az + scale2*(-ax) + 0.0f);
        rotateM3D.set(2, 0, scale1*az*ax + scale2*(-ay) + 0.0f);
        rotateM3D.set(2, 1, scale1*az*ay + scale2*(ax)  + 0.0f);
        rotateM3D.set(2, 2, scale1*az*az + 0.0f         + scale3);
        //rotateM3D.set(3, 3, scale1       + scale2       + scale3); // DOH!

        //System.out.println("!!!!!!!!!!!!!!!!!!!!!!!!!!!"); //debug
        //System.out.println("model.rotate("+ax+", "+ay+", "+az+", "+angle+"):"); //debug
        //System.out.println(rotateM3D);
        //System.out.println("!!!!!!!!!!!!!!!!!!!!!!!!!!!"); //debug

        compose(rotateM3D); // Multiply rotation matrix by this
    }

    public void lookAt(float eyex, float eyey, float eyez,
                       float atx,  float aty,  float atz,
                       float upx,  float upy,  float upz) {
        ///////////////////////////////////////////////////////////////
        //  Slide 36 of Lecture 12
        ///////////////////////////////////////////////////////////////
        //          this        =         this        * lookAt
        //  +-               -+   +-               -+
        //  | e00 e01 e02 e03 |   | e00 e01 e02 e03 |
        //  | e10 e11 e12 e13 | = | e10 e11 e12 e13 | * lookAt
        //  | e20 e21 e22 e23 |   | e20 e21 e22 e23 |
        //  | e30 e31 e32 e33 |   | e30 e31 e32 e33 |
        //  +-               -+   +-               -+
        //  Where:
        //             +-                                   -+
        //  lookAt =   |  r_hatx  r_haty  r_hatz  -r_hat*eye |
        //             |  u_hatx  u_haty  u_hatz  -u_hat*eye |
        //             | -l_hatx -l_haty -l_hatz   l_hat*eye |
        //             |       0       0       0           1 |
        //             +-                                   -+
        //
        //  l_hat = normalized(at - eye)
        //  r_hat = normalized(l x up)
        //  u_hat = normalized(r x l)
        //  ... and all other components are normalized
        ///////////////////////////////////////////////////////////////
        // Compute l_hat = normalized(at - eye)
        float l_hat[] = new float[3]; // l_hat = [l_hatx, l_haty, l_hatz]
        float lx = atx - eyex;
        float ly = aty - eyey;
        float lz = atz - eyez;
        float length = (float) java.lang.Math.sqrt((double) (lx*lx + ly*ly + lz*lz));
        l_hat[0] = lx / length; // normalize
        l_hat[1] = ly / length; // normalize
        l_hat[2] = lz / length; // normalize

        // Compute r_hat = normalized(l x up)
        float r_hat[] = new float[3]; // r_hat = [r_hatx, r_haty, r_hatz]
        // a x b := [ a[2] b[3] - a[3] b[2], a[3] b[1] - a[1] b[3], a[1] b[2] - a[2] b[1] ]
        float rx = ly*upz - lz*upy;
        float ry = lz*upx - lx*upz;
        float rz = lx*upy - ly*upx;
        length = (float) java.lang.Math.sqrt((double) (rx*rx + ry*ry + rz*rz));
        r_hat[0] = rx / length; // normalize
        r_hat[1] = ry / length; // normalize
        r_hat[2] = rz / length; // normalize
        //Point3D p = new Point3D(r_hat[0], r_hat[1], r_hat[2]);
        //System.out.println(p);

        // Compute u_hat = normalized(r x l)
        float u_hat[] = new float[3]; // u_hat = [u_hatx, u_haty, u_hatz]
        // a x b := [ a[2] b[3] - a[3] b[2], a[3] b[1] - a[1] b[3], a[1] b[2] - a[2] b[1] ]
        float ux = ry*lz - rz*ly;
        float uy = rz*lx - rx*lz;
        float uz = rx*ly - ry*lx;
        length = (float) java.lang.Math.sqrt((double) (ux*ux + uy*uy + uz*uz));
        u_hat[0] = ux / length; // normalize
        u_hat[1] = uy / length; // normalize
        u_hat[2] = uz / length; // normalize


        Matrix3D lookAtM3D = new Matrix3D(); // New 3D identity matrix
        lookAtM3D.set(0, 0, r_hat[0]); // r_hatx
        lookAtM3D.set(0, 1, r_hat[1]); // r_haty
        lookAtM3D.set(0, 2, r_hat[2]); // r_hatz
        lookAtM3D.set(0, 3, -(r_hat[0]*eyex +  // r_hatx*eyex +
                              r_hat[1]*eyey +  // r_haty*eyey +
                              r_hat[2]*eyez)); // r_hatz*eyez
        lookAtM3D.set(1, 0, u_hat[0]); // u_hatx
        lookAtM3D.set(1, 1, u_hat[1]); // u_haty
        lookAtM3D.set(1, 2, u_hat[2]); // u_hatz
        lookAtM3D.set(1, 3, -(u_hat[0]*eyex +  // u_hatx*eyex
                              u_hat[1]*eyey +  // u_haty*eyey
                              u_hat[2]*eyez)); // u_hatz*eyez
        lookAtM3D.set(2, 0, -l_hat[0]); // -l_hatx
        lookAtM3D.set(2, 1, -l_hat[1]); // -l_haty
        lookAtM3D.set(2, 2, -l_hat[2]); // -l_hatz
        lookAtM3D.set(2, 3, (l_hat[0]*eyex +  // l_hatx*eyex
                             l_hat[1]*eyey +  // l_haty*eyey
                             l_hat[2]*eyez)); // l_hatz*eyez
        // phew!
        compose(lookAtM3D); // Multiply lookAt matrix by this
    }

    ///////////////////////////////////////////////////////////////////
    //
    // Assume the following projection transformations
    // transform points into the canonical viewing space
    //
    ///////////////////////////////////////////////////////////////////
    public void perspective(float left, float right,
                            float bottom, float top,
                            float near, float far) {
        ///////////////////////////////////////////////////////////////
        //  Slide 44 of Lecture 12
        ///////////////////////////////////////////////////////////////
        //          this        =         this        * persp
        //  +-               -+   +-               -+
        //  | e00 e01 e02 e03 |   | e00 e01 e02 e03 |
        //  | e10 e11 e12 e13 | = | e10 e11 e12 e13 | * persp
        //  | e20 e21 e22 e23 |   | e20 e21 e22 e23 |
        //  | e30 e31 e32 e33 |   | e30 e31 e32 e33 |
        //  +-               -+   +-               -+
        //  Where:
        //            +-                                              -+
        //  persp =   |  2*N/(R-L)        0  -(R+L)/(R-L)           0  |
        //            |         0  2*N/(B-T) -(B+T)/(B-T)           0  |
        //            |         0         0   (F+N)/(F-N) -2*F*N/(F-N) |
        //            |         0         0            1            0  |
        //            +-                                              -+
        //  L = left
        //  R = right
        //  B = bottom
        //  T = top
        //  N = near
        //  F = far
        ///////////////////////////////////////////////////////////////
        Matrix3D perspM3D = new Matrix3D(); // New 3D identity matrix
        perspM3D.set(0, 0, 2*near/(right-left));        // 2*N/(R-L)
        perspM3D.set(0, 2, -(right+left)/(right-left)); // -(R+L)/(R-L)
        perspM3D.set(1, 1, 2*near/(bottom-top));        // 2*N/(B-T)
        perspM3D.set(1, 2, -(bottom+top)/(bottom-top)); // -(B+T)/(B-T)
        perspM3D.set(2, 2, (far+near)/(far-near));      // (F+N)/(F-N)
        perspM3D.set(2, 3, -2*far*near/(far-near));     // -2*F*N/(F-N)
        perspM3D.set(3, 2, 1.0f);
        perspM3D.set(3, 3, 0.0f);
        compose(perspM3D); // Multiply perspective matrix by this
    }

    public void orthographic(float left, float right,
                             float bottom, float top,
                             float near, float far) {
        ///////////////////////////////////////////////////////////////
        //  Slide 44 of Lecture 12
        ///////////////////////////////////////////////////////////////
        //          this        =         this
        //  +-               -+   +-               -+
        //  | e00 e01 e02 e03 |   | e00 e01 e02 e03 |
        //  | e10 e11 e12 e13 | = | e10 e11 e12 e13 | * ortho
        //  | e20 e21 e22 e23 |   | e20 e21 e22 e23 |
        //  | e30 e31 e32 e33 |   | e30 e31 e32 e33 |
        //  +-               -+   +-               -+
        //  Where:
        //            +-                                     -+
        //  ortho =   |  2/(R-L)      0       0  -(R+L)/(R-L) |
        //            |       0  2/(B-T)      0  -(B+T)/(B-T) |
        //            |       0       0  2/(F-N) -(F+N)/(F-N) |
        //            |       0       0       0            1  |
        //            +-                                     -+
        //  L = left
        //  R = right
        //  B = bottom
        //  T = top
        //  N = near
        //  F = far
        ///////////////////////////////////////////////////////////////
        Matrix3D orthoM3D = new Matrix3D(); // New 3D identity matrix
        orthoM3D.set(0, 0, 2/(right-left));             // 2/(R-L)
        orthoM3D.set(0, 3, -(right+left)/(right-left)); // -(R+L)/(R-L)
        orthoM3D.set(1, 1, 2/(bottom-top));             // 2/(B-T)
        orthoM3D.set(1, 3, -(bottom+top)/(bottom-top)); // -(B+T)/(B-T)
        orthoM3D.set(2, 2, 2/(far-near));               // 2/(F-N)
        orthoM3D.set(2, 3, -(far+near)/(far-near));     // (F+N)/(F-N)
        compose(orthoM3D); // Multiply orthographic matrix by this
    }

    public String toString() {
        ///////////////////////////////////////////////////////////////
        // Pretty prints current tranformation matrix to a string
        ///////////////////////////////////////////////////////////////
        String str;
        int col_max[] = new int[4];
        col_max[0] = (new Float(get(0,0))).toString().length();
        col_max[1] = (new Float(get(0,1))).toString().length();
        col_max[2] = (new Float(get(0,2))).toString().length();
        col_max[3] = (new Float(get(0,3))).toString().length();
        for (int col = 0; col<=3; col++) {
            for (int row = 1; row<=3; row++) {
                int num = (new Float(get(row,col))).toString().length();
                if ( num > col_max[col] )
                    col_max[col] = num;
            }
        }

        //for (int col = 0; col<=3; col++) {
        //    System.out.println("col_max["+col+"]="+col_max[col]);
        //}

        str = "+-";
        for (int i = 1; i<=(col_max[0]+col_max[1]+col_max[2]+col_max[3]+3); i++) {
            str += " ";
        }
        str += "-+\n";
        for (int row = 0; row<=3; row++) {
            str += "| ";
            for (int col = 0; col<=3; col++) {
                for (int i = (new Float(get(row,col))).toString().length(); i < col_max[col]; i++) {
                    str += " ";
                }
                str += get(row,col);
                str += " ";
            }
            str += "|\n";
        }
        str += "+-";
        for (int i = 1; i<=(col_max[0]+col_max[1]+col_max[2]+col_max[3]+3); i++) {
            str += " ";
        }
        str += "-+";

        return str;
    }

    public String[] toString2() {
        ///////////////////////////////////////////////////////////////
        // Pretty prints current tranformation matrix to an array
        // of strings, one string per line.
        ///////////////////////////////////////////////////////////////
        String str[] = new String[6];
        int col_max[] = new int[4];
        col_max[0] = (new Float(get(0,0))).toString().length();
        col_max[1] = (new Float(get(0,1))).toString().length();
        col_max[2] = (new Float(get(0,2))).toString().length();
        col_max[3] = (new Float(get(0,3))).toString().length();
        for (int col = 0; col<=3; col++) {
            for (int row = 1; row<=3; row++) {
                int num = (new Float(get(row,col))).toString().length();
                if ( num > col_max[col] )
                    col_max[col] = num;
            }
        }

        //for (int col = 0; col<=3; col++) {
        //    System.out.println("col_max["+col+"]="+col_max[col]);
        //}

        str[0] = "+-";
        for (int i = 1; i<=(col_max[0]+col_max[1]+col_max[2]+col_max[3]+3); i++) {
            str[0] += " ";
        }
        str[0] += "-+";
        str[5] = str[0];

        for (int row = 0; row<=3; row++) {
            str[1+row] = "| ";
            for (int col = 0; col<=3; col++) {
                for (int i = (new Float(get(row,col))).toString().length(); i < col_max[col]; i++) {
                    str[1+row] += " ";
                }
                str[1+row] += get(row,col);
                str[1+row] += " ";
            }
            str[1+row] += "|";
        }

        return str;
    }
}