/*
 *   __               .__       .__  ._____.
 * _/  |_  _______  __|__| ____ |  | |__\_ |__   ______
 * \   __\/  _ \  \/  /  |/ ___\|  | |  || __ \ /  ___/
 *  |  | (  <_> >    <|  \  \___|  |_|  || \_\ \\___ \
 *  |__|  \____/__/\_ \__|\___  >____/__||___  /____  >
 *                   \/       \/             \/     \/
 *
 * Copyright (c) 2006-2011 Karsten Schmidt
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * http://creativecommons.org/licenses/LGPL/2.1/
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA
 */
package toxi.geom;

import toxi.math.MathUtils;

/**
 * Implements a simple row-major 4x4 matrix class, all matrix operations are
 * applied to new instances. Use {@link #transpose()} to convert from
 * column-major formats...
 */
public class Matrix4x4 {

    private static final Matrix4x4 TEMP = new Matrix4x4();

    /**
     * Given a MxM array "matrix0", this function replaces it with the LU
     * decomposition of a row-wise permutation of itself. The input parameters
     * are "matrix0" and "dimen". The array "matrix0" is also an output
     * parameter. The vector "row_perm[4]" is an output parameter that contains
     * the row permutations resulting from partial pivoting. The output
     * parameter "even_row_xchg" is 1 when the number of row exchanges is even,
     * or -1 otherwise. Assumes data type is always double.
     *
     * This function is similar to luDecomposition, except that it is tuned
     * specifically for 4x4 matrices.
     *
     * Reference: Press, Flannery, Teukolsky, Vetterling,
     * _Numerical_Recipes_in_C_, Cambridge University Press, 1988, pp 40-45.
     *
     * @param matrix0
     * @param row_perm
     * @param width
     * @return true if the matrix is nonsingular, or false otherwise.
     */
    static boolean LUDecomposition(double[] matrix0, int[] row_perm, int width) {
        double row_scale[] = new double[width];
        // Determine implicit scaling information by looping over rows
        {
            int i, j;
            int ptr, rs;
            double big, temp;

            ptr = 0;
            rs = 0;

            // For each row ...
            i = width;
            while (i-- != 0) {
                big = 0.0;

                // For each column, find the largest element in the row
                j = width;
                while (j-- != 0) {
                    temp = matrix0[ptr++];
                    temp = Math.abs(temp);
                    if (temp > big) {
                        big = temp;
                    }
                }
                // Is the matrix singular?
                if (big == 0.0) {
                    return false;
                }
                row_scale[rs++] = 1.0 / big;
            }
        }

        {
            int j;
            int mtx = 0;

            // For all columns, execute Crout's method
            for (j = 0; j < width; j++) {
                int i, imax, k;
                int target, p1, p2;
                double sum, big, temp;

                // Determine elements of upper diagonal matrix U
                for (i = 0; i < j; i++) {
                    target = mtx + (width * i) + j;
                    sum = matrix0[target];
                    k = i;
                    p1 = mtx + (width * i);
                    p2 = mtx + j;
                    while (k-- != 0) {
                        sum -= matrix0[p1] * matrix0[p2];
                        p1++;
                        p2 += width;
                    }
                    matrix0[target] = sum;
                }

                // Search for largest pivot element and calculate
                // intermediate elements of lower diagonal matrix L.
                big = 0.0;
                imax = -1;
                for (i = j; i < width; i++) {
                    target = mtx + (width * i) + j;
                    sum = matrix0[target];
                    k = j;
                    p1 = mtx + (width * i);
                    p2 = mtx + j;
                    while (k-- != 0) {
                        sum -= matrix0[p1] * matrix0[p2];
                        p1++;
                        p2 += width;
                    }
                    matrix0[target] = sum;

                    // Is this the best pivot so far?
                    if ((temp = row_scale[i] * Math.abs(sum)) >= big) {
                        big = temp;
                        imax = i;
                    }
                }

                if (imax < 0) {
                    throw new RuntimeException();
                }

                // Is a row exchange necessary?
                if (j != imax) {
                    // Yes: exchange rows
                    k = width;
                    p1 = mtx + (width * imax);
                    p2 = mtx + (width * j);
                    while (k-- != 0) {
                        temp = matrix0[p1];
                        matrix0[p1++] = matrix0[p2];
                        matrix0[p2++] = temp;
                    }

                    // Record change in scale factor
                    row_scale[imax] = row_scale[j];
                }

                // Record row permutation
                row_perm[j] = imax;

                // Is the matrix singular
                if (matrix0[(mtx + (width * j) + j)] == 0.0) {
                    return false;
                }

                // Divide elements of lower diagonal matrix L by pivot
                if (j != width - 1) {
                    temp = 1.0 / (matrix0[(mtx + (width * j) + j)]);
                    target = mtx + (width * (j + 1)) + j;
                    i = (width - 1) - j;
                    while (i-- != 0) {
                        matrix0[target] *= temp;
                        target += width;
                    }
                }
            }
        }
        return true;
    }

    /**
     *
     */
    public double[][] matrix;

    /**
     *
     */
    protected double[] temp = new double[4];

    /**
     *
     */
    public Matrix4x4() {
        init();
        matrix[0][0] = 1;
        matrix[1][1] = 1;
        matrix[2][2] = 1;
        matrix[3][3] = 1;
    }

    /**
     *
     * @param v11
     * @param v12
     * @param v13
     * @param v14
     * @param v21
     * @param v22
     * @param v23
     * @param v24
     * @param v31
     * @param v32
     * @param v33
     * @param v34
     * @param v41
     * @param v42
     * @param v43
     * @param v44
     */
    public Matrix4x4(double v11, double v12, double v13, double v14,
            double v21, double v22, double v23, double v24, double v31,
            double v32, double v33, double v34, double v41, double v42,
            double v43, double v44) {
        init();
        double[] m = matrix[0];
        m[0] = v11;
        m[1] = v12;
        m[2] = v13;
        m[3] = v14;
        m = matrix[1];
        m[0] = v21;
        m[1] = v22;
        m[2] = v23;
        m[3] = v24;
        m = matrix[2];
        m[0] = v31;
        m[1] = v32;
        m[2] = v33;
        m[3] = v34;
        m = matrix[3];
        m[0] = v41;
        m[1] = v42;
        m[2] = v43;
        m[3] = v44;
    }

    /**
     * Initialising constructor from a 1d array. Assumes row-major ordering
     * (column index increases faster).
     *
     * @param array
     */
    public Matrix4x4(double[] array) {
        if (array.length != 9 && array.length != 16) {
            throw new RuntimeException("Array.length must == 9 or 16");
        }
        init();
        if (array.length == 16) {
            matrix[0][0] = array[0];
            matrix[0][1] = array[1];
            matrix[0][2] = array[2];
            matrix[0][3] = array[3];

            matrix[1][0] = array[4];
            matrix[1][1] = array[5];
            matrix[1][2] = array[6];
            matrix[1][3] = array[7];

            matrix[2][0] = array[8];
            matrix[2][1] = array[9];
            matrix[2][2] = array[10];
            matrix[2][3] = array[11];

            matrix[3][0] = array[12];
            matrix[3][1] = array[13];
            matrix[3][2] = array[14];
            matrix[3][3] = array[15];
        } else {
            matrix[0][0] = array[0];
            matrix[0][1] = array[1];
            matrix[0][2] = array[2];

            matrix[1][0] = array[3];
            matrix[1][1] = array[4];
            matrix[1][2] = array[5];

            matrix[2][0] = array[6];
            matrix[2][1] = array[7];
            matrix[2][2] = array[8];

            matrix[3][0] = array[9];
            matrix[3][1] = array[10];
            matrix[3][2] = array[11];
            matrix[3][3] = 1;
        }
    }

    /**
     *
     * @param m
     */
    public Matrix4x4(Matrix4x4 m) {
        init();
        for (int i = 0; i < 4; i++) {
            double[] mi = matrix[i];
            double[] mmi = m.matrix[i];
            mi[0] = mmi[0];
            mi[1] = mmi[1];
            mi[2] = mmi[2];
            mi[3] = mmi[3];
        }
    }

    /**
     *
     * @param rhs
     * @return
     */
    public Matrix4x4 add(Matrix4x4 rhs) {
        Matrix4x4 result = new Matrix4x4(this);
        return result.addSelf(rhs);
    }

    /**
     *
     * @param m
     * @return
     */
    public Matrix4x4 addSelf(Matrix4x4 m) {
        for (int i = 0; i < 4; i++) {
            double[] mi = matrix[i];
            double[] rhsm = m.matrix[i];
            mi[0] += rhsm[0];
            mi[1] += rhsm[1];
            mi[2] += rhsm[2];
            mi[3] += rhsm[3];
        }
        return this;
    }

    /**
     * Creates a copy of the given vector, transformed by this matrix.
     *
     * @param v
     * @return transformed vector
     */
    public Vec3D applyTo(ReadonlyVec3D v) {
        return applyToSelf(new Vec3D(v));
    }

    /**
     *
     * @param v
     * @return
     */
    public Vec3D applyToSelf(Vec3D v) {
        for (int i = 0; i < 4; i++) {
            double[] m = matrix[i];
            temp[i] = v.x * m[0] + v.y * m[1] + v.z * m[2] + m[3];
        }
        v.set((float) temp[0], (float) temp[1], (float) temp[2]).scaleSelf(
                (float) (1.0 / temp[3]));
        return v;
    }

    /**
     *
     * @return
     */
    public Matrix4x4 copy() {
        return new Matrix4x4(this);
    }

    /**
     *
     * @return
     */
    public Matrix4x4 getInverted() {
        return new Matrix4x4(this).invert();
    }

    /**
     *
     * @param axis
     * @param theta
     * @return
     */
    public Matrix4x4 getRotatedAroundAxis(ReadonlyVec3D axis, double theta) {
        return new Matrix4x4(this).rotateAroundAxis(axis, theta);
    }

    /**
     *
     * @param theta
     * @return
     */
    public Matrix4x4 getRotatedX(double theta) {
        return new Matrix4x4(this).rotateX(theta);
    }

    /**
     *
     * @param theta
     * @return
     */
    public Matrix4x4 getRotatedY(double theta) {
        return new Matrix4x4(this).rotateY(theta);
    }

    /**
     *
     * @param theta
     * @return
     */
    public Matrix4x4 getRotatedZ(double theta) {
        return new Matrix4x4(this).rotateZ(theta);
    }

    /**
     *
     * @return
     */
    public Matrix4x4 getTransposed() {
        return new Matrix4x4(this).transpose();
    }

    /**
     *
     * @return
     */
    public Matrix4x4 identity() {
        double[] m;
        m = matrix[0];
        m[1] = m[2] = m[3] = 0;
        m = matrix[1];
        m[0] = m[2] = m[3] = 0;
        m = matrix[2];
        m[0] = m[1] = m[3] = 0;
        m = matrix[3];
        m[0] = m[1] = m[2] = 0;
        matrix[0][0] = 1;
        matrix[1][1] = 1;
        matrix[2][2] = 1;
        matrix[3][3] = 1;
        return this;
    }

    private void init() {
        matrix = new double[][]{
            new double[4], new double[4], new double[4], new double[4]
        };
    }

    /**
     * Matrix Inversion using Cramer's Method Computes Adjoint matrix divided by
     * determinant Code modified from
     * http://www.intel.com/design/pentiumiii/sml/24504301.pdf
     *
     * @return itself
     */
    public Matrix4x4 invert() {
        final double[] tmp = new double[12];
        final double[] src = new double[16];
        final double[] dst = new double[16];
        final double[] mat = toArray(null);

        for (int i = 0; i < 4; i++) {
            int i4 = i << 2;
            src[i] = mat[i4];
            src[i + 4] = mat[i4 + 1];
            src[i + 8] = mat[i4 + 2];
            src[i + 12] = mat[i4 + 3];
        }

        // calculate pairs for first 8 elements (cofactors)
        tmp[0] = src[10] * src[15];
        tmp[1] = src[11] * src[14];
        tmp[2] = src[9] * src[15];
        tmp[3] = src[11] * src[13];
        tmp[4] = src[9] * src[14];
        tmp[5] = src[10] * src[13];
        tmp[6] = src[8] * src[15];
        tmp[7] = src[11] * src[12];
        tmp[8] = src[8] * src[14];
        tmp[9] = src[10] * src[12];
        tmp[10] = src[8] * src[13];
        tmp[11] = src[9] * src[12];

        // calculate first 8 elements (cofactors)
        double src0 = src[0];
        double src1 = src[1];
        double src2 = src[2];
        double src3 = src[3];
        double src4 = src[4];
        double src5 = src[5];
        double src6 = src[6];
        double src7 = src[7];
        dst[0] = tmp[0] * src5 + tmp[3] * src6 + tmp[4] * src7;
        dst[0] -= tmp[1] * src5 + tmp[2] * src6 + tmp[5] * src7;
        dst[1] = tmp[1] * src4 + tmp[6] * src6 + tmp[9] * src7;
        dst[1] -= tmp[0] * src4 + tmp[7] * src6 + tmp[8] * src7;
        dst[2] = tmp[2] * src4 + tmp[7] * src5 + tmp[10] * src7;
        dst[2] -= tmp[3] * src4 + tmp[6] * src5 + tmp[11] * src7;
        dst[3] = tmp[5] * src4 + tmp[8] * src5 + tmp[11] * src6;
        dst[3] -= tmp[4] * src4 + tmp[9] * src5 + tmp[10] * src6;
        dst[4] = tmp[1] * src1 + tmp[2] * src2 + tmp[5] * src3;
        dst[4] -= tmp[0] * src1 + tmp[3] * src2 + tmp[4] * src3;
        dst[5] = tmp[0] * src0 + tmp[7] * src2 + tmp[8] * src3;
        dst[5] -= tmp[1] * src0 + tmp[6] * src2 + tmp[9] * src3;
        dst[6] = tmp[3] * src0 + tmp[6] * src1 + tmp[11] * src3;
        dst[6] -= tmp[2] * src0 + tmp[7] * src1 + tmp[10] * src3;
        dst[7] = tmp[4] * src0 + tmp[9] * src1 + tmp[10] * src2;
        dst[7] -= tmp[5] * src0 + tmp[8] * src1 + tmp[11] * src2;

        // calculate pairs for second 8 elements (cofactors)
        tmp[0] = src2 * src7;
        tmp[1] = src3 * src6;
        tmp[2] = src1 * src7;
        tmp[3] = src3 * src5;
        tmp[4] = src1 * src6;
        tmp[5] = src2 * src5;
        tmp[6] = src0 * src7;
        tmp[7] = src3 * src4;
        tmp[8] = src0 * src6;
        tmp[9] = src2 * src4;
        tmp[10] = src0 * src5;
        tmp[11] = src1 * src4;

        // calculate second 8 elements (cofactors)
        src0 = src[8];
        src1 = src[9];
        src2 = src[10];
        src3 = src[11];
        src4 = src[12];
        src5 = src[13];
        src6 = src[14];
        src7 = src[15];
        dst[8] = tmp[0] * src5 + tmp[3] * src6 + tmp[4] * src7;
        dst[8] -= tmp[1] * src5 + tmp[2] * src6 + tmp[5] * src7;
        dst[9] = tmp[1] * src4 + tmp[6] * src6 + tmp[9] * src7;
        dst[9] -= tmp[0] * src4 + tmp[7] * src6 + tmp[8] * src7;
        dst[10] = tmp[2] * src4 + tmp[7] * src5 + tmp[10] * src7;
        dst[10] -= tmp[3] * src4 + tmp[6] * src5 + tmp[11] * src7;
        dst[11] = tmp[5] * src4 + tmp[8] * src5 + tmp[11] * src6;
        dst[11] -= tmp[4] * src4 + tmp[9] * src5 + tmp[10] * src6;
        dst[12] = tmp[2] * src2 + tmp[5] * src3 + tmp[1] * src1;
        dst[12] -= tmp[4] * src3 + tmp[0] * src1 + tmp[3] * src2;
        dst[13] = tmp[8] * src3 + tmp[0] * src0 + tmp[7] * src2;
        dst[13] -= tmp[6] * src2 + tmp[9] * src3 + tmp[1] * src0;
        dst[14] = tmp[6] * src1 + tmp[11] * src3 + tmp[3] * src0;
        dst[14] -= tmp[10] * src3 + tmp[2] * src0 + tmp[7] * src1;
        dst[15] = tmp[10] * src2 + tmp[4] * src0 + tmp[9] * src1;
        dst[15] -= tmp[8] * src1 + tmp[11] * src2 + tmp[5] * src0;

        double det = 1.0 / (src[0] * dst[0] + src[1] * dst[1] + src[2] * dst[2] + src[3]
                * dst[3]);

        for (int i = 0, k = 0; i < 4; i++) {
            double[] m = matrix[i];
            for (int j = 0; j < 4; j++) {
                m[j] = dst[k++] * det;
            }
        }
        return this;
    }

    /**
     *
     * @param eye
     * @param target
     * @param up
     * @return
     */
    public Matrix4x4 lookAt(ReadonlyVec3D eye, ReadonlyVec3D target,
            ReadonlyVec3D up) {
        Vec3D f = eye.sub(target).normalize();
        Vec3D s = up.cross(f).normalize();
        Vec3D t = f.cross(s).normalize();
        return set(s.x, s.y, s.z, -s.dot(eye), t.x, t.y, t.z, -t.dot(eye), f.x,
                f.y, f.z, -f.dot(eye), 0, 0, 0, 1);
    }

    /**
     *
     * @param factor
     * @return
     */
    public Matrix4x4 multiply(double factor) {
        return new Matrix4x4(this).multiplySelf(factor);
    }

    /**
     * Matrix-Matrix Right-multiplication.
     *
     * @param mat
     * @return product as new matrix
     */
    public Matrix4x4 multiply(Matrix4x4 mat) {
        return new Matrix4x4(this).multiplySelf(mat);
    }

    /**
     * In-place matrix-scalar multiplication.
     *
     * @param factor
     * @return product applied to this matrix.
     */
    public Matrix4x4 multiplySelf(double factor) {
        for (int i = 0; i < 4; i++) {
            double[] m = matrix[i];
            m[0] *= factor;
            m[1] *= factor;
            m[2] *= factor;
            m[3] *= factor;
        }
        return this;
    }

    /**
     *
     * @param mat
     * @return
     */
    public Matrix4x4 multiplySelf(Matrix4x4 mat) {
        double[] mm0 = mat.matrix[0];
        double[] mm1 = mat.matrix[1];
        double[] mm2 = mat.matrix[2];
        double[] mm3 = mat.matrix[3];
        for (int i = 0; i < 4; i++) {
            double[] m = matrix[i];
            for (int j = 0; j < 4; j++) {
                temp[j] = m[0] * mm0[j] + m[1] * mm1[j] + m[2] * mm2[j] + m[3]
                        * mm3[j];
            }
            m[0] = temp[0];
            m[1] = temp[1];
            m[2] = temp[2];
            m[3] = temp[3];
        }
        return this;
    }

    /**
     * Applies rotation about arbitrary axis to matrix
     *
     * @param axis
     * @param theta
     * @return rotation applied to this matrix
     */
    public Matrix4x4 rotateAroundAxis(ReadonlyVec3D axis, double theta) {
        double x, y, z, s, c, t, tx, ty;
        x = axis.x();
        y = axis.y();
        z = axis.z();
        s = Math.sin(theta);
        c = Math.cos(theta);
        t = 1 - c;
        tx = t * x;
        ty = t * y;
        TEMP.set(tx * x + c, tx * y + s * z, tx * z - s * y, 0, tx * y - s * z,
                ty * y + c, ty * z + s * x, 0, tx * z + s * y, ty * z - s * x,
                t * z * z + c, 0, 0, 0, 0, 1);
        return this.multiplySelf(TEMP);
    }

    /**
     * Applies rotation about X to this matrix.
     *
     * @param theta rotation angle in radians
     * @return itself
     */
    public Matrix4x4 rotateX(double theta) {
        TEMP.identity();
        TEMP.matrix[1][1] = TEMP.matrix[2][2] = Math.cos(theta);
        TEMP.matrix[2][1] = Math.sin(theta);
        TEMP.matrix[1][2] = -TEMP.matrix[2][1];
        return this.multiplySelf(TEMP);
    }

    /**
     * Applies rotation about Y to this matrix.
     *
     * @param theta rotation angle in radians
     * @return itself
     */
    public Matrix4x4 rotateY(double theta) {
        TEMP.identity();
        TEMP.matrix[0][0] = TEMP.matrix[2][2] = Math.cos(theta);
        TEMP.matrix[0][2] = Math.sin(theta);
        TEMP.matrix[2][0] = -TEMP.matrix[0][2];
        return this.multiplySelf(TEMP);
    }

    // Apply Rotation about Z to Matrix
    /**
     *
     * @param theta
     * @return
     */
    public Matrix4x4 rotateZ(double theta) {
        TEMP.identity();
        TEMP.matrix[0][0] = TEMP.matrix[1][1] = Math.cos(theta);
        TEMP.matrix[1][0] = Math.sin(theta);
        TEMP.matrix[0][1] = -TEMP.matrix[1][0];
        return this.multiplySelf(TEMP);
    }

    /**
     *
     * @param scale
     * @return
     */
    public Matrix4x4 scale(double scale) {
        return new Matrix4x4(this).scaleSelf(scale);
    }

    /**
     *
     * @param scaleX
     * @param scaleY
     * @param scaleZ
     * @return
     */
    public Matrix4x4 scale(double scaleX, double scaleY, double scaleZ) {
        return new Matrix4x4(this).scaleSelf(scaleX, scaleY, scaleZ);
    }

    /**
     *
     * @param scale
     * @return
     */
    public Matrix4x4 scale(ReadonlyVec3D scale) {
        return new Matrix4x4(this).scaleSelf(scale.x(), scale.y(), scale.z());
    }

    /**
     *
     * @param scale
     * @return
     */
    public Matrix4x4 scaleSelf(double scale) {
        return scaleSelf(scale, scale, scale);
    }

    /**
     *
     * @param scaleX
     * @param scaleY
     * @param scaleZ
     * @return
     */
    public Matrix4x4 scaleSelf(double scaleX, double scaleY, double scaleZ) {
        TEMP.identity();
        TEMP.setScale(scaleX, scaleY, scaleZ);
        return this.multiplySelf(TEMP);
    }

    /**
     *
     * @param scale
     * @return
     */
    public Matrix4x4 scaleSelf(ReadonlyVec3D scale) {
        return scaleSelf(scale.x(), scale.y(), scale.z());
    }

    /**
     *
     * @param a
     * @param b
     * @param c
     * @param d
     * @param e
     * @param f
     * @param g
     * @param h
     * @param i
     * @param j
     * @param k
     * @param l
     * @param m
     * @param n
     * @param o
     * @param p
     * @return
     */
    public Matrix4x4 set(double a, double b, double c, double d, double e,
            double f, double g, double h, double i, double j, double k,
            double l, double m, double n, double o, double p) {
        double[] mat = matrix[0];
        mat[0] = a;
        mat[1] = b;
        mat[2] = c;
        mat[3] = d;
        mat = matrix[1];
        mat[0] = e;
        mat[1] = f;
        mat[2] = g;
        mat[3] = h;
        mat = matrix[2];
        mat[0] = i;
        mat[1] = j;
        mat[2] = k;
        mat[3] = l;
        mat = matrix[3];
        mat[0] = m;
        mat[1] = n;
        mat[2] = o;
        mat[3] = p;
        return this;
    }

    /**
     *
     * @param mat
     * @return
     */
    public Matrix4x4 set(Matrix4x4 mat) {
        for (int i = 0; i < 4; i++) {
            double[] m = matrix[i];
            double[] n = mat.matrix[i];
            m[0] = n[0];
            m[1] = n[1];
            m[2] = n[2];
            m[3] = n[3];
        }
        return this;
    }

    /**
     *
     * @param left
     * @param right
     * @param top
     * @param bottom
     * @param near
     * @param far
     * @return
     */
    public Matrix4x4 setFrustum(double left, double right, double top,
            double bottom, double near, double far) {
        return set((2.0 * near) / (right - left), 0, (left + right)
                / (right - left), 0, 0, (2.0 * near) / (top - bottom),
                (top + bottom) / (top - bottom), 0, 0, 0, -(near + far)
                / (far - near), (-2 * near * far) / (far - near), 0, 0,
                -1, 0);
    }

    /**
     *
     * @param left
     * @param right
     * @param top
     * @param bottom
     * @param near
     * @param far
     * @return
     */
    public Matrix4x4 setOrtho(double left, double right, double top,
            double bottom, double near, double far) {
        return set(2.0 / (right - left), 0, 0, (left + right) / (right - left),
                0, 2.0 / (top - bottom), 0, (top + bottom) / (top - bottom), 0,
                0, -2.0 / (far - near), (far + near) / (far - near), 0, 0, 0, 1);
    }

    /**
     *
     * @param fov
     * @param aspect
     * @param near
     * @param far
     * @return
     */
    public Matrix4x4 setPerspective(double fov, double aspect, double near,
            double far) {
        double y = near * Math.tan(0.5 * MathUtils.radians(fov));
        double x = aspect * y;
        return setFrustum(-x, x, y, -y, near, far);
    }

    /**
     *
     * @param x
     * @param y
     * @param z
     * @return
     */
    public Matrix4x4 setPosition(double x, double y, double z) {
        matrix[0][3] = x;
        matrix[1][3] = y;
        matrix[2][3] = z;
        return this;
    }

    /**
     *
     * @param scaleX
     * @param scaleY
     * @param scaleZ
     * @return
     */
    public Matrix4x4 setScale(double scaleX, double scaleY, double scaleZ) {
        matrix[0][0] = scaleX;
        matrix[1][1] = scaleY;
        matrix[2][2] = scaleZ;
        return this;
    }

    /**
     *
     * @param m
     * @return
     */
    public Matrix4x4 sub(Matrix4x4 m) {
        return new Matrix4x4(this).subSelf(m);
    }

    /**
     *
     * @param mat
     * @return
     */
    public Matrix4x4 subSelf(Matrix4x4 mat) {
        for (int i = 0; i < 4; i++) {
            double[] m = matrix[i];
            double[] n = mat.matrix[i];
            m[0] -= n[0];
            m[1] -= n[1];
            m[2] -= n[2];
            m[3] -= n[3];
        }
        return this;
    }

    /**
     * Copies all matrix elements into an linear array.
     *
     * @param result array (or null to create a new one)
     * @return matrix as 16 element array
     */
    public double[] toArray(double[] result) {
        if (result == null) {
            result = new double[16];
        }
        for (int i = 0, k = 0; i < 4; i++) {
            double[] m = matrix[i];
            for (int j = 0; j < 4; j++) {
                result[k++] = m[j];
            }
        }
        return result;
    }

    /**
     *
     * @param result
     * @return
     */
    public float[] toFloatArray(float[] result) {
        if (result == null) {
            result = new float[16];
        }
        double[] tmp = toArray(null);
        for (int i = 0; i < 16; i++) {
            result[i] = (float) tmp[i];
        }
        return result;
    }

    /*
     * (non-Javadoc)
     *
     * @see java.lang.Object#toString()
     */
    /**
     *
     * @return
     */
    @Override
    public String toString() {
        return String.join("\n",
                String.format("| %f %f %f %f |",
                        matrix[0][0], matrix[0][1], matrix[0][2], matrix[0][3]),
                String.format("| %f %f %f %f |",
                        matrix[1][0], matrix[1][1], matrix[1][2], matrix[1][3]),
                String.format("| %f %f %f %f |",
                        matrix[2][0], matrix[2][1], matrix[2][2], matrix[2][3]),
                String.format("| %f %f %f %f |",
                        matrix[3][0], matrix[3][1], matrix[3][2], matrix[3][3])
        );
    }

    /**
     *
     * @param result
     * @return
     */
    public float[] toTransposedFloatArray(float[] result) {
        if (result == null) {
            result = new float[16];
        }
        for (int i = 0, k = 0; i < 4; i++) {
            for (int j = 0; j < 4; j++) {
                result[k++] = (float) matrix[j][i];
            }
        }
        return result;
    }

    /**
     *
     * @param dx
     * @param dy
     * @param dz
     * @return
     */
    public Matrix4x4 translate(double dx, double dy, double dz) {
        return new Matrix4x4(this).translateSelf(dx, dy, dz);
    }

    /**
     *
     * @param trans
     * @return
     */
    public Matrix4x4 translate(ReadonlyVec3D trans) {
        return new Matrix4x4(this).translateSelf(trans.x(), trans.y(),
                trans.z());
    }

    /**
     *
     * @param dx
     * @param dy
     * @param dz
     * @return
     */
    public Matrix4x4 translateSelf(double dx, double dy, double dz) {
        TEMP.identity();
        TEMP.setPosition(dx, dy, dz);
        return this.multiplySelf(TEMP);
    }

    /**
     *
     * @param trans
     * @return
     */
    public Matrix4x4 translateSelf(ReadonlyVec3D trans) {
        return translateSelf(trans.x(), trans.y(), trans.z());
    }

    /**
     * Converts the matrix (in-place) between column-major to row-major order
     * (and vice versa).
     *
     * @return itself
     */
    public Matrix4x4 transpose() {
        return set(matrix[0][0], matrix[1][0], matrix[2][0], matrix[3][0],
                matrix[0][1], matrix[1][1], matrix[2][1], matrix[3][1],
                matrix[0][2], matrix[1][2], matrix[2][2], matrix[3][2],
                matrix[0][3], matrix[1][3], matrix[2][3], matrix[3][3]);
    }
}