patch from Tomaz adding a proper matrix inverter, it should be able to invert any...

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

diff --git a/matrixlib.c b/matrixlib.c
index 2dd212a98eca0b53bb454cf33ef2a5c6726c13d6..7e4783e2c7ec01c44c07f382b10319be352fdfcd 100644 (file)
--- 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 917bd396c2cb701b75331b0a3312cce23f133de3..f510f3d9d567975b99ad0c624f89a0383d334871 100644 (file)
--- 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