From 655c0816bab6c37255fec198e818ba8bdec83b54 Mon Sep 17 00:00:00 2001
From: xiejiangzhi <xiejiangzhi@gmail.com>
Date: Wed, 22 Nov 2023 15:49:30 +0800
Subject: [PATCH] Add quat:ToEuler and quat:fromEuler

---
 src/api/l_math_vectors.c | 22 ++++++++++++++++++++++
 src/core/maf.h           | 30 ++++++++++++++++++++++++++++++
 2 files changed, 52 insertions(+)

diff --git a/src/api/l_math_vectors.c b/src/api/l_math_vectors.c
index 4565006bb..18a3596a0 100644
--- a/src/api/l_math_vectors.c
+++ b/src/api/l_math_vectors.c
@@ -1453,6 +1453,26 @@ static int l_lovrQuatSlerp(lua_State* L) {
   return 1;
 }
 
+static int l_lovrQuatToEuler(lua_State* L) {
+  quat q = luax_checkvector(L, 1, V_QUAT, NULL);
+  float roll, pitch, yaw;
+  quat_toEuler(q, &roll, &pitch, &yaw);
+  lua_pushnumber(L, roll);
+  lua_pushnumber(L, pitch);
+  lua_pushnumber(L, yaw);
+  return 3;
+}
+
+static int l_lovrQuatFromEuler(lua_State* L) {
+  quat q = luax_checkvector(L, 1, V_QUAT, NULL);
+  float roll = luax_checkfloat(L, 2);
+  float pitch = luax_checkfloat(L, 3);
+  float yaw = luax_checkfloat(L, 4);
+  quat_fromEuler(q, roll, pitch, yaw);
+  lua_settop(L, 1);
+  return 1;
+}
+
 static int l_lovrQuat__mul(lua_State* L) {
   quat q = luax_checkvector(L, 1, V_QUAT, NULL);
   VectorType type;
@@ -1574,6 +1594,8 @@ const luaL_Reg lovrQuat[] = {
   { "direction", l_lovrQuatDirection },
   { "conjugate", l_lovrQuatConjugate },
   { "slerp", l_lovrQuatSlerp },
+  { "toEuler", l_lovrQuatToEuler },
+  { "fromEuler", l_lovrQuatFromEuler },
   { "__mul", l_lovrQuat__mul },
   { "__len", l_lovrQuat__len },
   { "__tostring", l_lovrQuat__tostring },
diff --git a/src/core/maf.h b/src/core/maf.h
index 0949d4eee..730130a20 100644
--- a/src/core/maf.h
+++ b/src/core/maf.h
@@ -494,6 +494,36 @@ MAF quat quat_between(quat q, vec3 u, vec3 v) {
   return quat_normalize(q);
 }
 
+MAF void quat_toEuler(quat q, float* roll, float *pitch, float* yaw) {
+  double sinr_cosp = 2.0 * (q[3] * q[0] + q[1] * q[2]);
+  double cosr_cosp = 1.0 - 2.0 * (q[0] * q[0] + q[1] * q[1]);
+  *roll = atan2(sinr_cosp, cosr_cosp);
+
+  double sinp = sqrt(1 + 2 * (q[3] * q[1] - q[0] * q[2]));
+  double cosp = sqrt(1 - 2 * (q[3] * q[1] - q[0] * q[2]));
+  *pitch = 2.0 * atan2(sinp, cosp) - M_PI / 2.0;
+
+  double siny_cosp = +2.0 * (q[3] * q[2] + q[0] * q[1]);
+  double cosy_cosp = +1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]);
+  *yaw = atan2(siny_cosp, cosy_cosp);
+}
+
+MAF quat quat_fromEuler(quat q, float roll, float pitch, float yaw) {
+  double cy = cos(yaw * 0.5);
+  double sy = sin(yaw * 0.5);
+  double cp = cos(pitch * 0.5);
+  double sp = sin(pitch * 0.5);
+  double cr = cos(roll * 0.5);
+  double sr = sin(roll * 0.5);
+
+  return quat_set(q,
+    cy * sr * cp - sy * cr * sp,
+    cy * cr * sp + sy * sr * cp,
+    sy * cr * cp - cy * sr * sp,
+    cy * cr * cp + sy * sr * sp
+  );
+}
+
 // mat4
 
 #define MAT4_IDENTITY { 1.f, 0.f, 0.f, 0.f,  0.f, 1.f, 0.f, 0.f,  0.f, 0.f, 1.f, 0.f,  0.f, 0.f, 0.f, 1.f }