Here you can find the source of invertMatrix4x4(double[][] A)
| Parameter | Description |
|---|---|
| A | The 4 x 4 matrix to invert. |
public final static double[][] invertMatrix4x4(double[][] A)
//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; } }