qf/quakeforge
0
0
Fork 0
mirror of https://git.code.sf.net/p/quake/quakeforge synced 2024-11-26 22:31:05 +00:00
Code Issues Projects Releases Packages Wiki Activity

[util] Calculate quaternion to rotate between two vectors

Browse source 
The calculation fails (produces NaN) if the vectors are anti-parallel,
but works for all other combinations. I came up with this implementation
when I discovered Unity's Quaternion.FromToRotation could did not work
with very small angles. This implementation will produce a usable
quaternion below 0.00255 degrees (though it will be slightly larger than
unit). Unity's failed such that I could see KSP's skybox snap while it
rotated around my test vessel.
This commit is contained in:
Bill Currie 2020-12-19 17:29:08 +09:00
parent e991c44232
commit 62f3e1f428
3 changed files with 121 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

1
include/QF/math/quaternion.h
View file

@ -164,6 +164,7 @@ extern const vec_t *const quat_origin;
void QuatMult (const quat_t q1, const quat_t q2, quat_t out);
void QuatMultVec (const quat_t q, const vec3_t v, vec3_t out);
void QuatRotation (const vec3_t a, const vec3_t b, quat_t out);
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);

18
libs/util/mathlib.c
View file

@ -272,6 +272,24 @@ QuatMultVec (const quat_t q, const vec3_t v, vec3_t out)
VectorMultAdd (tv, s * s - dqq, v, out);
}
VISIBLE void
QuatRotation(const vec3_t a, const vec3_t b, quat_t out)
{
vec_t ma, mb;
vec_t den, mba_mab;
vec3_t t;
ma = VectorLength(a);
mb = VectorLength(b);
den = 2 * ma * mb;
VectorScale (a, mb, t);
VectorMultAdd(t, ma, b, t);
mba_mab = VectorLength(t);
CrossProduct (a, b, t);
VectorScale(t, 1 / mba_mab, out);
out[3] = mba_mab / den;
}
VISIBLE void
QuatInverse (const quat_t in, quat_t out)
{

102
libs/util/test/test-quat.c
View file

@ -29,6 +29,45 @@ static vec3_t test_angles[] = {
};
#define num_angle_tests (sizeof (test_angles) / sizeof (test_angles[0]))
static struct {
vec3_t a;
vec3_t b;
quat_t expect;
} quat_vector_rotation_tests[] = {
{{1, 0, 0}, {1, 0, 0}, {0, 0, 0, 1}},
{{0, 1, 0}, {0, 1, 0}, {0, 0, 0, 1}},
{{0, 0, 1}, {0, 0, 1}, {0, 0, 0, 1}},
{{1, 0, 0}, {8, 0, 0}, {0, 0, 0, 1}},
{{0, 8, 0}, {0, 1, 0}, {0, 0, 0, 1}},
{{0, 0, 8}, {0, 0, 8}, {0, 0, 0, 1}},
{{1, 0, 0}, {-1, 0, 0}, {0, 0, 0, 0}}, // x, y, z = NaN
{{0, -1, 0}, {0, 1, 0}, {0, 0, 0, 0}}, // x, y, z = NaN
{{0, 0, 1}, {0, 0, -1}, {0, 0, 0, 0}}, // x, y, z = NaN
{{-1, 0, 0}, {8, 0, 0}, {0, 0, 0, 0}}, // x, y, z = NaN
{{0, 8, 0}, {0, -1, 0}, {0, 0, 0, 0}}, // x, y, z = NaN
{{0, 0, -8}, {0, 0, 8}, {0, 0, 0, 0}}, // x, y, z = NaN
// excessive for float, but if vec_t becomes double...
// 1/50 second orbiting JNSQ Kerbin at 120km altitude. While this has
// nothing to do with quakeforge (yet?), it came up when testing camera
// rotation in KSP and Unity failed miserably. I don't remember the exact
// details, but I could see significant snapping in the rotation (I thought
// it was a few times per second, but these numbers indicate it must have
// been every few seconds).
// The quaternion is unit to 9 sigfigs (a little larger)
{{1720000, 0, 0}, {1719999.9983028059, 76.409082595828366, 0}, {0, 0, 2.22119434e-5, 1}},
// 1/20 second, same situation
{{1720000, 0, 0}, {1719999.9893925365, 191.02270615971355, 0}, {0, 0, 5.55298575e-5, 1}},
// 1/5 second, same situation
{{1720000, 0, 0}, {1719999.8302805868, 764.09080107761736, 0}, {0, 0, 2.2211943e-4, 1}},
// 1/4 second, same situation
{{1720000, 0, 0}, {1719999.7348134194, 955.11348367609469, 0}, {0, 0, 2.77649262e-4, 1}},
// 1/3 second, same situation. This is (float) 1 ulp in w: about 0.0424
// degrees.
// The quaternion is unit to 9 sigfigs (a little larger)
{{1720000, 0, 0}, {1719999.5285571995, 1273.4845939975546, 0}, {0, 0, 3.70199094e-4, 0.99999994}},
};
#define num_quat_vector_rotation_tests (sizeof (quat_vector_rotation_tests) / sizeof (quat_vector_rotation_tests[0]))
// return true if a and b are close enough (yay, floats)
static int
compare (vec_t a, vec_t b)
@ -200,6 +239,60 @@ fail:
return 0;
}
static int
test_rotation4 (const vec3_t a, const vec3_t b, const quat_t expect)
{
int i;
quat_t quat;
vec3_t t;
vec_t d = 0;
VectorZero (t);
// find the rotation vector between a and b
QuatRotation (a, b, quat);
if (quat[3] == 0) {
if (expect[3] != 0) {
goto fail;
}
// expect NaN for the vector components because the vectors are
// anti-parallel and thus the rotation axis is undefined
if (!(isnan(quat[0]) && isnan(quat[1]) && isnan(quat[2]))) {
goto fail;
}
} else {
// the vectors are not anti-parallel and thus the rotation axis is
// defined, so NaN is invalid
if (isnan(quat[0]) || isnan(quat[1]) || isnan(quat[2])) {
goto fail;
}
for (i = 0; i < 4; i++) {
// yes, float precision will make it difficult to set up expect
// but it is at least consistent (ie, the "errors" are not at all
// random and thus will be the same from run to run)
if (quat[i] != expect[i]) {
goto fail;
}
}
QuatMultVec(quat, a, t);
d = DotProduct (t, b) / (VectorLength (t) * VectorLength (b)) - 1;
if (d * d > 1e-8) {
goto fail;
}
}
return 1;
fail:
printf ("\ntest_rotation4\n");
printf ("%11.9g %11.9g %11.9g\n", VectorExpand(a));
printf ("%11.9g %11.9g %11.9g\n", VectorExpand(b));
printf ("%11.9g %11.9g %11.9g %11.9g\n", QuatExpand(quat));
printf ("%11.9g %11.9g %11.9g\n", VectorExpand(t));
printf ("%11.9g\n", d);
return 0;
}
#define s05 0.70710678118654757
static struct {
@ -285,6 +378,15 @@ main (int argc, const char **argv)
res = 1;
}
for (i = 0; i < num_quat_vector_rotation_tests; i++) {
vec_t *a = quat_vector_rotation_tests[i].a;
vec_t *b = quat_vector_rotation_tests[i].b;
vec_t *expect = quat_vector_rotation_tests[i].expect;
if (!test_rotation4 (a, b, expect)) {
res = 1;
}
}
for (i = 0; i < num_quat_mat_tests; i ++) {
vec_t *q = quat_mat_tests[i].q;
vec_t *expect = quat_mat_tests[i].expect;