From d6395d4fb8dbfb90864ab8a6fd3456165236a956 Mon Sep 17 00:00:00 2001
From: Recep Aslantas <info@recp.me>
Date: Fri, 13 Apr 2018 22:33:32 +0300
Subject: [PATCH] vec: optimize rotate vector using matrix

* add mat3 version
---
 docs/source/vec3.rst | 10 +++++++++
 include/cglm/vec3.h  | 50 +++++++++++++++++++++++++++++++++++++-------
 test/src/test_vec3.c | 12 +++++++++++
 3 files changed, 64 insertions(+), 8 deletions(-)

diff --git a/docs/source/vec3.rst b/docs/source/vec3.rst
index 77aebb6..fcbfbfb 100644
--- a/docs/source/vec3.rst
+++ b/docs/source/vec3.rst
@@ -60,6 +60,7 @@ Functions:
 #. :c:func:`glm_vec_angle`
 #. :c:func:`glm_vec_rotate`
 #. :c:func:`glm_vec_rotate_m4`
+#. :c:func:`glm_vec_rotate_m3`
 #. :c:func:`glm_vec_proj`
 #. :c:func:`glm_vec_center`
 #. :c:func:`glm_vec_maxv`
@@ -338,6 +339,15 @@ Functions documentation
       | *[in]*  **v**     vector
       | *[out]* **dest**  rotated vector
 
+.. c:function:: void  glm_vec_rotate_m3(mat3 m, vec3 v, vec3 dest)
+
+    apply rotation matrix to vector
+
+    Parameters:
+      | *[in]*  **m**     affine matrix or rot matrix
+      | *[in]*  **v**     vector
+      | *[out]* **dest**  rotated vector
+
 .. c:function:: void  glm_vec_proj(vec3 a, vec3 b, vec3 dest)
 
     project a vector onto b vector
diff --git a/include/cglm/vec3.h b/include/cglm/vec3.h
index 9add4c3..9976ad9 100644
--- a/include/cglm/vec3.h
+++ b/include/cglm/vec3.h
@@ -67,6 +67,7 @@
 #define cglm_vec3_h
 
 #include "common.h"
+#include "vec4.h"
 #include "vec3-ext.h"
 #include "util.h"
 
@@ -551,6 +552,12 @@ glm_vec_rotate(vec3 v, float angle, vec3 axis) {
 /*!
  * @brief apply rotation matrix to vector
  *
+ *  matrix format should be (no perspective):
+ *   a  b  c  x
+ *   e  f  g  y
+ *   i  j  k  z
+ *   0  0  0  w
+ *
  * @param[in]  m    affine matrix or rot matrix
  * @param[in]  v    vector
  * @param[out] dest rotated vector
@@ -558,17 +565,44 @@ glm_vec_rotate(vec3 v, float angle, vec3 axis) {
 CGLM_INLINE
 void
 glm_vec_rotate_m4(mat4 m, vec3 v, vec3 dest) {
-  vec3 res, x, y, z;
+  vec4 x, y, z, res;
 
-  glm_vec_normalize_to(m[0], x);
-  glm_vec_normalize_to(m[1], y);
-  glm_vec_normalize_to(m[2], z);
+  glm_vec4_normalize_to(m[0], x);
+  glm_vec4_normalize_to(m[1], y);
+  glm_vec4_normalize_to(m[2], z);
 
-  res[0] = x[0] * v[0] + y[0] * v[1] + z[0] * v[2];
-  res[1] = x[1] * v[0] + y[1] * v[1] + z[1] * v[2];
-  res[2] = x[2] * v[0] + y[2] * v[1] + z[2] * v[2];
+  glm_vec4_scale(x,   v[0], res);
+  glm_vec4_muladds(y, v[1], res);
+  glm_vec4_muladds(y, v[2], res);
 
-  glm_vec_copy(res, dest);
+  glm_vec3(res, dest);
+}
+
+/*!
+ * @brief apply rotation matrix to vector
+ *
+ * @param[in]  m    affine matrix or rot matrix
+ * @param[in]  v    vector
+ * @param[out] dest rotated vector
+ */
+CGLM_INLINE
+void
+glm_vec_rotate_m3(mat3 m, vec3 v, vec3 dest) {
+  vec4 res, x, y, z;
+
+  glm_vec4(m[0], 0.0f, x);
+  glm_vec4(m[1], 0.0f, y);
+  glm_vec4(m[2], 0.0f, z);
+
+  glm_vec4_normalize(x);
+  glm_vec4_normalize(y);
+  glm_vec4_normalize(z);
+
+  glm_vec4_scale(x,   v[0], res);
+  glm_vec4_muladds(y, v[1], res);
+  glm_vec4_muladds(y, v[2], res);
+
+  glm_vec3(res, dest);
 }
 
 /*!
diff --git a/test/src/test_vec3.c b/test/src/test_vec3.c
index 7c85d1f..dd0c55d 100644
--- a/test/src/test_vec3.c
+++ b/test/src/test_vec3.c
@@ -9,6 +9,8 @@
 
 void
 test_vec3(void **state) {
+  mat3 rot1m3;
+  mat4 rot1;
   vec3 v, v1, v2;
 
   /* test zero */
@@ -63,4 +65,14 @@ test_vec3(void **state) {
   glm_vec_broadcast(3, v2);
   glm_vec_muladd(v1, v2, v);
   assert_true(glmc_vec_eq_eps(v, 10));
+
+  /* rotate */
+  glm_vec_copy(GLM_YUP, v);
+  glm_rotate_make(rot1, glm_rad(90), GLM_XUP);
+  glm_vec_rotate_m4(rot1, v, v1);
+  glm_mat4_pick3(rot1, rot1m3);
+  glm_vec_rotate_m3(rot1m3, v, v2);
+
+  test_assert_vec3_eq(v1, v2);
+  test_assert_vec3_eq(v1, GLM_ZUP);
 }