From 6806798554012f2d994a33282c60795c330beeb3 Mon Sep 17 00:00:00 2001 From: havoc Date: Sun, 28 Jan 2007 02:52:41 +0000 Subject: [PATCH] patch from Tomaz adding a proper matrix inverter, it should be able to invert any matrix that is possible to invert, but is VERY slow, and consequently Matrix4x4_Invert_Simple has been kept for speed reasons it is probably possible to optimize this function significantly, and it would also be possible to add non-uniform-scaling inversion to Matrix4x4_Invert_Simple to make it more flexible git-svn-id: svn://svn.icculus.org/twilight/trunk/darkplaces@6759 d7cf8633-e32d-0410-b094-e92efae38249 --- matrixlib.c | 168 ++++++++++++++++++++++++++++++++++++++++++++++++++++ matrixlib.h | 6 ++ 2 files changed, 174 insertions(+) diff --git a/matrixlib.c b/matrixlib.c index 2dd212a9..7e4783e2 100644 --- a/matrixlib.c +++ b/matrixlib.c @@ -135,6 +135,174 @@ void Matrix4x4_Transpose (matrix4x4_t *out, const matrix4x4_t *in1) out->m[3][3] = in1->m[3][3]; } +int Matrix4x4_Invert_Full (matrix4x4_t *out, const matrix4x4_t *in1) +{ + double *temp; + double *r[4]; + double rtemp[4][8]; + double m[4]; + double s; + + r[0] = rtemp[0]; + r[1] = rtemp[1]; + r[2] = rtemp[2]; + r[3] = rtemp[3]; + +#ifdef MATRIX4x4_OPENGLORIENTATION + r[0][0] = in1->m[0][0]; r[0][1] = in1->m[1][0]; r[0][2] = in1->m[2][0]; r[0][3] = in1->m[3][0]; + r[0][4] = 1.0; r[0][5] = r[0][6] = r[0][7] = 0.0; + + r[1][0] = in1->m[0][1]; r[1][1] = in1->m[1][1]; r[1][2] = in1->m[2][1]; r[1][3] = in1->m[3][1]; + r[1][5] = 1.0; r[1][4] = r[1][6] = r[1][7] = 0.0; + + r[2][0] = in1->m[0][2]; r[2][1] = in1->m[1][2]; r[2][2] = in1->m[2][2]; r[2][3] = in1->m[3][2]; + r[2][6] = 1.0; r[2][4] = r[2][5] = r[2][7] = 0.0; + + r[3][0] = in1->m[0][3]; r[3][1] = in1->m[1][3]; r[3][2] = in1->m[2][3]; r[3][3] = in1->m[3][3]; + r[3][7] = 1.0; r[3][4] = r[3][5] = r[3][6] = 0.0; +#else + r[0][0] = in1->m[0][0]; r[0][1] = in1->m[0][1]; r[0][2] = in1->m[0][2]; r[0][3] = in1->m[0][3]; + r[0][4] = 1.0; r[0][5] = r[0][6] = r[0][7] = 0.0; + + r[1][0] = in1->m[1][0]; r[1][1] = in1->m[1][1]; r[1][2] = in1->m[1][2]; r[1][3] = in1->m[1][3]; + r[1][5] = 1.0; r[1][4] = r[1][6] = r[1][7] = 0.0; + + r[2][0] = in1->m[2][0]; r[2][1] = in1->m[2][1]; r[2][2] = in1->m[2][2]; r[2][3] = in1->m[2][3]; + r[2][6] = 1.0; r[2][4] = r[2][5] = r[2][7] = 0.0; + + r[3][0] = in1->m[3][0]; r[3][1] = in1->m[3][1]; r[3][2] = in1->m[3][2]; r[3][3] = in1->m[3][3]; + r[3][7] = 1.0; r[3][4] = r[3][5] = r[3][6] = 0.0; +#endif + + if (fabs (r[3][0]) > fabs (r[2][0])) { temp = r[3]; r[3] = r[2]; r[2] = temp; } + if (fabs (r[2][0]) > fabs (r[1][0])) { temp = r[2]; r[2] = r[1]; r[1] = temp; } + if (fabs (r[1][0]) > fabs (r[0][0])) { temp = r[1]; r[1] = r[0]; r[0] = temp; } + + if (r[0][0]) + { + m[1] = r[1][0] / r[0][0]; + m[2] = r[2][0] / r[0][0]; + m[3] = r[3][0] / r[0][0]; + + s = r[0][1]; r[1][1] -= m[1] * s; r[2][1] -= m[2] * s; r[3][1] -= m[3] * s; + s = r[0][2]; r[1][2] -= m[1] * s; r[2][2] -= m[2] * s; r[3][2] -= m[3] * s; + s = r[0][3]; r[1][3] -= m[1] * s; r[2][3] -= m[2] * s; r[3][3] -= m[3] * s; + + s = r[0][4]; if (s) { r[1][4] -= m[1] * s; r[2][4] -= m[2] * s; r[3][4] -= m[3] * s; } + s = r[0][5]; if (s) { r[1][5] -= m[1] * s; r[2][5] -= m[2] * s; r[3][5] -= m[3] * s; } + s = r[0][6]; if (s) { r[1][6] -= m[1] * s; r[2][6] -= m[2] * s; r[3][6] -= m[3] * s; } + s = r[0][7]; if (s) { r[1][7] -= m[1] * s; r[2][7] -= m[2] * s; r[3][7] -= m[3] * s; } + + if (fabs (r[3][1]) > fabs (r[2][1])) { temp = r[3]; r[3] = r[2]; r[2] = temp; } + if (fabs (r[2][1]) > fabs (r[1][1])) { temp = r[2]; r[2] = r[1]; r[1] = temp; } + + if (r[1][1]) + { + m[2] = r[2][1] / r[1][1]; + m[3] = r[3][1] / r[1][1]; + r[2][2] -= m[2] * r[1][2]; + r[3][2] -= m[3] * r[1][2]; + r[2][3] -= m[2] * r[1][3]; + r[3][3] -= m[3] * r[1][3]; + + s = r[1][4]; if (s) { r[2][4] -= m[2] * s; r[3][4] -= m[3] * s; } + s = r[1][5]; if (s) { r[2][5] -= m[2] * s; r[3][5] -= m[3] * s; } + s = r[1][6]; if (s) { r[2][6] -= m[2] * s; r[3][6] -= m[3] * s; } + s = r[1][7]; if (s) { r[2][7] -= m[2] * s; r[3][7] -= m[3] * s; } + + if (fabs (r[3][2]) > fabs (r[2][2])) { temp = r[3]; r[3] = r[2]; r[2] = temp; } + + if (r[2][2]) + { + m[3] = r[3][2] / r[2][2]; + r[3][3] -= m[3] * r[2][3]; + r[3][4] -= m[3] * r[2][4]; + r[3][5] -= m[3] * r[2][5]; + r[3][6] -= m[3] * r[2][6]; + r[3][7] -= m[3] * r[2][7]; + + if (r[3][3]) + { + s = 1.0 / r[3][3]; + r[3][4] *= s; + r[3][5] *= s; + r[3][6] *= s; + r[3][7] *= s; + + m[2] = r[2][3]; + s = 1.0 / r[2][2]; + r[2][4] = s * (r[2][4] - r[3][4] * m[2]); + r[2][5] = s * (r[2][5] - r[3][5] * m[2]); + r[2][6] = s * (r[2][6] - r[3][6] * m[2]); + r[2][7] = s * (r[2][7] - r[3][7] * m[2]); + + m[1] = r[1][3]; + r[1][4] -= r[3][4] * m[1], r[1][5] -= r[3][5] * m[1]; + r[1][6] -= r[3][6] * m[1], r[1][7] -= r[3][7] * m[1]; + + m[0] = r[0][3]; + r[0][4] -= r[3][4] * m[0], r[0][5] -= r[3][5] * m[0]; + r[0][6] -= r[3][6] * m[0], r[0][7] -= r[3][7] * m[0]; + + m[1] = r[1][2]; + s = 1.0 / r[1][1]; + r[1][4] = s * (r[1][4] - r[2][4] * m[1]), r[1][5] = s * (r[1][5] - r[2][5] * m[1]); + r[1][6] = s * (r[1][6] - r[2][6] * m[1]), r[1][7] = s * (r[1][7] - r[2][7] * m[1]); + + m[0] = r[0][2]; + r[0][4] -= r[2][4] * m[0], r[0][5] -= r[2][5] * m[0]; + r[0][6] -= r[2][6] * m[0], r[0][7] -= r[2][7] * m[0]; + + m[0] = r[0][1]; + s = 1.0 / r[0][0]; + r[0][4] = s * (r[0][4] - r[1][4] * m[0]), r[0][5] = s * (r[0][5] - r[1][5] * m[0]); + r[0][6] = s * (r[0][6] - r[1][6] * m[0]), r[0][7] = s * (r[0][7] - r[1][7] * m[0]); + +#ifdef MATRIX4x4_OPENGLORIENTATION + out->m[0][0] = r[0][4]; + out->m[0][1] = r[1][4]; + out->m[0][2] = r[2][4]; + out->m[0][3] = r[3][4]; + out->m[1][0] = r[0][5]; + out->m[1][1] = r[1][5]; + out->m[1][2] = r[2][5]; + out->m[1][3] = r[3][5]; + out->m[2][0] = r[0][6]; + out->m[2][1] = r[1][6]; + out->m[2][2] = r[2][6]; + out->m[2][3] = r[3][6]; + out->m[3][0] = r[0][7]; + out->m[3][1] = r[1][7]; + out->m[3][2] = r[2][7]; + out->m[3][3] = r[3][7]; +#else + out->m[0][0] = r[0][4]; + out->m[0][1] = r[0][5]; + out->m[0][2] = r[0][6]; + out->m[0][3] = r[0][7]; + out->m[1][0] = r[1][4]; + out->m[1][1] = r[1][5]; + out->m[1][2] = r[1][6]; + out->m[1][3] = r[1][7]; + out->m[2][0] = r[2][4]; + out->m[2][1] = r[2][5]; + out->m[2][2] = r[2][6]; + out->m[2][3] = r[2][7]; + out->m[3][0] = r[3][4]; + out->m[3][1] = r[3][5]; + out->m[3][2] = r[3][6]; + out->m[3][3] = r[3][7]; +#endif + + return 1; + } + } + } + } + + return 0; +} + void Matrix4x4_Invert_Simple (matrix4x4_t *out, const matrix4x4_t *in1) { // we only support uniform scaling, so assume the first row is enough diff --git a/matrixlib.h b/matrixlib.h index 917bd396..f510f3d9 100644 --- a/matrixlib.h +++ b/matrixlib.h @@ -31,6 +31,12 @@ void Matrix4x4_Concat (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4 // (is this useful for anything?) void Matrix4x4_Transpose (matrix4x4_t *out, const matrix4x4_t *in1); // creates a matrix that does the opposite of the matrix provided +// this is a full matrix inverter, it should be able to invert any matrix that +// is possible to invert +// (non-uniform scaling, rotation, shearing, and translation, possibly others) +// warning: this function is SLOW +int Matrix4x4_Invert_Full (matrix4x4_t *out, const matrix4x4_t *in1); +// creates a matrix that does the opposite of the matrix provided // only supports translate, rotate, scale (not scale3) matrices void Matrix4x4_Invert_Simple (matrix4x4_t *out, const matrix4x4_t *in1); // creates a matrix that does the same rotation and translation as the matrix -- 2.39.2