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.

1 files changed, 35 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);
+}