From c08ce6a5ee8750941821b28dcaa432159a058500 Mon Sep 17 00:00:00 2001
From: Recep Aslantas <info@recp.me>
Date: Wed, 12 Oct 2016 00:00:39 +0300
Subject: [PATCH] rigid-body matrix inverse

---
 include/cglm-affine-mat-sse2.h | 26 ++++++++++++++++++++++++++
 include/cglm-affine-mat.h      | 31 +++++++++++++++++++++++++++++++
 2 files changed, 57 insertions(+)

diff --git a/include/cglm-affine-mat-sse2.h b/include/cglm-affine-mat-sse2.h
index c3b1e1f..d384824 100644
--- a/include/cglm-affine-mat-sse2.h
+++ b/include/cglm-affine-mat-sse2.h
@@ -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 */
diff --git a/include/cglm-affine-mat.h b/include/cglm-affine-mat.h
index adb9f0d..00fcbcd 100644
--- a/include/cglm-affine-mat.h
+++ b/include/cglm-affine-mat.h
@@ -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 */