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.

2 files changed, 50 insertions, 0 deletions

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;
+}