mirror of
https://git.code.sf.net/p/quake/quakeforge
synced 2024-11-23 04:42:32 +00:00
Add support for duals and dual quaternions.
Not everything is unit-tested, but the currently important stuff is.
This commit is contained in:
Bill Currie
parent
3b938592c3
commit
f874aeb941
4 changed files with 276 additions and 1 deletions
|
|
@ -277,6 +277,157 @@ extern const vec_t *const quat_origin;
|
|||
//For printf etc
|
||||
#define QuatExpand(q) (q)[0], (q)[1], (q)[2], (q)[3]
|
||||
|
||||
#define DualAdd(a,b,c) \
|
||||
do { \
|
||||
(c).r = (a).r + (b).r; \
|
||||
(c).e = (a).e + (b).e; \
|
||||
} while (0)
|
||||
#define DualSubtract(a,b,c) \
|
||||
do { \
|
||||
(c).r = (a).r - (b).r; \
|
||||
(c).e = (a).e - (b).e; \
|
||||
} while (0)
|
||||
#define DualNegate(a,b) \
|
||||
do { \
|
||||
(b).r = -(a).r; \
|
||||
(b).e = -(a).e; \
|
||||
} while (0)
|
||||
#define DualConj(a,b) \
|
||||
do { \
|
||||
(b).r = (a).r; \
|
||||
(b).e = -(a).e; \
|
||||
} while (0)
|
||||
#define DualMult(a,b,c) \
|
||||
do { \
|
||||
(c).e = (a).r * (b).e + (a).e * (b).r; \
|
||||
(c).r = (a).r * (b).r; \
|
||||
} while (0)
|
||||
#define DualMultAdd(a,s,b,c) \
|
||||
do { \
|
||||
(c).r = (a).r + (s) * (b).r; \
|
||||
(c).e = (a).e + (s) * (b).e; \
|
||||
} while (0)
|
||||
#define DualMultSub(a,s,b,c) \
|
||||
do { \
|
||||
(c).r = (a).r - (s) * (b).r; \
|
||||
(c).e = (a).e - (s) * (b).e; \
|
||||
} while (0)
|
||||
#define DualNorm(a) ((a).r)
|
||||
#define DualScale(a,b,c) \
|
||||
do { \
|
||||
(c).r = (a).r * (b); \
|
||||
(c).e = (a).e * (b); \
|
||||
} while (0)
|
||||
#define DualCompCompare(x, op, y) ((x).r op (y).r) && ((x).e op (y).e)
|
||||
#define DualCompare(x, y) DualCompCompare (x, ==, y)
|
||||
#define DualIsZero(a) ((a).r == 0 && (a).e == 0)
|
||||
#define DualIsUnit(a) (((a).r - 1) * ((a).r - 1) < 1e-6 && (a).e * (a).e < 1e-6)
|
||||
#define DualSet(ar,ae,a) \
|
||||
do { \
|
||||
(a).ar = r; \
|
||||
(a).er = r; \
|
||||
} while (0)
|
||||
#define DualZero(a) \
|
||||
do { \
|
||||
(a).e = (a).r = 0; \
|
||||
} while (0)
|
||||
#define DualBlend(d1,d2,b,d) \
|
||||
do { \
|
||||
(d).r = (d1).r * (1 - (b)) + (d2).r * (b); \
|
||||
(d).e = (d1).e * (1 - (b)) + (d2).e * (b); \
|
||||
} while (0)
|
||||
#define DualExpand(d) (d).r, (d).e
|
||||
|
||||
#define DualQuatAdd(a,b,c) \
|
||||
do { \
|
||||
QuatAdd ((a).q0.q, (b).q0.q, (c).q0.q); \
|
||||
QuatAdd ((a).qe.q, (b).qe.q, (c).qe.q); \
|
||||
} while (0)
|
||||
#define DualQuatSubtract(a,b,c) \
|
||||
do { \
|
||||
QuatSub ((a).q0.q, (b).q0.q, (c).q0.q); \
|
||||
QuatSub ((a).qe.q, (b).qe.q, (c).qe.q); \
|
||||
} while (0)
|
||||
#define DualQuatNegate(a,b) \
|
||||
do { \
|
||||
QuatNegate ((a).q0.q, (b).q0.q); \
|
||||
QuatNegate ((a).qe.q, (b).qe.q); \
|
||||
} while (0)
|
||||
#define DualQuatConjQ(a,b) \
|
||||
do { \
|
||||
QuatConj ((a).q0.q, (b).q0.q); \
|
||||
QuatConj ((a).qe.q, (b).qe.q); \
|
||||
} while (0)
|
||||
#define DualQuatConjE(a,b) \
|
||||
do { \
|
||||
(b).q0 = (a).q0; \
|
||||
QuatNegate ((a).qe.q, (b).qe.q); \
|
||||
} while (0)
|
||||
#define DualQuatConjQE(a,b) \
|
||||
do { \
|
||||
QuatConj ((a).q0.q, (b).q0.q); \
|
||||
(b).qe.sv.s = -(a).qe.sv.s; \
|
||||
VectorCopy ((a).qe.sv.v, (b).qe.sv.v); \
|
||||
} while (0)
|
||||
#define DualQuatMult(a,b,c) \
|
||||
do { \
|
||||
Quat_t t; \
|
||||
QuatMult ((a).q0.q, (b).qe.q, t.q); \
|
||||
QuatMult ((a).qe.q, (b).q0.q, (c).qe.q); \
|
||||
QuatAdd (t.q, (c).qe.q, (c).qe.q); \
|
||||
QuatMult ((a).q0.q, (b).q0.q, (c).q0.q); \
|
||||
} while (0);
|
||||
#define DualQuatMultAdd(a,s,b,c) \
|
||||
do { \
|
||||
QuatMultAdd ((a).q0.q, s, (b).q0.q, (c).q0.q); \
|
||||
QuatMultAdd ((a).qe.q, s, (b).qe.q, (c).qe.q); \
|
||||
} while (0)
|
||||
#define DualQuatMultSub(a,s,b,c) \
|
||||
do { \
|
||||
QuatMultSub ((a).q0.q, s, (b).q0.q, (c).q0.q); \
|
||||
QuatMultSub ((a).qe.q, s, (b).qe.q, (c).qe.q); \
|
||||
} while (0)
|
||||
#define DualQuatNorm(a,b) \
|
||||
do { \
|
||||
(b).r = QuatLength ((a).q0.q); \
|
||||
(b).e = QDotProduct ((a).q0.q, (a).qe.q) / (b).r; \
|
||||
} while (0)
|
||||
#define DualQuatScale(a,b,c) \
|
||||
do { \
|
||||
QuatSub ((a).q0.q, (b), (c).q0.q); \
|
||||
QuatSub ((a).qe.q, (b), (c).qe.q); \
|
||||
} while (0)
|
||||
#define DualQuatCompCompare(x, op, y) \
|
||||
(QuatCompCompare ((x).q0.q, op, (y).q0.q) \
|
||||
&&QuatCompCompare ((x).qe.q, op, (y).qe.q))
|
||||
#define DualQuatCompare(x, y) DualQuatCompCompare (x, ==, y)
|
||||
#define DualQuatIsZero(a) (QuatIsZero ((a).q0.q) && QuatIsZero ((a).qe.q))
|
||||
#define DualQuatSetVect(vec, a) \
|
||||
do { \
|
||||
(a).q0.sv.s = 1; \
|
||||
VectorZero ((a).q0.sv.v); \
|
||||
(a).qe.sv.s = 0; \
|
||||
VectorCopy (vec, (a).qe.sv.v); \
|
||||
} while (0)
|
||||
#define DualQuatRotTrans(rot, trans, dq) \
|
||||
do { \
|
||||
QuatCopy (rot, (dq).q0.q); \
|
||||
(dq).qe.sv.s = 0; \
|
||||
VectorScale (trans, 0.5, (dq).qe.sv.v); \
|
||||
QuatMult ((dq).qe.q, (dq).q0.q, (dq).qe.q); \
|
||||
} while (0)
|
||||
#define DualQuatZero(a) \
|
||||
do { \
|
||||
QuatZero ((a).q0.q); \
|
||||
QuatZero ((a).qe.q); \
|
||||
} while (0)
|
||||
#define DualQuatBlend(dq1,dq2,b,dq) \
|
||||
do { \
|
||||
QuatBlend ((dq1).q0.q, (dq2).q0.q, (b), (dq).q0.q); \
|
||||
QuatBlend ((dq1).qe.q, (dq2).qe.q, (b), (dq).qe.q); \
|
||||
} while (0)
|
||||
#define DualQuatExpand(dq) QuatExpand ((dq).q0.q), QuatExpand ((dq).qe.q)
|
||||
|
||||
#define Mat4Copy(a, b) \
|
||||
do { \
|
||||
QuatCopy ((a) + 0, (b) + 0); \
|
||||
|
|
|
|||
|
|
@ -81,6 +81,14 @@ typedef union {
|
|||
} sv;
|
||||
quat_t q;
|
||||
} Quat_t;
|
||||
typedef struct {
|
||||
vec_t r;
|
||||
vec_t e;
|
||||
} Dual_t;
|
||||
typedef struct {
|
||||
Quat_t q0;
|
||||
Quat_t qe;
|
||||
} DualQuat_t;
|
||||
typedef int fixed4_t;
|
||||
typedef int fixed8_t;
|
||||
typedef int fixed16_t;
|
||||
|
|
|
|||
|
|
@ -2,7 +2,11 @@ AUTOMAKE_OPTIONS= foreign
|
|||
|
||||
INCLUDES= -I$(top_srcdir)/include
|
||||
|
||||
check_PROGRAMS=test-half test-qfs test-quat test-vrect
|
||||
check_PROGRAMS=test-dq test-half test-qfs test-quat test-vrect
|
||||
|
||||
test_dq_SOURCES=test-dq.c
|
||||
test_dq_LDADD=$(top_builddir)/libs/util/libQFutil.la
|
||||
test_dq_DEPENDENCIES=$(top_builddir)/libs/util/libQFutil.la
|
||||
|
||||
test_half_SOURCES=test-half.c
|
||||
test_half_LDADD=$(top_builddir)/libs/util/libQFutil.la
|
||||
|
|
|
|||
112
libs/util/test/test-dq.c
Normal file
112
libs/util/test/test-dq.c
Normal file
|
|
@ -0,0 +1,112 @@
|
|||
#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 vd, xd;
|
||||
Dual_t dual;
|
||||
|
||||
VectorZero (x);
|
||||
DualQuatZero (xd);
|
||||
|
||||
AngleQuat (angles, rotation);
|
||||
DualQuatSetVect (v, vd);
|
||||
DualQuatRotTrans (rotation, translation, transform);
|
||||
DualQuatConjQE (transform, conj);
|
||||
|
||||
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);
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
for (i = 0; i < 3; i++)
|
||||
if (!compare (xd.qe.sv.v[i], x[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));
|
||||
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;
|
||||
}
|
||||
Loading…
Reference in a new issue