Worldwind public release 0.2

[worldwind-tracker.git] / gov / nasa / worldwind / geom / Matrix4.java

/*
Copyright (C) 2001, 2006 United States Government
as represented by the Administrator of the
National Aeronautics and Space Administration.
All Rights Reserved.
*/
package gov.nasa.worldwind.geom;

import gov.nasa.worldwind.*;

/**
 * @author Tom Gaskins
 * @version $Id: Matrix4.java 1749 2007-05-06 19:48:14Z tgaskins $
 */
public class Matrix4 implements Matrix
{
    // TODO: scalar operations
    private double m11 = 1, m12, m13, m14, m21, m22 = 1, m23, m24, m31, m32, m33 = 1, m34, m41, m42, m43, m44 = 1;
    private boolean isOrthonormal = true;

    /**
     * Creates a new <code>Matrix4</code> as the identity matrix.
     */
    public Matrix4()
    {
    }

    /**
     * Creates a new <code>Matrix4</code> from an array of double precision floating point values. The caller must
     * provide at least sixteen values, and any values beyond the sixteenth are ignored.  Values are assigned in the
     * following order: (1, 1), (2, 1), (3, 1), (4, 1), (1, 2), (2, 2), (3, 2), (4, 2), (3, 3), (2, 3), (3, 3), (4, 3),
     * (1, 4), (2, 4), (3, 4), (4, 4).
     *
     * @param entries the values, must contain at least 16 values and may not be null
     * @throws IllegalArgumentException if <code>entries</code> is too short or null
     */
    public Matrix4(double[] entries)
    {
        if (entries == null)
        {
            String msg = WorldWind.retrieveErrMsg("nullValue.EntriesIsNull");
            WorldWind.logger().log(java.util.logging.Level.FINE, msg);
            throw new IllegalArgumentException(msg);
        }

        if (entries.length < 16)
        {
            String msg = WorldWind.retrieveErrMsg("geom.Matrix4.ArrayTooShort");
            WorldWind.logger().log(java.util.logging.Level.FINE, msg);
            throw new IllegalArgumentException(msg);
        }

        this.m11 = entries[0];
        this.m21 = entries[1];
        this.m31 = entries[2];
        this.m41 = entries[3];

        this.m12 = entries[4];
        this.m22 = entries[5];
        this.m32 = entries[6];
        this.m42 = entries[7];

        this.m13 = entries[8];
        this.m23 = entries[9];
        this.m33 = entries[10];
        this.m43 = entries[11];

        this.m14 = entries[12];
        this.m24 = entries[13];
        this.m34 = entries[14];
        this.m44 = entries[15];

        // TODO: Determine this by checking key entries
        this.isOrthonormal = false;
    }

    /**
     * Creates a new <code>Matrix4</code> from an array of single precision floating point values. The caller must
     * provide at least sixteen values, and any values beyond the sixteenth are ignored. Values are assigned in the
     * following order: (1, 1), (2, 1), (3, 1), (4, 1), (1, 2), (2, 2), (3, 2), (4, 2), (3, 3), (2, 3), (3, 3), (4, 3),
     * (1, 4), (2, 4), (3, 4), (4, 4).
     *
     * @param entries the values, must contain at least 16 values and may not be null
     * @throws IllegalArgumentException if <code>entries</code> is too short or null
     */
    public Matrix4(float[] entries)
    {
        if (entries == null)
        {
            String message = WorldWind.retrieveErrMsg("nullValue.EntriesIsNull");
            WorldWind.logger().log(java.util.logging.Level.FINE, message);
            throw new IllegalArgumentException(message);
        }

        if (entries.length < 16)
        {
            String msg = WorldWind.retrieveErrMsg("geom.Matrix4.ArrayTooShort");
            WorldWind.logger().log(java.util.logging.Level.FINE, msg);
            throw new IllegalArgumentException(msg);
        }

        this.m11 = entries[0];
        this.m21 = entries[1];
        this.m31 = entries[2];
        this.m41 = entries[3];

        this.m12 = entries[4];
        this.m22 = entries[5];
        this.m32 = entries[6];
        this.m42 = entries[7];

        this.m13 = entries[8];
        this.m23 = entries[9];
        this.m33 = entries[10];
        this.m43 = entries[11];

        this.m14 = entries[12];
        this.m24 = entries[13];
        this.m34 = entries[14];
        this.m44 = entries[15];

        // TODO: Optimize this by checking key entries
        this.isOrthonormal = false;
    }

    /**
     * Retrieves the entries comprising this <code>Matrix</code>. The returned array is always 16 entries long. Values
     * are place in as in the aray as follows: (1, 1), (2, 1), (3, 1), (4, 1), (1, 2), (2, 2), (3, 2), (4, 2), (3, 3),
     * (2, 3), (3, 3), (4, 3), (1, 4), (2, 4), (3, 4), (4, 4).
     *
     * @return an array, of length 16, containing this Matrices' entries.
     */
    public final double[] getEntries()
    {
        double[] entries = new double[16];

        entries[0] = this.m11;
        entries[1] = this.m21;
        entries[2] = this.m31;
        entries[3] = this.m41;

        entries[4] = this.m12;
        entries[5] = this.m22;
        entries[6] = this.m32;
        entries[7] = this.m42;

        entries[8] = this.m13;
        entries[9] = this.m23;
        entries[10] = this.m33;
        entries[11] = this.m43;

        entries[12] = this.m14;
        entries[13] = this.m24;
        entries[14] = this.m34;
        entries[15] = this.m44;

        return entries;
    }

    /**
     * Sets this <code>Matrix</code> to the identity matrix. This method causes internal changes to the
     * <code>Matrix</code> it operates on.
     *
     * @return <code>this</code>, set to the identity
     */
    public final Matrix setToIdentity()
    {
        this.m11 = 1;
        this.m12 = 0;
        this.m13 = 0;
        this.m14 = 0;
        this.m21 = 0;
        this.m22 = 1;
        this.m23 = 0;
        this.m24 = 0;
        this.m31 = 0;
        this.m32 = 0;
        this.m33 = 1;
        this.m34 = 0;
        this.m41 = 0;
        this.m42 = 0;
        this.m43 = 0;
        this.m44 = 1;

        this.isOrthonormal = true;

        return this;
    }

    /**
     * Obtains whether or not this <code>Matrix</code> is orthonormal. Orthonormal matrices possess unique properties
     * that can make algorithms more efficient.
     *
     * @return true if this is orthonormal, false otherwise
     */
    public final boolean isOrthonormal()
    {
        return this.isOrthonormal;
    }

    /**
     * Rotate this matrix by some angle around an arbitrary axis. A positive <code>Angle</code> indicates an
     * anti-clockwise direction. This method affects the internal state of this matrix.
     *
     * @param rotation the distance to rotate this matrix
     * @param axisX    the x component of the axis of rotation
     * @param axisY    the y component of the axis of rotation
     * @param axisZ    the z component of the axis of rotation
     * @return this <code>Matrix</code>, with the rotation applied
     * @throws IllegalArgumentException if <code>rotation</code> is null
     */
    public final Matrix rotate(Angle rotation, double axisX, double axisY, double axisZ)
    {
        if (rotation == null)
        {
            String message = WorldWind.retrieveErrMsg("nullValue.RotationAngleIsNull");
            WorldWind.logger().log(java.util.logging.Level.FINE, message);
            throw new IllegalArgumentException(message);
        }

        double ll = axisX * axisX + axisY * axisY + axisZ * axisZ;
        if (rotation.getDegrees() == 0 || ll == 0)
            return this;
        
        if (ll != 1) // if axis not unit length, normalize it.
        {
            double l = Math.sqrt(ll);
            axisX /= l;
            axisY /= l;
            axisZ /= l;
        }

        double c = rotation.cos();
        double s = rotation.sin();
        double c1 = 1 - c;
        Matrix4 o = new Matrix4();

        o.m11 = c + axisX * axisX * c1;
        o.m12 = axisX * axisY * c1 - axisZ * s;
        o.m13 = axisX * axisZ * c1 + axisY * s;
        o.m14 = 0;
        o.m21 = axisX * axisY * c1 + axisZ * s;
        o.m22 = c + axisY * axisY * c1;
        o.m23 = axisY * axisZ * c1 - axisX * s;
        o.m24 = 0;
        o.m31 = axisX * axisZ * c1 - axisY * s;
        o.m32 = axisY * axisZ * c1 + axisX * s;
        o.m33 = c + axisZ * axisZ * c1;
        o.m34 = 0;
        o.m41 = 0;
        o.m42 = 0;
        o.m43 = 0;
        o.m44 = 1;

        return this.multiply(o);
    }

    /**
     * Rotate this <code>Matrix</code> around the x-axis. A positive <code>Angle</code> indicates an anti-clockwise
     * direction. Changes the internal state of this <code>Matrix</code>.
     *
     * @param rotation the distance to rotate
     * @return this <code>Matrix</code>, rotated around the x-axis by <code>rotation</code> distance
     * @throws IllegalArgumentException if <code>rotation</code> is null
     */
    public final Matrix rotateX(Angle rotation)
    {
        if (rotation == null)
        {
            String message = WorldWind.retrieveErrMsg("nullValue.RotationAngleIsNull");
            WorldWind.logger().log(java.util.logging.Level.FINE, message);
            throw new IllegalArgumentException(message);
        }

        double c = rotation.cos();
        double s = rotation.sin();

        double n12 = this.m12 * c + this.m13 * s;
        double n13 = this.m12 * -s + this.m13 * c;

        double n22 = this.m22 * c + this.m23 * s;
        double n23 = this.m22 * -s + this.m23 * c;

        double n32 = this.m32 * c + this.m33 * s;
        double n33 = this.m32 * -s + this.m33 * c;

        double n42 = this.m42 * c + this.m43 * s;
        double n43 = this.m42 * -s + this.m43 * c;

        this.m12 = n12;
        this.m13 = n13;
        this.m22 = n22;
        this.m23 = n23;
        this.m32 = n32;
        this.m33 = n33;
        this.m42 = n42;
        this.m43 = n43;

        return this;
    }

    /**
     * Rotate this <code>Matrix</code> around the y-axis. A positive <code>Angle</code> indicates an anti-clockwise
     * direction. Changes the internal state of this <code>Matrix</code>.
     *
     * @param rotation the distance to rotate
     * @return this <code>Matrix</code>, rotated around the y-axis by <code>rotation</code> distance
     * @throws IllegalArgumentException if <code>rotation</code> is null
     */
    public final Matrix rotateY(Angle rotation)
    {
        if (rotation == null)
        {
            String message = WorldWind.retrieveErrMsg("nullValue.RotationAngleIsNull");
            WorldWind.logger().log(java.util.logging.Level.FINE, message);
            throw new IllegalArgumentException(message);
        }

        double c = rotation.cos();
        double s = rotation.sin();

        double n11 = this.m11 * c + this.m13 * -s;
        double n13 = this.m11 * s + this.m13 * c;

        double n21 = this.m21 * c + this.m23 * -s;
        double n23 = this.m21 * s + this.m23 * c;

        double n31 = this.m31 * c + this.m33 * -s;
        double n33 = this.m31 * s + this.m33 * c;

        double n41 = this.m41 * c + this.m43 * -s;
        double n43 = this.m41 * s + this.m43 * c;

        this.m11 = n11;
        this.m13 = n13;
        this.m21 = n21;
        this.m23 = n23;
        this.m31 = n31;
        this.m33 = n33;
        this.m41 = n41;
        this.m43 = n43;

        return this;
    }

    /**
     * Rotate this <code>Matrix</code> around the z-axis. A positive <code>Angle</code> indicates an anti-clockwise
     * direction. Changes the internal state of this <code>Matrix</code>.
     *
     * @param rotation the distance to rotate
     * @return this <code>Matrix</code>, rotated around the z-axis by <code>rotation</code> distance
     * @throws IllegalArgumentException if <code>rotation</code> is null
     */
    public final Matrix rotateZ(Angle rotation)
    {
        if (rotation == null)
        {
            String message = WorldWind.retrieveErrMsg("nullValue.RotationAngleIsNull");
            WorldWind.logger().log(java.util.logging.Level.FINE, message);
            throw new IllegalArgumentException(message);
        }

        double c = rotation.cos();
        double s = rotation.sin();

        double n11 = this.m11 * c + this.m12 * s;
        double n12 = this.m11 * -s + this.m12 * c;

        double n21 = this.m21 * c + this.m22 * s;
        double n22 = this.m21 * -s + this.m22 * c;

        double n31 = this.m31 * c + this.m32 * s;
        double n32 = this.m31 * -s + this.m32 * c;

        double n41 = this.m41 * c + this.m42 * s;
        double n42 = this.m41 * -s + this.m42 * c;

        this.m11 = n11;
        this.m12 = n12;
        this.m21 = n21;
        this.m22 = n22;
        this.m31 = n31;
        this.m32 = n32;
        this.m41 = n41;
        this.m42 = n42;

        return this;
    }

    /**
     * Translates this <code>Matrix</code> in three dimensional space. Changes the internal state of this
     * <code>Matrix</code>.
     *
     * @param x the distance to translate along the x-axis
     * @param y the distance to translate along the y-axis
     * @param z the distance to translate along the z-axis
     * @return this matrix, translated by (x, y, z)
     */
    public Matrix translate(double x, double y, double z)
    {
        this.m14 = this.m11 * x + this.m12 * y + this.m13 * z + this.m14;
        this.m24 = this.m21 * x + this.m22 * y + this.m23 * z + this.m24;
        this.m34 = this.m31 * x + this.m32 * y + this.m33 * z + this.m34;
        this.m44 = this.m41 * x + this.m42 * y + this.m43 * z + this.m44;

        return this;
    }

    /**
     * Translates this <code>Matrix</code> in three dimansional space. Changes the internal state of this
     * <code>Matrix</code>. The x, y and z co-ordinates are used to translate along the x, y and z axes respectively.
     *
     * @param p the x, y and z distances to translate as a <code>Point</code>
     * @return this <code>Matrix</code>, translated by the distances defined in <code>p</code>
     * @throws IllegalArgumentException if <code>p</code> is null
     */
    public final Matrix translate(Point p)
    {
        if (p == null)
        {
            String message = WorldWind.retrieveErrMsg("nullValue.PointIsNull");
            WorldWind.logger().log(java.util.logging.Level.FINE, message);
            throw new IllegalArgumentException(message);
        }

        return this.translate(p.x(), p.y(), p.z());
    }

    /**
     * Adds this another matrix to this one.
     *
     * @param m the <code>Matrix</code> to add to this one
     * @return this Matrix, with the <code>m</code> added to it
     * @throws IllegalArgumentException if <code>m</code> is null
     */
    public final Matrix add(Matrix m)
    {
        if (m == null)
        {
            String message = WorldWind.retrieveErrMsg("nullValue.MatrixIsNull");
            WorldWind.logger().log(java.util.logging.Level.FINE, message);
            throw new IllegalArgumentException(message);
        }

        Matrix4 o = (Matrix4) m;

        this.m11 += o.m11;
        this.m12 += o.m12;
        this.m13 += o.m13;
        this.m14 += o.m14;
        this.m21 += o.m21;
        this.m22 += o.m22;
        this.m23 += o.m23;
        this.m24 += o.m24;
        this.m31 += o.m31;
        this.m32 += o.m32;
        this.m33 += o.m33;
        this.m34 += o.m34;
        this.m41 += o.m41;
        this.m42 += o.m42;
        this.m43 += o.m43;
        this.m44 += o.m44;

        this.isOrthonormal = this.isOrthonormal || o.isOrthonormal;

        return this;
    }

    /**
     * Performs a cross multiplication with another <code>Matrix</code>. Alters the state of this <code>Matrix</code>.
     *
     * @param m another <code>Matrix</code>
     * @return this, postmultiplied by <code>m</code>
     * @throws IllegalArgumentException if <code>m</code> is null
     */
    public final Matrix multiply(Matrix m)
    {
        if (m == null)
        {
            String message = WorldWind.retrieveErrMsg("nullValue.MatrixIsNull");
            WorldWind.logger().log(java.util.logging.Level.FINE, message);
            throw new IllegalArgumentException(message);
        }

        Matrix4 o = (Matrix4) m;

        double n11 = this.m11 * o.m11 + this.m12 * o.m21 + this.m13 * o.m31 + this.m14 * o.m41;
        double n12 = this.m11 * o.m12 + this.m12 * o.m22 + this.m13 * o.m32 + this.m14 * o.m42;
        double n13 = this.m11 * o.m13 + this.m12 * o.m23 + this.m13 * o.m33 + this.m14 * o.m43;
        double n14 = this.m11 * o.m14 + this.m12 * o.m24 + this.m13 * o.m34 + this.m14 * o.m44;

        double n21 = this.m21 * o.m11 + this.m22 * o.m21 + this.m23 * o.m31 + this.m24 * o.m41;
        double n22 = this.m21 * o.m12 + this.m22 * o.m22 + this.m23 * o.m32 + this.m24 * o.m42;
        double n23 = this.m21 * o.m13 + this.m22 * o.m23 + this.m23 * o.m33 + this.m24 * o.m43;
        double n24 = this.m21 * o.m14 + this.m22 * o.m24 + this.m23 * o.m34 + this.m24 * o.m44;

        double n31 = this.m31 * o.m11 + this.m32 * o.m21 + this.m33 * o.m31 + this.m34 * o.m41;
        double n32 = this.m31 * o.m12 + this.m32 * o.m22 + this.m33 * o.m32 + this.m34 * o.m42;
        double n33 = this.m31 * o.m13 + this.m32 * o.m23 + this.m33 * o.m33 + this.m34 * o.m43;
        double n34 = this.m31 * o.m14 + this.m32 * o.m24 + this.m33 * o.m34 + this.m34 * o.m44;

        double n41 = this.m41 * o.m11 + this.m42 * o.m21 + this.m43 * o.m31 + this.m44 * o.m41;
        double n42 = this.m41 * o.m12 + this.m42 * o.m22 + this.m43 * o.m32 + this.m44 * o.m42;
        double n43 = this.m41 * o.m13 + this.m42 * o.m23 + this.m43 * o.m33 + this.m44 * o.m43;
        double n44 = this.m41 * o.m14 + this.m42 * o.m24 + this.m43 * o.m34 + this.m44 * o.m44;

        this.m11 = n11;
        this.m12 = n12;
        this.m13 = n13;
        this.m14 = n14;
        this.m21 = n21;
        this.m22 = n22;
        this.m23 = n23;
        this.m24 = n24;
        this.m31 = n31;
        this.m32 = n32;
        this.m33 = n33;
        this.m34 = n34;
        this.m41 = n41;
        this.m42 = n42;
        this.m43 = n43;
        this.m44 = n44;

        this.isOrthonormal = this.isOrthonormal || o.isOrthonormal;

        return this;
    }

    /**
     * Obtains the transpose of this <code>Matrix</code>. Does not alter the state of this <code>Matrix</code>.
     *
     * @return the transpoase of this <code>Matrix</code>
     */
    public final Matrix getTranspose()
    {
        Matrix4 transpose = new Matrix4();

        transpose.m11 = this.m11;
        transpose.m12 = this.m21;
        transpose.m13 = this.m31;
        transpose.m14 = this.m41;
        transpose.m21 = this.m12;
        transpose.m22 = this.m22;
        transpose.m23 = this.m32;
        transpose.m24 = this.m42;
        transpose.m31 = this.m13;
        transpose.m32 = this.m23;
        transpose.m33 = this.m33;
        transpose.m34 = this.m43;
        transpose.m41 = this.m14;
        transpose.m42 = this.m24;
        transpose.m43 = this.m34;
        transpose.m44 = this.m44;

        transpose.isOrthonormal = this.isOrthonormal;

        return transpose;
    }

    /**
     * Obtain the inverse of this <code>Matrix</code>.
     *
     * @return the inverse of this <code>Matrix</code>.
     */
    public final Matrix getInverse()
    {
        Matrix4 inverse;

        if (this.isOrthonormal)
        {
            inverse = this.orthonormalInverse();
        }
        else
        {
            inverse = this.generalInverse();
        }

        inverse.isOrthonormal = this.isOrthonormal;

        return inverse;
    }

    private Matrix4 orthonormalInverse()
    {
        Matrix4 inverse = new Matrix4();

        // Transpose of upper 3x3.
        inverse.m11 = this.m11;
        inverse.m12 = this.m21;
        inverse.m13 = this.m31;

        inverse.m21 = this.m12;
        inverse.m22 = this.m22;
        inverse.m23 = this.m32;

        inverse.m31 = this.m13;
        inverse.m32 = this.m23;
        inverse.m33 = this.m33;

        // Upper 3x3 inverse times current translation (4th column).
        inverse.m14 = -(inverse.m11 * this.m14 + inverse.m12 * this.m24 + inverse.m13 * this.m34);
        inverse.m24 = -(inverse.m21 * this.m14 + inverse.m22 * this.m24 + inverse.m23 * this.m34);
        inverse.m34 = -(inverse.m31 * this.m14 + inverse.m32 * this.m24 + inverse.m33 * this.m34);

        return inverse;
    }

    // TODO: Fix generalInverse. It's not producing correct inverses.
    private Matrix4 generalInverse()
    {
        double d = this.determinant();
        if (d == 0)
        {
            return null;
        }

        double id = 1 / d;
        Matrix4 inverse = new Matrix4();

        // Form the adjoint matrix.
        double a1 = this.m33 * this.m44 - this.m34 * this.m43;
        double a2 = this.m34 * this.m42 - this.m32 * this.m44;
        double a3 = this.m32 * this.m43 - this.m33 * this.m42;
        double a4 = this.m23 * this.m44 - this.m24 * this.m43;
        double a5 = this.m24 * this.m42 - this.m22 * this.m44;
        double a6 = this.m23 * this.m34 - this.m24 * this.m33;
        double a7 = this.m22 * this.m33 - this.m23 * this.m32;
        double a8 = this.m34 * this.m41 - this.m31 * this.m44;
        double a9 = this.m31 * this.m43 - this.m33 * this.m41;
        double a21 = this.m24 * this.m41 - this.m21 * this.m44;
        double a22 = this.m24 * this.m31 - this.m21 * this.m34;
        double a23 = this.m32 * this.m44 - this.m34 * this.m42;
        double a24 = this.m31 * this.m42 - this.m32 * this.m41;
        double a14 = this.m21 * this.m42 - this.m22 * this.m41;
        double a15 = this.m21 * this.m32 - this.m22 * this.m31;
        double a16 = this.m33 * this.m41 - this.m31 * this.m43;

        inverse.m11 = id
            * this.m22 * a1
            + this.m23 * a2
            + this.m24 * a3;
        inverse.m12 = -id
            * this.m12 * a1
            + this.m13 * a2
            + this.m14 * a3;
        inverse.m13 = id
            * this.m12 * a4
            + this.m13 * (this.m24 * this.m42 - this.m22 * this.m44)
            + this.m14 * a5;
        inverse.m14 = -id
            * this.m12 * a6
            + this.m13 * (this.m24 * this.m32 - this.m22 * this.m34)
            + this.m14 * a7;
        inverse.m21 = -id
            * this.m21 * a1
            + this.m23 * a8
            + this.m24 * a9;
        inverse.m22 = id
            * this.m11 * a1
            + this.m13 * a8
            + this.m14 * a9;
        inverse.m23 = -id
            * this.m11 * a4
            + this.m13 * a21
            + this.m14 * (this.m21 * this.m43 - this.m23 * this.m41);
        inverse.m24 = -id
            * this.m11 * a6
            + this.m13 * a22
            + this.m14 * (this.m21 * this.m33 - this.m23 * this.m31);
        inverse.m31 = id
            * this.m21 * a23
            + this.m22 * a8
            + this.m24 * a24;
        inverse.m32 = -id
            * this.m11 * a23
            + this.m12 * a8
            + this.m14 * a24;
        inverse.m33 = -id
            * this.m11 * (this.m22 * this.m44 - this.m24 * this.m42)
            + this.m12 * a21
            + this.m14 * a14;
        inverse.m34 = id
            * this.m11 * (this.m22 * this.m34 - this.m24 * this.m32)
            + this.m12 * a22
            + this.m14 * a15;
        inverse.m41 = -id
            * this.m21 * a3
            + this.m22 * a16
            + this.m23 * a24;
        inverse.m42 = -id
            * this.m11 * a3
            + this.m12 * a16
            + this.m13 * a24;
        inverse.m43 = id
            * this.m11 * a5
            + this.m12 * (this.m23 * this.m41 - this.m21 * this.m43)
            + this.m13 * a14;
        inverse.m44 = -id
            * this.m11 * a7
            + this.m12 * (this.m23 * this.m31 - this.m21 * this.m33)
            + this.m13 * a15;

        return inverse;
    }

    /**
     * Obtains the determinant of this <code>Matrix</code>.
     *
     * @return the determinant
     */
    public final double determinant()
    {
        return
            this.m11 * (
                (this.m22 * this.m33 * this.m44 + this.m23 * this.m34 * this.m42 + this.m24 * this.m32 * this.m43)
                    - this.m24 * this.m33 * this.m42
                    - this.m22 * this.m34 * this.m43
                    - this.m23 * this.m32 * this.m44)
                - this.m12 * (
                (this.m21 * this.m33 * this.m44 + this.m23 * this.m34 * this.m41 + this.m24 * this.m31 * this.m43)
                    - this.m24 * this.m33 * this.m41
                    - this.m21 * this.m34 * this.m43
                    - this.m23 * this.m31 * this.m44)
                + this.m13 * (
                (this.m21 * this.m32 * this.m44 + this.m22 * this.m34 * this.m41 + this.m24 * this.m31 * this.m42)
                    - this.m24 * this.m32 * this.m41
                    - this.m21 * this.m34 * this.m42
                    - this.m22 * this.m31 * this.m44)
                - this.m14 * (
                (this.m21 * this.m32 * this.m43 + this.m22 * this.m33 * this.m41 + this.m23 * this.m31 * this.m42)
                    - this.m23 * this.m32 * this.m41
                    - this.m21 * this.m33 * this.m42
                    - this.m22 * this.m31 * this.m43);
    }

    /**
     * Applies this <code>Matrix</code> to a <code>Point</code>.
     *
     * @param p the <code>Point</code> to transform
     * @return p, trasformed by the current state of this <code>Matrix</code>
     * @throws IllegalArgumentException if <code>p</code> is null
     */
    public final Point transform(Point p)
    {
        if (p == null)
        {
            String message = WorldWind.retrieveErrMsg("nullValue.PointIsNull");
            WorldWind.logger().log(java.util.logging.Level.FINE, message);
            throw new IllegalArgumentException(message);
        }

        double x = this.m11 * p.x() + this.m12 * p.y() + this.m13 * p.z() + this.m14 * p.w();
        double y = this.m21 * p.x() + this.m22 * p.y() + this.m23 * p.z() + this.m24 * p.w();
        double z = this.m31 * p.x() + this.m32 * p.y() + this.m33 * p.z() + this.m34 * p.w();
        double w = this.m41 * p.x() + this.m42 * p.y() + this.m43 * p.z() + this.m44 * p.w();

        return new Point(x, y, z, w);
    }

    @Override
    public boolean equals(Object o)
    {
        if (this == o)
            return true;
        if (o == null || getClass() != o.getClass())
            return false;

        final gov.nasa.worldwind.geom.Matrix4 matrix4 = (gov.nasa.worldwind.geom.Matrix4) o;

        if (isOrthonormal != matrix4.isOrthonormal)
            return false;
        if (Double.compare(matrix4.m11, m11) != 0)
            return false;
        if (Double.compare(matrix4.m12, m12) != 0)
            return false;
        if (Double.compare(matrix4.m13, m13) != 0)
            return false;
        if (Double.compare(matrix4.m14, m14) != 0)
            return false;
        if (Double.compare(matrix4.m21, m21) != 0)
            return false;
        if (Double.compare(matrix4.m22, m22) != 0)
            return false;
        if (Double.compare(matrix4.m23, m23) != 0)
            return false;
        if (Double.compare(matrix4.m24, m24) != 0)
            return false;
        if (Double.compare(matrix4.m31, m31) != 0)
            return false;
        if (Double.compare(matrix4.m32, m32) != 0)
            return false;
        if (Double.compare(matrix4.m33, m33) != 0)
            return false;
        if (Double.compare(matrix4.m34, m34) != 0)
            return false;
        if (Double.compare(matrix4.m41, m41) != 0)
            return false;
        if (Double.compare(matrix4.m42, m42) != 0)
            return false;
        if (Double.compare(matrix4.m43, m43) != 0)
            return false;
        if (Double.compare(matrix4.m44, m44) != 0)
            return false;

        return true;
    }

    @Override
    public int hashCode()
    {
        int result;
        long temp;
        temp = m11 != +0.0d ? Double.doubleToLongBits(m11) : 0L;
        result = (int) (temp ^ (temp >>> 32));
        temp = m12 != +0.0d ? Double.doubleToLongBits(m12) : 0L;
        result = 29 * result + (int) (temp ^ (temp >>> 32));
        temp = m13 != +0.0d ? Double.doubleToLongBits(m13) : 0L;
        result = 29 * result + (int) (temp ^ (temp >>> 32));
        temp = m14 != +0.0d ? Double.doubleToLongBits(m14) : 0L;
        result = 29 * result + (int) (temp ^ (temp >>> 32));
        temp = m21 != +0.0d ? Double.doubleToLongBits(m21) : 0L;
        result = 29 * result + (int) (temp ^ (temp >>> 32));
        temp = m22 != +0.0d ? Double.doubleToLongBits(m22) : 0L;
        result = 29 * result + (int) (temp ^ (temp >>> 32));
        temp = m23 != +0.0d ? Double.doubleToLongBits(m23) : 0L;
        result = 29 * result + (int) (temp ^ (temp >>> 32));
        temp = m24 != +0.0d ? Double.doubleToLongBits(m24) : 0L;
        result = 29 * result + (int) (temp ^ (temp >>> 32));
        temp = m31 != +0.0d ? Double.doubleToLongBits(m31) : 0L;
        result = 29 * result + (int) (temp ^ (temp >>> 32));
        temp = m32 != +0.0d ? Double.doubleToLongBits(m32) : 0L;
        result = 29 * result + (int) (temp ^ (temp >>> 32));
        temp = m33 != +0.0d ? Double.doubleToLongBits(m33) : 0L;
        result = 29 * result + (int) (temp ^ (temp >>> 32));
        temp = m34 != +0.0d ? Double.doubleToLongBits(m34) : 0L;
        result = 29 * result + (int) (temp ^ (temp >>> 32));
        temp = m41 != +0.0d ? Double.doubleToLongBits(m41) : 0L;
        result = 29 * result + (int) (temp ^ (temp >>> 32));
        temp = m42 != +0.0d ? Double.doubleToLongBits(m42) : 0L;
        result = 29 * result + (int) (temp ^ (temp >>> 32));
        temp = m43 != +0.0d ? Double.doubleToLongBits(m43) : 0L;
        result = 29 * result + (int) (temp ^ (temp >>> 32));
        temp = m44 != +0.0d ? Double.doubleToLongBits(m44) : 0L;
        result = 29 * result + (int) (temp ^ (temp >>> 32));
        result = 29 * result + (isOrthonormal ? 1 : 0);
        return result;
    }

    @Override
    public String toString()
    {
        return "Matrix4 :\n[ " + this.m11 + ", " + this.m12 + ", " + this.m13 + ", " + this.m14 + ",\n "
            + this.m21 + ", " + this.m22 + ", " + this.m23 + ", " + this.m24 + ",\n "
            + this.m31 + ", " + this.m32 + ", " + this.m33 + ", " + this.m34 + ",\n "
            + this.m41 + ", " + this.m42 + ", " + this.m43 + ", " + this.m44 + " ]";
    }
}