math.h

/* -*- mode: C; c-basic-offset: 4; intent-tabs-mode: nil -*-
 *
 * Sifteo SDK
 *
 * Copyright <c> 2012 Sifteo, Inc.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 * 
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#pragma once
#ifdef NOT_USERSPACE
#   error This is a userspace-only header, not allowed by the current build.
#endif

#include <sifteo/abi.h>
#include <sifteo/macros.h>

namespace Sifteo {

/*
 * Common mathematical constants
 */
 
#define M_E         2.71828182845904523536028747135266250   
#define M_LOG2E     1.44269504088896340735992468100189214   
#define M_LOG10E    0.434294481903251827651128918916605082  
#define M_LN2       0.693147180559945309417232121458176568  
#define M_LN10      2.30258509299404568401799145468436421   
#define M_PI        3.14159265358979323846264338327950288   
#define M_TAU       6.2831853071795862                      
#define M_PI_2      1.57079632679489661923132169163975144   
#define M_PI_4      0.785398163397448309615660845819875721  
#define M_1_PI      0.318309886183790671537767526745028724  
#define M_2_PI      0.636619772367581343075535053490057448  
#define M_2_SQRTPI  1.12837916709551257389615890312154517   
#define M_SQRT2     1.41421356237309504880168872420969808   
#define M_SQRT1_2   0.707106781186547524400844362104849039  
#define MAXFLOAT    ((float)3.40282346638528860e+38)        
#define NAN         __builtin_nanf("0x7fc00000")            


template <typename T> inline T clamp(const T& value, const T& low, const T& high)
{
    if (value < low) {
        return low;
    }
    if (value > high) {
        return high;
    }
    return value;
}

template <typename T> inline T abs(const T& value)
{
    if (value < 0) {
        return -value;
    }
    return value;
}

template <typename T> inline T lslc(const T& value, int bits)
{
    if (bits < 0)
        return value;
    if (bits >= sizeof(T) * 8)
        return 0;
    return value << (unsigned)bits;
}

template <typename T> inline T lsrc(const T& value, int bits)
{
    if (bits < 0)
        return value;
    if (bits >= sizeof(T) * 8)
        return 0;
    return value >> (unsigned)bits;
}

template <typename T> inline T bitRange(int begin, int end)
{
    return lslc((T)-1, begin) & ~lslc((T)-1, end);
}

inline unsigned clz(uint32_t word)
{
    return __builtin_clz(word);
}

inline unsigned ffs(uint32_t word)
{
    // Reference: http://en.wikipedia.org/wiki/Find_first_set#Properties_and_relations
    return 32 - __builtin_clz(word & -word);
}

float inline fmod(float a, float b)
{
    uint32_t r = _SYS_fmodf(reinterpret_cast<uint32_t&>(a), reinterpret_cast<uint32_t&>(b));
    return reinterpret_cast<float&>(r);
}

double inline fmod(double a, double b)
{
    uint64_t ia = reinterpret_cast<uint64_t&>(a);
    uint64_t ib = reinterpret_cast<uint64_t&>(b);
    uint64_t r = _SYS_fmod(ia, ia >> 32, ib, ib >> 32);
    return reinterpret_cast<double&>(r);
}

float inline pow(float a, float b)
{
    uint32_t r = _SYS_powf(reinterpret_cast<uint32_t&>(a), reinterpret_cast<uint32_t&>(b));
    return reinterpret_cast<float&>(r);
}

double inline pow(double a, double b)
{
    uint64_t ia = reinterpret_cast<uint64_t&>(a);
    uint64_t ib = reinterpret_cast<uint64_t&>(b);
    uint64_t r = _SYS_pow(ia, ia >> 32, ib, ib >> 32);
    return reinterpret_cast<double&>(r);
}

unsigned inline umod(int a, int b)
{
    int r = a % b;
    if (r < 0)
        r += b;
    return r;
}

template <typename T> inline T ceildiv(T numerator, T denominator) {
    return (numerator + (denominator - 1)) / denominator;
}

template <typename T> inline T roundup(T numerator, T denominator) {
    return ceildiv(numerator, denominator) * denominator;
}

float inline log(float a)
{
    uint32_t r = _SYS_logf(reinterpret_cast<uint32_t&>(a));
    return reinterpret_cast<float&>(r);
}

double inline log(double a)
{
    uint64_t ia = reinterpret_cast<uint64_t&>(a);
    uint64_t r = _SYS_logd(ia, ia >> 32);
    return reinterpret_cast<double&>(r);
}

float inline sqrt(float a)
{
    uint32_t r = _SYS_sqrtf(reinterpret_cast<uint32_t&>(a));
    return reinterpret_cast<float&>(r);
}

double inline sqrt(double a)
{
    uint64_t ia = reinterpret_cast<uint64_t&>(a);
    uint64_t r = _SYS_sqrt(ia, ia >> 32);
    return reinterpret_cast<double&>(r);
}

float inline sin(float x)
{
    uint32_t r = _SYS_sinf(reinterpret_cast<uint32_t&>(x));
    return reinterpret_cast<float&>(r);
}

float inline cos(float x)
{
    uint32_t r = _SYS_cosf(reinterpret_cast<uint32_t&>(x));
    return reinterpret_cast<float&>(r);
}

float inline tan(float x)
{
    uint32_t r = _SYS_tanf(reinterpret_cast<uint32_t&>(x));
    return reinterpret_cast<float&>(r);
}

float inline atan(float x)
{
    uint32_t r = _SYS_atanf(reinterpret_cast<uint32_t&>(x));
    return reinterpret_cast<float&>(r);
}

float inline atan2(float a, float b)
{
    uint32_t r = _SYS_atan2f(reinterpret_cast<uint32_t&>(a), reinterpret_cast<uint32_t&>(b));
    return reinterpret_cast<float&>(r);
}

void inline sincos(float x, float *s, float *c)
{
    _SYS_sincosf(reinterpret_cast<uint32_t&>(x), s, c);
}

float inline tsin(float x)
{
    uint32_t r = _SYS_tsinf(reinterpret_cast<uint32_t&>(x));
    return reinterpret_cast<float&>(r);
}

float inline tcos(float x)
{
    uint32_t r = _SYS_tcosf(reinterpret_cast<uint32_t&>(x));
    return reinterpret_cast<float&>(r);
}

int inline tsini(int x)
{
    return _SYS_tsini(x);
}

int inline tcosi(int x)
{
    return _SYS_tcosi(x);
}

bool inline isunordered(float a, float b = 0.f)
{
    return __builtin_isunordered(a, b);
}

bool inline isunordered(double a, double b = 0.f)
{
    return __builtin_isunordered(a, b);
}

template <typename T> inline long ceil(const T value)
{
    long result = value;
    if (result >= 0 && result < value) {
        return result+1;
    }
    return result;
}

template <typename T> inline long floor(const T value)
{
   long result = value;
   if (result <= 0 && result > value) {
       return result - 1;
   }
   return result;
}

template <typename T> inline long round(const T value)
{
    return floor(value + 0.5);
}

template <typename T> inline bool almostEqual(const T a, const T b, const T epsilon)
{
    return abs(a-b) < epsilon;
}


template <typename T> struct Vector2 {
    T x;    
    T y;    

    void set(T _x, T _y) {
        x = _x;
        y = _y;
    }

    void setPolar(float angle, float magnitude) {
        float s, c;
        sincos(angle, &s, &c);
        x = c * magnitude;
        y = s * magnitude;
    }

    Vector2<T> rotate(float angle) const {
        float s, c;
        sincos(angle, &s, &c);
        Vector2<T> result = { x*c - y*s, x*s + y*c };
        return result;
    }

    Vector2<T> rotateI(int angle) const {
        Vector2<T> a1 = { -y,  x };
        Vector2<T> a2 = { -x, -y };
        Vector2<T> a3 = {  y, -x };
        switch (angle) {
            default: ASSERT(0);
            case 0: return *this;
            case 1: return a1;
            case 2: return a2;
            case 3: return a3;
        }
    }

    static Vector2<T> unit(int side) {
        Vector2<T> a0 = {  0, -1 };
        Vector2<T> a1 = { -1,  0 };
        Vector2<T> a2 = {  0,  1 };
        Vector2<T> a3 = {  1,  0 };
        switch(side) {
            default: ASSERT(0);
            case 0: return a0;
            case 1: return a1;
            case 2: return a2;
            case 3: return a3;
        }
    }

    T len2() const {
        return ( x * x + y * y );
    }

    T len() const {
        return sqrt(len2());
    }

    T lenManhattan() const {
        return abs(x) + abs(y);
    }

    Vector2<T> normalize() const {
        return *this / len();
    }
    
    Vector2<int> round() const {
        Vector2<int> result = { x + 0.5f, y + 0.5f };
        return result;
    }

    template <typename R> Vector2<R> cast() const {
        Vector2<R> result = { x, y };
        return result;
    }

    Vector2<T> cmul(Vector2<T> u) const {
        Vector2<T> result = { x*u.x - y*u.y, x*u.y + y*u.x };
        return result;
    }

    Vector2<int> toInt() const { return cast<int>(); }
    Vector2<float> toFloat() const { return cast<float>(); }
    Vector2<double> toDouble() const { return cast<double>(); }
    
    // Implicit casts
    operator Vector2<int>            () const { return cast<int>(); }
    operator Vector2<unsigned>       () const { return cast<unsigned>(); }
    operator Vector2<short>          () const { return cast<short>(); }
    operator Vector2<unsigned short> () const { return cast<unsigned short>(); }
    operator Vector2<int8_t>         () const { return cast<int8_t>(); }
    operator Vector2<uint8_t>        () const { return cast<uint8_t>(); }
    operator Vector2<float>          () const { return cast<float>(); }
    operator Vector2<double>         () const { return cast<double>(); }
};

typedef Vector2<int>                Int2;       
typedef Vector2<unsigned>           UInt2;      
typedef Vector2<short>              Short2;     
typedef Vector2<unsigned short>     UShort2;    
typedef Vector2<int8_t>             Byte2;      
typedef Vector2<uint8_t>            UByte2;     
typedef Vector2<float>              Float2;     
typedef Vector2<double>             Double2;    

template <typename T> inline Vector2<T> vec(T x, T y) {
    Vector2<T> result = { x, y };
    return result;
}

template <typename T> inline T dot(Vector2<T> u, Vector2<T> v) {
    return u.x * v.x + u.y * v.y;
}

template <typename T> inline Vector2<T> polar(T angle, T magnitude) {
    Vector2<T> result;
    result.setPolar(angle, magnitude);
    return result;
}

// Vector operations
template <typename T> inline Vector2<T> operator-(Vector2<T> u) { return vec<T>(-u.x, -u.y); }
template <typename T> inline Vector2<T> operator+=(Vector2<T> &u, Vector2<T> v) { return vec<T>(u.x+=v.x, u.y+=v.y); }
template <typename T> inline Vector2<T> operator-=(Vector2<T> &u, Vector2<T> v) { return vec<T>(u.x-=v.x, u.y-=v.y); }
template <typename T> inline Vector2<T> operator+(Vector2<T> u, Vector2<T> v) { return vec<T>(u.x+v.x, u.y+v.y); }
template <typename T> inline Vector2<T> operator-(Vector2<T> u, Vector2<T> v) { return vec<T>(u.x-v.x, u.y-v.y); }
template <typename T> inline Vector2<T> operator*(Vector2<T> u, Vector2<T> v) { return vec<T>(u.x*v.x, u.y*v.y); }
template <typename T> inline bool operator==(Vector2<T> u, Vector2<T> v) { return u.x == v.x && u.y == v.y; }
template <typename T> inline bool operator!=(Vector2<T> u, Vector2<T> v) { return u.x != v.x || u.y != v.y; }

// Scalar int promotion
template <typename T> inline Int2 operator*(int k, Vector2<T> v) { return vec<int>(k*v.x, k*v.y); }
template <typename T> inline Int2 operator*(Vector2<T> v, int k) { return vec<int>(k*v.x, k*v.y); }
template <typename T> inline Int2 operator/(Vector2<T> v, int k) { return vec<int>(v.x/k, v.y/k); }
template <typename T> inline Int2 operator+=(Vector2<T> &u, int k) { return vec<int>(u.x+=k, u.y+=k); }
template <typename T> inline Int2 operator*=(Vector2<T> &u, int k) { return vec<int>(u.x*=k, u.y*=k); }
template <typename T> inline Int2 operator<<(Vector2<T> u, int shift) { return vec<int>(u.x<<shift, u.y<<shift); }
template <typename T> inline Int2 operator>>(Vector2<T> u, int shift) { return vec<int>(u.x>>shift, u.y>>shift); }

// Scalar float promotion
template <typename T> inline Float2 operator*(float k, Vector2<T> v) { return vec<float>(k*v.x, k*v.y); }
template <typename T> inline Float2 operator*(Vector2<T> v, float k) { return vec<float>(k*v.x, k*v.y); }
template <typename T> inline Float2 operator/(Vector2<T> v, float k) { return vec<float>(v.x/k, v.y/k); }
template <typename T> inline Float2 operator+=(Vector2<T> &u, float k) { return vec<float>(u.x+=k, u.y+=k); }
template <typename T> inline Float2 operator*=(Vector2<T> &u, float k) { return vec<float>(u.x*=k, u.y*=k); }

// Scalar double promotion
template <typename T> inline Double2 operator*(double k, Vector2<T> v) { return vec<double>(k*v.x, k*v.y); }
template <typename T> inline Double2 operator*(Vector2<T> v, double k) { return vec<double>(k*v.x, k*v.y); }
template <typename T> inline Double2 operator/(Vector2<T> v, double k) { return vec<double>(v.x/k, v.y/k); }
template <typename T> inline Double2 operator+=(Vector2<T> &u, double k) { return vec<double>(u.x+=k, u.y+=k); }
template <typename T> inline Double2 operator*=(Vector2<T> &u, double k) { return vec<double>(u.x*=k, u.y*=k); }

// Vector int -> float promotion
inline Vector2<float> operator+=(Vector2<float> &u, Int2 v) { return vec(u.x+=v.x, u.y+=v.y); }
inline Vector2<float> operator-=(Vector2<float> &u, Int2 v) { return vec(u.x-=v.x, u.y-=v.y); }
inline Vector2<float> operator+(Vector2<float> u, Int2 v) { return vec(u.x+v.x, u.y+v.y); }
inline Vector2<float> operator-(Vector2<float> u, Int2 v) { return vec(u.x-v.x, u.y-v.y); }
inline Vector2<float> operator*(Vector2<float> u, Int2 v) { return vec(u.x*v.x, u.y*v.y); }
inline Vector2<float> operator+(Int2 u, Vector2<float> v) { return vec(u.x+v.x, u.y+v.y); }
inline Vector2<float> operator-(Int2 u, Vector2<float> v) { return vec(u.x-v.x, u.y-v.y); }
inline Vector2<float> operator*(Int2 u, Vector2<float> v) { return vec(u.x*v.x, u.y*v.y); }


template <typename T> struct Vector3 {
    T x;    
    T y;    
    T z;    

    void set(T _x, T _y, T _z) {
        x = _x;
        y = _y;
        z = _z;
    }
    
    Vector2<T> xy() const { return vec(x, y); } 
    Vector2<T> xz() const { return vec(x, z); } 
    Vector2<T> yx() const { return vec(y, x); } 
    Vector2<T> yz() const { return vec(y, z); } 
    Vector2<T> zx() const { return vec(z, x); } 
    Vector2<T> zy() const { return vec(z, y); } 

    T len2() const {
        return ( x * x + y * y + z * z );
    }

    T len() const {
        return sqrt(len2());
    }

    T lenManhattan() const {
        return abs(x) + abs(y) + abs(z);
    }

    Vector3<T> normalize() const {
        return *this / len();
    }

    Vector3<int> round() const {
        Vector3<int> result = { x + 0.5f, y + 0.5f, z + 0.5f };
        return result;
    }

    Vector3<T> zRotate(float angle) const {
        float s, c;
        sincos(angle, &s, &c);
        Vector3<T> result = { x*c - y*s, x*s + y*c, z };
        return result;
    }

    Vector3<T> zRotateI(int angle) const {
        Vector3<T> a1 = { -y,  x, z };
        Vector3<T> a2 = { -x, -y, z };
        Vector3<T> a3 = {  y, -x, z };
        switch (angle) {
            default: ASSERT(0);
            case 0: return *this;
            case 1: return a1;
            case 2: return a2;
            case 3: return a3;
        }
    }

    template <typename R> Vector3<R> cast() const {
        Vector3<R> result = { x, y, z };
        return result;
    }
    
    Vector3<int> toInt() const { return cast<int>(); }
    Vector3<float> toFloat() const { return cast<float>(); }
    Vector3<double> toDouble() const { return cast<double>(); }
    
    // Implicit casts
    operator Vector3<int>            () const { return cast<int>(); }
    operator Vector3<unsigned>       () const { return cast<unsigned>(); }
    operator Vector3<short>          () const { return cast<short>(); }
    operator Vector3<unsigned short> () const { return cast<unsigned short>(); }
    operator Vector3<int8_t>         () const { return cast<int8_t>(); }
    operator Vector3<uint8_t>        () const { return cast<uint8_t>(); }
    operator Vector3<float>          () const { return cast<float>(); }
    operator Vector3<double>         () const { return cast<double>(); }
};

typedef Vector3<int>                Int3;       
typedef Vector3<unsigned>           UInt3;      
typedef Vector3<short>              Short3;     
typedef Vector3<unsigned short>     UShort3;    
typedef Vector3<int8_t>             Byte3;      
typedef Vector3<uint8_t>            UByte3;     
typedef Vector3<float>              Float3;     
typedef Vector3<double>             Double3;    

template <typename T> inline Vector3<T> vec(T x, T y, T z) {
    Vector3<T> result = { x, y, z };
    return result;
}

template <typename T> inline T dot(Vector3<T> u, Vector3<T> v) {
    return u.x * v.x + u.y * v.y + u.z * v.z;
}

template <typename T> inline T cross(T u, T v) {
    return vec(u.y * v.z - u.z * v.y,
               u.z * v.x - u.x * v.z,
               u.x * v.y - u.y * v.x);
}

// Vector operations
template <typename T> inline Vector3<T> operator-(Vector3<T> u) { return vec<T>(-u.x, -u.y, -u.z); }
template <typename T> inline Vector3<T> operator+=(Vector3<T> &u, Vector3<T> v) { return vec<T>(u.x+=v.x, u.y+=v.y, u.z+=v.z); }
template <typename T> inline Vector3<T> operator-=(Vector3<T> &u, Vector3<T> v) { return vec<T>(u.x-=v.x, u.y-=v.y, u.z-=v.z); }
template <typename T> inline Vector3<T> operator+(Vector3<T> u, Vector3<T> v) { return vec<T>(u.x+v.x, u.y+v.y, u.z+v.z); }
template <typename T> inline Vector3<T> operator-(Vector3<T> u, Vector3<T> v) { return vec<T>(u.x-v.x, u.y-v.y, u.z-v.z); }
template <typename T> inline Vector3<T> operator*(Vector3<T> u, Vector3<T> v) { return vec<T>(u.x*v.x, u.y*v.y, u.z*v.z); }
template <typename T> inline bool operator==(Vector3<T> u, Vector3<T> v) { return u.x == v.x && u.y == v.y && u.z == v.z; }
template <typename T> inline bool operator!=(Vector3<T> u, Vector3<T> v) { return u.x != v.x || u.y != v.y || u.z != v.z; }

// Scalar int promotion
template <typename T> inline Int3 operator*(int k, Vector3<T> v) { return vec<int>(k*v.x, k*v.y, k*v.z); }
template <typename T> inline Int3 operator*(Vector3<T> v, int k) { return vec<int>(k*v.x, k*v.y, k*v.z); }
template <typename T> inline Int3 operator/(Vector3<T> v, int k) { return vec<int>(v.x/k, v.y/k, v.z/k); }
template <typename T> inline Int3 operator+=(Vector3<T> &u, int k) { return vec<int>(u.x+=k, u.y+=k, u.z+=k); }
template <typename T> inline Int3 operator*=(Vector3<T> &u, int k) { return vec<int>(u.x*=k, u.y*=k, u.z*=k); }
template <typename T> inline Int3 operator<<(Vector3<T> u, int shift) { return vec<int>(u.x<<shift, u.y<<shift, u.z<<shift); }
template <typename T> inline Int3 operator>>(Vector3<T> u, int shift) { return vec<int>(u.x>>shift, u.y>>shift, u.z>>shift); }

// Scalar float promotion
template <typename T> inline Float3 operator*(float k, Vector3<T> v) { return vec<float>(k*v.x, k*v.y, k*v.z); }
template <typename T> inline Float3 operator*(Vector3<T> v, float k) { return vec<float>(k*v.x, k*v.y, k*v.z); }
template <typename T> inline Float3 operator/(Vector3<T> v, float k) { return vec<float>(v.x/k, v.y/k, v.z/k); }
template <typename T> inline Float3 operator+=(Vector3<T> &u, float k) { return vec<float>(u.x+=k, u.y+=k, u.z+=k); }
template <typename T> inline Float3 operator*=(Vector3<T> &u, float k) { return vec<float>(u.x*=k, u.y*=k, u.z*=k); }

// Scalar double promotion
template <typename T> inline Double3 operator*(double k, Vector3<T> v) { return vec<double>(k*v.x, k*v.y, k*v.z); }
template <typename T> inline Double3 operator*(Vector3<T> v, double k) { return vec<double>(k*v.x, k*v.y, k*v.z); }
template <typename T> inline Double3 operator/(Vector3<T> v, double k) { return vec<double>(v.x/k, v.y/k, v.z/k); }
template <typename T> inline Double3 operator+=(Vector3<T> &u, double k) { return vec<double>(u.x+=k, u.y+=k, u.z+=k); }
template <typename T> inline Double3 operator*=(Vector3<T> &u, double k) { return vec<double>(u.x*=k, u.y*=k, u.z*=k); }

// Vector int -> float promotion
inline Float3 operator+=(Float3 &u, Int3 v) { return vec(u.x+=v.x, u.y+=v.y, u.z+=v.z); }
inline Float3 operator-=(Float3 &u, Int3 v) { return vec(u.x-=v.x, u.y-=v.y, u.z-=v.z); }
inline Float3 operator+(Float3 u, Int3 v) { return vec(u.x+v.x, u.y+v.y, u.z+v.z); }
inline Float3 operator-(Float3 u, Int3 v) { return vec(u.x-v.x, u.y-v.y, u.z-v.z); }
inline Float3 operator*(Float3 u, Int3 v) { return vec(u.x*v.x, u.y*v.y, u.z*v.z); }
inline Float3 operator+(Int3 u, Float3 v) { return vec(u.x+v.x, u.y+v.y, u.z+v.z); }
inline Float3 operator-(Int3 u, Float3 v) { return vec(u.x-v.x, u.y-v.y, u.z-v.z); }
inline Float3 operator*(Int3 u, Float3 v) { return vec(u.x*v.x, u.y*v.y, u.z*v.z); }


struct Random {
    _SYSPseudoRandomState state;

    Random() {
        seed();
    }
    
    Random(uint32_t s) {
        seed(s);
    }
    
    void seed(uint32_t s) {
        _SYS_prng_init(&state, s);
    }

    void seed() {
        seed((uint32_t) _SYS_ticks_ns());
    }

    uint32_t raw() {
        return _SYS_prng_value(&state);
    }
    
    float random() {
        return raw() * (1.0f / 0xFFFFFFFF);
    }
    
    float expovariate(float averageInterval) {
        return -averageInterval * log(1.0f - random());
    }
    
    bool chance(float probability) {
        // Use 31 bits, to give us range to represent probability=1.
        const uint32_t mask = 0x7FFFFFFF;
        uint32_t threshold = probability * (mask + 1);
        return (raw() & mask) < threshold;
    }

    float uniform(float a, float b) {
        // Order of operations here allows constant folding if the endpoints are constant
        return a + raw() * ((b-a) / 0xFFFFFFFF);
    }

    template <typename T>
    T randint(T a, T b) {
        return T(a + _SYS_prng_valueBounded(&state, b - a));
    }

    template <typename T>
    T randrange(T a, T b) {
        return randint<T>(a, T(b - 1));
    }

    template <typename T>
    T randrange(T count) {
        return randrange<T>(T(0), count);
    }

};


struct AffineMatrix {
    float cx;   
    float cy;   
    float xx;   
    float xy;   
    float yx;   
    float yy;   

    AffineMatrix() {}

    AffineMatrix(float _xx, float _yx, float _cx,
                 float _xy, float _yy, float _cy)
        : cx(_cx), cy(_cy), xx(_xx),
          xy(_xy), yx(_yx), yy(_yy) {}
    
    static AffineMatrix identity() {
        return AffineMatrix(1, 0, 0,
                            0, 1, 0);
    }

    static AffineMatrix scaling(float s) {
        float inv_s = 1.0f / s;
        return AffineMatrix(inv_s, 0, 0,
                            0, inv_s, 0);
    }

    static AffineMatrix translation(float x, float y) {
        return AffineMatrix(1, 0, x,
                            0, 1, y);
    }

    static AffineMatrix translation(Float2 v) {
        return AffineMatrix(1, 0, v.x,
                            0, 1, v.y);
    }

    static AffineMatrix rotation(float angle) {
        float s, c;
        sincos(angle, &s, &c);
        return AffineMatrix(c, -s, 0,
                            s, c, 0);
    }

    void operator*= (const AffineMatrix &m) {
        AffineMatrix n;

        n.cx = xx*m.cx + yx*m.cy + cx;
        n.cy = xy*m.cx + yy*m.cy + cy;
        n.xx = xx*m.xx + yx*m.xy;
        n.xy = xy*m.xx + yy*m.xy;
        n.yx = xx*m.yx + yx*m.yy;
        n.yy = xy*m.yx + yy*m.yy;

        *this = n;
    }

    void translate(float x, float y) {
        *this *= translation(x, y);
    }

    void translate(Float2 v) {
        *this *= translation(v);
    }

    void rotate(float angle) {
        *this *= rotation(angle);
    }

    void scale(float s) {
        *this *= scaling(s);
    }
};

}   // namespace Sifteo