irrMath.hpp

// Copyright (C) 2002-2012 Nikolaus Gebhardt
// This file is part of the "Irrlicht Engine".
// For conditions of distribution and use, see copyright notice in duckcpp/duckcpp.hpp
 
#ifndef DCPP_MATH_HPP_INCLUDED
#define DCPP_MATH_HPP_INCLUDED
 
#include <duckcpp/core/engine/DcppCompileConfig.hpp>
#include <duckcpp/core/engine/dcpp_types.hpp>
#include <math.h>
 
#if defined(_DCPP_SOLARIS_PLATFORM_) || defined(__BORLANDC__) || defined (__BCPLUSPLUS__) || defined (_WIN32_WCE)
    #define sqrtf(X) (dcpp::float32_kt)sqrt((dcpp::float64_kt)(X))
    #define sinf(X) (dcpp::float32_kt)sin((dcpp::float64_kt)(X))
    #define cosf(X) (dcpp::float32_kt)cos((dcpp::float64_kt)(X))
    #define asinf(X) (dcpp::float32_kt)asin((dcpp::float64_kt)(X))
    #define acosf(X) (dcpp::float32_kt)acos((dcpp::float64_kt)(X))
    #define atan2f(X,Y) (dcpp::float32_kt)atan2((dcpp::float64_kt)(X),(dcpp::float64_kt)(Y))
    #define ceilf(X) (dcpp::float32_kt)ceil((dcpp::float64_kt)(X))
    #define floorf(X) (dcpp::float32_kt)floor((dcpp::float64_kt)(X))
    #define powf(X,Y) (dcpp::float32_kt)pow((dcpp::float64_kt)(X),(dcpp::float64_kt)(Y))
    #define fmodf(X,Y) (dcpp::float32_kt)fmod((dcpp::float64_kt)(X),(dcpp::float64_kt)(Y))
    #define fabsf(X) (dcpp::float32_kt)fabs((dcpp::float64_kt)(X))
    #define logf(X) (dcpp::float32_kt)log((dcpp::float64_kt)(X))
#endif
 
#ifndef FLT_MAX
#define FLT_MAX 3.402823466E+38F
#endif
 
#ifndef FLT_MIN
#define FLT_MIN 1.17549435e-38F
#endif
 
namespace dcpp
{
namespace nub
{
 
 
    const dcpp::int32_kt ROUNDING_ERROR_INT32 = 0;
 
#ifdef __DCPP_HAS_INT64
    const dcpp::int64_kt ROUNDING_ERROR_INT64 = 0;
#endif
    const dcpp::float32_kt ROUNDING_ERROR_Float32 = 0.000001f;
    const dcpp::float64_kt ROUNDING_ERROR_Float64 = 0.00000001;
 
#ifdef PI // make sure we don't collide with a define
#undef PI
#endif
    const dcpp::float32_kt PI = 3.14159265359f;
 
    const dcpp::float32_kt RECIPROCAL_PI = 1.0f/PI;
 
    const dcpp::float32_kt HALF_PI = PI/2.0f;
 
#ifdef PI64 // make sure we don't collide with a define
#undef PI64
#endif
    const dcpp::float64_kt PI64 = 3.1415926535897932384626433832795028841971693993751;
 
    const dcpp::float64_kt RECIPROCAL_PI64 = 1.0/PI64;
 
    const dcpp::float32_kt DEGTORAD = PI / 180.0f;
 
    const dcpp::float32_kt RADTODEG   = 180.0f / PI;
 
    const dcpp::float64_kt DEGTORAD64 = PI64 / 180.0;
 
    const dcpp::float64_kt RADTODEG64 = 180.0 / PI64;
 
 
    inline dcpp::float32_kt radToDeg(dcpp::float32_kt radians)
    {
        return RADTODEG * radians;
    }
 
 
    inline dcpp::float64_kt radToDeg(dcpp::float64_kt radians)
    {
        return RADTODEG64 * radians;
    }
 
 
    inline dcpp::float32_kt degToRad(dcpp::float32_kt degrees)
    {
        return DEGTORAD * degrees;
    }
 
 
    inline dcpp::float64_kt degToRad(dcpp::float64_kt 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 dcpp::float32_kt 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 Duckcpp 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 dcpp::float32_kt roundingError()
    {
        return ROUNDING_ERROR_Float32;
    }
 
    template <>
    inline dcpp::float64_kt roundingError()
    {
        return ROUNDING_ERROR_Float64;
    }
 
    template <>
    inline dcpp::int32_kt roundingError()
    {
        return ROUNDING_ERROR_INT32;
    }
 
    template <>
    inline dcpp::uint32_kt roundingError()
    {
        return ROUNDING_ERROR_INT32;
    }
 
#ifdef __DCPP_HAS_INT64
    template <>
    inline dcpp::int64_kt roundingError()
    {
        return ROUNDING_ERROR_INT64;
    }
 
    template <>
    inline dcpp::uint64_kt roundingError()
    {
        return ROUNDING_ERROR_INT64;
    }
#endif
 
    template <class T>
    inline T relativeErrorFactor()
    {
        return 1;
    }
 
    template <>
    inline dcpp::float32_kt relativeErrorFactor()
    {
        return 4;
    }
 
    template <>
    inline dcpp::float64_kt 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; }
 
        dcpp::int32_kt i;
        dcpp::float32_kt f;
    };
 
    //\result true when numbers have a ULP <= maxUlpDiff AND have the same sign.
    inline bool equalsByUlp(dcpp::float32_kt a, dcpp::float32_kt 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.
 
        const FloatIntUnion32 fa(a);
        const 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.
        const int ulpsDiff = abs_(fa.i- fb.i);
        if (ulpsDiff <= maxUlpDiff)
            return true;
 
        return false;
    }
 
    inline bool iszero(const dcpp::float64_kt a, const dcpp::float64_kt tolerance = ROUNDING_ERROR_Float64)
    {
        return fabs(a) <= tolerance;
    }
 
    inline bool iszero(const dcpp::float32_kt a, const dcpp::float32_kt tolerance = ROUNDING_ERROR_Float32)
    {
        return fabsf(a) <= tolerance;
    }
 
    inline bool isnotzero(const dcpp::float32_kt a, const dcpp::float32_kt tolerance = ROUNDING_ERROR_Float32)
    {
        return fabsf(a) > tolerance;
    }
 
    inline bool iszero(const dcpp::int32_kt a, const dcpp::int32_kt tolerance = 0)
    {
        return ( a & 0x7ffffff ) <= tolerance;
    }
 
    inline bool iszero(const dcpp::uint32_kt a, const dcpp::uint32_kt tolerance = 0)
    {
        return a <= tolerance;
    }
 
#ifdef __DCPP_HAS_INT64
    inline bool iszero(const dcpp::int64_kt a, const dcpp::int64_kt tolerance = 0)
    {
        return abs_(a) <= tolerance;
    }
#endif
 
    inline dcpp::int32_kt int32_min(dcpp::int32_kt a, dcpp::int32_kt b)
    {
        const dcpp::int32_kt mask = (a - b) >> 31;
        return (a & mask) | (b & ~mask);
    }
 
    inline dcpp::int32_kt int32_max(dcpp::int32_kt a, dcpp::int32_kt b)
    {
        const dcpp::int32_kt mask = (a - b) >> 31;
        return (b & mask) | (a & ~mask);
    }
 
    inline dcpp::int32_kt int32_clamp (dcpp::int32_kt value, dcpp::int32_kt low, dcpp::int32_kt high)
    {
        return int32_min(int32_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)
    */
 
    using inttofloat = union { dcpp::uint32_kt u; dcpp::int32_kt s; dcpp::float32_kt f; };
 
    #define FLOAT32_AS_INT32(f)     (*((dcpp::int32_kt *) &(f)))
    #define FLOAT32_AS_UINT32(f)        (*((dcpp::uint32_kt *) &(f)))
    #define FLOAT32_AS_UINT32_POINTER(f)    ( ((dcpp::uint32_kt *) &(f)))
 
    #define FLOAT32_VALUE_0     0x00000000
    #define FLOAT32_VALUE_1     0x3f800000
    #define FLOAT32_SIGN_BIT        0x80000000U
    #define FLOAT32_EXPON_MANTISSA  0x7FFFFFFFU
 
#ifdef DUCKCPP_FAST_MATH
    #define IR(x)           ((dcpp::uint32_kt&)(x))
#else
    inline dcpp::uint32_kt IR(dcpp::float32_kt x) {inttofloat tmp; tmp.f=x; return tmp.u;}
#endif
 
    #define AIR(x)          (IR(x)&0x7fffffff)
 
#ifdef DUCKCPP_FAST_MATH
    #define FR(x)           ((dcpp::float32_kt&)(x))
#else
    inline dcpp::float32_kt FR(dcpp::uint32_kt x) {inttofloat tmp; tmp.u=x; return tmp.f;}
    inline dcpp::float32_kt FR(dcpp::int32_kt x) {inttofloat tmp; tmp.s=x; return tmp.f;}
#endif
 
    #define IEEE_1_0        0x3f800000
    #define IEEE_255_0      0x437f0000
 
#ifdef DUCKCPP_FAST_MATH
    #define FLOAT32_LOWER_0(f)      (FLOAT32_AS_UINT32(f) >  FLOAT32_SIGN_BIT)
    #define FLOAT32_LOWER_EQUAL_0(f)    (FLOAT32_AS_INT32(f) <= FLOAT32_VALUE_0)
    #define FLOAT32_GREATER_0(f)    (FLOAT32_AS_INT32(f) >  FLOAT32_VALUE_0)
    #define FLOAT32_GREATER_EQUAL_0(f)  (FLOAT32_AS_UINT32(f) <= FLOAT32_SIGN_BIT)
    #define FLOAT32_EQUAL_1(f)      (FLOAT32_AS_UINT32(f) == FLOAT32_VALUE_1)
    #define FLOAT32_EQUAL_0(f)      ( (FLOAT32_AS_UINT32(f) & FLOAT32_EXPON_MANTISSA ) == FLOAT32_VALUE_0)
 
    // only same sign
    #define FLOAT32_A_GREATER_B(a,b)    (FLOAT32_AS_INT32((a)) > FLOAT32_AS_INT32((b)))
 
#else
 
    #define FLOAT32_LOWER_0(n)      ((n) <  0.0f)
    #define FLOAT32_LOWER_EQUAL_0(n)    ((n) <= 0.0f)
    #define FLOAT32_GREATER_0(n)    ((n) >  0.0f)
    #define FLOAT32_GREATER_EQUAL_0(n)  ((n) >= 0.0f)
    #define FLOAT32_EQUAL_1(n)      ((n) == 1.0f)
    #define FLOAT32_EQUAL_0(n)      ((n) == 0.0f)
    #define FLOAT32_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 dcpp::uint32_kt if_c_a_else_b ( const dcpp::char_kt condition, const dcpp::uint32_kt a, const dcpp::uint32_kt b )
    {
        return ( ( -condition >> 7 ) & ( a ^ b ) ) ^ b;
    }
 
    REALINLINE dcpp::uint32_kt if_c_a_else_0 ( const dcpp::char_kt condition, const dcpp::uint32_kt a )
    {
        return ( -condition >> 31 ) & a;
    }
#else
 
    REALINLINE dcpp::uint32_kt if_c_a_else_b ( const dcpp::int32_kt condition, const dcpp::uint32_kt a, const dcpp::uint32_kt b )
    {
        return ( ( -condition >> 31 ) & ( a ^ b ) ) ^ b;
    }
 
    REALINLINE dcpp::uint16_kt if_c_a_else_b ( const dcpp::int16_kt condition, const dcpp::uint16_kt a, const dcpp::uint16_kt b )
    {
        return ( ( -condition >> 15 ) & ( a ^ b ) ) ^ b;
    }
 
    REALINLINE dcpp::uint32_kt if_c_a_else_0 ( const dcpp::int32_kt condition, const dcpp::uint32_kt a )
    {
        return ( -condition >> 31 ) & a;
    }
#endif
 
    /*
        if (condition) state |= m; else state &= ~m;
    */
    REALINLINE void setbit_cond ( dcpp::uint32_kt &state, dcpp::int32_kt condition, dcpp::uint32_kt mask )
    {
        // 0, or any positive to mask
        //dcpp::int32_kt conmask = -condition >> 31;
        state ^= ( ( -condition >> 31 ) ^ state ) & mask;
    }
 
    // NOTE: This is not as exact as the c99/c++11 round function, especially at high numbers starting with 8388609
    //       (only low number which seems to go wrong is 0.49999997 which is rounded to 1)
    //      Also negative 0.5 is rounded up not down unlike with the standard function (p.E. input -0.5 will be 0 and not -1)
    inline dcpp::float32_kt round_( dcpp::float32_kt x )
    {
        return floorf( x + 0.5f );
    }
 
    // calculate: sqrt ( x )
    REALINLINE dcpp::float32_kt squareroot(const dcpp::float32_kt f)
    {
        return sqrtf(f);
    }
 
    // calculate: sqrt ( x )
    REALINLINE dcpp::float64_kt squareroot(const dcpp::float64_kt f)
    {
        return sqrt(f);
    }
 
    // calculate: sqrt ( x )
    REALINLINE dcpp::int32_kt squareroot(const dcpp::int32_kt f)
    {
        return static_cast<dcpp::int32_kt>(squareroot(static_cast<dcpp::float32_kt>(f)));
    }
 
#ifdef __DCPP_HAS_INT64
    // calculate: sqrt ( x )
    REALINLINE dcpp::int64_kt squareroot(const dcpp::int64_kt f)
    {
        return static_cast<dcpp::int64_kt>(squareroot(static_cast<dcpp::float64_kt>(f)));
    }
#endif
 
    // calculate: 1 / sqrt ( x )
    REALINLINE dcpp::float64_kt reciprocal_squareroot(const dcpp::float64_kt x)
    {
        return 1.0 / sqrt(x);
    }
 
    // calculate: 1 / sqrtf ( x )
    REALINLINE dcpp::float32_kt reciprocal_squareroot(const dcpp::float32_kt f)
    {
#if defined ( DUCKCPP_FAST_MATH )
        // NOTE: Unlike comment below says I found inaccuracies already at 4'th significant bit.
        // p.E: Input 1, expected 1, got 0.999755859
 
    #if defined(_MSC_VER) && !defined(_WIN64)
        // SSE reciprocal square root estimate, accurate to 12 significant
        // bits of the mantissa
        dcpp::float32_kt recsqrt;
        __asm rsqrtss xmm0, f           // xmm0 = rsqrtss(f)
        __asm movss recsqrt, xmm0       // return xmm0
        return recsqrt;
 
/*
        // comes from Nvidia
        dcpp::uint32_kt tmp = (dcpp::uint32_kt(IEEE_1_0 << 1) + IEEE_1_0 - *(dcpp::uint32_kt*)&x) >> 1;
        dcpp::float32_kt y = *(dcpp::float32_kt*)&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 dcpp::int32_kt reciprocal_squareroot(const dcpp::int32_kt x)
    {
        return static_cast<dcpp::int32_kt>(reciprocal_squareroot(static_cast<dcpp::float32_kt>(x)));
    }
 
    // calculate: 1 / x
    REALINLINE dcpp::float32_kt reciprocal( const dcpp::float32_kt f )
    {
#if defined (DUCKCPP_FAST_MATH)
        // NOTE: Unlike with 1.f / f the values very close to 0 return -nan instead of inf
 
        // 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) && !defined(_WIN64)
        dcpp::float32_kt 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
        //dcpp::uint32_kt 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 dcpp::float64_kt reciprocal ( const dcpp::float64_kt f )
    {
        return 1.0 / f;
    }
 
 
    // calculate: 1 / x, low precision allowed
    REALINLINE dcpp::float32_kt reciprocal_approxim ( const dcpp::float32_kt f )
    {
#if defined( DUCKCPP_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) && !defined(_WIN64)
        dcpp::float32_kt 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
        dcpp::float32_kt rec;
        __asm rcpss xmm0, f             // xmm0 = rcpss(f)
        __asm movss rec , xmm0          // return xmm0
        return rec;
*/
/*
        dcpp::uint32_kt x = 0x7F000000 - IR ( p );
        const dcpp::float32_kt r = FR ( x );
        return r * (2.0f - p * r);
*/
#else // no fast math
        return 1.f / f;
#endif
    }
 
 
    REALINLINE dcpp::int32_kt floor32(dcpp::float32_kt x)
    {
        return (dcpp::int32_kt) floorf ( x );
    }
 
    REALINLINE dcpp::int32_kt ceil32 ( dcpp::float32_kt x )
    {
        return (dcpp::int32_kt) ceilf ( x );
    }
 
    // NOTE: Please check round_ documentation about some inaccuracies in this compared to standard library round function.
    REALINLINE dcpp::int32_kt round32(dcpp::float32_kt x)
    {
        return (dcpp::int32_kt) round_(x);
    }
 
    inline dcpp::float32_kt Float32_max3(const dcpp::float32_kt a, const dcpp::float32_kt b, const dcpp::float32_kt c)
    {
        return a > b ? (a > c ? a : c) : (b > c ? b : c);
    }
 
    inline dcpp::float32_kt Float32_min3(const dcpp::float32_kt a, const dcpp::float32_kt b, const dcpp::float32_kt c)
    {
        return a < b ? (a < c ? a : c) : (b < c ? b : c);
    }
 
    inline dcpp::float32_kt fract ( dcpp::float32_kt x )
    {
        return x - floorf ( x );
    }
 
} // end namespace nub
} // end namespace dcpp
 
#ifndef DUCKCPP_FAST_MATH
    using dcpp::nub::IR;
    using dcpp::nub::FR;
#endif
 
#endif