133 lines
3.2 KiB
C
133 lines
3.2 KiB
C
#ifdef HAVE_CONFIG_H
|
|
# include "config.h"
|
|
#endif
|
|
|
|
#include "QF/mathlib.h"
|
|
|
|
//PITCH YAW ROLL
|
|
static vec3_t test_transforms[][2] = {
|
|
{{ 0, 0, 0}, { 0, 0, 0}},
|
|
{{ 0, 0, 0}, { 1, 2, 3}},
|
|
{{45, 0, 0}, { 0, 0, 0}},
|
|
{{45, 0, 0}, {-1, 2, 3}},
|
|
{{ 0, 45, 0}, { 0, 0, 0}},
|
|
{{ 0, 45, 0}, { 1,-2, 3}},
|
|
{{ 0, 0, 45}, { 0, 0, 0}},
|
|
{{ 0, 0, 45}, { 1, 2,-3}},
|
|
{{45, 45, 0}, { 0, 0, 0}},
|
|
{{45, 45, 0}, {-1,-2, 3}},
|
|
{{ 0, 45, 45}, { 0, 0, 0}},
|
|
{{ 0, 45, 45}, {-1, 2,-3}},
|
|
{{45, 0, 45}, { 0, 0, 0}},
|
|
{{45, 0, 45}, { 1,-2,-3}},
|
|
{{45, 45, 45}, { 0, 0, 0}},
|
|
{{45, 45, 45}, {-1,-2,-3}},
|
|
};
|
|
#define num_transform_tests \
|
|
(sizeof (test_transforms) / sizeof (test_transforms[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_transform (const vec3_t angles, const vec3_t translation)
|
|
{
|
|
int i;
|
|
const vec3_t v = {4,5,6};
|
|
vec3_t x;
|
|
quat_t rotation;
|
|
DualQuat_t transform, conj;
|
|
DualQuat_t inverse, iconj;
|
|
DualQuat_t vd, xd, ix;
|
|
Dual_t dual;
|
|
|
|
VectorZero (x);
|
|
DualQuatZero (xd);
|
|
DualQuatZero (ix);
|
|
|
|
AngleQuat (angles, rotation);
|
|
DualQuatSetVect (v, vd);
|
|
DualQuatRotTrans (rotation, translation, transform);
|
|
DualQuatConjQE (transform, conj);
|
|
|
|
DualQuatConjQ (transform, inverse);
|
|
DualQuatConjQE (inverse, iconj);
|
|
|
|
DualQuatNorm (vd, dual);
|
|
if (!DualIsUnit (dual)) {
|
|
printf ("dual vector not unit: "
|
|
"[(%g %g %g %g) (%g %g %g %g)] -> [%g %g]\n",
|
|
DualQuatExpand (vd), DualExpand (dual));
|
|
goto fail;
|
|
}
|
|
|
|
DualQuatNorm (transform, dual);
|
|
if (!DualIsUnit (dual)) {
|
|
printf ("dual quat not unit: "
|
|
"[(%g %g %g %g) (%g %g %g %g)] -> [%g %g]\n",
|
|
DualQuatExpand (transform), DualExpand (dual));
|
|
goto fail;
|
|
}
|
|
|
|
QuatMultVec (rotation, v, x);
|
|
VectorAdd (x, translation, x);
|
|
|
|
DualQuatMult (transform, vd, xd);
|
|
DualQuatMult (xd, conj, xd);
|
|
|
|
DualQuatMult (inverse, xd, ix);
|
|
DualQuatMult (ix, iconj, ix);
|
|
|
|
DualQuatNorm (xd, dual);
|
|
if (!DualIsUnit (dual)) {
|
|
printf ("dual result not unit: "
|
|
"[(%g %g %g %g) (%g %g %g %g)] -> [%g %g]\n",
|
|
DualQuatExpand (xd), DualExpand (dual));
|
|
goto fail;
|
|
}
|
|
|
|
DualQuatNorm (ix, dual);
|
|
if (!DualIsUnit (dual)) {
|
|
printf ("dual inverse not unit: "
|
|
"[(%g %g %g %g) (%g %g %g %g)] -> [%g %g]\n",
|
|
DualQuatExpand (ix), DualExpand (dual));
|
|
goto fail;
|
|
}
|
|
|
|
for (i = 0; i < 3; i++)
|
|
if (!compare (xd.qe.sv.v[i], x[i]))
|
|
goto fail;
|
|
for (i = 0; i < 3; i++)
|
|
if (!compare (ix.qe.sv.v[i], v[i]))
|
|
goto fail;
|
|
return 1;
|
|
fail:
|
|
printf ("\n\n(%g %g %g) (%g %g %g)\n",
|
|
VectorExpand (angles), VectorExpand (translation));
|
|
printf (" [(%g %g %g %g) (%g %g %g %g)]\n", DualQuatExpand (transform));
|
|
printf (" [(%g %g %g %g) (%g %g %g %g)]\n", DualQuatExpand (vd));
|
|
printf (" [(%g %g %g %g) (%g %g %g %g)]\n", DualQuatExpand (conj));
|
|
printf (" (%g %g %g)\n", VectorExpand (x));
|
|
printf (" (%g %g %g)\n", VectorExpand (xd.qe.sv.v));
|
|
printf (" (%g %g %g)\n", VectorExpand (ix.qe.sv.v));
|
|
return 0;
|
|
}
|
|
|
|
int
|
|
main (int argc, const char **argv)
|
|
{
|
|
int res = 0;
|
|
size_t i;
|
|
|
|
for (i = 0; i < num_transform_tests; i ++) {
|
|
if (!test_transform (test_transforms[i][0], test_transforms[i][1]))
|
|
res = 1;
|
|
}
|
|
return res;
|
|
}
|