Add support and tests for 3x3 matrices.
This commit is contained in:
Bill Currie
parent
cc35209f86
commit
6f484ee757
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -0,0 +1,213 @@
|
|||
#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 ("\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;
|
||||
}
|
||||
Loading…
Reference in New Issue