public void invert(Matrix srcMatrix) { double src[][] = srcMatrix.getData(); double dst[][] = this.data; // COMPUTE ADJOINT COFACTOR MATRIX FOR THE ROTATION+SCALE 3x3 for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) { int i0 = (i + 1) % 3, i1 = (i + 2) % 3; int j0 = (j + 1) % 3, j1 = (j + 2) % 3; dst[j][i] = src[i0][j0] * src[i1][j1] - src[i0][j1] * src[i1][j0]; } // RENORMALIZE BY DETERMINANT TO GET ROTATION+SCALE 3x3 INVERSE double determinant = src[0][0] * dst[0][0] + src[1][0] * dst[0][1] + src[2][0] * dst[0][2]; for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) dst[i][j] /= determinant; // COMPUTE INVERSE TRANSLATION for (int i = 0; i < 3; i++) dst[i][3] = - dst[i][0] * src[0][3] - dst[i][1] * src[1][3] - dst[i][2] * src[2][3]; dst[15] = 1; }