mirror of
https://git.code.sf.net/p/quake/quakeforge
synced 2024-12-11 21:31:30 +00:00
5fb28d7c38
And add a unary op macro. Having VectorCompOp makes it easy to write macros that work for multiple data widths, which is why it and its users now use (dst, ...) instead of (..., dst) as in the past. I'll sort out the other macros later now that I know the compiler handily gives messages about the switched order (uninitialized vars etc).
218 lines
4.5 KiB
C
218 lines
4.5 KiB
C
#ifdef HAVE_CONFIG_H
|
|
# include "config.h"
|
|
#endif
|
|
|
|
#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]))
|
|
|
|
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 ("\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;
|
|
}
|
|
|
|
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 (x, scale, 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 ("\ntest_transform\n");
|
|
printf ("(%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;
|
|
quat_t rot;
|
|
vec3_t sc, sh;
|
|
|
|
VectorCopy (v, x);
|
|
AngleQuat (angles, rotation);
|
|
VectorCompMult (x, scale, 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 (y, sc, 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;
|
|
}
|
|
|
|
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 ("\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));
|
|
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;
|
|
}
|