Inverts a 4 x 4 matrix using Gaussian Elimination. - Java java.lang

Description

Inverts a 4 x 4 matrix using Gaussian Elimination.

Demo Code

//package com.java2s;

public class Main {
    /**// w w  w  . 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;
    }
}

• Previous
• Next

Related Tutorials

• matrix rotate 180 degrees
• matrix rotate Clock Wise
• matrix rotate Counter Clock Wise
• Multiply a matrix with another matrix that only contains elements on its diagonal.
• Multiplies two matrices.