From 5344599ae4b00c685eae28b0a316067d3a8a6a2e Mon Sep 17 00:00:00 2001
From: Recep Aslantas <info@recp.me>
Date: Tue, 11 Oct 2016 00:15:57 +0300
Subject: [PATCH] pick mat3 from mat4

---
 include/cglm-mat-simd-sse2.h | 19 +++++++++++++++++++
 include/cglm-mat.h           | 26 ++++++++++++++++++++++++++
 2 files changed, 45 insertions(+)

diff --git a/include/cglm-mat-simd-sse2.h b/include/cglm-mat-simd-sse2.h
index 9080e5f..1eeac37 100644
--- a/include/cglm-mat-simd-sse2.h
+++ b/include/cglm-mat-simd-sse2.h
@@ -11,6 +11,25 @@
 
 #include "cglm-intrin.h"
 
+CGLM_INLINE
+void
+glm_mat4_pick3_sse2(mat4 mat, mat3 dest) {
+  __m128 x0, x1, x2, x3;
+
+  x0 = _mm_load_ps(mat[0]);
+  x1 = _mm_load_ps(mat[1]);
+  x2 = _mm_load_ps(mat[2]);
+
+  x3 = _mm_shuffle_ps(x0, x1, _MM_SHUFFLE(0, 0, 2, 2));
+  x0 = _mm_shuffle_ps(x0, x3, _MM_SHUFFLE(2, 0, 1, 0));
+  x1 = _mm_shuffle_ps(x1, x2, _MM_SHUFFLE(1, 0, 2, 1));
+
+  _mm_storeu_ps(&dest[0][0], x0);
+  _mm_storeu_ps(&dest[1][1], x1);
+
+  dest[2][2] = mat[2][2];
+}
+
 CGLM_INLINE
 void
 glm_mat4_scale_sse2(mat4 m, float s){
diff --git a/include/cglm-mat.h b/include/cglm-mat.h
index ccc164c..9ba3aa9 100644
--- a/include/cglm-mat.h
+++ b/include/cglm-mat.h
@@ -63,6 +63,32 @@ glm_mat4_dup(mat4 mat, mat4 dest) {
 #endif
 }
 
+/*!
+ * @brief copy top-left of mat4 to mat3
+ *
+ * @param[in]  mat  source
+ * @param[out] dest destination
+ */
+CGLM_INLINE
+void
+glm_mat4_pick3(mat4 mat, mat3 dest) {
+#if defined( __SSE__ ) || defined( __SSE2__ )
+  glm_mat4_pick3_sse2(mat, dest);
+#else
+  dest[0][0] = mat[0][0];
+  dest[0][1] = mat[0][1];
+  dest[0][2] = mat[0][2];
+
+  dest[1][0] = mat[1][0];
+  dest[1][1] = mat[1][1];
+  dest[1][2] = mat[1][2];
+
+  dest[2][0] = mat[2][0];
+  dest[2][1] = mat[2][1];
+  dest[2][2] = mat[2][2];
+#endif
+}
+
 /*!
  * @brief multiply m1 and m2 to dest
  *