External/cglm
1
0
Fork 0
mirror of https://github.com/recp/cglm.git synced 2025-12-26 10:35:10 +00:00
Code Issues Packages Projects Releases Wiki Activity

quat: add lerp and improve slerp

Browse Source
This commit is contained in:
Recep Aslantas
2018-04-10 12:38:54 +03:00
parent 416e2f4452
commit 290bcf134c
2 changed files with 35 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files

66
include/cglm/quat.h
View File

@@ -432,60 +432,64 @@ glm_quat_mat3(versor q, mat3 dest) {
dest[0][2] = xz - wy;
}
/*!
* @brief interpolates between two quaternions
* using linear interpolation (LERP)
*
* @param[in] from from
* @param[in] to to
* @param[in] t interpolant (amount) clamped between 0 and 1
* @param[out] dest result quaternion
*/
CGLM_INLINE
void
glm_quat_lerp(versor from, versor to, float t, versor dest) {
glm_vec4_lerp(from, to, t, dest);
}
/*!
* @brief interpolates between two quaternions
* using spherical linear interpolation (SLERP)
*
* @param[in] q from
* @param[in] r to
* @param[in] from from
* @param[in] to to
* @param[in] t amout
* @param[out] dest result quaternion
*/
CGLM_INLINE
void
glm_quat_slerp(versor q, versor r, float t, versor dest) {
glm_quat_slerp(versor from, versor to, float t, versor dest) {
vec4 q1, q2;
float cosTheta, sinTheta, angle;
/* https://en.wikipedia.org/wiki/Slerp */
#if defined( __SSE__ ) || defined( __SSE2__ )
glm_quat_slerp_sse2(q, r, t, dest);
#else
float cosTheta, sinTheta, angle, a, b, c;
cosTheta = glm_quat_dot(from, to);
glm_quat_copy(from, q1);
cosTheta = glm_quat_dot(q, r);
if (cosTheta < 0.0f) {
glm_vec4_flipsign(q);
cosTheta = -cosTheta;
if (fabsf(cosTheta) >= 1.0f) {
glm_quat_copy(q1, dest);
return;
}
if (fabs(cosTheta) >= 1.0f) {
glm_quat_copy(q, dest);
return;
if (cosTheta < 0.0f) {
glm_vec4_flipsign(q1);
cosTheta = -cosTheta;
}
sinTheta = sqrtf(1.0f - cosTheta * cosTheta);
c = 1.0f - t;
/* LERP */
/* TODO: FLT_EPSILON vs 0.001? */
if (sinTheta < 0.001f) {
dest[0] = c * q[0] + t * r[0];
dest[1] = c * q[1] + t * r[1];
dest[2] = c * q[2] + t * r[2];
dest[3] = c * q[3] + t * r[3];
/* LERP to avoid zero division */
if (fabsf(sinTheta) < 0.001f) {
glm_quat_lerp(from, to, t, dest);
return;
}
/* SLERP */
angle = acosf(cosTheta);
a = sinf(c * angle);
b = sinf(t * angle);
glm_vec4_scale(q1, sinf((1.0f - t) * angle), q1);
glm_vec4_scale(to, sinf(t * angle), q2);
dest[0] = (q[0] * a + r[0] * b) / sinTheta;
dest[1] = (q[1] * a + r[1] * b) / sinTheta;
dest[2] = (q[2] * a + r[2] * b) / sinTheta;
dest[3] = (q[3] * a + r[3] * b) / sinTheta;
#endif
glm_vec4_add(q1, q2, q1);
glm_vec4_scale(q1, 1.0f / sinTheta, dest);
}
#endif /* cglm_quat_h */

52
include/cglm/simd/sse2/quat.h
View File

@@ -41,58 +41,6 @@ glm_quat_mul_sse2(versor p, versor q, versor dest) {
_mm_store_ps(dest, r);
}
CGLM_INLINE
void
glm_quat_slerp_sse2(versor q,
versor r,
float t,
versor dest) {
/* https://en.wikipedia.org/wiki/Slerp */
float cosTheta, sinTheta, angle, a, b, c;
__m128 xmm_q;
xmm_q = _mm_load_ps(q);
cosTheta = glm_vec4_dot(q, r);
if (cosTheta < 0.0f) {
_mm_store_ps(q,
_mm_xor_ps(xmm_q,
_mm_set1_ps(-0.f))) ;
cosTheta = -cosTheta;
}
if (cosTheta >= 1.0f) {
_mm_store_ps(dest, xmm_q);
return;
}
sinTheta = sqrtf(1.0f - cosTheta * cosTheta);
c = 1.0f - t;
/* LERP */
if (sinTheta < 0.001f) {
_mm_store_ps(dest, _mm_add_ps(_mm_mul_ps(_mm_set1_ps(c),
xmm_q),
_mm_mul_ps(_mm_set1_ps(t),
_mm_load_ps(r))));
return;
}
/* SLERP */
angle = acosf(cosTheta);
a = sinf(c * angle);
b = sinf(t * angle);
_mm_store_ps(dest,
_mm_div_ps(_mm_add_ps(_mm_mul_ps(_mm_set1_ps(a),
xmm_q),
_mm_mul_ps(_mm_set1_ps(b),
_mm_load_ps(r))),
_mm_set1_ps(sinTheta)));
}
#endif
#endif /* cglm_quat_simd_h */