quakeforge/libs/util/test/test-mat3.c

219 lines
4.5 KiB
C
Raw Normal View History

#ifdef HAVE_CONFIG_H
# include "config.h"
#endif
2012-05-10 05:42:40 +00:00
#ifdef HAVE_STRING_H
# include <string.h>
#endif
#ifdef HAVE_STRINGS_H
# include <strings.h>
#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]))
2012-05-09 10:07:27 +00:00
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]))
2012-05-09 10:07:27 +00:00
// 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 ("\ntest_angle\n");
printf ("(%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;
}
2012-05-09 10:07:27 +00:00
static int
test_transform (const vec3_t angles, const vec3_t scale)
2012-05-09 10:07:27 +00:00
{
int i;
const vec3_t v = {4,5,6};
vec3_t x, y;
quat_t rotation;
mat3_t mat;
2012-05-09 10:07:27 +00:00
VectorCopy (v, x);
AngleQuat (angles, rotation);
VectorCompMult (scale, x, x);
QuatMultVec (rotation, x, x);
Mat3Init (rotation, scale, mat);
Mat3MultVec (mat, v, y);
2012-05-09 10:07:27 +00:00
for (i = 0; i < 3; i++)
if (!compare (x[i], y[i]))
goto fail;
2012-05-09 10:07:27 +00:00
return 1;
fail:
printf ("\ntest_transform\n");
printf ("(%g %g %g) (%g %g %g)\n", VectorExpand (angles),
VectorExpand (scale));
2012-05-09 10:07:27 +00:00
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;
quat_t rot;
vec3_t 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 ("\ntest_transform2\n");
printf ("(%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;
}
2012-05-10 05:42:40 +00:00
static int
test_inverse (const vec3_t angles, const vec3_t scale)
2012-05-10 05:42:40 +00:00
{
int i;
quat_t rotation;
mat3_t mat, inv, I, res;
2012-05-10 05:42:40 +00:00
AngleQuat (angles, rotation);
Mat3Init (rotation, scale, mat);
2012-05-10 05:42:40 +00:00
Mat3Identity (I);
Mat3Inverse (mat, inv);
Mat3Mult (mat, inv, res);
2012-05-10 05:42:40 +00:00
for (i = 0; i < 3 * 3; i++)
2012-05-10 05:42:40 +00:00
if (!compare (I[i], res[i]))
goto fail;
return 1;
fail:
printf ("\ntest_inverse\n");
printf ("(%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));
2012-05-10 05:42:40 +00:00
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;
}
2012-05-09 10:07:27 +00:00
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;
2012-05-09 10:07:27 +00:00
}
}
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;
}
}
2012-05-10 05:42:40 +00:00
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;
2012-05-10 05:42:40 +00:00
}
}
return res;
}