diff --git a/include/QF/mathlib.h b/include/QF/mathlib.h index ba75e61a5..3e9af925c 100644 --- a/include/QF/mathlib.h +++ b/include/QF/mathlib.h @@ -587,6 +587,8 @@ 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 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); 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); diff --git a/libs/util/mathlib.c b/libs/util/mathlib.c index b64e3f90d..ec4ca56fd 100644 --- a/libs/util/mathlib.c +++ b/libs/util/mathlib.c @@ -798,6 +798,16 @@ Invert24To16 (fixed16_t val) } #endif +void +Mat4Init (const quat_t rot, const vec3_t scale, const vec3_t trans, mat4_t mat) +{ + QuatToMatrix (rot, mat, 1, 1); + VectorScale (mat + 0, scale[0], mat + 0); + VectorScale (mat + 4, scale[1], mat + 4); + VectorScale (mat + 8, scale[2], mat + 8); + VectorCopy (trans, mat + 12); +} + void Mat4Transpose (const mat4_t a, mat4_t b) { diff --git a/libs/util/test/test-mat.c b/libs/util/test/test-mat.c index 23323d804..ef1329e45 100644 --- a/libs/util/test/test-mat.c +++ b/libs/util/test/test-mat.c @@ -7,20 +7,12 @@ //PITCH YAW ROLL static vec3_t test_angles[] = { { 0, 0, 0}, - { 0, 0, 0}, - {45, 0, 0}, {45, 0, 0}, { 0, 45, 0}, - { 0, 45, 0}, - { 0, 0, 45}, { 0, 0, 45}, {45, 45, 0}, - {45, 45, 0}, - { 0, 45, 45}, { 0, 45, 45}, {45, 0, 45}, - {45, 0, 45}, - {45, 45, 45}, {45, 45, 45}, {0, 180, 180}, {180, 0, 180}, @@ -29,6 +21,39 @@ static vec3_t test_angles[] = { #define num_angle_tests \ (sizeof (test_angles) / sizeof (test_angles[0])) +static vec3_t test_translations[] = { + { 0, 0, 0}, + { 1, 2, 3}, + {-1, 2, 3}, + { 1,-2, 3}, + { 1, 2,-3}, + {-1,-2, 3}, + {-1, 2,-3}, + { 1,-2,-3}, + {-1,-2,-3}, +}; +#define num_translation_tests \ + (sizeof (test_translations) / sizeof (test_translations[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_translation_tests \ + (sizeof (test_translations) / sizeof (test_translations[0])) + // return true if a and b are close enough (yay, floats) static int compare (vec_t a, vec_t b) @@ -70,15 +95,55 @@ fail: return 0; } +static int +test_transform (const vec3_t angles, const vec3_t scale, + const vec3_t translation) +{ + int i; + const vec3_t v = {4,5,6}; + vec3_t x, y; + quat_t rotation; + mat4_t mat; + + VectorCopy (v, x); + AngleQuat (angles, rotation); + VectorCompMult (scale, x, x); + QuatMultVec (rotation, x, x); + VectorAdd (x, translation, x); + + Mat4Init (rotation, scale, translation, mat); + Mat4MultVec (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) (%g %g %g)\n", VectorExpand (angles), + VectorExpand (scale), VectorExpand (translation)); + printf (" (%g %g %g)\n", VectorExpand (x)); + printf (" (%g %g %g)\n", VectorExpand (y)); + return 0; +} + int main (int argc, const char **argv) { int res = 0; - size_t i; + size_t i, j, k; 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_translation_tests; j ++) { + for (k = 0; k < num_translation_tests; k ++) { + if (!test_transform (test_angles[i], test_scales[j], + test_translations[k])) + res = 1; + } + } + } return res; }