matrix.java

world wind java sdk 源码

        // b = (x2-x1)/(lat2-lat1) - a*(lon2-lon1)/(lat2-lat1)
        // c = x1 - a*lon1 - b*lat1
        //
        // d0 = (lon3-lon1) - (lon2-lon1)*(lat3-lat1)/(lat2-lat1)
        // d = (1/d0) * [(y3-y1) - (y2-y1)*(lat3-lat1)/(lat2-lat1)]
        // e = (y2-y1)/(lat2-lat1) - d*(lon2-lon1)/(lat2-lat1)
        // f = y1 - d*lon1 - e*lat1

        double lat1 = geoPoints[0].getLatitude().degrees;
        double lat2 = geoPoints[1].getLatitude().degrees;
        double lat3 = geoPoints[2].getLatitude().degrees;
        double lon1 = geoPoints[0].getLongitude().degrees;
        double lon2 = geoPoints[1].getLongitude().degrees;
        double lon3 = geoPoints[2].getLongitude().degrees;

        double x1 = imagePoints[0].getX();
        double x2 = imagePoints[1].getX();
        double x3 = imagePoints[2].getX();
        double y1 = imagePoints[0].getY();
        double y2 = imagePoints[1].getY();
        double y3 = imagePoints[2].getY();

        double a0 = (lon3-lon1) - (lon2-lon1)*(lat3-lat1)/(lat2-lat1);
        double a = (1/a0) * ((x3-x1) - (x2-x1)*(lat3-lat1)/(lat2-lat1));
        double b = (x2-x1)/(lat2-lat1) - a*(lon2-lon1)/(lat2-lat1);
        double c = x1 - a*lon1 - b*lat1;

        double d0 = (lon3-lon1) - (lon2-lon1)*(lat3-lat1)/(lat2-lat1);
        double d = (1/d0) * ((y3-y1) - (y2-y1)*(lat3-lat1)/(lat2-lat1));
        double e = (y2-y1)/(lat2-lat1) - d*(lon2-lon1)/(lat2-lat1);
        double f = y1 - d*lon1 - e*lat1;

        return new Matrix(
            a,   b,   c,   0.0,
            d,   e,   f,   0.0,
            0.0, 0.0, 1.0, 0.0,
            0.0, 0.0, 0.0, 0.0);
    }

    /**
     * Computes a Matrix that will map the geographic region defined by sector onto a Cartesian region of the specified
     * <code>width</code> and <code>height</code> and centered at the point <code>(x, y)</code>.
     *
     * @param sector the geographic region which will be mapped to the Cartesian region
     * @param x x-coordinate of lower left hand corner of the Cartesian region
     * @param y y-coordinate of lower left hand corner of the Cartesian region
     * @param width width of the Cartesian region, extending to the right from the x-coordinate
     * @param height height of the Cartesian region, extending up from the y-coordinate
     *
     * @return Matrix that will map from the geographic region to the Cartesian region.
     * @throws IllegalArgumentException if <code>sector</code> is null, or if <code>width</code> or <code>height</code>
     *                                  are less than zero.
     */
    public static Matrix fromGeographicToViewport(Sector sector, int x, int y, int width, int height)
    {
        if (sector == null)
        {
            String message = Logging.getMessage("nullValue.SectorIsNull");
            Logging.logger().severe(message);
            throw new IllegalArgumentException(message);
        }

        if (width <= 0)
        {
            String message = Logging.getMessage("Geom.WidthInvalid", width);
            Logging.logger().severe(message);
            throw new IllegalArgumentException(message);
        }

        if (height <= 0)
        {
            String message = Logging.getMessage("Geom.HeightInvalid", height);
            Logging.logger().severe(message);
            throw new IllegalArgumentException(message);
        }

        Matrix transform = Matrix.IDENTITY;
        transform = transform.multiply(
            Matrix.fromTranslation(-x, -y, 0.0));
        transform = transform.multiply(
            Matrix.fromScale(width / sector.getDeltaLonDegrees(), height / sector.getDeltaLatDegrees(), 1.0));
        transform = transform.multiply(
            Matrix.fromTranslation(-sector.getMinLongitude().degrees, -sector.getMinLatitude().degrees, 0.0));

        return transform;
    }

    // ============== Arithmetic Functions ======================= //
    // ============== Arithmetic Functions ======================= //
    // ============== Arithmetic Functions ======================= //

    public final Matrix add(Matrix matrix)
    {
        if (matrix == null)
        {
            String msg = Logging.getMessage("nullValue.MatrixIsNull");
            Logging.logger().severe(msg);
            throw new IllegalArgumentException(msg);
        }

        return new Matrix(
            this.m11 + matrix.m11, this.m12 + matrix.m12, this.m13 + matrix.m13, this.m14 + matrix.m14,
            this.m21 + matrix.m21, this.m22 + matrix.m22, this.m23 + matrix.m23, this.m24 + matrix.m24,
            this.m31 + matrix.m31, this.m32 + matrix.m32, this.m33 + matrix.m33, this.m34 + matrix.m34,
            this.m41 + matrix.m41, this.m42 + matrix.m42, this.m43 + matrix.m43, this.m44 + matrix.m44);
    }

    public final Matrix subtract(Matrix matrix)
    {
        if (matrix == null)
        {
            String msg = Logging.getMessage("nullValue.MatrixIsNull");
            Logging.logger().severe(msg);
            throw new IllegalArgumentException(msg);
        }

        return new Matrix(
            this.m11 - matrix.m11, this.m12 - matrix.m12, this.m13 - matrix.m13, this.m14 - matrix.m14,
            this.m21 - matrix.m21, this.m22 - matrix.m22, this.m23 - matrix.m23, this.m24 - matrix.m24,
            this.m31 - matrix.m31, this.m32 - matrix.m32, this.m33 - matrix.m33, this.m34 - matrix.m34,
            this.m41 - matrix.m41, this.m42 - matrix.m42, this.m43 - matrix.m43, this.m44 - matrix.m44);
    }

    public final Matrix multiplyComponents(double value)
    {
        return new Matrix(
            this.m11 * value, this.m12 * value, this.m13 * value, this.m14 * value,
            this.m21 * value, this.m22 * value, this.m23 * value, this.m24 * value,
            this.m31 * value, this.m32 * value, this.m33 * value, this.m34 * value,
            this.m41 * value, this.m42 * value, this.m43 * value, this.m44 * value);
    }

    public final Matrix multiply(Matrix matrix)
    {
        if (matrix == null)
        {
            String msg = Logging.getMessage("nullValue.MatrixIsNull");
            Logging.logger().severe(msg);
            throw new IllegalArgumentException(msg);
        }

        return new Matrix(
            // Row 1
            (this.m11 * matrix.m11) + (this.m12 * matrix.m21) + (this.m13 * matrix.m31) + (this.m14 * matrix.m41),
            (this.m11 * matrix.m12) + (this.m12 * matrix.m22) + (this.m13 * matrix.m32) + (this.m14 * matrix.m42),
            (this.m11 * matrix.m13) + (this.m12 * matrix.m23) + (this.m13 * matrix.m33) + (this.m14 * matrix.m43),
            (this.m11 * matrix.m14) + (this.m12 * matrix.m24) + (this.m13 * matrix.m34) + (this.m14 * matrix.m44),
            // Row 2
            (this.m21 * matrix.m11) + (this.m22 * matrix.m21) + (this.m23 * matrix.m31) + (this.m24 * matrix.m41),
            (this.m21 * matrix.m12) + (this.m22 * matrix.m22) + (this.m23 * matrix.m32) + (this.m24 * matrix.m42),
            (this.m21 * matrix.m13) + (this.m22 * matrix.m23) + (this.m23 * matrix.m33) + (this.m24 * matrix.m43),
            (this.m21 * matrix.m14) + (this.m22 * matrix.m24) + (this.m23 * matrix.m34) + (this.m24 * matrix.m44),
            // Row 3
            (this.m31 * matrix.m11) + (this.m32 * matrix.m21) + (this.m33 * matrix.m31) + (this.m34 * matrix.m41),
            (this.m31 * matrix.m12) + (this.m32 * matrix.m22) + (this.m33 * matrix.m32) + (this.m34 * matrix.m42),
            (this.m31 * matrix.m13) + (this.m32 * matrix.m23) + (this.m33 * matrix.m33) + (this.m34 * matrix.m43),
            (this.m31 * matrix.m14) + (this.m32 * matrix.m24) + (this.m33 * matrix.m34) + (this.m34 * matrix.m44),
            // Row 4
            (this.m41 * matrix.m11) + (this.m42 * matrix.m21) + (this.m43 * matrix.m31) + (this.m44 * matrix.m41),
            (this.m41 * matrix.m12) + (this.m42 * matrix.m22) + (this.m43 * matrix.m32) + (this.m44 * matrix.m42),
            (this.m41 * matrix.m13) + (this.m42 * matrix.m23) + (this.m43 * matrix.m33) + (this.m44 * matrix.m43),
            (this.m41 * matrix.m14) + (this.m42 * matrix.m24) + (this.m43 * matrix.m34) + (this.m44 * matrix.m44),
            // Product of orthonormal 3D transformHACK matrices is also an orthonormal 3D transformHACK.
            this.isOrthonormalTransform && matrix.isOrthonormalTransform);
    }

    public final Matrix divideComponents(double value)
    {
        if (isZero(value))
        {
            String msg = Logging.getMessage("generic.ArgumentOutOfRange", value);
            Logging.logger().severe(msg);
            throw new IllegalArgumentException(msg);
        }

        return new Matrix(
            this.m11 / value, this.m12 / value, this.m13 / value, this.m14 / value,
            this.m21 / value, this.m22 / value, this.m23 / value, this.m24 / value,
            this.m31 / value, this.m32 / value, this.m33 / value, this.m34 / value,
            this.m41 / value, this.m42 / value, this.m43 / value, this.m44 / value);
    }

    public final Matrix divideComponents(Matrix matrix)
    {
        if (matrix == null)
        {
            String msg = Logging.getMessage("nullValue.MatrixIsNull");
            Logging.logger().severe(msg);
            throw new IllegalArgumentException(msg);
        }

        return new Matrix(
            this.m11 / matrix.m11, this.m12 / matrix.m12, this.m13 / matrix.m13, this.m14 / matrix.m14,
            this.m21 / matrix.m21, this.m22 / matrix.m22, this.m23 / matrix.m23, this.m24 / matrix.m24,
            this.m31 / matrix.m31, this.m32 / matrix.m32, this.m33 / matrix.m33, this.m34 / matrix.m34,
            this.m41 / matrix.m41, this.m42 / matrix.m42, this.m43 / matrix.m43, this.m44 / matrix.m44);
    }

    public final Matrix negate()
    {
        return new Matrix(
            0.0 - this.m11, 0.0 - this.m12, 0.0 - this.m13, 0.0 - this.m14,
            0.0 - this.m21, 0.0 - this.m22, 0.0 - this.m23, 0.0 - this.m24,
            0.0 - this.m31, 0.0 - this.m32, 0.0 - this.m33, 0.0 - this.m34,
            0.0 - this.m41, 0.0 - this.m42, 0.0 - this.m43, 0.0 - this.m44,
            // Negative of orthonormal 3D transformHACK matrix is also an orthonormal 3D transformHACK.
            this.isOrthonormalTransform);
    }

    // ============== Matrix Arithmetic Functions ======================= //
    // ============== Matrix Arithmetic Functions ======================= //
    // ============== Matrix Arithmetic Functions ======================= //

    public final double getDeterminant()
    {
        double result = 0.0;
        // Columns 2, 3, 4.
        result += this.m11 *
            (this.m22 * (this.m33 * this.m44 - this.m43 * this.m34)
                - this.m23 * (this.m32 * this.m44 - this.m42 * this.m34)
                + this.m24 * (this.m32 * this.m43 - this.m42 * this.m33));
        // Columns 1, 3, 4.
        result -= this.m12 *
            (this.m21 * (this.m33 * this.m44 - this.m43 * this.m34)
                - this.m23 * (this.m31 * this.m44 - this.m41 * this.m34)
                + this.m24 * (this.m31 * this.m43 - this.m41 * this.m33));
        // Columns 1, 2, 4.
        result += this.m13 *
            (this.m21 * (this.m32 * this.m44 - this.m42 * this.m34)
                - this.m22 * (this.m31 * this.m44 - this.m41 * this.m34)
                + this.m24 * (this.m31 * this.m42 - this.m41 * this.m32));
        // Columns 1, 2, 3.
        result -= this.m14 *
            (this.m21 * (this.m32 * this.m43 - this.m42 - this.m33)
                - this.m22 * (this.m31 * this.m43 - this.m41 * this.m33)
                + this.m23 * (this.m31 * this.m42 - this.m41 * this.m32));
        return result;
    }

    public final Matrix getTranspose()
    {
        // Swap rows with columns.
        return new Matrix(
            this.m11, this.m21, this.m31, this.m41,
            this.m12, this.m22, this.m32, this.m42,
            this.m13, this.m23, this.m33, this.m43,
            this.m14, this.m24, this.m34, this.m44,
            // Transpose of orthonormal 3D transformHACK matrix is not an orthonormal 3D transformHACK matrix.
            false);
    }

    public final double getTrace()
    {
        return this.m11 + this.m22 + this.m33 + this.m44;
    }

    public final Matrix getInverse()
    {
        if (this.isOrthonormalTransform)
            return computeTransformInverse(this);
        else
            return computeGeneralInverse(this);
    }
    
    private static Matrix computeGeneralInverse(Matrix a)
    {
        double cf_11 = a.m22 * (a.m33 * a.m44 - a.m43 * a.m34)
            - a.m23 * (a.m32 * a.m44 - a.m42 * a.m34)
            + a.m24 * (a.m32 * a.m43 - a.m42 * a.m33);
        double cf_12 = -(a.m21 * (a.m33 * a.m44 - a.m43 * a.m34)
            - a.m23 * (a.m31 * a.m44 - a.m41 * a.m34)
            + a.m24 * (a.m31 * a.m43 - a.m41 * a.m33));
        double cf_13 = a.m21 * (a.m32 * a.m44 - a.m42 * a.m34)
            - a.m22 * (a.m31 * a.m44 - a.m41 * a.m34)
            + a.m24 * (a.m31 * a.m42 - a.m41 * a.m32);
        double cf_14 = -(a.m21 * (a.m32 * a.m43 - a.m42 - a.m33)
            - a.m22 * (a.m31 * a.m43 - a.m41 * a.m33)
            + a.m23 * (a.m31 * a.m42 - a.m41 * a.m32));
        double cf_21 = a.m12 * (a.m33 * a.m44 - a.m43 - a.m34)
            - a.m13 * (a.m32 * a.m44 - a.m42 * a.m34)
            + a.m14 * (a.m32 * a.m43 - a.m42 * a.m33);
        double cf_22 = -(a.m11 * (a.m33 * a.m44 - a.m43 * a.m34)
            - a.m13 * (a.m31 * a.m44 - a.m41 * a.m34)
            + a.m14 * (a.m31 * a.m43 - a.m41 * a.m33));
        double cf_23 = a.m11 * (a.m32 * a.m44 - a.m42 * a.m34)
            - a.m12 * (a.m31 * a.m44 - a.m41 * a.m34)
            + a.m14 * (a.m31 * a.m42 - a.m41 * a.m32);
        double cf_24 = -(a.m11 * (a.m32 * a.m43 - a.m42 * a.m33)
            - a.m12 * (a.m31 * a.m43 - a.m41 * a.m33)
            + a.m13 * (a.m31 * a.m42 - a.m41 * a.m32));
        double cf_31 = a.m12 * (a.m23 * a.m44 - a.m43 * a.m24)
            - a.m13 * (a.m22 * a.m44 - a.m42 * a.m24)
            + a.m14 * (a.m22 * a.m43 - a.m42 * a.m23);
        double cf_32 = -(a.m11 * (a.m23 * a.m44 - a.m43 * a.m24)
            - a.m13 * (a.m21 * a.m44 - a.m41 * a.m24)
            + a.m14 * (a.m24 * a.m43 - a.m41 * a.m23));
        double cf_33 = a.m11 * (a.m22 * a.m44 - a.m42 * a.m24)
            - a.m12 * (a.m21 * a.m44 - a.m41 * a.m24)
            + a.m14 * (a.m21 * a.m42 - a.m41 * a.m22);
        double cf_34 = -(a.m11 * (a.m22 * a.m33 - a.m32 * a.m23)
            - a.m12 * (a.m21 * a.m33 - a.m31 * a.m23)
            + a.m13 * (a.m21 * a.m32 - a.m31 * a.m22));
        double cf_41 = a.m12 * (a.m23 * a.m34 - a.m33 * a.m24)
            - a.m13 * (a.m22 * a.m34 - a.m32 * a.m24)
            + a.m14 * (a.m22 * a.m33 - a.m32 * a.m23);
        double cf_42 = -(a.m11 * (a.m23 * a.m34 - a.m33 * a.m24)
            - a.m13 * (a.m21 * a.m34 - a.m31 * a.m24)
            + a.m14 * (a.m21 * a.m33 - a.m31 * a.m23));
        double cf_43 = a.m11 * (a.m22 * a.m34 - a.m32 * a.m24)
            - a.m12 * (a.m21 * a.m34 - a.m31 * a.m24)
            + a.m14 * (a.m21 * a.m32 - a.m31 * a.m22);
        double cf_44 = -(a.m11 * (a.m22 * a.m33 - a.m32 * a.m23)
            - a.m12 * (a.m21 * a.m33 - a.m31 * a.m23)
            + a.m13 * (a.m21 * a.m32 - a.m31 * a.m22));
        double det = (a.m11 * cf_11) + (a.m12 * cf_12) + (a.m13 * cf_13) + (a.m14 * cf_14);

        if (isZero(det))
            return null;
        return new Matrix(
            cf_11 / det, cf_21 / det, cf_31 / det, cf_41 / det,
            cf_12 / det, cf_22 / det, cf_32 / det, cf_42 / det,
            cf_13 / det, cf_23 / det, cf_33 / det, cf_43 / det,
            cf_14 / det, cf_24 / det, cf_34 / det, cf_44 / det);
    }

    private static Matrix computeTransformInverse(Matrix a)
    {
        // 'a' is assumed to contain a 3D transformation matrix.
        // Upper-3x3 is inverted, translation is transformed by inverted-upper-3x3 and negated.
        return new Matrix(
            a.m11, a.m21, a.m31, 0.0 - (a.m11 * a.m14) - (a.m21 * a.m24) - (a.m31 * a.m34),
            a.m12, a.m22, a.m32, 0.0 - (a.m12 * a.m14) - (a.m22 * a.m24) - (a.m32 * a.m34),
            a.m13, a.m23, a.m33, 0.0 - (a.m13 * a.m14) - (a.m23 * a.m24) - (a.m33 * a.m34),
            0.0, 0.0, 0.0, 1.0,
            // Inverse of an orthogonal, 3D transformHACK matrix is not an orthogonal 3D transformHACK.
            false);
    }

    // ============== Accessor Functions ======================= //
    // ============== Accessor Functions ======================= //
    // ============== Accessor Functions ======================= //
    
    public final Angle getRotationX()
    {
        double yRadians = Math.asin(this.m13);
        double cosY = Math.cos(yRadians);
        if (isZero(cosY))
            return null;

        double xRadians;
        // No Gimball lock.
        if (Math.abs(cosY) > 0.005)
        {
            xRadians = Math.atan2(-this.m23 / cosY, this.m33 / cosY);
        }
        // Gimball lock has occurred. Rotation around X axis becomes rotation around Z axis.
        else
        {
            xRadians = 0;
        }

        if (Double.isNaN(xRadians))
            return null;

        return Angle.fromRadians(xRadians);
    }

    public final Angle getRotationY()
    {
        double yRadians = Math.asin(this.m13);
        if (Double.isNaN(yRadians))
            return null;
        
        return Angle.fromRadians(yRadians);
    }

    public final Angle getRotationZ()
    {
        double yRadians = Math.asin(this.m13);
        double cosY = Math.cos(yRadians);
        if (isZero(cosY))
            return null;

        double zRadians;
        // No Gimball lock.
        if (Math.abs(cosY) > 0.005)
        {
            zRadians = Math.atan2(-this.m12 / cosY, this.m11 / cosY);
        }
        // Gimball lock has occurred. Rotation around X axis becomes rotation around Z axis.
        else
        {
            zRadians = Math.atan2(this.m21, this.m22);
        }

        if (Double.isNaN(zRadians))
            return null;

        return Angle.fromRadians(zRadians);
    }

    public final Vec4 getTranslation()
    {
        return new Vec4(this.m14, this.m24, this.m34);
    }

    // ============== Helper Functions ======================= //
    // ============== Helper Functions ======================= //
    // ============== Helper Functions ======================= //

    private static final Double POSITIVE_ZERO = +0.0d;

    private static final Double NEGATIVE_ZERO = -0.0d;

    private static boolean isZero(double value)
    {
        return (POSITIVE_ZERO.compareTo(value) == 0)
            || (NEGATIVE_ZERO.compareTo(value) == 0);
    }
}