mirror of
https://git.code.sf.net/p/quake/quakeforge
synced 2024-11-10 15:22:04 +00:00
[util] Create simd quaternion to matrix function
This seems to be pretty close to as fast as it gets (might be able to do better with some shuffles of the negation constants instead of loading separate constants).
This commit is contained in:
Bill Currie
parent
0366b72d4a
commit
4a97bc3ba5
3 changed files with 163 additions and 1 deletions
|
|
@ -38,6 +38,7 @@ 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);
|
||||
GNU89INLINE inline void mat4fquat (mat4f_t m, vec4f_t q);
|
||||
|
||||
#ifndef IMPLEMENT_MAT4F_Funcs
|
||||
GNU89INLINE inline
|
||||
|
|
@ -120,4 +121,53 @@ mat4fidentity (mat4f_t m)
|
|||
m[3] = (vec4f_t) { 0, 0, 0, 1 };
|
||||
}
|
||||
|
||||
#ifndef IMPLEMENT_MAT4F_Funcs
|
||||
GNU89INLINE inline
|
||||
#else
|
||||
VISIBLE
|
||||
#endif
|
||||
void
|
||||
mat4fquat (mat4f_t m, vec4f_t q)
|
||||
{
|
||||
vec4f_t xq = q[0] * q;
|
||||
vec4f_t yq = q[1] * q;
|
||||
vec4f_t zq = q[2] * q;
|
||||
vec4f_t wq = q[3] * q;
|
||||
|
||||
static const vec4i_t shuff103 = { 1, 0, 3, 2 };
|
||||
static const vec4i_t shuff230 = { 2, 3, 0, 1 };
|
||||
static const vec4i_t shuff321 = { 3, 2, 1, 0 };
|
||||
#define p (0)
|
||||
#define m (1u << 31)
|
||||
static const vec4i_t mpm = { m, p, m, 0 };
|
||||
static const vec4i_t pmm = { p, m, m, 0 };
|
||||
static const vec4i_t mmp = { m, m, p, 0 };
|
||||
static const vec4i_t mask = { ~0u, ~0u, ~0u, 0 };
|
||||
#undef p
|
||||
#undef m
|
||||
{
|
||||
vec4f_t a = xq;
|
||||
vec4f_t b = _mm_xor_ps (__builtin_shuffle (yq, shuff103), (__m128) mpm);
|
||||
vec4f_t c = _mm_xor_ps (__builtin_shuffle (zq, shuff230), (__m128) pmm);
|
||||
vec4f_t d = _mm_xor_ps (__builtin_shuffle (wq, shuff321), (__m128) mmp);
|
||||
|
||||
m[0] = _mm_and_ps (a + b - c - d, (__m128) mask);
|
||||
}
|
||||
{
|
||||
vec4f_t a = _mm_xor_ps (__builtin_shuffle (xq, shuff103), (__m128) mpm);
|
||||
vec4f_t b = yq;
|
||||
vec4f_t c = _mm_xor_ps (__builtin_shuffle (zq, shuff321), (__m128) mmp);
|
||||
vec4f_t d = _mm_xor_ps (__builtin_shuffle (wq, shuff230), (__m128) pmm);
|
||||
m[1] = _mm_and_ps (b + c - a - d, (__m128) mask);
|
||||
}
|
||||
{
|
||||
vec4f_t a = _mm_xor_ps (__builtin_shuffle (xq, shuff230), (__m128) pmm);
|
||||
vec4f_t b = _mm_xor_ps (__builtin_shuffle (yq, shuff321), (__m128) mmp);
|
||||
vec4f_t c = zq;
|
||||
vec4f_t d = _mm_xor_ps (__builtin_shuffle (wq, shuff103), (__m128) mpm);
|
||||
m[2] = _mm_and_ps (a - b + c - d, (__m128) mask);
|
||||
}
|
||||
m[3] = (vec4f_t) { 0, 0, 0, 1 };
|
||||
}
|
||||
|
||||
#endif//__QF_simd_mat4f_h
|
||||
|
|
|
|||
|
|
@ -362,6 +362,31 @@ fail:
|
|||
return 0;
|
||||
}
|
||||
|
||||
static int
|
||||
test_quat_mat2(const quat_t q, const quat_t expect)
|
||||
{
|
||||
int i;
|
||||
vec_t m[9];
|
||||
|
||||
QuatToMatrix (q, m, 0, 1);
|
||||
Mat3Transpose (m, m);
|
||||
|
||||
for (i = 0; i < 9; i++)
|
||||
if (m[i] != expect[i]) // exact tests here
|
||||
goto fail;
|
||||
return 1;
|
||||
fail:
|
||||
printf ("\ntest_quat_mat\n");
|
||||
printf ("%11.9g %11.9g %11.9g %11.9g\n", QuatExpand (q));
|
||||
printf ("%11.9g %11.9g %11.9g %11.9g %11.9g %11.9g\n",
|
||||
VectorExpand (m + 0), VectorExpand (expect + 0));
|
||||
printf ("%11.9g %11.9g %11.9g %11.9g %11.9g %11.9g\n",
|
||||
VectorExpand (m + 3), VectorExpand (expect + 3));
|
||||
printf ("%11.9g %11.9g %11.9g %11.9g %11.9g %11.9g\n",
|
||||
VectorExpand (m + 6), VectorExpand (expect + 6));
|
||||
return 0;
|
||||
}
|
||||
|
||||
int
|
||||
main (int argc, const char **argv)
|
||||
{
|
||||
|
|
@ -407,5 +432,12 @@ main (int argc, const char **argv)
|
|||
res = 1;
|
||||
}
|
||||
|
||||
for (i = 0; i < num_quat_mat_tests; i ++) {
|
||||
vec_t *q = quat_mat_tests[i].q;
|
||||
vec_t *expect = quat_mat_tests[i].expect;
|
||||
if (!test_quat_mat2 (q, expect))
|
||||
res = 1;
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -7,6 +7,7 @@
|
|||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include "QF/mathlib.h"
|
||||
#include "QF/simd/vec4d.h"
|
||||
#include "QF/simd/vec4f.h"
|
||||
#include "QF/simd/mat4f.h"
|
||||
|
|
@ -76,6 +77,13 @@ typedef struct {
|
|||
vec4f_t ulp_errors;
|
||||
} mv4f_test_t;
|
||||
|
||||
typedef struct {
|
||||
void (*op) (mat4f_t m, vec4f_t q);
|
||||
vec4f_t q;
|
||||
mat4f_t expect;
|
||||
mat4f_t ulp_errors;
|
||||
} mq4f_test_t;
|
||||
|
||||
static vec4d_t tvtruncd (vec4d_t v, vec4d_t ignore)
|
||||
{
|
||||
return vtruncd (v);
|
||||
|
|
@ -367,6 +375,20 @@ static mv4f_test_t mv4f_tests[] = {
|
|||
};
|
||||
#define num_mv4f_tests (sizeof (mv4f_tests) / (sizeof (mv4f_tests[0])))
|
||||
|
||||
// expect filled in using non-simd QuatToMatrix (has its own tests)
|
||||
static mq4f_test_t mq4f_tests[] = {
|
||||
{ mat4fquat, { 0, 0, 0, 1 } },
|
||||
{ mat4fquat, { 0.5, 0.5, 0.5, 0.5 } },
|
||||
{ mat4fquat, { 0.5, 0.5, -0.5, 0.5 } },
|
||||
{ mat4fquat, { 0.5, -0.5, 0.5, 0.5 } },
|
||||
{ mat4fquat, { 0.5, -0.5, -0.5, 0.5 } },
|
||||
{ mat4fquat, { -0.5, 0.5, 0.5, 0.5 } },
|
||||
{ mat4fquat, { -0.5, 0.5, -0.5, 0.5 } },
|
||||
{ mat4fquat, { -0.5, -0.5, 0.5, 0.5 } },
|
||||
{ mat4fquat, { -0.5, -0.5, -0.5, 0.5 } },
|
||||
};
|
||||
#define num_mq4f_tests (sizeof (mq4f_tests) / (sizeof (mq4f_tests[0])))
|
||||
|
||||
static int
|
||||
run_vec4d_tests (void)
|
||||
{
|
||||
|
|
@ -485,7 +507,7 @@ run_mv4f_tests (void)
|
|||
|
||||
if (res[0] || res[1] || res[2] || res[3]) {
|
||||
ret |= 1;
|
||||
printf ("\nrun_mat4f_tests %zd\n", i);
|
||||
printf ("\nrun_mv4f_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));
|
||||
|
|
@ -501,6 +523,63 @@ run_mv4f_tests (void)
|
|||
return ret;
|
||||
}
|
||||
|
||||
static int
|
||||
run_mq4f_tests (void)
|
||||
{
|
||||
int ret = 0;
|
||||
|
||||
for (size_t i = 0; i < num_mq4f_tests; i++) {
|
||||
__auto_type test = &mq4f_tests[i];
|
||||
quat_t q;
|
||||
vec_t m[16];
|
||||
memcpy (q, &test->q, sizeof (quat_t));
|
||||
QuatToMatrix (q, m, 1, 1);
|
||||
memcpy (&test->expect, m, sizeof (mat4f_t));
|
||||
}
|
||||
for (size_t i = 0; i < num_mq4f_tests; i++) {
|
||||
__auto_type test = &mq4f_tests[i];
|
||||
mat4f_t result;
|
||||
mat4f_t expect;
|
||||
mat4i_t res = {};
|
||||
|
||||
test->op (result, test->q);
|
||||
maddf (expect, test->expect, test->ulp_errors);
|
||||
memcpy (expect, (void *) &test->expect, sizeof (mat4f_t));
|
||||
|
||||
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_mq4f_tests %zd\n", i);
|
||||
printf ("q: " VEC4F_FMT "\n", VEC4_EXP(test->q));
|
||||
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;
|
||||
}
|
||||
|
||||
int
|
||||
main (void)
|
||||
{
|
||||
|
|
@ -509,5 +588,6 @@ main (void)
|
|||
ret |= run_vec4f_tests ();
|
||||
ret |= run_mat4f_tests ();
|
||||
ret |= run_mv4f_tests ();
|
||||
ret |= run_mq4f_tests ();
|
||||
return ret;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in a new issue