mirror of
https://git.code.sf.net/p/quake/quakeforge
synced 2025-02-17 01:11:45 +00:00
[util] Add vector-quaternion shortcut functions
Care needs to be taken to ensure the right function is used with the right arguments, but with these, the need to use qconj(d|f) for a one-off inverse rotation is removed.
This commit is contained in:
Bill Currie
parent
7bf90e5f4a
commit
015cee7b6f
3 changed files with 99 additions and 2 deletions
|
|
@ -61,6 +61,8 @@ GNU89INLINE inline vec4d_t dotd (vec4d_t a, vec4d_t b) __attribute__((const));
|
|||
*/
|
||||
GNU89INLINE inline vec4d_t qmuld (vec4d_t a, vec4d_t b) __attribute__((const));
|
||||
/** Optimized quaterion-vector multiplication for vector rotation.
|
||||
*
|
||||
* \note This is the inverse of vqmulf
|
||||
*
|
||||
* If the vector's w component is not zero, then the result's w component
|
||||
* is the cosine of the full rotation angle scaled by the vector's w component.
|
||||
|
|
@ -68,6 +70,16 @@ GNU89INLINE inline vec4d_t qmuld (vec4d_t a, vec4d_t b) __attribute__((const));
|
|||
* vector will be scaled by the square of the quaternion's magnitude.
|
||||
*/
|
||||
GNU89INLINE inline vec4d_t qvmuld (vec4d_t q, vec4d_t v) __attribute__((const));
|
||||
/** Optimized vector-quaterion multiplication for vector rotation.
|
||||
*
|
||||
* \note This is the inverse of qvmulf
|
||||
*
|
||||
* If the vector's w component is not zero, then the result's w component
|
||||
* is the cosine of the full rotation angle scaled by the vector's w component.
|
||||
* The quaternion is assumed to be unit. If the quaternion is not unit, the
|
||||
* vector will be scaled by the square of the quaternion's magnitude.
|
||||
*/
|
||||
GNU89INLINE inline vec4d_t vqmuld (vec4d_t v, vec4d_t q) __attribute__((const));
|
||||
/** Create the quaternion representing the shortest rotation from a to b.
|
||||
*
|
||||
* Both a and b are assumed to be 3D vectors (w components 0), but a resonable
|
||||
|
|
@ -183,6 +195,7 @@ VISIBLE
|
|||
#endif
|
||||
vec4d_t
|
||||
qvmuld (vec4d_t q, vec4d_t v)
|
||||
// ^^^ ^^^
|
||||
{
|
||||
double s = q[3];
|
||||
// zero the scalar of the quaternion. Results in an extra operation, but
|
||||
|
|
@ -193,9 +206,32 @@ qvmuld (vec4d_t q, vec4d_t v)
|
|||
vec4d_t qv = dotd (q, v); // q.w is 0 so v.w is irrelevant
|
||||
vec4d_t qq = dotd (q, q);
|
||||
|
||||
// vvv
|
||||
return (s * s - qq) * v + 2 * (qv * q + s * c);
|
||||
}
|
||||
|
||||
#ifndef IMPLEMENT_VEC4D_Funcs
|
||||
GNU89INLINE inline
|
||||
#else
|
||||
VISIBLE
|
||||
#endif
|
||||
vec4d_t
|
||||
vqmuld (vec4d_t v, vec4d_t q)
|
||||
// ^^^ ^^^
|
||||
{
|
||||
double s = q[3];
|
||||
// zero the scalar of the quaternion. Results in an extra operation, but
|
||||
// avoids adding precision issues.
|
||||
vec4d_t z = {};
|
||||
q = _mm256_blend_pd (q, z, 0x08);
|
||||
vec4d_t c = crossd (q, v);
|
||||
vec4d_t qv = dotd (q, v); // q.w is 0 so v.w is irrelevant
|
||||
vec4d_t qq = dotd (q, q);
|
||||
|
||||
// vvv
|
||||
return (s * s - qq) * v + 2 * (qv * q - s * c);
|
||||
}
|
||||
|
||||
#ifndef IMPLEMENT_VEC4D_Funcs
|
||||
GNU89INLINE inline
|
||||
#else
|
||||
|
|
|
|||
|
|
@ -61,12 +61,23 @@ GNU89INLINE inline vec4f_t dotf (vec4f_t a, vec4f_t b) __attribute__((const));
|
|||
*/
|
||||
GNU89INLINE inline vec4f_t qmulf (vec4f_t a, vec4f_t b) __attribute__((const));
|
||||
/** Optimized quaterion-vector multiplication for vector rotation.
|
||||
*
|
||||
* \note This is the inverse of vqmulf
|
||||
*
|
||||
* If the vector's w component is not zero, then the result's w component
|
||||
* is the cosine of the full rotation angle scaled by the vector's w component.
|
||||
* The quaternion is assumed to be unit.
|
||||
*/
|
||||
GNU89INLINE inline vec4f_t qvmulf (vec4f_t q, vec4f_t v) __attribute__((const));
|
||||
/** Optimized vector-quaterion multiplication for vector rotation.
|
||||
*
|
||||
* \note This is the inverse of qvmulf
|
||||
*
|
||||
* If the vector's w component is not zero, then the result's w component
|
||||
* is the cosine of the full rotation angle scaled by the vector's w component.
|
||||
* The quaternion is assumed to be unit.
|
||||
*/
|
||||
GNU89INLINE inline vec4f_t vqmulf (vec4f_t v, vec4f_t q) __attribute__((const));
|
||||
/** Create the quaternion representing the shortest rotation from a to b.
|
||||
*
|
||||
* Both a and b are assumed to be 3D vectors (w components 0), but a resonable
|
||||
|
|
@ -190,6 +201,25 @@ qvmulf (vec4f_t q, vec4f_t v)
|
|||
return (s * s - qq) * v + 2 * (qv * q + s * c);
|
||||
}
|
||||
|
||||
#ifndef IMPLEMENT_VEC4F_Funcs
|
||||
GNU89INLINE inline
|
||||
#else
|
||||
VISIBLE
|
||||
#endif
|
||||
vec4f_t
|
||||
vqmulf (vec4f_t v, vec4f_t q)
|
||||
{
|
||||
float s = q[3];
|
||||
// zero the scalar of the quaternion. Results in an extra operation, but
|
||||
// avoids adding precision issues.
|
||||
q = _mm_insert_ps (q, q, 0xf8);
|
||||
vec4f_t c = crossf (q, v);
|
||||
vec4f_t qv = dotf (q, v); // q.w is 0 so v.w is irrelevant
|
||||
vec4f_t qq = dotf (q, q);
|
||||
|
||||
return (s * s - qq) * v + 2 * (qv * q - s * c);
|
||||
}
|
||||
|
||||
#ifndef IMPLEMENT_VEC4F_Funcs
|
||||
GNU89INLINE inline
|
||||
#else
|
||||
|
|
|
|||
|
|
@ -153,11 +153,22 @@ static vec4d_test_t vec4d_tests[] = {
|
|||
{ qvmuld, one, up, { 4, 0, 0, 0 } },
|
||||
{ qvmuld, one, {1,1,1,0}, { 4, 4, 4, 0 } },
|
||||
{ qvmuld, one, one, { 4, 4, 4, -2 } },
|
||||
// inverse rotation, so x->z->y->x
|
||||
{ vqmuld, right, one, { 0, 0, 4, 0 } },
|
||||
{ vqmuld, forward, one, { 4, 0, 0, 0 } },
|
||||
{ vqmuld, up, one, { 0, 4, 0, 0 } },
|
||||
{ vqmuld, {1,1,1,0}, one, { 4, 4, 4, 0 } },
|
||||
{ vqmuld, one, one, { 4, 4, 4, -2 } },
|
||||
// The half vector is unit.
|
||||
{ qvmuld, half, right, forward },
|
||||
{ qvmuld, half, forward, up },
|
||||
{ qvmuld, half, up, right },
|
||||
{ qvmuld, half, {1,1,1,0}, { 1, 1, 1, 0 } },
|
||||
// inverse
|
||||
{ vqmuld, right, half, up },
|
||||
{ vqmuld, forward, half, right },
|
||||
{ vqmuld, up, half, forward },
|
||||
{ vqmuld, {1,1,1,0}, half, { 1, 1, 1, 0 } },
|
||||
// one is a 4D vector and qvmuld is meant for 3D vectors. However, it
|
||||
// seems that the vector's w has no effect on the 3d portion of the
|
||||
// result, but the result's w is cosine of the full rotation angle
|
||||
|
|
@ -168,6 +179,13 @@ static vec4d_test_t vec4d_tests[] = {
|
|||
{ qvmuld, qtest, forward, {0.6144, 0.1808, 0.768, 0},
|
||||
{0, -2.7e-17, 0, 0} },
|
||||
{ qvmuld, qtest, up, {0.576, -0.768, -0.28, 0} },
|
||||
// inverse
|
||||
{ vqmuld, one, half, { 1, 1, 1, -0.5 } },
|
||||
{ vqmuld, {2,2,2,2}, half, { 2, 2, 2, -1 } },
|
||||
{ vqmuld, right, qtest, {0.5392, 0.6144, 0.576, 0} },
|
||||
{ vqmuld, forward, qtest, {0.6144, 0.1808, -0.768, 0},
|
||||
{0, -2.7e-17, 0, 0} },
|
||||
{ vqmuld, up, qtest, {-0.576, 0.768, -0.28, 0} },
|
||||
|
||||
{ qrotd, right, right, qident },
|
||||
{ qrotd, right, forward, { 0, 0, s05, s05 },
|
||||
|
|
@ -259,12 +277,25 @@ static vec4f_test_t vec4f_tests[] = {
|
|||
{ qvmulf, one, up, { 4, 0, 0, 0 } },
|
||||
{ qvmulf, one, {1,1,1,0}, { 4, 4, 4, 0 } },
|
||||
{ qvmulf, one, one, { 4, 4, 4, -2 } },
|
||||
// inverse rotation, so x->z->y->x
|
||||
{ vqmulf, right, one, { 0, 0, 4, 0 } },
|
||||
{ vqmulf, forward, one, { 4, 0, 0, 0 } },
|
||||
{ vqmulf, up, one, { 0, 4, 0, 0 } },
|
||||
{ vqmulf, {1,1,1,0}, one, { 4, 4, 4, 0 } },
|
||||
{ vqmulf, one, one, { 4, 4, 4, -2 } },
|
||||
//
|
||||
{ qvmulf, qtest, right, {0.5392, 0.6144, -0.576, 0},
|
||||
{0, -5.9e-08, -6e-8, 0} },
|
||||
{0, -5.9e-8, -6e-8, 0} },
|
||||
{ qvmulf, qtest, forward, {0.6144, 0.1808, 0.768, 0},
|
||||
{-5.9e-08, 1.5e-08, 0, 0} },
|
||||
{-5.9e-8, 1.5e-8, 0, 0} },
|
||||
{ qvmulf, qtest, up, {0.576, -0.768, -0.28, 0},
|
||||
{6e-8, 0, 3e-8, 0} },
|
||||
{ vqmulf, right, qtest, {0.5392, 0.6144, 0.576, 0},
|
||||
{0, -5.9e-8, 5.9e-8, 0} },
|
||||
{ vqmulf, forward, qtest, {0.6144, 0.1808, -0.768, 0},
|
||||
{-5.9e-8, 1.5e-8, 0, 0} },
|
||||
{ vqmulf, up, qtest, {-0.576, 0.768, -0.28, 0},
|
||||
{-5.9e-8, 0, 3e-8, 0} },
|
||||
|
||||
{ qrotf, right, right, qident },
|
||||
{ qrotf, right, forward, { 0, 0, s05, s05 } },
|
||||
|
|
|
|||
Loading…
Reference in a new issue