solve method
Solve A
* x
= b
.
Implementation
static void solve(Matrix4 A, Vector4 x, Vector4 b) {
final double a00 = A._m4storage[0];
final double a01 = A._m4storage[1];
final double a02 = A._m4storage[2];
final double a03 = A._m4storage[3];
final double a10 = A._m4storage[4];
final double a11 = A._m4storage[5];
final double a12 = A._m4storage[6];
final double a13 = A._m4storage[7];
final double a20 = A._m4storage[8];
final double a21 = A._m4storage[9];
final double a22 = A._m4storage[10];
final double a23 = A._m4storage[11];
final double a30 = A._m4storage[12];
final double a31 = A._m4storage[13];
final double a32 = A._m4storage[14];
final double a33 = A._m4storage[15];
final double b00 = a00 * a11 - a01 * a10;
final double b01 = a00 * a12 - a02 * a10;
final double b02 = a00 * a13 - a03 * a10;
final double b03 = a01 * a12 - a02 * a11;
final double b04 = a01 * a13 - a03 * a11;
final double b05 = a02 * a13 - a03 * a12;
final double b06 = a20 * a31 - a21 * a30;
final double b07 = a20 * a32 - a22 * a30;
final double b08 = a20 * a33 - a23 * a30;
final double b09 = a21 * a32 - a22 * a31;
final double b10 = a21 * a33 - a23 * a31;
final double b11 = a22 * a33 - a23 * a32;
final double bX = b.storage[0];
final double bY = b.storage[1];
final double bZ = b.storage[2];
final double bW = b.storage[3];
double det =
b00 * b11 - b01 * b10 + b02 * b09 + b03 * b08 - b04 * b07 + b05 * b06;
if (det != 0.0) {
det = 1.0 / det;
}
x
..x = det *
((a11 * b11 - a12 * b10 + a13 * b09) * bX -
(a10 * b11 - a12 * b08 + a13 * b07) * bY +
(a10 * b10 - a11 * b08 + a13 * b06) * bZ -
(a10 * b09 - a11 * b07 + a12 * b06) * bW)
..y = det *
-((a01 * b11 - a02 * b10 + a03 * b09) * bX -
(a00 * b11 - a02 * b08 + a03 * b07) * bY +
(a00 * b10 - a01 * b08 + a03 * b06) * bZ -
(a00 * b09 - a01 * b07 + a02 * b06) * bW)
..z = det *
((a31 * b05 - a32 * b04 + a33 * b03) * bX -
(a30 * b05 - a32 * b02 + a33 * b01) * bY +
(a30 * b04 - a31 * b02 + a33 * b00) * bZ -
(a30 * b03 - a31 * b01 + a32 * b00) * bW)
..w = det *
-((a21 * b05 - a22 * b04 + a23 * b03) * bX -
(a20 * b05 - a22 * b02 + a23 * b01) * bY +
(a20 * b04 - a21 * b02 + a23 * b00) * bZ -
(a20 * b03 - a21 * b01 + a22 * b00) * bW);
}