Skip to content

Commit

Permalink
implement inverse matrix calculation
Browse files Browse the repository at this point in the history 
implement mat4 * vec4
  • Loading branch information
@Zubnix
Zubnix authored and Erik De Rijcke committed Feb 26, 2015
1 parent d1f9e44 commit 4c7f3e4
Show file tree
Hide file tree
Showing 4 changed files with 233 additions and 2 deletions.
7 changes: 7 additions & 0 deletions src/main/java/com/hackoeur/jglm/Mat4.java
View file
Expand Up @@ -400,6 +400,13 @@ public Mat4 multiply(final Mat4 right) {
nm30, nm31, nm32, nm33
);
}

public Vec4 multiply(final Vec4 right) {
return new Vec4(this.m00 * right.x + this.m10 * right.y + this.m20 * right.z + this.m30 * right.w,
this.m01 * right.x + this.m11 * right.y + this.m21 * right.z + this.m31 * right.w,
this.m02 * right.x + this.m12 * right.y + this.m22 * right.z + this.m32 * right.w,
this.m03 * right.x + this.m13 * right.y + this.m23 * right.z + this.m33 * right.w);
}

/**
* Subtract other matrix from this one and return the result ( this - right )
Expand Down
114 changes: 113 additions & 1 deletion src/main/java/com/hackoeur/jglm/Matrices.java
View file
Expand Up @@ -179,4 +179,116 @@ public static Mat4 rotate(final float phi, final Vec3 axis) {
return new Mat4(v1, v2, v3, v4);
}

}
public static Mat4 invert(final Mat4 matrix){

final float a[][] = new float[][]{{matrix.m00, matrix.m10, matrix.m20, matrix.m30},
{matrix.m01, matrix.m11, matrix.m21, matrix.m31},
{matrix.m02, matrix.m12, matrix.m22, matrix.m32},
{matrix.m03, matrix.m13, matrix.m23, matrix.m33}};
final int n = 4;
float[][] inverted = invert(a,
n);
return new Mat4(inverted[0][0], inverted[1][0], inverted[2][0], inverted[3][0],
inverted[0][1], inverted[1][1], inverted[2][1], inverted[3][1],
inverted[0][2], inverted[1][2], inverted[2][2], inverted[3][2],
inverted[0][3], inverted[1][3], inverted[2][3], inverted[3][3]);
}

public static Mat3 invert(final Mat3 matrix){

final float a[][] = new float[][]{{matrix.m00, matrix.m10, matrix.m20},
{matrix.m01, matrix.m11, matrix.m21},
{matrix.m02, matrix.m12, matrix.m22}};
final int n = 3;
float[][] inverted = invert(a,
n);
return new Mat3(inverted[0][0], inverted[1][0], inverted[2][0],
inverted[0][1], inverted[1][1], inverted[2][1],
inverted[0][2], inverted[1][2], inverted[2][2]);
}

private static float[][] invert(final float[][] a,
final int n){
float x[][] = new float[n][n];
float b[][] = new float[n][n];
int index[] = new int[n];

for (int i = 0; i < n; ++i) {
b[i][i] = 1;
}
// Transform the a into an upper triangle
gaussian(a, index);
// Update the a b[i][j] with the ratios stored
for (int i = 0; i < n - 1; ++i){
for (int j = i + 1; j < n; ++j){
for (int k = 0; k < n; ++k){
b[index[j]][k] -= a[index[j]][i] * b[index[i]][k];
}
}
}
// Perform backward substitutions
for (int i = 0; i < n; ++i){
x[n - 1][i] = b[index[n - 1]][i] / a[index[n - 1]][n - 1];
for (int j = n - 2; j >= 0; --j){
x[j][i] = b[index[j]][i];
for (int k = j + 1; k < n; ++k){
x[j][i] -= a[index[j]][k] * x[k][i];
}
x[j][i] /= a[index[j]][j];
}
}
return x;
}


// Method to carry out the partial-pivoting Gaussian
// elimination. Here index[] stores pivoting order.
private static void gaussian(float a[][],
int index[]) {
int n = index.length;
float c[] = new float[n];
// Initialize the index
for (int i = 0; i < n; ++i) {
index[i] = i;
}
// Find the rescaling factors, one from each row
for (int i = 0; i < n; ++i){
float c1 = 0;
for (int j = 0; j < n; ++j){
float c0 = Math.abs(a[i][j]);
if (c0 > c1) {
c1 = c0;
}
}
c[i] = c1;
}
// Search the pivoting element from each column
int k = 0;
for (int j = 0; j < n - 1; ++j){
float pi1 = 0;
for (int i = j; i < n; ++i){
float pi0 = Math.abs(a[index[i]][j]);
pi0 /= c[index[i]];
if (pi0 > pi1) {

pi1 = pi0;
k = i;
}
}
// Interchange rows according to the pivoting order
int itmp = index[j];
index[j] = index[k];
index[k] = itmp;
for (int i = j + 1; i < n; ++i){
float pj = a[index[i]][j] / a[index[j]][j];
// Record pivoting ratios below the diagonal
a[index[i]][j] = pj;

// Modify other elements accordingly
for (int l = j + 1; l < n; ++l){
a[index[i]][l] -= pj * a[index[j]][l];
}
}
}
}
}
28 changes: 28 additions & 0 deletions src/test/java/com/hackoeur/jglm/Mat4Test.java
View file
Expand Up @@ -178,6 +178,34 @@ public void testMultiplication() {

Assert.assertEquals(expectedMul, multiplied);
}

@Test
public void testMultiplicationWithVector(){
Mat4 m1 = new Mat4(
49.f, 50.f,29.f, 12.f,
23.f, 90.f, 45.f, 32.f,
5.f, 47.f, 46.f, 89.f,
86.f, 88.f, 20.f, 58.f
);

Vec4 v1 = new Vec4(
54.f,
43.f,
28.f,
64.f
);

Vec4 expectedMul = new Vec4(
9279.f,
13518.f,
6069.f,
8228.f
);

Vec4 multiplied = m1.multiply(v1);

Assert.assertEquals(expectedMul, multiplied);
}

@Test
public void testAddition() {
Expand Down
86 changes: 85 additions & 1 deletion src/test/java/com/hackoeur/jglm/MatricesTest.java
View file
Expand Up @@ -133,4 +133,88 @@ public void testRotate() {
Assert.assertEquals(expected, got);
}

}
@Test
public void testInverseMat4(){
final Mat4 expected = Mat4.MAT4_IDENTITY;

Mat4 matrix = new Mat4(+10.00000000f, +12.00000000f, +30.00000000f, +40.00000000f,
+20.00000000f, +30.00000000f, +40.00000000f, +50.00000000f,
+30.00000000f, +18.00000000f, +15.00000000f, +60.00000000f,
+40.00000000f, +50.00000000f, +60.00000000f, +70.00000000f);
Mat4 invert = Matrices.invert(matrix);
Mat4 got = matrix.multiply(invert);
Assert.assertTrue(expected.equalsWithEpsilon(got, 1f));

matrix = new Mat4(+20.00000000f, +20.00000000f, +30.00000000f, +40.00000000f,
+20.00000000f, +30.00000000f, +40.00000000f, +50.00000000f,
+30.00000000f, +30.00000000f, +30.00000000f, +60.00000000f,
+40.00000000f, +50.00000000f, +60.00000000f, +70.00000000f);
invert = Matrices.invert(matrix);
got = matrix.multiply(invert);
Assert.assertTrue(expected.equalsWithEpsilon(got, 1f));

matrix = new Mat4(+500.00000000f, +800.00000000f, +33.00000000f, +44.00000000f,
+22.00000000f, +33.00000000f, +44.00000000f, +55.00000000f,
+33.00000000f, +33.00000000f, +33.00000000f, +66.00000000f,
+44.00000000f, +55.00000000f, +66.00000000f, +77.00000000f);
invert = Matrices.invert(matrix);
got = matrix.multiply(invert);
Assert.assertTrue(expected.equalsWithEpsilon(got, 1f));

matrix = new Mat4(-0.33333474f, +0.66666520f, +0.66666530f, +0.0000000E+0f,
+0.66666530f, -0.33333474f, +0.66666520f, +0.0000000E+0f,
+0.66666520f, +0.66666530f, -0.33333474f, +0.0000000E+0f,
+0.00000000f, +0.00000000f, +0.00000000f, +1.0000000E+0f);
invert = Matrices.invert(matrix);
got = matrix.multiply(invert);
Assert.assertTrue(expected.equalsWithEpsilon(got, 1f));

matrix = new Mat4(+1.50000000f, +0.00000000f, +0.00000000f, +0.00000000f,
+0.00000000f, +1.50000000f, +0.00000000f, +0.00000000f,
+0.00000000f, +0.00000000f, -1.16216218f, -1.00000000f,
+0.00000000f, +0.00000000f, -3.24324322f, +0.00000000f);
invert = Matrices.invert(matrix);
got = matrix.multiply(invert);
Assert.assertTrue(expected.equalsWithEpsilon(got, 1f));
}

@Test
public void testInverseMat3(){
final Mat3 expected = Mat3.MAT3_IDENTITY;

Mat3 matrix = new Mat3(+10.00000000f, +12.00000000f, +30.00000000f,
+20.00000000f, +30.00000000f, +40.00000000f,
+30.00000000f, +18.00000000f, +15.00000000f);
Mat3 invert = Matrices.invert(matrix);
Mat3 got = matrix.multiply(invert);
Assert.assertTrue(expected.equalsWithEpsilon(got, 1f));

matrix = new Mat3(+20.00000000f, +20.00000000f, +30.00000000f,
+20.00000000f, +30.00000000f, +40.00000000f,
+30.00000000f, +30.00000000f, +30.00000000f);
invert = Matrices.invert(matrix);
got = matrix.multiply(invert);
Assert.assertTrue(expected.equalsWithEpsilon(got, 1f));

matrix = new Mat3(+500.00000000f, +800.00000000f, +33.00000000f,
+22.00000000f, +33.00000000f, +44.00000000f,
+33.00000000f, +33.00000000f, +33.00000000f);
invert = Matrices.invert(matrix);
got = matrix.multiply(invert);
Assert.assertTrue(expected.equalsWithEpsilon(got, 1f));

matrix = new Mat3(-0.33333474f, +0.66666520f, +0.66666530f,
+0.66666530f, -0.33333474f, +0.66666520f,
+0.66666520f, +0.66666530f, -0.33333474f);
invert = Matrices.invert(matrix);
got = matrix.multiply(invert);
Assert.assertTrue(expected.equalsWithEpsilon(got, 1f));

matrix = new Mat3(+1.50000000f, +0.00000000f, +0.00000000f,
+0.00000000f, +1.50000000f, +0.00000000f,
+0.00000000f, +0.00000000f, -1.16216218f);
invert = Matrices.invert(matrix);
got = matrix.multiply(invert);
Assert.assertTrue(expected.equalsWithEpsilon(got, 1f));
}
}

0 comments on commit 4c7f3e4

Please sign in to comment.