irrMath.h

Go to the documentation of this file.

// Copyright (C) 2002-2012 Nikolaus Gebhardt
// This file is part of the "Irrlicht Engine".
// For conditions of distribution and use, see copyright notice in irrlicht.h
 
#ifndef __IRR_MATH_H_INCLUDED__
#define __IRR_MATH_H_INCLUDED__
 
#include "SagaConfig.h"
#include <cmath>
#include <cfloat>
#include <cstdlib> // for abs() etc.
#include <climits> // For INT_MAX / UINT_MAX
#include <cstdint>
#include <glm/vec3.hpp>
#include <glm/gtx/norm.hpp>
 
namespace saga
{
namespace core
{
 
 
    const std::int32_t ROUNDING_ERROR_S32 = 0;
 
#ifdef __IRR_HAS_S64
    const std::int64_t ROUNDING_ERROR_S64 = 0;
#endif
    const float ROUNDING_ERROR_float = 0.000001f;
    const double ROUNDING_ERROR_double = 0.00000001;
 
#ifdef PI // make sure we don't collide with a define
#undef PI
#endif
    const float PI = 3.14159265359f;
 
    const float RECIPROCAL_PI = 1.0f/PI;
 
    const float HALF_PI = PI/2.0f;
 
#ifdef PI64 // make sure we don't collide with a define
#undef PI64
#endif
    const double PI64 = 3.1415926535897932384626433832795028841971693993751;
 
    const double RECIPROCAL_PI64 = 1.0/PI64;
 
    const float DEGTORAD = PI / 180.0f;
 
    const float RADTODEG   = 180.0f / PI;
 
    const double DEGTORAD64 = PI64 / 180.0;
 
    const double RADTODEG64 = 180.0 / PI64;
 
    inline bool isBetweenPoints(const glm::vec3& point, const glm::vec3& begin, const glm::vec3& end)
    {
        const auto f = glm::length2(end - begin);
        return glm::distance2(point, begin) <= f && glm::distance2(point, end) <= f;
    }
 
    inline glm::vec3 getHorizontalAngle(const glm::vec3& vec)
    {
        glm::vec3 angle;
 
        const double tmp = (atan2((double)vec.x, (double)vec.z) * RADTODEG64);
        angle.y = tmp;
 
        if (angle.y < 0)
            angle.y += 360;
        if (angle.y >= 360)
            angle.y -= 360;
 
        const double z1 = glm::sqrt(vec.x*vec.x + vec.z*vec.z);
 
        angle.x = (atan2((double)z1, (double)vec.y) * RADTODEG64 - 90.0);
 
        if (angle.x < 0)
            angle.x += 360;
        if (angle.x >= 360)
            angle.x -= 360;
 
        return angle;
    }
 
    inline glm::vec3 rotationToDirection(const glm::vec3& vec, const glm::vec3& forwards = {0, 0, 1})
    {
        const double cr = cos( core::DEGTORAD64 * vec.x );
        const double sr = sin( core::DEGTORAD64 * vec.x );
        const double cp = cos( core::DEGTORAD64 * vec.y );
        const double sp = sin( core::DEGTORAD64 * vec.y );
        const double cy = cos( core::DEGTORAD64 * vec.z );
        const double sy = sin( core::DEGTORAD64 * vec.z );
 
        const double srsp = sr*sp;
        const double crsp = cr*sp;
 
        const double pseudoMatrix[] = {
            ( cp*cy ), ( cp*sy ), ( -sp ),
            ( srsp*cy-cr*sy ), ( srsp*sy+cr*cy ), ( sr*cp ),
            ( crsp*cy+sr*sy ), ( crsp*sy-sr*cy ), ( cr*cp )};
 
        return {
            (forwards.x * pseudoMatrix[0] +
                forwards.y * pseudoMatrix[3] +
                forwards.z * pseudoMatrix[6]),
            (forwards.x * pseudoMatrix[1] +
                forwards.y * pseudoMatrix[4] +
                forwards.z * pseudoMatrix[7]),
            (forwards.x * pseudoMatrix[2] +
                forwards.y * pseudoMatrix[5] +
                forwards.z * pseudoMatrix[8])
        };
    }
 
 
    inline float radToDeg(float radians)
    {
        return RADTODEG * radians;
    }
 
 
    inline double radToDeg(double radians)
    {
        return RADTODEG64 * radians;
    }
 
 
    inline float degToRad(float degrees)
    {
        return DEGTORAD * degrees;
    }
 
 
    inline double degToRad(double degrees)
    {
        return DEGTORAD64 * degrees;
    }
 
    template<class T>
    inline const T& min_(const T& a, const T& b)
    {
        return a < b ? a : b;
    }
 
    template<class T>
    inline const T& min_(const T& a, const T& b, const T& c)
    {
        return a < b ? min_(a, c) : min_(b, c);
    }
 
    template<class T>
    inline const T& max_(const T& a, const T& b)
    {
        return a < b ? b : a;
    }
 
    template<class T>
    inline const T& max_(const T& a, const T& b, const T& c)
    {
        return a < b ? max_(b, c) : max_(a, c);
    }
 
    template<class T>
    inline T abs_(const T& a)
    {
        return a < (T)0 ? -a : a;
    }
 
    template<class T>
    inline T lerp(const T& a, const T& b, const float t)
    {
        return (T)(a*(1.f-t)) + (b*t);
    }
 
    template <class T>
    inline const T clamp (const T& value, const T& low, const T& high)
    {
        return min_ (max_(value,low), high);
    }
 
    // Note: We use the same trick as boost and use two template arguments to
    // avoid ambiguity when swapping objects of an Irrlicht type that has not
    // it's own swap overload. Otherwise we get conflicts with some compilers
    // in combination with stl.
    template <class T1, class T2>
    inline void swap(T1& a, T2& b)
    {
        T1 c(a);
        a = b;
        b = c;
    }
 
    template <class T>
    inline T roundingError();
 
    template <>
    inline float roundingError()
    {
        return ROUNDING_ERROR_float;
    }
 
    template <>
    inline double roundingError()
    {
        return ROUNDING_ERROR_double;
    }
 
    template <>
    inline std::int32_t roundingError()
    {
        return ROUNDING_ERROR_S32;
    }
 
    template <>
    inline std::uint32_t roundingError()
    {
        return ROUNDING_ERROR_S32;
    }
 
#ifdef __IRR_HAS_S64
    template <>
    inline std::int64_t roundingError()
    {
        return ROUNDING_ERROR_S64;
    }
 
    template <>
    inline std::uint64_t roundingError()
    {
        return ROUNDING_ERROR_S64;
    }
#endif
 
    template <class T>
    inline T relativeErrorFactor()
    {
        return 1;
    }
 
    template <>
    inline float relativeErrorFactor()
    {
        return 4;
    }
 
    template <>
    inline double relativeErrorFactor()
    {
        return 8;
    }
 
    template <class T>
    inline bool equals(const T a, const T b, const T tolerance = roundingError<T>())
    {
        return (a + tolerance >= b) && (a - tolerance <= b);
    }
 
 
    template <class T>
    inline bool equalsRelative(const T a, const T b, const T factor = relativeErrorFactor<T>())
    {
        //https://eagergames.wordpress.com/2017/04/01/fast-parallel-lines-and-vectors-test/
 
        const T maxi = max_(a, b);
        const T mini = min_(a, b);
        const T maxMagnitude = max_(maxi, -mini);
 
        return  (maxMagnitude*factor + maxi) == (maxMagnitude*factor + mini); // MAD Wise
    }
 
    union FloatIntUnion32
    {
        FloatIntUnion32(float f1 = 0.0f) : f(f1) {}
        // Portable sign-extraction
        bool sign() const { return (i >> 31) != 0; }
 
        std::int32_t i;
        float f;
    };
 
    //\result true when numbers have a ULP <= maxUlpDiff AND have the same sign.
    inline bool equalsByUlp(float a, float b, int maxUlpDiff)
    {
        // Based on the ideas and code from Bruce Dawson on
        // http://www.altdevblogaday.com/2012/02/22/comparing-floating-point-numbers-2012-edition/
        // When floats are interpreted as integers the two nearest possible float numbers differ just
        // by one integer number. Also works the other way round, an integer of 1 interpreted as float
        // is for example the smallest possible float number.
 
        FloatIntUnion32 fa(a);
        FloatIntUnion32 fb(b);
 
        // Different signs, we could maybe get difference to 0, but so close to 0 using epsilons is better.
        if (fa.sign() != fb.sign())
        {
            // Check for equality to make sure +0==-0
            if (fa.i == fb.i)
                return true;
            return false;
        }
 
        // Find the difference in ULPs.
        int ulpsDiff = abs_(fa.i- fb.i);
        if (ulpsDiff <= maxUlpDiff)
            return true;
 
        return false;
    }
 
    inline bool iszero(const double a, const double tolerance = ROUNDING_ERROR_double)
    {
        return fabs(a) <= tolerance;
    }
 
    inline bool iszero(const float a, const float tolerance = ROUNDING_ERROR_float)
    {
        return fabsf(a) <= tolerance;
    }
 
    inline bool isnotzero(const float a, const float tolerance = ROUNDING_ERROR_float)
    {
        return fabsf(a) > tolerance;
    }
 
    inline bool iszero(const std::int32_t a, const std::int32_t tolerance = 0)
    {
        return (a & 0x7ffffff) <= tolerance;
    }
 
    inline bool iszero(const std::uint32_t a, const std::uint32_t tolerance = 0)
    {
        return a <= tolerance;
    }
 
#ifdef __IRR_HAS_S64
    inline bool iszero(const std::int64_t a, const std::int64_t tolerance = 0)
    {
        return abs_(a) <= tolerance;
    }
#endif
 
    inline std::int32_t s32_min(std::int32_t a, std::int32_t b)
    {
        const std::int32_t mask = (a - b) >> 31;
        return (a & mask) | (b & ~mask);
    }
 
    inline std::int32_t s32_max(std::int32_t a, std::int32_t b)
    {
        const std::int32_t mask = (a - b) >> 31;
        return (b & mask) | (a & ~mask);
    }
 
    inline std::int32_t s32_clamp(std::int32_t value, std::int32_t low, std::int32_t high)
    {
        return s32_min(s32_max(value,low), high);
    }
 
    /*
        float IEEE-754 bit representation
 
        0      0x00000000
        1.0    0x3f800000
        0.5    0x3f000000
        3      0x40400000
        +inf   0x7f800000
        -inf   0xff800000
        +NaN   0x7fc00000 or 0x7ff00000
        in general: number = (sign ? -1:1) * 2^(exponent) * 1.(mantissa bits)
    */
 
    typedef union { std::uint32_t u; std::int32_t s; float f; } inttofloat;
 
    #define F32_AS_S32(f)    (*((std::int32_t *) &(f)))
    #define F32_AS_U32(f)    (*((std::uint32_t *) &(f)))
    #define F32_AS_U32_POINTER(f)  (((std::uint32_t *) &(f)))
 
    #define F32_VALUE_0    0x00000000
    #define F32_VALUE_1    0x3f800000
    #define F32_SIGN_BIT    0x80000000U
    #define F32_EXPON_MANTISSA  0x7FFFFFFFU
 
#ifdef IRRLICHT_FAST_MATH
    #define IR(x)      ((std::uint32_t&)(x))
#else
    inline std::uint32_t IR(float x) {inttofloat tmp; tmp.f=x; return tmp.u;}
#endif
 
    #define AIR(x)      (IR(x)&0x7fffffff)
 
#ifdef IRRLICHT_FAST_MATH
    #define FR(x)      ((float&)(x))
#else
    inline float FR(std::uint32_t x) {inttofloat tmp; tmp.u=x; return tmp.f;}
    inline float FR(std::int32_t x) {inttofloat tmp; tmp.s=x; return tmp.f;}
#endif
 
    #define IEEE_1_0    0x3f800000
    #define IEEE_255_0    0x437f0000
 
#ifdef IRRLICHT_FAST_MATH
    #define  F32_LOWER_0(f)    (F32_AS_U32(f) >  F32_SIGN_BIT)
    #define  F32_LOWER_EQUAL_0(f)  (F32_AS_S32(f) <= F32_VALUE_0)
    #define  F32_GREATER_0(f)  (F32_AS_S32(f) >  F32_VALUE_0)
    #define  F32_GREATER_EQUAL_0(f)  (F32_AS_U32(f) <= F32_SIGN_BIT)
    #define  F32_EQUAL_1(f)    (F32_AS_U32(f) == F32_VALUE_1)
    #define  F32_EQUAL_0(f)    ((F32_AS_U32(f) & F32_EXPON_MANTISSA) == F32_VALUE_0)
 
    // only same sign
    #define  F32_A_GREATER_B(a,b)  (F32_AS_S32((a)) > F32_AS_S32((b)))
 
#else
 
    #define  F32_LOWER_0(n)    ((n) <  0.0f)
    #define  F32_LOWER_EQUAL_0(n)  ((n) <= 0.0f)
    #define  F32_GREATER_0(n)  ((n) >  0.0f)
    #define  F32_GREATER_EQUAL_0(n)  ((n) >= 0.0f)
    #define  F32_EQUAL_1(n)    ((n) == 1.0f)
    #define  F32_EQUAL_0(n)    ((n) == 0.0f)
    #define  F32_A_GREATER_B(a,b)  ((a) > (b))
#endif
 
 
#ifndef REALINLINE
    #ifdef _MSC_VER
        #define REALINLINE __forceinline
    #else
        #define REALINLINE inline
    #endif
#endif
 
#if defined(__BORLANDC__) || defined (__BCPLUSPLUS__)
 
    // 8-bit bools in Borland builder
 
    REALINLINE std::uint32_t if_c_a_else_b (const char condition, const std::uint32_t a, const std::uint32_t b)
    {
        return ((-condition >> 7) & (a ^ b)) ^ b;
    }
 
    REALINLINE std::uint32_t if_c_a_else_0 (const char condition, const std::uint32_t a)
    {
        return (-condition >> 31) & a;
    }
#else
 
    REALINLINE std::uint32_t if_c_a_else_b (const std::int32_t condition, const std::uint32_t a, const std::uint32_t b)
    {
        return ((-condition >> 31) & (a ^ b)) ^ b;
    }
 
    REALINLINE std::uint16_t if_c_a_else_b (const std::int16_t condition, const std::uint16_t a, const std::uint16_t b)
    {
        return ((-condition >> 15) & (a ^ b)) ^ b;
    }
 
    REALINLINE std::uint32_t if_c_a_else_0 (const std::int32_t condition, const std::uint32_t a)
    {
        return (-condition >> 31) & a;
    }
#endif
 
    /*
        if (condition) state |= m; else state &= ~m;
    */
    REALINLINE void setbit_cond (std::uint32_t &state, std::int32_t condition, std::uint32_t mask)
    {
        // 0, or any positive to mask
        //std::int32_t conmask = -condition >> 31;
        state ^= ((-condition >> 31) ^ state) & mask;
    }
 
    inline float round_(float x)
    {
        return floorf(x + 0.5f);
    }
 
    REALINLINE void clearFPUException ()
    {
#ifdef IRRLICHT_FAST_MATH
        return;
    #ifdef feclearexcept
        feclearexcept(FE_ALL_EXCEPT);
    #elif defined(_MSC_VER)
        __asm fnclex;
    #elif defined(__GNUC__) && defined(__x86__)
        __asm__ __volatile__ ("fclex \n\t");
    #endif
#endif
    }
 
    // calculate: sqrt (x)
    REALINLINE float squareroot(const float f)
    {
        return sqrtf(f);
    }
 
    // calculate: sqrt (x)
    REALINLINE double squareroot(const double f)
    {
        return sqrt(f);
    }
 
    // calculate: sqrt (x)
    REALINLINE std::int32_t squareroot(const std::int32_t f)
    {
        return static_cast<std::int32_t>(squareroot(static_cast<float>(f)));
    }
 
#ifdef __IRR_HAS_S64
    // calculate: sqrt (x)
    REALINLINE std::int64_t squareroot(const std::int64_t f)
    {
        return static_cast<std::int64_t>(squareroot(static_cast<double>(f)));
    }
#endif
 
    // calculate: 1 / sqrt (x)
    REALINLINE double reciprocal_squareroot(const double x)
    {
        return 1.0 / sqrt(x);
    }
 
    // calculate: 1 / sqrtf (x)
    REALINLINE float reciprocal_squareroot(const float f)
    {
#if defined (IRRLICHT_FAST_MATH)
    #if defined(_MSC_VER)
        // SSE reciprocal square root estimate, accurate to 12 significant
        // bits of the mantissa
        float recsqrt;
        __asm rsqrtss xmm0, f           // xmm0 = rsqrtss(f)
        __asm movss recsqrt, xmm0       // return xmm0
        return recsqrt;
 
/*
        // comes from Nvidia
        std::uint32_t tmp = (std::uint32_t(IEEE_1_0 << 1) + IEEE_1_0 - *(std::uint32_t*)&x) >> 1;
        float y = *(float*)&tmp;
        return y * (1.47f - 0.47f * x * y * y);
*/
    #else
        return 1.f / sqrtf(f);
    #endif
#else // no fast math
        return 1.f / sqrtf(f);
#endif
    }
 
    // calculate: 1 / sqrtf(x)
    REALINLINE std::int32_t reciprocal_squareroot(const std::int32_t x)
    {
        return static_cast<std::int32_t>(reciprocal_squareroot(static_cast<float>(x)));
    }
 
    // calculate: 1 / x
    REALINLINE float reciprocal(const float f)
    {
#if defined (IRRLICHT_FAST_MATH)
 
        // SSE Newton-Raphson reciprocal estimate, accurate to 23 significant
        // bi ts of the mantissa
        // One Newton-Raphson Iteration:
        // f(i+1) = 2 * rcpss(f) - f * rcpss(f) * rcpss(f)
#if defined(_MSC_VER)
        float rec;
        __asm rcpss xmm0, f               // xmm0 = rcpss(f)
        __asm movss xmm1, f               // xmm1 = f
        __asm mulss xmm1, xmm0            // xmm1 = f * rcpss(f)
        __asm mulss xmm1, xmm0            // xmm2 = f * rcpss(f) * rcpss(f)
        __asm addss xmm0, xmm0            // xmm0 = 2 * rcpss(f)
        __asm subss xmm0, xmm1            // xmm0 = 2 * rcpss(f)
                                          //        - f * rcpss(f) * rcpss(f)
        __asm movss rec, xmm0             // return xmm0
        return rec;
#else // no support yet for other compilers
        return 1.f / f;
#endif
        // instead set f to a high value to get a return value near zero..
        // -1000000000000.f.. is use minus to stay negative..
        // must test's here (plane.normal dot anything) checks on <= 0.f
        //std::uint32_t x = (-(AIR(f) != 0) >> 31) & (IR(f) ^ 0xd368d4a5) ^ 0xd368d4a5;
        //return 1.f / FR (x);
 
#else // no fast math
        return 1.f / f;
#endif
    }
 
    // calculate: 1 / x
    REALINLINE double reciprocal (const double f)
    {
        return 1.0 / f;
    }
 
 
    // calculate: 1 / x, low precision allowed
    REALINLINE float reciprocal_approxim (const float f)
    {
#if defined(IRRLICHT_FAST_MATH)
 
        // SSE Newton-Raphson reciprocal estimate, accurate to 23 significant
        // bi ts of the mantissa
        // One Newton-Raphson Iteration:
        // f(i+1) = 2 * rcpss(f) - f * rcpss(f) * rcpss(f)
#if defined(_MSC_VER)
        float rec;
        __asm rcpss xmm0, f               // xmm0 = rcpss(f)
        __asm movss xmm1, f               // xmm1 = f
        __asm mulss xmm1, xmm0            // xmm1 = f * rcpss(f)
        __asm mulss xmm1, xmm0            // xmm2 = f * rcpss(f) * rcpss(f)
        __asm addss xmm0, xmm0            // xmm0 = 2 * rcpss(f)
        __asm subss xmm0, xmm1            // xmm0 = 2 * rcpss(f)
                                          //        - f * rcpss(f) * rcpss(f)
        __asm movss rec, xmm0             // return xmm0
        return rec;
#else // no support yet for other compilers
        return 1.f / f;
#endif
 
/*
        // SSE reciprocal estimate, accurate to 12 significant bits of
        float rec;
        __asm rcpss xmm0, f             // xmm0 = rcpss(f)
        __asm movss rec , xmm0          // return xmm0
        return rec;
*/
/*
        register std::uint32_t x = 0x7F000000 - IR (p);
        const float r = FR (x);
        return r * (2.0f - p * r);
*/
#else // no fast math
        return 1.f / f;
#endif
    }
 
 
    REALINLINE std::int32_t floor32(float x)
    {
#ifdef IRRLICHT_FAST_MATH
        const float h = 0.5f;
 
        std::int32_t t;
 
#if defined(_MSC_VER)
        __asm
        {
            fld  x
            fsub  h
            fistp  t
        }
#elif defined(__GNUC__)
        __asm__ __volatile__ (
            "fsub %2 \n\t"
            "fistpl %0"
            : "=m" (t)
            : "t" (x), "f" (h)
            : "st"
           );
#else
        return (std::int32_t) floorf (x);
#endif
        return t;
#else // no fast math
        return (std::int32_t) floorf (x);
#endif
    }
 
 
    REALINLINE std::int32_t ceil32 (float x)
    {
#ifdef IRRLICHT_FAST_MATH
        const float h = 0.5f;
 
        std::int32_t t;
 
#if defined(_MSC_VER)
        __asm
        {
            fld  x
            fadd  h
            fistp  t
        }
#elif defined(__GNUC__)
        __asm__ __volatile__ (
            "fadd %2 \n\t"
            "fistpl %0 \n\t"
            : "=m"(t)
            : "t"(x), "f"(h)
            : "st"
           );
#else
        return (std::int32_t) ceilf (x);
#endif
        return t;
#else // not fast math
        return (std::int32_t) ceilf (x);
#endif
    }
 
 
 
    REALINLINE std::int32_t round32(float x)
    {
#if defined(IRRLICHT_FAST_MATH)
        std::int32_t t;
 
#if defined(_MSC_VER)
        __asm
        {
            fld   x
            fistp t
        }
#elif defined(__GNUC__)
        __asm__ __volatile__ (
            "fistpl %0 \n\t"
            : "=m"(t)
            : "t"(x)
            : "st"
           );
#else
        return (std::int32_t) round_(x);
#endif
        return t;
#else // no fast math
        return (std::int32_t) round_(x);
#endif
    }
 
    inline float float_max3(const float a, const float b, const float c)
    {
        return a > b ? (a > c ? a : c) : (b > c ? b : c);
    }
 
    inline float float_min3(const float a, const float b, const float c)
    {
        return a < b ? (a < c ? a : c) : (b < c ? b : c);
    }
 
    inline float fract (float x)
    {
        return x - floorf (x);
    }
 
} // namespace core
} // namespace saga
 
#ifndef IRRLICHT_FAST_MATH
    using saga::core::IR;
    using saga::core::FR;
#endif
 
#endif