Java Matrix Invert invertMatrix4x4(double[][] A)

Here you can find the source of invertMatrix4x4(double[][] A)

Description

Inverts a 4 x 4 matrix using Gaussian Elimination.

License

Open Source License

Parameter

Parameter Description
A The 4 x 4 matrix to invert.

Return

A new inverted matrix (4 x 4).

Declaration

public final static double[][] invertMatrix4x4(double[][] A) 

Method Source Code

//package com.java2s;
//License from project: Open Source License 

public class Main {
    /**//from   www  .  j a v  a 2  s. c o m
     * Inverts a 4 x 4 matrix using Gaussian Elimination.
     * C code from
     * http://www.gamedev.net/community/forums/topic.asp?topic_id=211424&PageSize=25&WhichPage=1
     * ported to java. Not very elegant but it works.
     * @param A The 4 x 4 matrix to invert.
     * @return A new inverted matrix (4 x 4).
     */
    public final static double[][] invertMatrix4x4(double[][] A) {

        double s1, s2, s3, s4;
        double negX, div;
        double r1fab, r2fab, r3fab, r4fab;

        double[] A_row0 = A[0];
        double[] A_row1 = A[1];
        double[] A_row2 = A[2];
        double[] A_row3 = A[3];

        double[][] res = new double[4][4];
        double[] res_row0 = res[0];
        double[] res_row1 = res[1];
        double[] res_row2 = res[2];
        double[] res_row3 = res[3];

        for (int i = 0; i < 4; i++)
            res[i][i] = 1;

        /*========================= Step 1 of Gaussian Elimination ==============================*/

        r1fab = Math.abs(A_row0[0]);
        r2fab = Math.abs(A_row1[0]);
        r3fab = Math.abs(A_row2[0]);
        r4fab = Math.abs(A_row3[0]);

        // Pivot Point Test on Column 1
        if (r2fab > r1fab) {
            s1 = A_row0[0];
            s2 = A_row0[1];
            s3 = A_row0[2];
            s4 = A_row0[3];
            A_row0[0] = A_row1[0];
            A_row0[1] = A_row1[1];
            A_row0[2] = A_row1[2];
            A_row0[3] = A_row1[3];
            A_row1[0] = s1;
            A_row1[1] = s2;
            A_row1[2] = s3;
            A_row1[3] = s4;

            s1 = res_row0[0];
            s2 = res_row0[1];
            s3 = res_row0[2];
            s4 = res_row0[3];
            res_row0[0] = res_row1[0];
            res_row0[1] = res_row1[1];
            res_row0[2] = res_row1[2];
            res_row0[3] = res_row1[3];
            res_row1[0] = s1;
            res_row1[1] = s2;
            res_row1[2] = s3;
            res_row1[3] = s4;
        }

        if (r3fab > r1fab) {
            s1 = A_row0[0];
            s2 = A_row0[1];
            s3 = A_row0[2];
            s4 = A_row0[3];
            A_row0[0] = A_row2[0];
            A_row0[1] = A_row2[1];
            A_row0[2] = A_row2[2];
            A_row0[3] = A_row2[3];
            A_row2[0] = s1;
            A_row2[1] = s2;
            A_row2[2] = s3;
            A_row2[3] = s4;

            s1 = res_row0[0];
            s2 = res_row0[1];
            s3 = res_row0[2];
            s4 = res_row0[3];
            res_row0[0] = res_row2[0];
            res_row0[1] = res_row2[1];
            res_row0[2] = res_row2[2];
            res_row0[3] = res_row2[3];
            res_row2[0] = s1;
            res_row2[1] = s2;
            res_row2[2] = s3;
            res_row2[3] = s4;

        }
        if (r4fab > r1fab) {
            s1 = A_row0[0];
            s2 = A_row0[1];
            s3 = A_row0[2];
            s4 = A_row0[3];
            A_row0[0] = A_row3[0];
            A_row0[1] = A_row3[1];
            A_row0[2] = A_row3[2];
            A_row0[3] = A_row3[3];
            A_row3[0] = s1;
            A_row3[1] = s2;
            A_row3[2] = s3;
            A_row3[3] = s4;

            s1 = res_row0[0];
            s2 = res_row0[1];
            s3 = res_row0[2];
            s4 = res_row0[3];
            res_row0[0] = res_row3[0];
            res_row0[1] = res_row3[1];
            res_row0[2] = res_row3[2];
            res_row0[3] = res_row3[3];
            res_row3[0] = s1;
            res_row3[1] = s2;
            res_row3[2] = s3;
            res_row3[3] = s4;

        }

        /*if column 1 was not pivoted then do Step 1 in Gaussian Elimination
             
        row 1 math
             
        Divide first row by A_row0[0] to make A_row0[0] == 1
         */

        div = A_row0[0];
        A_row0[0] /= div;
        A_row0[1] /= div;
        A_row0[2] /= div;
        A_row0[3] /= div;
        res_row0[0] = res_row0[0] / div;
        res_row0[1] = res_row0[1] / div;
        res_row0[2] = res_row0[2] / div;
        res_row0[3] = res_row0[3] / div;

        // row 2 math
        negX = -A_row1[0];
        A_row1[0] += A_row0[0] * negX;
        A_row1[1] += A_row0[1] * negX;
        A_row1[2] += A_row0[2] * negX;
        A_row1[3] += A_row0[3] * negX;
        res_row1[0] += res_row0[0] * negX;
        res_row1[1] += res_row0[1] * negX;
        res_row1[2] += res_row0[2] * negX;
        res_row1[3] += res_row0[3] * negX;

        // row 3 math
        negX = -A_row2[0];
        A_row2[0] += negX;
        A_row2[1] += A_row0[1] * negX;
        A_row2[2] += A_row0[2] * negX;
        A_row2[3] += A_row0[3] * negX;
        res_row2[0] += res_row0[0] * negX;
        res_row2[1] += res_row0[1] * negX;
        res_row2[2] += res_row0[2] * negX;
        res_row2[3] += res_row0[3] * negX;

        // row 4 math
        negX = -A_row3[0];
        A_row3[0] += negX;
        A_row3[1] += A_row0[1] * negX;
        A_row3[2] += A_row0[2] * negX;
        A_row3[3] += A_row0[3] * negX;
        res_row3[0] += res_row0[0] * negX;
        res_row3[1] += res_row0[1] * negX;
        res_row3[2] += res_row0[2] * negX;
        res_row3[3] += res_row0[3] * negX;

        /*======================== Step 2 of Gaussian Elimination ========*/
        // Pivot Point Test for Column 2 excluding row 1
        r2fab = Math.abs(A_row1[1]);
        r3fab = Math.abs(A_row2[1]);
        r4fab = Math.abs(A_row3[1]);

        if (r3fab > r2fab) {
            s1 = A_row1[0];
            s2 = A_row1[1];
            s3 = A_row1[2];
            s4 = A_row1[3];
            A_row1[0] = A_row2[0];
            A_row1[1] = A_row2[1];
            A_row1[2] = A_row2[2];
            A_row1[3] = A_row2[3];
            A_row2[0] = s1;
            A_row2[1] = s2;
            A_row2[2] = s3;
            A_row2[3] = s4;

            s1 = res_row1[0];
            s2 = res_row1[1];
            s3 = res_row1[2];
            s4 = res_row1[3];
            res_row1[0] = res_row2[0];
            res_row1[1] = res_row2[1];
            res_row1[2] = res_row2[2];
            res_row1[3] = res_row2[3];
            res_row2[0] = s1;
            res_row2[1] = s2;
            res_row2[2] = s3;
            res_row2[3] = s4;

        }
        if (r4fab > r2fab) {
            s1 = A_row1[0];
            s2 = A_row1[1];
            s3 = A_row1[2];
            s4 = A_row1[3];
            A_row1[0] = A_row3[0];
            A_row1[1] = A_row3[1];
            A_row1[2] = A_row3[2];
            A_row1[3] = A_row3[3];
            A_row3[0] = s1;
            A_row3[1] = s2;
            A_row3[2] = s3;
            A_row3[3] = s4;

            s1 = res_row1[0];
            s2 = res_row1[1];
            s3 = res_row1[2];
            s4 = res_row1[3];
            res_row1[0] = res_row3[0];
            res_row1[1] = res_row3[1];
            res_row1[2] = res_row3[2];
            res_row1[3] = res_row3[3];
            res_row3[0] = s1;
            res_row3[1] = s2;
            res_row3[2] = s3;
            res_row3[3] = s4;

        }

        /* row 2 math
        Divide row 2 by A_row1[1] to make A_row1[1] == 1
         */
        div = A_row1[1];
        A_row1[1] /= div;
        A_row1[2] /= div;
        A_row1[3] /= div;
        res_row1[0] /= div;
        res_row1[1] /= div;
        res_row1[2] /= div;
        res_row1[3] /= div;

        // row 1 math
        negX = -A_row0[1];
        A_row0[1] += negX;
        A_row0[2] += A_row1[2] * negX;
        A_row0[3] += A_row1[3] * negX;
        res_row0[0] += negX * res_row1[0];
        res_row0[1] += negX * res_row1[1];
        res_row0[2] += negX * res_row1[2];
        res_row0[3] += negX * res_row1[3];

        // row 3 math
        negX = -A_row2[1];
        A_row2[1] += negX;
        A_row2[2] += A_row1[2] * negX;
        A_row2[3] += A_row1[3] * negX;
        res_row2[0] += negX * res_row1[0];
        res_row2[1] += negX * res_row1[1];
        res_row2[2] += negX * res_row1[2];
        res_row2[3] += negX * res_row1[3];

        // row 4 math
        negX = -A_row3[1];
        A_row3[1] += negX;
        A_row3[2] += A_row1[2] * negX;
        A_row3[3] += A_row1[3] * negX;
        res_row3[0] += negX * res_row1[0];
        res_row3[1] += negX * res_row1[1];
        res_row3[2] += negX * res_row1[2];
        res_row3[3] += negX * res_row1[3];

        /*====================== Step 3 of Gaussian Elimination ================================*/

        r3fab = Math.abs(A_row2[2]);
        r4fab = Math.abs(A_row3[2]);

        // Pivot Point Test for Column 3 exluding row 1 and 2

        if (r4fab > r3fab) {
            s1 = A_row2[0];
            s2 = A_row2[1];
            s3 = A_row2[2];
            s4 = A_row2[3];
            A_row2[0] = A_row3[0];
            A_row2[1] = A_row3[1];
            A_row2[2] = A_row3[2];
            A_row2[3] = A_row3[3];
            A_row3[0] = s1;
            A_row3[1] = s2;
            A_row3[2] = s3;
            A_row3[3] = s4;

            s1 = res_row2[0];
            s2 = res_row2[1];
            s3 = res_row2[2];
            s4 = res_row2[3];
            res_row2[0] = res_row3[0];
            res_row2[1] = res_row3[1];
            res_row2[2] = res_row3[2];
            res_row2[3] = res_row3[3];
            res_row3[0] = s1;
            res_row3[1] = s2;
            res_row3[2] = s3;
            res_row3[3] = s4;

        }
        // row 3 math
        div = A_row2[2];
        A_row2[2] /= div;
        A_row2[3] /= div;
        res_row2[0] /= div;
        res_row2[1] /= div;
        res_row2[2] /= div;
        res_row2[3] /= div;

        //row 1 math
        negX = -A_row0[2];
        A_row0[2] += negX;
        A_row0[3] += A_row2[3] * negX;
        res_row0[0] += negX * res_row2[0];
        res_row0[1] += negX * res_row2[1];
        res_row0[2] += negX * res_row2[2];
        res_row0[3] += negX * res_row2[3];

        // row 2 math
        negX = -A_row1[2];
        A_row1[2] += negX;
        A_row1[3] += A_row2[3] * negX;
        res_row1[0] += negX * res_row2[0];
        res_row1[1] += negX * res_row2[1];
        res_row1[2] += negX * res_row2[2];
        res_row1[3] += negX * res_row2[3];

        // row 4 math
        negX = -A_row3[2];
        A_row3[2] += negX;
        A_row3[3] += A_row2[3] * negX;
        res_row3[0] += negX * res_row2[0];
        res_row3[1] += negX * res_row2[1];
        res_row3[2] += negX * res_row2[2];
        res_row3[3] += negX * res_row2[3];

        /*======================= Step 4 of Gaussian Elimination ===============================*/

        // No Pivot Point Test since we are at row 4

        // row 4 math

        div = A_row3[3];

        A_row3[3] /= div;
        res_row3[0] /= div;
        res_row3[1] /= div;
        res_row3[2] /= div;
        res_row3[3] /= div;

        // row 1 math
        negX = -A_row0[3];
        A_row0[3] += negX;
        res_row0[0] += negX * res_row3[0];
        res_row0[1] += negX * res_row3[1];
        res_row0[2] += negX * res_row3[2];
        res_row0[3] += negX * res_row3[3];

        // row 2 math
        negX = -A_row1[3];
        A_row1[3] = negX + A_row1[3];
        res_row1[0] += negX * res_row3[0];
        res_row1[1] += negX * res_row3[1];
        res_row1[2] += negX * res_row3[2];
        res_row1[3] += negX * res_row3[3];

        // row 3 math
        negX = -A_row2[3];
        A_row2[3] = negX + A_row2[3];
        res_row2[0] += negX * res_row3[0];
        res_row2[1] += negX * res_row3[1];
        res_row2[2] += negX * res_row3[2];
        res_row2[3] += negX * res_row3[3];

        return res;
    }
}

Related

  1. invert3x3(float[] mat)
  2. invert44Matrix(final double[] m, final double[] invOut)
  3. invertDataSet(double[][] dataSet)
  4. invertMatrix(int[][] matrix)
  5. invertMatrix3x3(float[] result, float[] m)
  6. invertRegions(int[][] image)
  7. invertSymmetric2x2(final double[][] m, final double[][] inverse)
  8. invertSymmetric3x3(final double[][] m, final double[][] inverse)
  9. invertUpperTriangular(double[][] r)