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

quaternion multiplication

Browse Source 
* convert quaternion multiplication to xyzw
* previous implementation may be wrong, wikipedia version implemented
* implement SSE version
This commit is contained in:
Recep Aslantas
2018-04-09 23:56:09 +03:00
parent 93a08fce17
commit 6f69da361b
7 changed files with 116 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
include/cglm/call/quat.h
View File

@@ -39,7 +39,7 @@ glmc_quat_dot(versor q, versor r);
CGLM_EXPORT
void
glmc_quat_mulv(versor q1, versor q2, versor dest);
glmc_quat_mul(versor p, versor q, versor dest);
CGLM_EXPORT
void

77
include/cglm/quat.h
View File

@@ -11,15 +11,27 @@
GLM_QUAT_IDENTITY
Functions:
CGLM_INLINE void glm_quat_identity(versor q);
CGLM_INLINE void glm_quat(versor q, float angle, float x, float y, float z);
CGLM_INLINE void glm_quatv(versor q, float angle, vec3 axis);
CGLM_INLINE void glm_quat_identity(versor q);
CGLM_INLINE void glm_quat_init(versor q, float x, float y, float z, float w);
CGLM_INLINE void glm_quat(versor q, float angle, float x, float y, float z);
CGLM_INLINE void glm_quatv(versor q, float angle, vec3 axis);
CGLM_INLINE float glm_quat_norm(versor q);
CGLM_INLINE void glm_quat_normalize(versor q);
CGLM_INLINE float glm_quat_dot(versor q, versor r);
CGLM_INLINE void glm_quat_mulv(versor q1, versor q2, versor dest);
CGLM_INLINE void glm_quat_mat4(versor q, mat4 dest);
CGLM_INLINE void glm_quat_slerp(versor q, versor r, float t, versor dest);
CGLM_INLINE void glm_quat_normalize(versor q);
CGLM_INLINE float glm_quat_dot(versor q1, versor q2);
CGLM_INLINE void glm_quat_conjugate(versor q, versor dest);
CGLM_INLINE void glm_quat_inv(versor q, versor dest);
CGLM_INLINE void glm_quat_add(versor p, versor q, versor dest);
CGLM_INLINE void glm_quat_sub(versor p, versor q, versor dest);
CGLM_INLINE float glm_quat_real(versor q);
CGLM_INLINE void glm_quat_imag(versor q, vec3 dest);
CGLM_INLINE void glm_quat_imagn(versor q, vec3 dest);
CGLM_INLINE float glm_quat_imaglen(versor q);
CGLM_INLINE float glm_quat_angle(versor q);
CGLM_INLINE void glm_quat_axis(versor q, versor dest);
CGLM_INLINE void glm_quat_mul(versor p, versor q, versor dest);
CGLM_INLINE void glm_quat_mat4(versor q, mat4 dest);
CGLM_INLINE void glm_quat_mat3(versor q, mat3 dest)
CGLM_INLINE void glm_quat_slerp(versor q, versor r, float t, versor dest);
*/
#ifndef cglm_quat_h
@@ -164,24 +176,6 @@ glm_quat_dot(versor q1, versor q2) {
return glm_vec4_dot(q1, q2);
}
/*!
* @brief multiplies two quaternion and stores result in dest
*
* @param[in] q1 quaternion 1
* @param[in] q2 quaternion 2
* @param[out] dest result quaternion
*/
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);
}
/*!
* @brief conjugate of quaternion
*
@@ -310,6 +304,37 @@ glm_quat_axis(versor q, versor dest) {
glm_quat_imagn(q, dest);
}
/*!
* @brief multiplies two quaternion and stores result in dest
* this is also called Hamilton Product
*
* According to WikiPedia:
* The product of two rotation quaternions [clarification needed] will be
* equivalent to the rotation q followed by the rotation p
*
* @param[in] p quaternion 1
* @param[in] q quaternion 2
* @param[out] dest result quaternion
*/
CGLM_INLINE
void
glm_quat_mul(versor p, versor q, versor dest) {
/*
+ (a1 b2 + b1 a2 + c1 d2 − d1 c2)i
+ (a1 c2 − b1 d2 + c1 a2 + d1 b2)j
+ (a1 d2 + b1 c2 − c1 b2 + d1 a2)k
a1 a2 − b1 b2 − c1 c2 − d1 d2
*/
#if defined( __SSE__ ) || defined( __SSE2__ )
glm_quat_mul_sse2(p, q, dest);
#else
dest[0] = p[3] * q[0] + p[0] * q[3] + p[1] * q[2] - p[2] * q[1];
dest[1] = p[3] * q[1] - p[0] * q[2] + p[1] * q[3] + p[2] * q[0];
dest[2] = p[3] * q[2] + p[0] * q[1] - p[1] * q[0] + p[2] * q[3];
dest[3] = p[3] * q[3] - p[0] * q[0] - p[1] * q[1] - p[2] * q[2];
#endif
}
/*!
* @brief convert quaternion to mat4
*

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

@@ -12,6 +12,35 @@
#include "../../common.h"
#include "../intrin.h"
CGLM_INLINE
void
glm_quat_mul_sse2(versor p, versor q, versor dest) {
/*
+ (a1 b2 + b1 a2 + c1 d2 − d1 c2)i
+ (a1 c2 − b1 d2 + c1 a2 + d1 b2)j
+ (a1 d2 + b1 c2 − c1 b2 + d1 a2)k
a1 a2 − b1 b2 − c1 c2 − d1 d2
*/
__m128 xp, xq, x0, r;
xp = _mm_load_ps(p); /* 3 2 1 0 */
xq = _mm_load_ps(q);
r = _mm_mul_ps(_mm_shuffle1_ps1(xp, 3), xq);
x0 = _mm_xor_ps(_mm_shuffle1_ps1(xp, 0), _mm_set_ps(-0.f, 0.f, -0.f, 0.f));
r = _mm_add_ps(r, _mm_mul_ps(x0, _mm_shuffle1_ps(xq, 0, 1, 2, 3)));
x0 = _mm_xor_ps(_mm_shuffle1_ps1(xp, 1), _mm_set_ps(-0.f, -0.f, 0.f, 0.f));
r = _mm_add_ps(r, _mm_mul_ps(x0, _mm_shuffle1_ps(xq, 1, 0, 3, 2)));
x0 = _mm_xor_ps(_mm_shuffle1_ps1(xp, 2), _mm_set_ps(-0.f, 0.f, 0.f, -0.f));
r = _mm_add_ps(r, _mm_mul_ps(x0, _mm_shuffle1_ps(xq, 2, 3, 0, 1)));
_mm_store_ps(dest, r);
}
CGLM_INLINE
void
glm_quat_slerp_sse2(versor q,

9
src/quat.c
View File

@@ -46,8 +46,8 @@ glmc_quat_dot(versor q, versor r) {
CGLM_EXPORT
void
glmc_quat_mulv(versor q1, versor q2, versor dest) {
glm_quat_mul(q1, q2, dest);
glmc_quat_mul(versor p, versor q, versor dest) {
glm_quat_mul(p, q, dest);
}
CGLM_EXPORT
@@ -64,9 +64,6 @@ glmc_quat_mat3(versor q, mat3 dest) {
CGLM_EXPORT
void
glmc_quat_slerp(versor q,
versor r,
float t,
versor dest) {
glmc_quat_slerp(versor q, versor r, float t, versor dest) {
glm_quat_slerp(q, r, t, dest);
}

18
test/src/test_common.c
View File

@@ -92,10 +92,18 @@ test_assert_vec3_eq(vec3 v1, vec3 v2) {
}
void
test_assert_quat_eq(versor v1, versor v2) {
assert_true(fabsf(v1[0] - v2[0]) <= 0.0009); /* rounding errors */
assert_true(fabsf(v1[1] - v2[1]) <= 0.0009);
assert_true(fabsf(v1[2] - v2[2]) <= 0.0009);
assert_true(fabsf(v1[3] - v2[3]) <= 0.0009);
test_assert_quat_eq_abs(versor v1, versor v2) {
assert_true(fabsf(fabsf(v1[0]) - fabsf(v2[0])) <= 0.0009); /* rounding errors */
assert_true(fabsf(fabsf(v1[1]) - fabsf(v2[1])) <= 0.0009);
assert_true(fabsf(fabsf(v1[2]) - fabsf(v2[2])) <= 0.0009);
assert_true(fabsf(fabsf(v1[3]) - fabsf(v2[3])) <= 0.0009);
}
void
test_assert_quat_eq(versor v1, versor v2) {
assert_true(fabsf(v1[0] - v2[0]) <= 0.0000009); /* rounding errors */
assert_true(fabsf(v1[1] - v2[1]) <= 0.0000009);
assert_true(fabsf(v1[2] - v2[2]) <= 0.0000009);
assert_true(fabsf(v1[3] - v2[3]) <= 0.0000009);
}

3
test/src/test_common.h
View File

@@ -37,6 +37,9 @@ test_assert_vec3_eq(vec3 v1, vec3 v2);
void
test_assert_quat_eq(versor v1, versor v2);
void
test_assert_quat_eq_abs(versor v1, versor v2);
void
test_rand_vec3(vec3 dest);

18
test/src/test_quat.c
View File

@@ -7,10 +7,19 @@
#include "test_common.h"
CGLM_INLINE
void
test_quat_mul_raw(versor p, versor q, versor dest) {
dest[0] = p[3] * q[0] + p[0] * q[3] + p[1] * q[2] - p[2] * q[1];
dest[1] = p[3] * q[1] - p[0] * q[2] + p[1] * q[3] + p[2] * q[0];
dest[2] = p[3] * q[2] + p[0] * q[1] - p[1] * q[0] + p[2] * q[3];
dest[3] = p[3] * q[3] - p[0] * q[0] - p[1] * q[1] - p[2] * q[2];
}
void
test_quat(void **state) {
mat4 inRot, outRot;
versor inQuat, outQuat;
versor inQuat, outQuat, q3, q4;
int i;
for (i = 0; i < 1000; i++) {
@@ -18,7 +27,12 @@ test_quat(void **state) {
glmc_quat_mat4(inQuat, inRot);
glmc_mat4_quat(inRot, outQuat);
glmc_quat_mat4(outQuat, outRot);
test_assert_quat_eq(inQuat, outQuat);
test_assert_quat_eq_abs(inQuat, outQuat);
test_assert_mat4_eq2(inRot, outRot, 0.000009); /* almost equal */
test_quat_mul_raw(inQuat, outQuat, q3);
glm_quat_mul_sse2(inQuat, outQuat, q4);
test_assert_quat_eq(q3, q4);
}
}