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

rigid-body matrix inverse

Browse Source
This commit is contained in:
Recep Aslantas
2016-10-12 00:00:39 +03:00
parent b7ed89fcf0
commit c08ce6a5ee
2 changed files with 57 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
include/cglm-affine-mat-sse2.h
View File

@@ -49,5 +49,31 @@ glm_affine_mul_sse2(mat4 m1, mat4 m2, mat4 dest) {
_mm_mul_ps(_mm_shuffle1_ps1(r, 3), l3))));
}
CGLM_INLINE
void
glm_affine_inv_tr_sse2(mat4 mat) {
__m128 r0, r1, r2, r3, x0;
r0 = _mm_load_ps(mat[0]);
r1 = _mm_load_ps(mat[1]);
r2 = _mm_load_ps(mat[2]);
r3 = _mm_load_ps(mat[3]);
x0 = _mm_add_ps(_mm_mul_ps(r0, _mm_shuffle1_ps(r3, 0, 0, 0, 0)),
_mm_mul_ps(r1, _mm_shuffle1_ps(r3, 1, 1, 1, 1)));
x0 = _mm_add_ps(x0, _mm_mul_ps(r2, _mm_shuffle1_ps(r3, 2, 2, 2, 2)));
x0 = _mm_xor_ps(x0, _mm_set1_ps(-0.f));
r3 = _mm_set_ps(1.0f, 0.0f, 0.0f, 0.0f);
x0 = _mm_add_ps(x0, r3);
_MM_TRANSPOSE4_PS(r0, r1, r2, r3);
_mm_store_ps(mat[0], r0);
_mm_store_ps(mat[1], r1);
_mm_store_ps(mat[2], r2);
_mm_store_ps(mat[3], x0);
}
#endif
#endif /* cglm_affine_mat_sse2_h */

31
include/cglm-affine-mat.h
View File

@@ -10,6 +10,7 @@
#include "cglm.h"
#include "cglm-mat.h"
#include "cglm-mat3.h"
#include "cglm-affine-mat-sse2.h"
#include "cglm-affine-mat-avx.h"
#include <assert.h>
@@ -59,4 +60,34 @@ glm_affine_mul(mat4 m1, mat4 m2, mat4 dest) {
#endif
}
/*!
* @brief inverse orthonormal rotation + translation matrix (ridig-body)
*
* @code
* X = | R T | X' = | R' -R'T |
* | 0 1 | | 0 1 |
* @endcode
*
* @param[in,out] mat matrix
*/
CGLM_INLINE
void
glm_affine_inv_tr(mat4 mat) {
#if defined( __SSE__ ) || defined( __SSE2__ )
glm_affine_inv_tr_sse2(mat);
#else
CGLM_ALIGN(16) mat3 r;
CGLM_ALIGN(16) vec3 t;
/* rotate */
glm_mat4_pick3t(mat, r);
glm_mat4_ins3(r, mat);
/* translate */
glm_mat3_mulv(r, mat[3], t);
glm_vec_flipsign(t);
glm_vec_dup(t, mat[3]);
#endif
}
#endif /* cglm_affine_mat_h */