Use true pitch/yaw/roll rotations without loss of precision by pgimeno (#8019)

[minetest.git] / src / util / numeric.h

diff --git a/src/util/numeric.h b/src/util/numeric.h
index 61370a3f4ba2f2d6ae2d288d50ff29a920420916..6f82a18c1b4d858019e29d8f5a9c0da064b6a3a1 100644 (file)
--- 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))
@@ -182,6 +183,23 @@ inline float wrapDegrees_0_360(float f)
 }
 
 
+/** Returns \p v3f wrapped to the range [0, 360]
+  */
+inline v3f wrapDegrees_0_360_v3f(v3f v)
+{
+       v3f value_v3f;
+       value_v3f.X = modulo360f(v.X);
+       value_v3f.Y = modulo360f(v.Y);
+       value_v3f.Z = modulo360f(v.Z);
+
+       // Now that values are wrapped, use to get values for certain ranges
+       value_v3f.X = value_v3f.X < 0 ? value_v3f.X + 360 : value_v3f.X;
+       value_v3f.Y = value_v3f.Y < 0 ? value_v3f.Y + 360 : value_v3f.Y;
+       value_v3f.Z = value_v3f.Z < 0 ? value_v3f.Z + 360 : value_v3f.Z;
+       return value_v3f;
+}
+
+
 /** Returns \p f wrapped to the range [-180, 180]
   */
 inline float wrapDegrees_180(float f)
@@ -243,6 +261,11 @@ inline s32 myround(f32 f)
        return (s32)(f < 0.f ? (f - 0.5f) : (f + 0.5f));
 }
 
+inline constexpr f32 sqr(f32 f)
+{
+       return f * f;
+}
+
 /*
        Returns integer position of node in given floating point position
 */
@@ -395,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;
+}