Add functions to calculate the determinant and inverse of a 4x4 matrix

5 files changed, 194 insertions, 1 deletions

diff --git a/.gitignore b/.gitignore
index 0cf9dcc..8428217 100644
--- a/.gitignore
+++ b/.gitignore
@@ -19,4 +19,5 @@ src/.deps
 test/.deps
 test/add4v_4v
 test/lookat4v
+test/det
 docs/
diff --git a/include/glu3.h b/include/glu3.h
index b735ba8..3dff417 100644
--- a/include/glu3.h
+++ b/include/glu3.h
@@ -580,6 +580,21 @@ void gluOrtho6f(GLUmat4 *result, GLfloat left, GLfloat right, GLfloat bottom,
 void gluTranspose4m(GLUmat4 *result, const GLUmat4 *m);
 
 /**
+ * Calculate the determinant of a matrix.
+ */
+GLfloat gluDeterminant4_4m(const GLUmat4 *m);
+
+/**
+ * Calculate the inverse of a matrix.
+ *
+ * \return
+ * If the matrix is invertable (i.e., the determinant is not zero), \c GL_TRUE
+ * is returned.  Otherwise GL_FALSE is returned.
+ */
+GLboolean gluInverse4_4m(GLUmat4 *result, const GLUmat4 *m);
+
+
+/**
  * Identity matrix
  *
  * Global constant containing the matrix:
diff --git a/src/matrix.c b/src/matrix.c
index e2f02e4..b4859d0 100644
--- a/src/matrix.c
+++ b/src/matrix.c
@@ -229,3 +229,109 @@ gluOrtho4f(GLUmat4 *result, GLfloat left, GLfloat right, GLfloat bottom,
 {
 	gluOrtho6f(result, left, right, bottom, top, -1.0, 1.0);
 }
+
+
+static double
+det3(const GLUmat4 *m, unsigned i, unsigned j)
+{
+	unsigned r;
+	unsigned c;
+	double det = 0.0;
+	GLUvec4 col[6];
+
+
+	/* Generate a 3x3 matrix from the original matrix with the ith column
+	 * and the jth row removed.  The columns of the matrix are duplicated
+	 * to make the 'c - r' column addressing, below, work out easier.
+	 */
+	for (c = 0; c < 4; c++) {
+		if (c < i) {
+			col[c + 0] = m->col[c];
+			col[c + 3] = m->col[c];
+		} else if (c > i) {
+			col[c - 1] = m->col[c];
+			col[c + 2] = m->col[c];
+		}
+	}
+
+	for (r = j; r < 3; r++) {
+		col[0].values[r] = col[0].values[r + 1];
+		col[1].values[r] = col[1].values[r + 1];
+		col[2].values[r] = col[2].values[r + 1];
+		col[3].values[r] = col[3].values[r + 1];
+		col[4].values[r] = col[4].values[r + 1];
+		col[5].values[r] = col[5].values[r + 1];
+	}
+
+
+	/* Calculate the determinant of the resulting 3x3 matrix.
+	 */
+	for (c = 0; c < 3; c++) {
+		double diag1 = col[c].values[0];
+		double diag2 = col[c].values[0];
+
+		for (r = 1; r < 3; r++) {
+			diag1 *= col[(0 + c) + r].values[r];
+			diag2 *= col[(3 + c) - r].values[r];
+		}
+
+		det += (diag1 - diag2);
+	}
+
+	return det;
+}
+
+
+GLfloat
+gluDeterminant4_4m(const GLUmat4 *m)
+{
+	double det = 0.0;
+	unsigned c;
+
+	for (c = 0; c < 4; c++) {
+		if (m->col[c].values[3] != 0.0) {
+			/* The usual equation is -1**(i+j) where i and j are
+			 * the row and column of the matrix on the range
+			 * [1, rows] and [1, cols].  Note that r and c are on
+			 * the range [0, rows - 1] and [0, cols - 1].
+			 */
+			const double sign = ((c ^ 3) & 1) ? -1.0 : 1.0;
+			const double d = det3(m, c, 3);
+
+			det += sign * m->col[c].values[3] * d;
+		}
+	}
+
+	return det;
+}
+
+
+GLboolean
+gluInverse4_4m(GLUmat4 *result, const GLUmat4 *m)
+{
+	const double det = gluDeterminant4_4m(m);
+	double inv_det;
+	unsigned c;
+	unsigned r;
+
+
+	if (det == 0.0)
+		return GL_FALSE;
+
+	inv_det = 1.0 / det;
+	for (c = 0; c < 4; c++) {
+		for (r = 0; r < 4; r++) {
+			/* The usual equation is -1**(i+j) where i and j are
+			 * the row and column of the matrix on the range
+			 * [1, rows] and [1, cols].  Note that r and c are on
+			 * the range [0, rows - 1] and [0, cols - 1].
+			 */
+			const double sign = ((c ^ r) & 1) ? -1.0 : 1.0;
+			const double d = det3(m, c, r);
+
+			result->col[r].values[c] = sign * inv_det * d;
+		}
+	}
+
+	return GL_TRUE;
+}
diff --git a/test/Makefile.am b/test/Makefile.am
index a0e33d2..4e4338e 100644
--- a/test/Makefile.am
+++ b/test/Makefile.am
@@ -3,7 +3,8 @@ LDADD = ../src/libGLU3.a -lm
 
 TESTS = \
 	add4v_4v \
-	lookat4v
+	lookat4v \
+	det
 
 EXTRA_PROGRAMS = $(TESTS)
 CLEANFILES = $(EXTRA_PROGRAMS)
diff --git a/test/det.c b/test/det.c
new file mode 100644
index 0000000..c3c7461
--- /dev/null
+++ b/test/det.c
@@ -0,0 +1,70 @@
+/*
+ * Copyright © 2009 Ian D. Romanick
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ */
+
+#include <glu3.h>
+#include <math.h>
+#include <assert.h>
+
+struct foo {
+	GLUmat4 matrix;
+	GLfloat det;
+};
+
+static const struct foo test_cases[] = {
+	{
+		{{
+			{{1.0, 0.0, 0.0, 0.0}},
+			{{0.0, 1.0, 0.0, 0.0}},
+			{{0.0, 0.0, 1.0, 0.0}},
+			{{0.0, 0.0, 0.0, 1.0}}
+		}},
+		1.0
+	},
+	{
+		{{
+			{{1.0, 0.0, 0.0, 0.0}},
+			{{0.0, 1.0, 0.0, 0.0}},
+			{{0.0, 0.0, 1.0, 0.0}},
+			{{0.0, 0.0, 0.0, 1.0}}
+		}},
+		1.0
+	},
+};
+
+#define ELEMENTS_OF(a)  (sizeof(a) / sizeof(a[0]))
+
+int main(int argc, char **argv)
+{
+	unsigned i;
+
+	(void)argc;
+	(void)argv;
+
+	for (i = 0; i < ELEMENTS_OF(test_cases); i++) {
+		const GLfloat det = gluDeterminant4_4m(& test_cases[i].matrix);
+
+		assert(fabs(det - test_cases[i].det) < 0.0000001);
+	}
+
+	return 0;
+}