[util] Add simd 4x4 matrix functions

Currently just add, subtract, multiply (m m and m v).

include/QF/simd/mat4f.h

@@ -0,0 +1,123 @@
+/*
+	QF/simd/mat4f.h
+
+	Matrix functions for mat4f_t (ie, float precision)
+
+	Copyright (C) 2021  Bill Currie <bill@taniwha.org>
+
+	This program is free software; you can redistribute it and/or
+	modify it under the terms of the GNU General Public License
+	as published by the Free Software Foundation; either version 2
+	of the License, or (at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+
+	See the GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program; if not, write to:
+
+		Free Software Foundation, Inc.
+		59 Temple Place - Suite 330
+		Boston, MA  02111-1307, USA
+
+*/
+
+#ifndef __QF_simd_mat4f_h
+#define __QF_simd_mat4f_h
+
+#include <immintrin.h>
+
+#include "QF/simd/types.h"
+
+GNU89INLINE inline void maddf (mat4f_t c, const mat4f_t a, const mat4f_t b);
+GNU89INLINE inline void msubf (mat4f_t c, const mat4f_t a, const mat4f_t b);
+GNU89INLINE inline void mmulf (mat4f_t c, const mat4f_t a, const mat4f_t b);
+GNU89INLINE inline vec4f_t mvmulf (const mat4f_t m, vec4f_t v) __attribute__((const));
+GNU89INLINE inline vec4f_t m3vmulf (const mat4f_t m, vec4f_t v) __attribute__((const));
+GNU89INLINE inline void mat4fidentity (mat4f_t m);
+
+#ifndef IMPLEMENT_MAT4F_Funcs
+GNU89INLINE inline
+#else
+VISIBLE
+#endif
+void
+maddf (mat4f_t c, const mat4f_t a, const mat4f_t b)
+{
+	c[0] = a[0] + b[0];
+	c[1] = a[1] + b[1];
+	c[2] = a[2] + b[2];
+	c[3] = a[3] + b[3];
+}
+
+#ifndef IMPLEMENT_MAT4F_Funcs
+GNU89INLINE inline
+#else
+VISIBLE
+#endif
+void
+msubf (mat4f_t c, const mat4f_t a, const mat4f_t b)
+{
+	c[0] = a[0] - b[0];
+	c[1] = a[1] - b[1];
+	c[2] = a[2] - b[2];
+	c[3] = a[3] - b[3];
+}
+
+#ifndef IMPLEMENT_MAT4F_Funcs
+GNU89INLINE inline
+#else
+VISIBLE
+#endif
+void
+mmulf (mat4f_t c, const mat4f_t a, const mat4f_t b)
+{
+	c[0] = a[0] * b[0][0] + a[1] * b[0][1] + a[2] * b[0][2] + a[3] * b[0][3];
+	c[1] = a[0] * b[1][0] + a[1] * b[1][1] + a[2] * b[1][2] + a[3] * b[1][3];
+	c[2] = a[0] * b[2][0] + a[1] * b[2][1] + a[2] * b[2][2] + a[3] * b[2][3];
+	c[3] = a[0] * b[3][0] + a[1] * b[3][1] + a[2] * b[3][2] + a[3] * b[3][3];
+}
+
+#ifndef IMPLEMENT_MAT4F_Funcs
+GNU89INLINE inline
+#else
+VISIBLE
+#endif
+vec4f_t
+mvmulf (const mat4f_t m, vec4f_t v)
+{
+	return m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3] * v[3];
+}
+
+#ifndef IMPLEMENT_MAT4F_Funcs
+GNU89INLINE inline
+#else
+VISIBLE
+#endif
+vec4f_t
+m3vmulf (const mat4f_t m, vec4f_t v)
+{
+	vec4f_t     w;
+	w = m[0] * v[0] + m[1] * v[1] + m[2] * v[2];
+	w[3] = 1;
+	return w;
+}
+
+#ifndef IMPLEMENT_MAT4F_Funcs
+GNU89INLINE inline
+#else
+VISIBLE
+#endif
+void
+mat4fidentity (mat4f_t m)
+{
+	m[0] = (vec4f_t) { 1, 0, 0, 0 };
+	m[1] = (vec4f_t) { 0, 1, 0, 0 };
+	m[2] = (vec4f_t) { 0, 0, 1, 0 };
+	m[3] = (vec4f_t) { 0, 0, 0, 1 };
+}
+
+#endif//__QF_simd_mat4f_h

include/QF/simd/types.h

@@ -82,4 +82,8 @@ VEC_TYPE (int, vec4i_t);
 #define VEC4I_FMT "[%d, %d, %d, %d]"
 #define VEC4_EXP(v) (v)[0], (v)[1], (v)[2], (v)[3]
 
+#define MAT4_ROW(m, r) (m)[0][r], (m)[1][r], (m)[2][r], (m)[3][r]
+
+typedef vec4f_t mat4f_t[4];
+typedef vec4i_t mat4i_t[4];
 #endif//__QF_simd_types_h

libs/util/simd.c

@@ -32,6 +32,8 @@
 
 #define IMPLEMENT_VEC4F_Funcs
 #define IMPLEMENT_VEC4D_Funcs
+#define IMPLEMENT_MAT4F_Funcs
 
 #include "QF/simd/vec4d.h"
 #include "QF/simd/vec4f.h"
+#include "QF/simd/mat4f.h"

libs/util/test/test-simd.c

@@ -9,6 +9,7 @@
 
 #include "QF/simd/vec4d.h"
 #include "QF/simd/vec4f.h"
+#include "QF/simd/mat4f.h"
 
 #define right   { 1, 0, 0 }
 #define forward { 0, 1, 0 }
@@ -25,6 +26,22 @@
 #define none     { -1, -1, -1, -1 }
 #define nqident  {  0,  0,  0, -1 }
 
+#define identity \
+	{	{ 1, 0, 0, 0 }, \
+		{ 0, 1, 0, 0 }, \
+		{ 0, 0, 1, 0 }, \
+		{ 0, 0, 0, 1 } }
+#define rotate120 \
+	{	{ 0, 1, 0, 0 }, \
+		{ 0, 0, 1, 0 }, \
+		{ 1, 0, 0, 0 }, \
+		{ 0, 0, 0, 1 } }
+#define rotate240 \
+	{	{ 0, 0, 1, 0 }, \
+		{ 1, 0, 0, 0 }, \
+		{ 0, 1, 0, 0 }, \
+		{ 0, 0, 0, 1 } }
+
 #define s05 0.70710678118654757
 
 typedef  struct {
@@ -43,6 +60,22 @@ typedef  struct {
 	vec4f_t     ulp_errors;
 } vec4f_test_t;
 
+typedef  struct {
+	void   (*op) (mat4f_t c, const mat4f_t a, const mat4f_t b);
+	mat4f_t     a;
+	mat4f_t     b;
+	mat4f_t     expect;
+	mat4f_t     ulp_errors;
+} mat4f_test_t;
+
+typedef  struct {
+	vec4f_t   (*op) (const mat4f_t a, vec4f_t b);
+	mat4f_t     a;
+	vec4f_t     b;
+	vec4f_t     expect;
+	vec4f_t     ulp_errors;
+} mv4f_test_t;
+
 static vec4d_t tvtruncd (vec4d_t v, vec4d_t ignore)
 {
 	return vtruncd (v);
@@ -314,6 +347,26 @@ static vec4f_test_t vec4f_tests[] = {
 };
 #define num_vec4f_tests (sizeof (vec4f_tests) / (sizeof (vec4f_tests[0])))
 
+static mat4f_test_t mat4f_tests[] = {
+	{ mmulf, identity, identity, identity },
+	{ mmulf, rotate120, identity, rotate120 },
+	{ mmulf, identity, rotate120, rotate120 },
+	{ mmulf, rotate120, rotate120, rotate240 },
+	{ mmulf, rotate120, rotate240, identity },
+	{ mmulf, rotate240, rotate120, identity },
+};
+#define num_mat4f_tests (sizeof (mat4f_tests) / (sizeof (mat4f_tests[0])))
+
+static mv4f_test_t mv4f_tests[] = {
+	{ mvmulf, identity, { 1, 0, 0, 0 }, { 1, 0, 0, 0 } },
+	{ mvmulf, identity, { 0, 1, 0, 0 }, { 0, 1, 0, 0 } },
+	{ mvmulf, identity, { 0, 0, 1, 0 }, { 0, 0, 1, 0 } },
+	{ mvmulf, identity, { 0, 0, 0, 1 }, { 0, 0, 0, 1 } },
+	{ mvmulf, rotate120, { 1, 2, 3, 4 }, { 3, 1, 2, 4 } },
+	{ mvmulf, rotate240, { 1, 2, 3, 4 }, { 2, 3, 1, 4 } },
+};
+#define num_mv4f_tests (sizeof (mv4f_tests) / (sizeof (mv4f_tests[0])))
+
 static int
 run_vec4d_tests (void)
 {
@@ -326,7 +379,7 @@ run_vec4d_tests (void)
 		vec4l_t     res = result != expect;
 		if (res[0] || res[1] || res[2] || res[3]) {
 			ret |= 1;
-			printf ("\nrun_vec4d_tests\n");
+			printf ("\nrun_vec4d_tests %zd\n", i);
 			printf ("a: " VEC4D_FMT "\n", VEC4_EXP(test->a));
 			printf ("b: " VEC4D_FMT "\n", VEC4_EXP(test->b));
 			printf ("r: " VEC4D_FMT "\n", VEC4_EXP(result));
@@ -351,7 +404,7 @@ run_vec4f_tests (void)
 		vec4i_t     res = result != expect;
 		if (res[0] || res[1] || res[2] || res[3]) {
 			ret |= 1;
-			printf ("\nrun_vec4f_tests\n");
+			printf ("\nrun_vec4f_tests %zd\n", i);
 			printf ("a: " VEC4F_FMT "\n", VEC4_EXP(test->a));
 			printf ("b: " VEC4F_FMT "\n", VEC4_EXP(test->b));
 			printf ("r: " VEC4F_FMT "\n", VEC4_EXP(result));
@@ -364,11 +417,97 @@ run_vec4f_tests (void)
 	return ret;
 }
 
+static int
+run_mat4f_tests (void)
+{
+	int         ret = 0;
+
+	for (size_t i = 0; i < num_mat4f_tests; i++) {
+		__auto_type test = &mat4f_tests[i];
+		mat4f_t     result;
+		mat4f_t     expect;
+		mat4i_t     res = {};
+
+		test->op (result, test->a, test->b);
+		maddf (expect, test->expect, test->ulp_errors);
+
+		int         fail = 0;
+		for (int j = 0; j < 4; j++) {
+			res[j] = result[j] != expect[j];
+			fail |= res[j][0] || res[j][1] || res[j][2] || res[j][3];
+		}
+		if (fail) {
+			ret |= 1;
+			printf ("\nrun_mat4f_tests %zd\n", i);
+			printf ("a: " VEC4F_FMT "\n", MAT4_ROW(test->a, 0));
+			printf ("   " VEC4F_FMT "\n", MAT4_ROW(test->a, 1));
+			printf ("   " VEC4F_FMT "\n", MAT4_ROW(test->a, 2));
+			printf ("   " VEC4F_FMT "\n", MAT4_ROW(test->a, 3));
+			printf ("b: " VEC4F_FMT "\n", MAT4_ROW(test->b, 0));
+			printf ("   " VEC4F_FMT "\n", MAT4_ROW(test->b, 1));
+			printf ("   " VEC4F_FMT "\n", MAT4_ROW(test->b, 2));
+			printf ("   " VEC4F_FMT "\n", MAT4_ROW(test->b, 3));
+			printf ("r: " VEC4F_FMT "\n", MAT4_ROW(result, 0));
+			printf ("   " VEC4F_FMT "\n", MAT4_ROW(result, 1));
+			printf ("   " VEC4F_FMT "\n", MAT4_ROW(result, 2));
+			printf ("   " VEC4F_FMT "\n", MAT4_ROW(result, 3));
+			printf ("t: " VEC4I_FMT "\n", MAT4_ROW(res, 0));
+			printf ("   " VEC4I_FMT "\n", MAT4_ROW(res, 1));
+			printf ("   " VEC4I_FMT "\n", MAT4_ROW(res, 2));
+			printf ("   " VEC4I_FMT "\n", MAT4_ROW(res, 3));
+			printf ("E: " VEC4F_FMT "\n", MAT4_ROW(expect, 0));
+			printf ("   " VEC4F_FMT "\n", MAT4_ROW(expect, 1));
+			printf ("   " VEC4F_FMT "\n", MAT4_ROW(expect, 2));
+			printf ("   " VEC4F_FMT "\n", MAT4_ROW(expect, 3));
+			printf ("e: " VEC4F_FMT "\n", MAT4_ROW(test->expect, 0));
+			printf ("   " VEC4F_FMT "\n", MAT4_ROW(test->expect, 1));
+			printf ("   " VEC4F_FMT "\n", MAT4_ROW(test->expect, 2));
+			printf ("   " VEC4F_FMT "\n", MAT4_ROW(test->expect, 3));
+			printf ("u: " VEC4F_FMT "\n", MAT4_ROW(test->ulp_errors, 0));
+			printf ("   " VEC4F_FMT "\n", MAT4_ROW(test->ulp_errors, 1));
+			printf ("   " VEC4F_FMT "\n", MAT4_ROW(test->ulp_errors, 2));
+			printf ("   " VEC4F_FMT "\n", MAT4_ROW(test->ulp_errors, 3));
+		}
+	}
+	return ret;
+}
+
+static int
+run_mv4f_tests (void)
+{
+	int         ret = 0;
+
+	for (size_t i = 0; i < num_mv4f_tests; i++) {
+		__auto_type test = &mv4f_tests[i];
+		vec4f_t     result = test->op (test->a, test->b);
+		vec4f_t     expect = test->expect + test->ulp_errors;
+		vec4i_t     res = result != expect;
+
+		if (res[0] || res[1] || res[2] || res[3]) {
+			ret |= 1;
+			printf ("\nrun_mat4f_tests %zd\n", i);
+			printf ("a: " VEC4F_FMT "\n", MAT4_ROW(test->a, 0));
+			printf ("   " VEC4F_FMT "\n", MAT4_ROW(test->a, 1));
+			printf ("   " VEC4F_FMT "\n", MAT4_ROW(test->a, 2));
+			printf ("   " VEC4F_FMT "\n", MAT4_ROW(test->a, 3));
+			printf ("b: " VEC4F_FMT "\n", VEC4_EXP(test->b));
+			printf ("r: " VEC4F_FMT "\n", VEC4_EXP(result));
+			printf ("t: " VEC4I_FMT "\n", VEC4_EXP(res));
+			printf ("E: " VEC4F_FMT "\n", VEC4_EXP(expect));
+			printf ("e: " VEC4F_FMT "\n", VEC4_EXP(test->expect));
+			printf ("u: " VEC4F_FMT "\n", VEC4_EXP(test->ulp_errors));
+		}
+	}
+	return ret;
+}
+
 int
 main (void)
 {
 	int         ret = 0;
 	ret |= run_vec4d_tests ();
 	ret |= run_vec4f_tests ();
+	ret |= run_mat4f_tests ();
+	ret |= run_mv4f_tests ();
 	return ret;
 }