fastmath.h

Go to the documentation of this file.

/*
* Copyright 2016 Autodesk, Inc. All rights reserved.
* Use of this software is subject to the terms of the Autodesk license agreement and any attachments or Appendices thereto provided at the time of installation or download,
* or which otherwise accompanies this software in either electronic or hard copy form, or which is signed by you and accepted by Autodesk.
*/

#pragma once

#include "gwnavruntime/base/types.h"
#include <math.h>

namespace Kaim
{

//----------------------------------------------------------------------------------------
// Fsel

KY_INLINE KyFloat32 Fsel(KyFloat32 a, KyFloat32 x, KyFloat32 y) { return a >= 0.0f ? x : y; }

//----------------------------------------------------------------------------------------
// Isel Lsel

KY_INLINE KyInt32 Isel(KyInt32 a, KyInt32 x, KyInt32 y)
{
    // mask=0xFFFFFFFFFFFFFFFF if (a < 0)   mask=0x0000000000000000 if (a >= 0)
    const KyInt32 mask = a >> 31;
    return x + ((y - x) & mask);
};

KY_INLINE KyInt64 Lsel(KyInt64 a, KyInt64 x, KyInt64 y)
{
    // mask=0xFFFFFFFFFFFFFFFF if a<0, mask=0x0000000000000000 if a>=0
    const KyInt64 mask = a >> 63;
    return x + ((y - x) & mask);
};

KY_INLINE KyInt64 Lsign(KyInt64 x) { return (x >> 63) - ((-x) >> 63); };
KY_INLINE KyInt32 Isign(KyInt32 x) { return (x >> 31) - ((-x) >> 31); };

//----------------------------------------------------------------------------------------
// Min / Max

template <typename T> KY_INLINE T Min(const T& a, const T& b) { return (a < b) ? a : b; }
template <typename T> KY_INLINE T Max(const T& a, const T& b) { return (a < b) ? b : a; }

template <typename T> KY_INLINE T Min(const T& a, const T& b, const T& c) { return Min(Min(a, b), c); }
template <typename T> KY_INLINE T Max(const T& a, const T& b, const T& c) { return Max(Max(a, b), c); }

template <typename T> KY_INLINE T Min(const T& a, const T& b, const T& c, const T& d) { return Min(Min(a, b), Min(c, d)); }
template <typename T> KY_INLINE T Max(const T& a, const T& b, const T& c, const T& d) { return Max(Max(a, b), Max(c, d)); }

template <typename T> KY_INLINE T Min(const T& a, const T& b, const T& c, const T& d, const T& e) { return Min(Min(a, b, c, d), e); }
template <typename T> KY_INLINE T Max(const T& a, const T& b, const T& c, const T& d, const T& e) { return Max(Max(a, b, c, d), e); }

template <typename T> KY_INLINE T Clamp(const T& x, const T& min_, const T& max_)
{
    if (x > max_) return max_;
    if (x < min_) return min_;
    return x;
}

template <typename T> inline bool Less(const T& a, const T& b, const T& c) { return a < b && b < c; }
template <typename T> inline bool LessEq(const T& a, const T& b, const T& c) { return a <= b && b <= c; }

//---------------------------------------------------------------------
// Fsel specialization for floats
#if defined(KY_CPU_PPC) || defined(KY_CPU_PPC64)

template<>
KY_INLINE KyFloat32 Min(const KyFloat32& a, const KyFloat32& b) { return Fsel(a - b, b, a);}
template<>
KY_INLINE KyFloat32 Max(const KyFloat32& a, const KyFloat32& b) { return Fsel(a - b, a, b); }

#endif
//---------------------------------------------------------------------

KY_INLINE KyUInt32 FastDivisionBy3(KyUInt32 i) { return (i * 0x0000AAAB) >> 17; }   // WARNING : Can overflow
KY_INLINE KyUInt32 FastModuloBy3(KyUInt32 i) { return i - 3 * FastDivisionBy3(i); } // WARNING : Can overflow


KY_INLINE KyInt32 FastMin(KyInt32 a, KyInt32 b) { return Isel(a - b, b, a); } // WARNING : Can overflow because of the a - b
KY_INLINE KyInt32 FastMax(KyInt32 a, KyInt32 b) { return Isel(a - b, a, b); } // WARNING : Can overflow because of the a - b

KY_INLINE KyInt32 FastMin(KyInt32 a, KyInt32 b, KyInt32 c) { return FastMin(FastMin(a, b), c); }
KY_INLINE KyInt32 FastMax(KyInt32 a, KyInt32 b, KyInt32 c) { return FastMax(FastMax(a, b), c); }
KY_INLINE KyInt32 FastMin(KyInt32 a, KyInt32 b, KyInt32 c, KyInt32 d) { return FastMin(FastMin(FastMin(a, b), c), d); }
KY_INLINE KyInt32 FastMax(KyInt32 a, KyInt32 b, KyInt32 c, KyInt32 d) { return FastMin(FastMax(FastMax(a, b), c), d); }


KY_INLINE KyInt64 FastMin(KyInt64 a, KyInt64 b) { return Lsel(a - b, b, a); } // WARNING : Can overflow because of the a - b
KY_INLINE KyInt64 FastMax(KyInt64 a, KyInt64 b) { return Lsel(a - b, a, b); } // WARNING : Can overflow because of the a - b

KY_INLINE KyInt64 FastMin(KyInt64 a, KyInt64 b, KyInt64 c) { return FastMin(FastMin(a, b), c); }
KY_INLINE KyInt64 FastMax(KyInt64 a, KyInt64 b, KyInt64 c) { return FastMax(FastMax(a, b), c); }
KY_INLINE KyInt64 FastMin(KyInt64 a, KyInt64 b, KyInt64 c, KyInt64 d) { return FastMin(FastMin(FastMin(a, b), c), d); }
KY_INLINE KyInt64 FastMax(KyInt64 a, KyInt64 b, KyInt64 c, KyInt64 d) { return FastMin(FastMax(FastMax(a, b), c), d); }


// remove this function and replace it with template<class T> struct OperatorLess
template <typename T1, typename T2 = T1>
class DefaultLess
{
public:
    KY_INLINE bool operator()(const T1& lhs, const T2& rhs) const { return lhs < rhs; }
};


//----------------------------------------------------------------------------------------
// Float32 to signed int32

KY_INLINE KyInt32 NearestInt(KyFloat32 x) { return (KyInt32)(x + Kaim::Fsel(x, 0.5f, -0.5f)); }

// LowerInt( 1.0f) =  1
// LowerInt(-1.0f) = -1
KY_INLINE KyInt32 LowerInt(KyFloat32 x) { return (KyInt32)(floorf(x)); }

// FastLowerInt( 1.0f) =  1
// FastLowerInt(-1.0f) = -2 !!
KY_INLINE KyInt32 FastLowerInt(KyFloat32 x) { return (KyInt32)(x + Kaim::Fsel(x, 0.0f, -1.0f)); }

// UpperInt( 1.0f) =  1
// UpperInt(-1.0f) = -1
KY_INLINE KyInt32 UpperInt(KyFloat32 x) { return (KyInt32)(ceilf(x)); }


// -----------------------------------------------------------
// Rotation and angles

// Cap the input to [-1.0, 1.0] to be robust to float precision errors and to avoid returning NaN
KY_INLINE KyFloat32 Acosf(KyFloat32 x) { return acosf(Clamp(x, -1.0f, 1.0f)); }

KY_INLINE KyFloat32 Sqrtf(KyFloat32 x)       { return sqrtf(x); }
KY_INLINE KyFloat32 Cosf(KyFloat32 angleRad) { return cosf(angleRad); }
KY_INLINE KyFloat32 Sinf(KyFloat32 angleRad) { return sinf(angleRad); }

static const KyFloat32 KY_PI = 3.14159265f;  
static const KyFloat32 KY_2_PI = 2.0f * KY_PI; 
static const KyFloat32 KY_PI_DIVIDED_BY_180 = KY_PI / 180.0f; 
static const KyFloat32 KY_180_DIVIDED_BY_PI = 180.0f / KY_PI; 
KY_INLINE KyFloat32 RadFromDeg(KyFloat32 degrees) { return degrees * KY_PI_DIVIDED_BY_180; } 
KY_INLINE KyFloat32 DegFromRad(KyFloat32 radians) { return radians * KY_180_DIVIDED_BY_PI; } 

// Commented because never used at the moment
//KY_INLINE KyFloat32 Asinf(KyFloat32 input)
//{
//  // Cap the input to [-1.0, 1.0] to be robust to float precision errors and to avoid returning a NaN
//  const KyFloat32 normalizeDotProd = Kaim::Min(1.f, Kaim::Max(-1.f, input));
//  return asinf(normalizeDotProd);
//}

//----------------------------------------------------------------------------------------
// Floating points

static const KyFloat32 KY_MIN_NORMALIZED_FLOAT = FLT_MIN; // min normalized positive value
static const KyFloat32 KY_EPSILON = FLT_EPSILON;  // smallest such that 1.0+FLT_EPSILON != 1.0

KY_INLINE bool IsFloatDenormalized(KyFloat32 x)    { return x  < KY_MIN_NORMALIZED_FLOAT && x >  -KY_MIN_NORMALIZED_FLOAT; }
KY_INLINE bool IsFloatNormalized(KyFloat32 x)      { return x >= KY_MIN_NORMALIZED_FLOAT || x <= -KY_MIN_NORMALIZED_FLOAT; }
KY_INLINE bool CanDivideBy(KyFloat32 x)            { return x >= KY_MIN_NORMALIZED_FLOAT || x <= -KY_MIN_NORMALIZED_FLOAT; }

KY_INLINE bool IsEpsilonPositive(KyFloat32 x) { return x > KY_EPSILON; }
KY_INLINE bool IsEpsilonNegative(KyFloat32 x) { return x < -KY_EPSILON; }
KY_INLINE bool IsEpsilonZero(KyFloat32 x)     { return x <= KY_EPSILON && x >= -KY_EPSILON; }
KY_INLINE bool IsEpsilonEqual(KyFloat32 a, KyFloat32 b) { return IsEpsilonZero(a - b); };

KY_INLINE bool IsNearZero(KyFloat32 x, KyFloat32 dist) { return x <= dist && x >= -dist; }
KY_INLINE bool IsNearEqual(KyFloat32 a, KyFloat32 b, KyFloat32 dist) { return IsNearZero(a - b, dist); };

//----------------------------------------------------------------------------------------
// Other stuff

namespace Winding { enum Enum { CCW = 0, CW = 1}; }

template<typename T>
KY_INLINE T Square(const T& x) { return x * x; }

#define KY_SQRT1_2 ((KyFloat32)0.70710678118654752440f) // 1/sqrt(2)


//----------------------------------------------------------------------------------------
// integer division rounded toward nearest
//                                                   5/3=1.66  -5/3=-1.66  5/2=1.5  -5/2=-1.5
// regular integer division rounds towards zero    : 5/3=1     -5/3=-1     5/2=1    -5/2=-1
// DivNearest() rounds towards the nearest integer : 5/3=2     -5/3=-2     5/2=1    -5/2=-2
KY_INLINE KyInt64 DivNearest(KyInt64 a, KyInt64 b)
{
    KyInt64 a2 = a * 2;
    KyInt64 d2 = a2 / b;
    KyInt64 m2 = a2 % b;
    KyInt64 sab = (a ^ b) >> 63; // a/b<0 ? -1 : 0
    KyInt64 c = (m2 == 0 && sab == 0) ? 0 : 2 * sab + 1;
    return (d2 + c) / 2;
}

// 20% slower than DivNearest but does not multiply a by 2, so has 1 less bit chance to overflow
KY_INLINE KyInt64 DivNearestAlt(KyInt64 a, KyInt64 b)
{
    KyInt64 sab = ((a ^ b) >> 63);
    KyInt64 sb = 2 * (b >> 63) + 1;
    KyInt64 c = (1 + sab) * (b - sb) + sab * b; // c(sab=0)=(b - sb)  c(sab=-1)=-b
    return (a + c / 2) / b;
}

//----------------------------------------------------------------------------------------
// integer division rounded toward -infinite
//                                                5/3=1.66  -5/3=-1.66  5/2=1.5  -5/2=-1.5
// regular integer division rounds towards zero : 5/3=1     -5/3=-1     5/2=1    -5/2=-1
// DivFloor() rounds towards the lower integer  : 5/3=1     -5/3=-2     5/2=1    -5/2=-2
inline KyInt64 DivFloor(KyInt64 a, KyInt64 b)
{
    KyInt64 sab = (a ^ b) >> 63;    // a/b<0 ? -1 : 0
    KyInt64 sb = 2 * (b >> 63) + 1; //   b<0 ? -1 : 1
    KyInt64 c = sab * (b - sb);
    return (a + c) / b;
}

}