From d5456da69de6d74206a8513fc53db38c7dd4bd22 Mon Sep 17 00:00:00 2001
From: Paul Ouellette <oue.paul18@gmail.com>
Date: Thu, 7 Feb 2019 16:26:06 -0500
Subject: Use true pitch/yaw/roll rotations without loss of precision by
 pgimeno (#8019)

Store the rotation in the node as a 4x4 transformation matrix internally (through IDummyTransformationSceneNode), which allows more manipulations without losing precision or having gimbal lock issues.

Network rotation is still transmitted as Eulers, though, not as matrix. But it will stay this way in 5.0.
---
 src/util/numeric.cpp | 35 +++++++++++++++++++++++++++++++++++
 src/util/numeric.h   | 15 +++++++++++++++
 2 files changed, 50 insertions(+)

(limited to 'src/util')

diff --git a/src/util/numeric.cpp b/src/util/numeric.cpp
index a120e3207..bd298e94e 100644
--- a/src/util/numeric.cpp
+++ b/src/util/numeric.cpp
@@ -174,3 +174,38 @@ s16 adjustDist(s16 dist, float zoom_fov)
 	return std::round(dist * std::cbrt((1.0f - std::cos(threshold_fov)) /
 		(1.0f - std::cos(zoom_fov / 2.0f))));
 }
+
+void setPitchYawRollRad(core::matrix4 &m, const v3f &rot)
+{
+	f64 a1 = rot.Z, a2 = rot.X, a3 = rot.Y;
+	f64 c1 = cos(a1), s1 = sin(a1);
+	f64 c2 = cos(a2), s2 = sin(a2);
+	f64 c3 = cos(a3), s3 = sin(a3);
+	f32 *M = m.pointer();
+
+	M[0] = s1 * s2 * s3 + c1 * c3;
+	M[1] = s1 * c2;
+	M[2] = s1 * s2 * c3 - c1 * s3;
+
+	M[4] = c1 * s2 * s3 - s1 * c3;
+	M[5] = c1 * c2;
+	M[6] = c1 * s2 * c3 + s1 * s3;
+
+	M[8] = c2 * s3;
+	M[9] = -s2;
+	M[10] = c2 * c3;
+}
+
+v3f getPitchYawRollRad(const core::matrix4 &m)
+{
+	const f32 *M = m.pointer();
+
+	f64 a1 = atan2(M[1], M[5]);
+	f64 c2 = sqrt(M[10]*M[10] + M[8]*M[8]);
+	f32 a2 = atan2f(-M[9], c2);
+	f64 c1 = cos(a1);
+	f64 s1 = sin(a1);
+	f32 a3 = atan2f(s1*M[6] - c1*M[2], c1*M[0] - s1*M[4]);
+
+	return v3f(a2, a3, a1);
+}
diff --git a/src/util/numeric.h b/src/util/numeric.h
index bfdff84a0..6f82a18c1 100644
--- a/src/util/numeric.h
+++ b/src/util/numeric.h
@@ -24,6 +24,7 @@ with this program; if not, write to the Free Software Foundation, Inc.,
 #include "irr_v2d.h"
 #include "irr_v3d.h"
 #include "irr_aabb3d.h"
+#include <matrix4.h>
 
 #define rangelim(d, min, max) ((d) < (min) ? (min) : ((d) > (max) ? (max) : (d)))
 #define myfloor(x) ((x) < 0.0 ? (int)(x) - 1 : (int)(x))
@@ -417,3 +418,17 @@ inline void wrappedApproachShortest(T &current, const T target, const T stepsize
 		current = target;
 	}
 }
+
+void setPitchYawRollRad(core::matrix4 &m, const v3f &rot);
+
+inline void setPitchYawRoll(core::matrix4 &m, const v3f &rot)
+{
+	setPitchYawRollRad(m, rot * core::DEGTORAD64);
+}
+
+v3f getPitchYawRollRad(const core::matrix4 &m);
+
+inline v3f getPitchYawRoll(const core::matrix4 &m)
+{
+	return getPitchYawRollRad(m) * core::RADTODEG64;
+}
-- 
cgit v1.2.3