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

quaternions

Browse Source
This commit is contained in:
Recep Aslantas
2016-09-18 00:53:24 +03:00
parent 5a28f2ec60
commit 6c0cfe310a
4 changed files with 189 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
include/cglm-mat.h
View File

@@ -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) {

66
include/cglm-quat-simd.h Normal file
View File

@@ -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 */

137
include/cglm-quat.h
View File

@@ -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 <math.h>
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;
}

27
include/cglm-vec.h
View File

@@ -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) {