From 6f484ee757f75f6275811d578c68b100267d3e3f Mon Sep 17 00:00:00 2001 From: Bill Currie Date: Sat, 18 Aug 2012 16:29:57 +0900 Subject: [PATCH] Add support and tests for 3x3 matrices. --- include/QF/mathlib.h | 91 ++++++++- include/QF/qtypes.h | 1 + libs/util/mathlib.c | 177 +++++++++++++++-- libs/util/test/Makefile.am | 13 +- libs/util/test/test-mat3.c | 213 +++++++++++++++++++++ libs/util/test/{test-mat.c => test-mat4.c} | 0 6 files changed, 473 insertions(+), 22 deletions(-) create mode 100644 libs/util/test/test-mat3.c rename libs/util/test/{test-mat.c => test-mat4.c} (100%) diff --git a/include/QF/mathlib.h b/include/QF/mathlib.h index bdc2b958d..0c45b2e7b 100644 --- a/include/QF/mathlib.h +++ b/include/QF/mathlib.h @@ -466,6 +466,75 @@ extern const vec_t *const quat_origin; } while (0) #define DualQuatExpand(dq) QuatExpand ((dq).q0.q), QuatExpand ((dq).qe.q) +#define Mat3Copy(a, b) \ + do { \ + VectorCopy ((a) + 0, (b) + 0); \ + VectorCopy ((a) + 3, (b) + 3); \ + VectorCopy ((a) + 6, (b) + 6); \ + } while (0) +#define Mat3Add(a, b, c) \ + do { \ + VectorAdd ((a) + 0, (b) + 0, (c) + 0); \ + VectorAdd ((a) + 3, (b) + 3, (c) + 3); \ + VectorAdd ((a) + 6, (b) + 6, (c) + 6); \ + } while (0) +#define Mat3Subtract(a, b, c) \ + do { \ + VectorSubtract ((a) + 0, (b) + 0, (c) + 0); \ + VectorSubtract ((a) + 3, (b) + 3, (c) + 3); \ + VectorSubtract ((a) + 6, (b) + 6, (c) + 6); \ + } while (0) +#define Mat3Scale(a, b, c) \ + do { \ + VectorScale ((a) + 0, (b), (c) + 0); \ + VectorScale ((a) + 3, (b), (c) + 3); \ + VectorScale ((a) + 6, (b), (c) + 6); \ + } while (0) +#define Mat3CompMult(a, b, c) \ + do { \ + VectorCompMult ((a) + 0, (b) + 0, (c) + 0); \ + VectorCompMult ((a) + 3, (b) + 3, (c) + 3); \ + VectorCompMult ((a) + 6, (b) + 6, (c) + 6); \ + } while (0) +#define Mat3Zero(a) \ + memset ((a), 0, 9 * sizeof (a)[0]) +#define Mat3Identity(a) \ + do { \ + Mat3Zero (a); \ + (a)[8] = (a)[4] = (a)[0] = 1; \ + } while (0) +#define Mat3Expand(a) \ + VectorExpand ((a) + 0), \ + VectorExpand ((a) + 3), \ + VectorExpand ((a) + 6) +#define Mat3Blend(m1,m2,b,m) \ + do { \ + VectorBlend ((m1) + 0, (m2) + 0, (b), (m) + 0); \ + VectorBlend ((m1) + 3, (m2) + 3, (b), (m) + 3); \ + VectorBlend ((m1) + 6, (m2) + 6, (b), (m) + 6); \ + } while (0) +#define Mat3MultAdd(a,s,b,c) \ + do { \ + VectorMultAdd ((a) + 0, s, (b) + 0, (c) + 0); \ + VectorMultAdd ((a) + 3, s, (b) + 3, (c) + 3); \ + VectorMultAdd ((a) + 6, s, (b) + 6, (c) + 6); \ + } while (0) + +#define Mat3toMat4(a, b) \ + do { \ + VectorCopy ((a) + 0, (b) + 0); (b)[3] = 0; \ + VectorCopy ((a) + 3, (b) + 4); (b)[7] = 0; \ + VectorCopy ((a) + 6, (b) + 8); (b)[11] = 0; \ + QuatZero ((b) + 12); \ + } while (0) +#define Mat4toMat3(a, b) \ + do { \ + VectorCopy ((a) + 0, (b) + 0); \ + VectorCopy ((a) + 4, (b) + 3); \ + VectorCopy ((a) + 8, (b) + 6); \ + } while (0) +#define Mat3Trace(a) ((a)[0] + (a)[4] + (a)[8]) + #define Mat4Copy(a, b) \ do { \ QuatCopy ((a) + 0, (b) + 0); \ @@ -527,6 +596,7 @@ extern const vec_t *const quat_origin; QuatMultAdd ((a) + 8, s, (b) + 8, (c) + 8); \ QuatMultAdd ((a) + 12, s, (b) + 12, (c) + 12); \ } while (0) +#define Mat4Trace(a) ((a)[0] + (a)[5] + (a)[10] + (a)[15]) #define qfrandom(MAX) ((float) MAX * (rand() * (1.0 / (RAND_MAX + 1.0)))) @@ -626,6 +696,25 @@ void QuatInverse (const quat_t in, quat_t out); void QuatExp (const quat_t a, quat_t b); void QuatToMatrix (const quat_t q, vec_t *m, int homogenous, int vertical); +void Mat3Init (const quat_t rot, const vec3_t scale, mat3_t mat); +void Mat3Transpose (const mat3_t a, mat3_t b); +vec_t Mat3Determinant (const mat3_t m); +int Mat3Inverse (const mat3_t a, mat3_t b); +void Mat3Mult (const mat3_t a, const mat3_t b, mat3_t c); +void Mat3MultVec (const mat3_t a, const vec3_t b, vec3_t c); +void Mat3SymEigen (const mat3_t m, vec3_t e); +/** Decompose a 3x3 column major matrix into its component transformations. + + This gives the matrix's rotation as a quaternion, shear (XY, XZ, YZ), + and scale. Using the following sequence will give the same result as + multiplying \a v by \a mat. + + QuatMultVec (rot, v, v); + VectorShear (shear, v, v); + VectorCompMult (scale, v, v); +*/ +int Mat3Decompose (const mat4_t mat, quat_t rot, vec3_t shear, vec3_t scale); + void Mat4Init (const quat_t rot, const vec3_t scale, const vec3_t trans, mat4_t mat); void Mat4Transpose (const mat4_t a, mat4_t b); @@ -633,7 +722,7 @@ int Mat4Inverse (const mat4_t a, mat4_t b); void Mat4Mult (const mat4_t a, const mat4_t b, mat4_t c); void Mat4MultVec (const mat4_t a, const vec3_t b, vec3_t c); void Mat4as3MultVec (const mat4_t a, const vec3_t b, vec3_t c); -/** Decompose a column major matrix into its component transformations. +/** Decompose a 4x4 column major matrix into its component transformations. This gives the matrix's rotation as a quaternion, shear (XY, XZ, YZ), scale, and translation. Using the following sequence will give the diff --git a/include/QF/qtypes.h b/include/QF/qtypes.h index 4cec24f88..ada2fa931 100644 --- a/include/QF/qtypes.h +++ b/include/QF/qtypes.h @@ -93,6 +93,7 @@ typedef int fixed4_t; typedef int fixed8_t; typedef int fixed16_t; +typedef float mat3_t[3 * 3]; typedef float mat4_t[4 * 4]; #define SIDE_FRONT 0 diff --git a/libs/util/mathlib.c b/libs/util/mathlib.c index d5876b268..728f41495 100644 --- a/libs/util/mathlib.c +++ b/libs/util/mathlib.c @@ -798,6 +798,150 @@ Invert24To16 (fixed16_t val) } #endif +void +Mat3Init (const quat_t rot, const vec3_t scale, mat3_t mat) +{ + QuatToMatrix (rot, mat, 0, 1); + VectorScale (mat + 0, scale[0], mat + 0); + VectorScale (mat + 3, scale[1], mat + 3); + VectorScale (mat + 6, scale[2], mat + 6); +} + +void +Mat3Transpose (const mat3_t a, mat3_t b) +{ + vec_t t; + int i, j; + + for (i = 0; i < 2; i++) { + b[i * 3 + i] = a[i * 3 + i]; // in case b != a + for (j = i + 1; j < 3; j++) { + t = a[i * 3 + j]; // in case b == a + b[i * 3 + j] = a[j * 3 + i]; + b[j * 3 + i] = t; + } + } + b[i * 3 + i] = a[i * 3 + i]; // in case b != a +} + +vec_t +Mat3Determinant (const mat3_t m) +{ + vec3_t t; + CrossProduct (m + 3, m + 6, t); + return DotProduct (m + 0, t); +} + +typedef vec_t mat2_t[2 * 2]; + +static void +Mat3Sub2 (const mat3_t m3, mat2_t m2, int i, int j) +{ + int si, sj, di, dj; + + for (di = 0; di < 2; di++) { + for (dj = 0; dj < 2; dj++) { + si = di + ((di >= i) ? 1 : 0); + sj = dj + ((dj >= j) ? 1 : 0); + m2[di * 2 + dj] = m3[si * 3 + sj]; + } + } +} + +static vec_t +Mat2Det (const mat2_t m) +{ + return m[0] * m[3] - m[1] * m[2]; +} + +int +Mat3Inverse (const mat3_t a, mat3_t b) +{ + mat3_t tb; + mat2_t m2; + vec_t *m = b; + int i, j; + vec_t det; + vec_t sign[2] = { 1, -1}; + + det = Mat3Determinant (a); + if (det * det < 1e-6) { + Mat3Identity (b); + return 0; + } + if (b == a) + m = tb; + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + Mat3Sub2 (a, m2, i, j); + m[j * 3 + i] = sign[(i + j) & 1] * Mat2Det (m2) / det; + } + } + if (m != b) + Mat3Copy (m, b); + return 1; +} + +void Mat3Mult (const mat3_t a, const mat3_t b, mat3_t c) +{ + mat3_t ta, tb; // in case c == b or c == a + int i, j, k; + + Mat3Transpose (a, ta); // transpose so we can use dot + Mat3Copy (b, tb); + + k = 0; + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + c[k++] = DotProduct (ta + 3 * j, tb + 3 * i); + } + } +} + +void Mat3MultVec (const mat3_t a, const vec3_t b, vec3_t c) +{ + int i; + vec3_t tb; + + VectorCopy (b, tb); + for (i = 0; i < 3; i++) + c[i] = a[i + 0] * tb[0] + a[i + 3] * b[1] + a[i + 6] * b[2]; +} + +#define sqr(x) ((x) * (x)) +void Mat3SymEigen (const mat3_t m, vec3_t e) +{ + vec_t p, q, r; + vec_t phi; + mat3_t B; + + p = sqr (m[1]) + sqr (m[2]) + sqr (m[5]); + if (p < 1e-6) { + e[0] = m[0]; + e[1] = m[4]; + e[2] = m[8]; + return; + } + q = Mat3Trace (m) / 3; + p = sqr (m[0] - q) + sqr (m[4] - q) + sqr (m[8] - q) + 2 * p; + p = sqrt (p); + Mat3Zero (B); + B[0] = B[4] = B[8] = q; + Mat3Subtract (m, B, B); + Mat3Scale (B, 1.0 / p, B); + r = Mat3Determinant (B) / 2; + if (r >= 1) + phi = 0; + else if (r <= -1) + phi = M_PI / 3; + else + phi = acos (r) / 3; + + e[0] = q + 2 * p * cos (phi); + e[2] = q + 2 * p * cos (phi + M_PI * 2 / 3); + e[1] = 3 * q - e[0] - e[2]; +} + void Mat4Init (const quat_t rot, const vec3_t scale, const vec3_t trans, mat4_t mat) { @@ -825,16 +969,6 @@ Mat4Transpose (const mat4_t a, mat4_t b) b[i * 4 + i] = a[i * 4 + i]; // in case b != a } -typedef vec_t mat3_t[3 * 3]; - -static vec_t -Mat3Det (const mat3_t m) -{ - vec3_t t; - CrossProduct (m + 3, m + 6, t); - return DotProduct (m, t); -} - static void Mat4Sub3 (const mat4_t m4, mat3_t m3, int i, int j) { @@ -858,7 +992,7 @@ Mat4Det (const mat4_t m) for (i = 0; i < 4; i++, s = -s) { Mat4Sub3 (m, t, 0, i); - det = Mat3Det (t); + det = Mat3Determinant (t); res += m[i] * det * s; } return res; @@ -884,7 +1018,7 @@ Mat4Inverse (const mat4_t a, mat4_t b) for (i = 0; i < 4; i++) { for (j = 0; j < 4; j++) { Mat4Sub3 (a, m3, i, j); - m[j * 4 + i] = sign[(i + j) & 1] * Mat3Det (m3) / det; + m[j * 4 + i] = sign[(i + j) & 1] * Mat3Determinant (m3) / det; } } if (m != b) @@ -932,8 +1066,7 @@ Mat4as3MultVec (const mat4_t a, const vec3_t b, vec3_t c) } int -Mat4Decompose (const mat4_t mat, quat_t rot, vec3_t shear, vec3_t scale, - vec3_t trans) +Mat3Decompose (const mat3_t mat, quat_t rot, vec3_t shear, vec3_t scale) { vec3_t row[3], shr, scl; vec_t l, t; @@ -941,7 +1074,7 @@ Mat4Decompose (const mat4_t mat, quat_t rot, vec3_t shear, vec3_t scale, for (i = 0; i < 3; i++) for (j = 0; j < 3; j++) - row[j][i] = mat[i * 4 + j]; + row[j][i] = mat[i * 3 + j]; l = DotProduct (row[0], row[0]); if (l < 1e-5) @@ -973,8 +1106,6 @@ Mat4Decompose (const mat4_t mat, quat_t rot, vec3_t shear, vec3_t scale, VectorCopy (scl, scale); if (shear) VectorCopy (shr, shear); - if (trans) - VectorCopy (mat + 12, trans); if (!rot) return 1; @@ -1008,3 +1139,15 @@ Mat4Decompose (const mat4_t mat, quat_t rot, vec3_t shear, vec3_t scale, } return 1; } + +int +Mat4Decompose (const mat4_t mat, quat_t rot, vec3_t shear, vec3_t scale, + vec3_t trans) +{ + mat3_t m3; + + if (trans) + VectorCopy (mat + 12, trans); + Mat4toMat3 (mat, m3); + return Mat3Decompose (m3, rot, shear, scale); +} diff --git a/libs/util/test/Makefile.am b/libs/util/test/Makefile.am index eb477eb84..e858ed6ef 100644 --- a/libs/util/test/Makefile.am +++ b/libs/util/test/Makefile.am @@ -2,7 +2,8 @@ AUTOMAKE_OPTIONS= foreign INCLUDES= -I$(top_srcdir)/include -check_PROGRAMS=test-dq test-half test-mat test-qfs test-quat test-vrect +check_PROGRAMS= \ + test-dq test-half test-mat3 test-mat4 test-qfs test-quat test-vrect test_dq_SOURCES=test-dq.c test_dq_LDADD=$(top_builddir)/libs/util/libQFutil.la @@ -12,9 +13,13 @@ test_half_SOURCES=test-half.c test_half_LDADD=$(top_builddir)/libs/util/libQFutil.la test_half_DEPENDENCIES=$(top_builddir)/libs/util/libQFutil.la -test_mat_SOURCES=test-mat.c -test_mat_LDADD=$(top_builddir)/libs/util/libQFutil.la -test_mat_DEPENDENCIES=$(top_builddir)/libs/util/libQFutil.la +test_mat3_SOURCES=test-mat3.c +test_mat3_LDADD=$(top_builddir)/libs/util/libQFutil.la +test_mat3_DEPENDENCIES=$(top_builddir)/libs/util/libQFutil.la + +test_mat4_SOURCES=test-mat4.c +test_mat4_LDADD=$(top_builddir)/libs/util/libQFutil.la +test_mat4_DEPENDENCIES=$(top_builddir)/libs/util/libQFutil.la test_qfs_SOURCES=test-qfs.c test_qfs_LDADD=$(top_builddir)/libs/util/libQFutil.la diff --git a/libs/util/test/test-mat3.c b/libs/util/test/test-mat3.c new file mode 100644 index 000000000..385200750 --- /dev/null +++ b/libs/util/test/test-mat3.c @@ -0,0 +1,213 @@ +#ifdef HAVE_CONFIG_H +# include "config.h" +#endif + +#ifdef HAVE_STRING_H +# include +#endif +#ifdef HAVE_STRINGS_H +# include +#endif + +#include "QF/mathlib.h" + +//PITCH YAW ROLL +static vec3_t test_angles[] = { + { 0, 0, 0}, + {45, 0, 0}, + { 0, 45, 0}, + { 0, 0, 45}, + {45, 45, 0}, + { 0, 45, 45}, + {45, 0, 45}, + {45, 45, 45}, + {0, 180, 180}, + {180, 0, 180}, + {180, 180, 0}, +}; +#define num_angle_tests \ + (sizeof (test_angles) / sizeof (test_angles[0])) + +static vec3_t test_scales[] = { + { 1, 1, 1}, + { 2, 1, 1}, + { 1, 2, 1}, + { 1, 1, 2}, + { 2, 2, 1}, + { 1, 2, 2}, + { 2, 1, 2}, + { 2, 2, 2}, + { 1, 2, 3}, + { 1, 3, 2}, + { 2, 1, 3}, + { 2, 3, 1}, + { 3, 1, 2}, + { 3, 2, 1}, +}; +#define num_scale_tests \ + (sizeof (test_scales) / sizeof (test_scales[0])) + +// return true if a and b are close enough (yay, floats) +static int +compare (vec_t a, vec_t b) +{ + vec_t diff = a - b; + return diff * diff < 0.001; +} + +static int +test_angle (const vec3_t angles) +{ + int i; + quat_t rotation, r; + vec3_t scale, shear; + mat3_t mat; + + AngleQuat (angles, rotation); + QuatToMatrix (rotation, mat, 0, 1); + Mat3Decompose (mat, r, shear, scale); + for (i = 0; i < 4; i++) + if (!compare (rotation[i], r[i])) + goto negate; + return 1; +negate: + // Mat3Decompose always sets the rotation quaternion's scalar to +ve + // but AngleQuat might produce a -ve scalar. + QuatNegate (r, r); + for (i = 0; i < 4; i++) + if (!compare (rotation[i], r[i])) + goto fail; + + return 1; +fail: + printf ("\n\n(%g %g %g)\n", VectorExpand (angles)); + printf (" [%g %g %g %g]\n", QuatExpand (rotation)); + printf (" [%g %g %g %g] [%g %g %g] [%g %g %g]\n", + QuatExpand (r), VectorExpand (scale), VectorExpand (shear)); + return 0; +} + +static int +test_transform (const vec3_t angles, const vec3_t scale) +{ + int i; + const vec3_t v = {4,5,6}; + vec3_t x, y; + quat_t rotation; + mat3_t mat; + + VectorCopy (v, x); + AngleQuat (angles, rotation); + VectorCompMult (scale, x, x); + QuatMultVec (rotation, x, x); + + Mat3Init (rotation, scale, mat); + Mat3MultVec (mat, v, y); + + for (i = 0; i < 3; i++) + if (!compare (x[i], y[i])) + goto fail; + + return 1; +fail: + printf ("\n\n(%g %g %g) (%g %g %g)\n", VectorExpand (angles), + VectorExpand (scale)); + printf (" (%g %g %g)\n", VectorExpand (x)); + printf (" (%g %g %g)\n", VectorExpand (y)); + return 0; +} + +static int +test_transform2 (const vec3_t angles, const vec3_t scale) +{ + int i; + const vec3_t v = {4,5,6}; + vec3_t x, y; + quat_t rotation; + mat3_t mat; + vec3_t rot, sc, sh; + + VectorCopy (v, x); + AngleQuat (angles, rotation); + VectorCompMult (scale, x, x); + QuatMultVec (rotation, x, x); + + Mat3Init (rotation, scale, mat); + Mat3Decompose (mat, rot, sh, sc); + + VectorCopy (v, y); + QuatMultVec (rot, y, y); + VectorShear (sh, y, y); + VectorCompMult (sc, y, y);//scale + + for (i = 0; i < 3; i++) + if (!compare (x[i], y[i])) + goto fail; + + return 1; +fail: + printf ("\n\n(%g %g %g) (%g %g %g) (%g %g %g)\n", + VectorExpand (angles), VectorExpand (scale), VectorExpand (v)); + printf (" (%g %g %g)\n", VectorExpand (x)); + printf (" (%g %g %g)\n", VectorExpand (y)); + return 0; +} + +static int +test_inverse (const vec3_t angles, const vec3_t scale) +{ + int i; + quat_t rotation; + mat3_t mat, inv, I, res; + + AngleQuat (angles, rotation); + Mat3Init (rotation, scale, mat); + + Mat3Identity (I); + Mat3Inverse (mat, inv); + Mat3Mult (mat, inv, res); + + for (i = 0; i < 3 * 3; i++) + if (!compare (I[i], res[i])) + goto fail; + + return 1; +fail: + printf ("\n\n(%g %g %g) (%g %g %g)\n", + VectorExpand (angles), VectorExpand (scale)); + printf (" [%g %g %g]\n [%g %g %g]\n [%g %g %g]\n\n", Mat3Expand (mat)); + printf (" [%g %g %g]\n [%g %g %g]\n [%g %g %g]\n\n", Mat3Expand (inv)); + printf (" [%g %g %g]\n [%g %g %g]\n [%g %g %g]\n\n", Mat3Expand (res)); + return 0; +} + +int +main (int argc, const char **argv) +{ + int res = 0; + size_t i, j; + + for (i = 0; i < num_angle_tests; i ++) { + if (!test_angle (test_angles[i])) + res = 1; + } + for (i = 0; i < num_angle_tests; i ++) { + for (j = 0; j < num_scale_tests; j ++) { + if (!test_transform (test_angles[i], test_scales[j])) + res = 1; + } + } + for (i = 0; i < num_angle_tests; i ++) { + for (j = 0; j < num_scale_tests; j ++) { + if (!test_transform2 (test_angles[i], test_scales[j])) + res = 1; + } + } + for (i = 0; i < num_angle_tests; i ++) { + for (j = 0; j < num_scale_tests; j ++) { + if (!test_inverse (test_angles[i], test_scales[j])) + res = 1; + } + } + return res; +} diff --git a/libs/util/test/test-mat.c b/libs/util/test/test-mat4.c similarity index 100% rename from libs/util/test/test-mat.c rename to libs/util/test/test-mat4.c