From: John Tsiombikas <nuclear@member.fsf.org>
Date: Tue, 2 Oct 2018 06:37:04 +0000 (+0300)
Subject: more untested matrix function (inverse, cofactors, etc)
X-Git-Url: http://git.mutantstargoat.com/user/nuclear/?a=commitdiff_plain;ds=inline;h=ceeb6a82a05fb8e4a741adf5916085da5fadb11c;hp=1acd12168f51c4cd19f9f40c3b4bd5d77d424801;p=gph-cmath

more untested matrix function (inverse, cofactors, etc)
---

diff --git a/src/cgmath.h b/src/cgmath.h
index 36632f2..9ec2fc8 100644
--- a/src/cgmath.h
+++ b/src/cgmath.h
@@ -138,7 +138,14 @@ static inline void cgm_mgetrow_v4(cgm_vec4 *v, const float *m, int idx);
 static inline void cgm_mgetcol_v3(cgm_vec3 *v, const float *m, int idx);
 static inline void cgm_mgetcol_v4(cgm_vec4 *v, const float *m, int idx);
 
+static inline void cgm_msubmatrix(float *m, int row, int col);
 static inline void cgm_mupper3(float *m);
+static inline float cgm_msubdet(float *m, int row, int col);
+static inline float cgm_mcofactor(float *m, int row, int col);
+static inline float cgm_mdet(float *m);
+static inline void cgm_mtranspose(float *m);
+static inline void cgm_mcofmatrix(float *m);
+static inline int cgm_minverse(float *m);	/* returns 0 on success, -1 for singular */
 
 #include "cgmvec3.inl"
 #include "cgmvec4.inl"
diff --git a/src/cgmmat.inl b/src/cgmmat.inl
index 5afb224..74b27cc 100644
--- a/src/cgmmat.inl
+++ b/src/cgmmat.inl
@@ -79,19 +79,25 @@ static inline void cgm_mgetcol_v4(cgm_vec4 *v, const float *m, int idx)
 
 static inline void cgm_msubmatrix(float *m, int row, int col)
 {
-	int i, j;
+	float orig[16];
+	int i, j, subi, subj;
+
+	cgm_mcopy(orig, m);
+
+	subi = 0;
 	for(i=0; i<4; i++) {
-		for(j=0; j<4; j++) {
-			int si = i;
-			int sj = j;
-			if(i >= col) si++;
-			if(j >= row) sj++;
+		if(i == col) continue;
 
-			if(si == i && sj == j) continue;
+		subj = 0;
+		for(j=0; j<4; j++) {
+			if(j == row) continue;
 
-			m[i * 4 + j] = m[si * 4 + sj];
+			m[subi * 4 + subj++] = orig[i * 4 + j];
 		}
+		subi++;
 	}
+
+	cgm_mupper3(m);
 }
 
 static inline void cgm_mupper3(float *m)
@@ -99,3 +105,67 @@ static inline void cgm_mupper3(float *m)
 	m[3] = m[7] = m[11] = m[12] = m[13] = m[14] = 0.0f;
 	m[15] = 1.0f;
 }
+
+static inline float cgm_msubdet(float *m, int row, int col)
+{
+	cgm_msubmatrix(m, row, col);
+	return cgm_mdet(m);
+}
+
+static inline float cgm_mcofactor(float *m, int row, int col)
+{
+	float min = cgm_msubdet(m, row, col);
+	return (row + col) & 1 ? -min : min;
+}
+
+static inline float cgm_mdet(float *m)
+{
+	return m[0] * cgm_msubdet(m, 0, 0) - m[1] * cgm_msubdet(m, 0, 1) +
+		m[2] * cgm_msubdet(m, 0, 2) - m[3] * cgm_msubdet(m, 0, 3);
+}
+
+static inline void cgm_mtranspose(float *m)
+{
+	int i, j;
+	for(i=0; i<4; i++) {
+		for(j=0; j<i; j++) {
+			int a = i * 4 + j;
+			int b = j * 4 + i;
+			float tmp = m[a];
+			m[a] = m[b];
+			m[b] = tmp;
+		}
+	}
+}
+
+static inline void cgm_mcofmatrix(float *m)
+{
+	float tmp[16];
+	int i, j;
+
+	cgm_mcopy(tmp, m);
+
+	for(i=0; i<4; i++) {
+		for(j=0; j<4; j++) {
+			m[i * 4 + j] = cgm_mcofactor(tmp, i, j);
+		}
+	}
+}
+
+static inline int cgm_minverse(float *m)
+{
+	int i, j;
+	float tmp[16];
+	float inv_det;
+	float det = cgm_mdet(m);
+	if(det == 0.0f) return -1;
+	inv_det = 1.0f / det;
+
+	cgm_mcopy(tmp, m);
+
+	for(i=0; i<4; i++) {
+		for(j=0; j<4; j++) {
+			m[i * 4 + j] = cgm_mcofactor(tmp, j, i) * inv_det;	/* transposed */
+		}
+	}
+}