From 6c0cfe310a67a6ccb1a0088c258c10cc7183b708 Mon Sep 17 00:00:00 2001 From: Recep Aslantas Date: Sun, 18 Sep 2016 00:53:24 +0300 Subject: [PATCH] quaternions --- include/cglm-mat.h | 9 +++ include/cglm-quat-simd.h | 66 +++++++++++++++++++ include/cglm-quat.h | 137 +++++++++++++++++++++++++-------------- include/cglm-vec.h | 27 +++++++- 4 files changed, 189 insertions(+), 50 deletions(-) create mode 100644 include/cglm-quat-simd.h diff --git a/include/cglm-mat.h b/include/cglm-mat.h index 9e04bd5..3f0743a 100644 --- a/include/cglm-mat.h +++ b/include/cglm-mat.h @@ -84,6 +84,15 @@ glm_mat4_mulN(mat4 * __restrict matrices[], int len, mat4 dest) { dest); } +CGLM_INLINE +void +glm_mat4_mulv(mat4 m, vec4 v, vec4 dest) { + dest[0] = m[0][0] * v[0] + m[1][0] * v[1] + m[2][0] * v[2] + m[3][0] * v[3]; + dest[0] = m[0][1] * v[0] + m[1][1] * v[1] + m[2][1] * v[2] + m[3][1] * v[3]; + dest[0] = m[0][2] * v[0] + m[1][2] * v[1] + m[2][2] * v[2] + m[3][2] * v[3]; + dest[0] = m[0][3] * v[0] + m[1][3] * v[1] + m[2][3] * v[2] + m[3][3] * v[3]; +} + CGLM_INLINE void glm_mat4_transpose_to(mat4 m, mat4 dest) { diff --git a/include/cglm-quat-simd.h b/include/cglm-quat-simd.h new file mode 100644 index 0000000..567cc82 --- /dev/null +++ b/include/cglm-quat-simd.h @@ -0,0 +1,66 @@ +/* + * Copyright (c), Recep Aslantas. + * + * MIT License (MIT), http://opensource.org/licenses/MIT + * Full license can be found in the LICENSE file + */ + +#ifndef cglm_quat_simd_h +#define cglm_quat_simd_h + +#include "cglm-intrin.h" + +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 = sqrt(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 = acos(cosTheta); + a = sin(c * angle); + b = sin(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 /* cglm_quat_simd_h */ diff --git a/include/cglm-quat.h b/include/cglm-quat.h index d82f6f6..3bde637 100644 --- a/include/cglm-quat.h +++ b/include/cglm-quat.h @@ -9,13 +9,93 @@ #define cglm_quat_h #include "cglm.h" +#include "cglm-vec.h" #include "cglm-intrin.h" +#include "cglm-quat-simd.h" #include +CGLM_INLINE +void +glm_quat_init(versor q, + float angle, + float x, + float y, + float z) { + q[0] = cosf(angle / 2.0f); + q[1] = sinf(angle / 2.0f) * x; + q[2] = sinf(angle / 2.0f) * y; + q[3] = sinf(angle / 2.0f) * z; +} + + +CGLM_INLINE +float +glm_quat_norm(versor q) { + return glm_vec4_norm(q); +} + +CGLM_INLINE +void +glm_quat_normalize(versor q) { + float sum, norm; + + sum = q[0] * q[0] + q[1] * q[1] + + q[2] * q[2] + q[3] * q[3]; + + if (fabs(1.0f - sum) < 0.0001f) + return; + + norm = sqrt(sum); + + q[0] = q[0] / norm; + q[1] = q[1] / norm; + q[2] = q[2] / norm; + q[3] = q[3] / norm; +} + CGLM_INLINE float glm_quat_dot(versor q, versor r) { - return q[0] * r[0] + q[1] * r[1] + q[2] * r[2] + q[3] * r[3]; + return glm_vec4_dot(q, r); +} + +CGLM_INLINE +void +glm_quat_mulv(versor q1, versor q2, versor dest) { + dest[0] = q2[0] * q1[0] - q2[1] * q1[1] - q2[2] * q1[2] - q2[3] * q1[3]; + dest[1] = q2[0] * q1[1] + q2[1] * q1[0] - q2[2] * q1[3] + q2[3] * q1[2]; + dest[2] = q2[0] * q1[2] + q2[1] * q1[3] + q2[2] * q1[0] - q2[3] * q1[1]; + dest[3] = q2[0] * q1[3] - q2[1] * q1[2] + q2[2] * q1[1] + q2[3] * q1[0]; + + glm_quat_normalize(dest); +} + +CGLM_INLINE +void +glm_quat_mat4(versor q, mat4 dest) { + float w, x, y, z; + + w = q[0]; + x = q[1]; + y = q[2]; + z = q[3]; + + dest[0][0] = 1.0f - 2.0f * (y * y + z * z); + dest[0][1] = 2.0f * (x * y + w * z); + dest[0][2] = 2.0f * (x * z - w * y); + dest[0][3] = 0.0f; + dest[1][0] = 2.0f * (x * y - w * z); + dest[1][1] = 1.0f - 2.0f * (x * x + z * z); + dest[1][2] = 2.0f * (y * z + w * x); + dest[1][3] = 0.0f; + dest[2][0] = 2.0f * (x * z + w * y); + dest[2][1] = 2.0f * (y * z - w * x); + dest[2][2] = 1.0f - 2.0f * (x * x + y * y); + dest[2][3] = 0.0f; + dest[3][0] = 0.0f; + dest[3][1] = 0.0f; + dest[3][2] = 0.0f; + dest[3][3] = 1.0f; } CGLM_INLINE @@ -24,52 +104,12 @@ glm_quat_slerp(versor q, versor r, float t, versor dest) { + /* 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; -#if defined( __SSE__ ) || defined( __SSE2__ ) - __m128 xmm_q; - - xmm_q = _mm_load_ps(q); - - cosTheta = glm_quat_dot(q, r); - if (cosTheta < 0.0f) { - _mm_store_ps(q, - _mm_xor_ps(xmm_q, - _mm_set1_ps(-0.f))) ; - - cosTheta = glm_quat_dot(q, r); - } - - if (fabs(cosTheta) >= 1.0f) { - _mm_store_ps(dest, xmm_q); - return; - } - - sinTheta = sqrt(1.0f - cosTheta * cosTheta); - - c = 1.0f - t; - - /* LERP */ - if (fabs(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 = acos(cosTheta); - a = sin(c * angle); - b = sin(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))); -#else cosTheta = glm_quat_dot(q, r); if (cosTheta < 0.0f) { q[0] *= -1.0f; @@ -77,7 +117,7 @@ glm_quat_slerp(versor q, q[2] *= -1.0f; q[3] *= -1.0f; - cosTheta = glm_quat_dot(q, r); + cosTheta = -cosTheta; } if (fabs(cosTheta) >= 1.0f) { @@ -85,7 +125,6 @@ glm_quat_slerp(versor q, dest[1] = q[1]; dest[2] = q[2]; dest[3] = q[3]; - return; } @@ -94,12 +133,12 @@ glm_quat_slerp(versor q, c = 1.0f - t; /* LERP */ - if (fabs(sinTheta) < 0.001f) { + /* 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]; - return; } diff --git a/include/cglm-vec.h b/include/cglm-vec.h index 5aa8859..184a6c4 100644 --- a/include/cglm-vec.h +++ b/include/cglm-vec.h @@ -41,7 +41,7 @@ glm_vec_dot(vec3 a, vec3 b) { CGLM_INLINE float -glm_vec_dot4(vec4 a, vec4 b) { +glm_vec4_dot(vec4 a, vec4 b) { return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3]; } @@ -60,6 +60,13 @@ glm_vec_norm(vec3 vec) { return sqrtf(vec[0] * vec[0] + vec[1] * vec[1] + vec[2] * vec[2]); } +CGLM_INLINE +float +glm_vec4_norm(vec3 vec) { + return sqrtf(vec[0] * vec[0] + vec[1] * vec[1] + + vec[2] * vec[2] + vec[3] * vec[3]); +} + CGLM_INLINE void glm_vec_normalize_to(vec3 vec, vec3 dest) { @@ -94,6 +101,24 @@ glm_vec_normalize(vec3 v) { v[2] = v[2] / norm; } +CGLM_INLINE +void +glm_vec4_normalize(vec4 v) { + float norm; + + norm = glm_vec_norm(v); + + if (norm == 0.0f) { + v[0] = v[1] = v[2] = v[3] = 0.0f; + return; + } + + v[0] = v[0] / norm; + v[1] = v[1] / norm; + v[2] = v[2] / norm; + v[3] = v[3] / norm; +} + CGLM_INLINE void glm_vec_add(vec3 v1, vec3 v2, vec3 dest) {