diff options
Diffstat (limited to 'src/util')
| -rw-r--r-- | src/util/numeric.cpp | 35 | ||||
| -rw-r--r-- | src/util/numeric.h | 15 |
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 ¤t, 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; +} |