pico_math.h

Go to the documentation of this file.

 
#ifndef PICO_MATH_H
#define PICO_MATH_H
 
#include <float.h>   // FLT_MIN/MAX, DBL_MIN/MAX
#include <math.h>    // sqrt(f), cos(f), sin(f), atan2(f)...
#include <stdbool.h> // bool, true, false
#include <stdint.h>  // uint32_t
 
#ifdef __cplusplus
extern "C" {
#endif
 
// Common
 
#if defined(_MSC_VER)
    #define PM_INLINE __forceinline
#elif defined(__GNUC__) || defined(__clang__)
    #define PM_INLINE static inline __attribute((always_inline))
#else
    #define PM_INLINE
#endif
 
/*==============================================================================
 * Types, constants, and function aliases
 *============================================================================*/
 
#ifdef PICO_MATH_DOUBLE
    typedef double pfloat;
 
    #define PM_EPSILON 1e-7
 
    #define PM_PI   3.14159265358979323846264338327950288
    #define PM_PI2 (2.0 * PM_PI)
    #define PM_E    2.71828182845904523536028747135266250
 
    #define PM_FLOAT_MIN DBL_MIN
    #define PM_FLOAT_MAX DBL_MAX
 
    #define pf_sqrt  sqrt
    #define pf_cos   cos
    #define pf_sin   sin
    #define pf_acos  acos
    #define pf_asin  asin
    #define pf_atan2 atan2
    #define pf_abs   fabs
    #define pf_fmod  fmod
    #define pf_exp   exp
    #define pf_pow   pow
    #define pf_floor floor
    #define pf_ceil  ceil
    #define pf_log2  log2
    #define pf_max   fmax
    #define pf_min   fmin
 
#else
    typedef float pfloat;
 
    #define PM_EPSILON 1e-5f
 
    #define PM_PI   3.14159265359f
    #define PM_PI2 (2.0f * PM_PI)
    #define PM_E    2.71828182845f
 
    #define PM_FLOAT_MIN FLT_MIN
    #define PM_FLOAT_MAX FLT_MAX
 
    #define pf_sqrt  sqrtf
    #define pf_cos   cosf
    #define pf_sin   sinf
    #define pf_acos  acosf
    #define pf_asin  asinf
    #define pf_atan2 atan2f
    #define pf_abs   fabsf
    #define pf_fmod  fmodf
    #define pf_exp   expf
    #define pf_pow   powf
    #define pf_floor floorf
    #define pf_ceil  ceilf
    #define pf_log2  log2f
    #define pf_max   fmaxf
    #define pf_min   fminf
#endif
 
/*==============================================================================
 * Data structures
 *============================================================================*/
 
typedef struct
{
    pfloat x, y;
} pv2;
 
typedef struct
{
    pfloat t00, t10, t01, t11, tx, ty;
} pt2;
 
typedef struct
{
    pv2 min, max;
} pb2;
 
/*==============================================================================
 * Scalar functions and macros
 *============================================================================*/
 
PM_INLINE pfloat pf_clamp(pfloat val, pfloat min, pfloat max)
 {
    return ((val < min) ? min : ((val > max) ? max : val));
 }
 
PM_INLINE pfloat pf_sign(pfloat val)
{
    return ((0 == val) ? 0.0f : ((val > 0.0f) ? 1.0f : -1.0f));
}
 
PM_INLINE bool pf_equal(pfloat c1, pfloat c2)
{
    return pf_abs(c1 - c2) < PM_EPSILON;
}
 
PM_INLINE pfloat pf_lerp(pfloat a, pfloat b, pfloat alpha)
{
    return a + (b - a) * alpha;
}
 
pfloat pf_lerp_angle(pfloat angle1, pfloat angle2, pfloat alpha);
 
PM_INLINE pfloat pf_normalize_angle(pfloat angle)
{
    while (angle >= PM_PI2)
        angle -= PM_PI2;
 
    while (angle < 0.0f)
        angle += PM_PI2;
 
    return angle;
}
 
/*==============================================================================
 * Vectors
 *============================================================================*/
 
#define pv2_make(x, y) ((const pv2){ x, y })
 
PM_INLINE bool pv2_equal(pv2 v1, pv2 v2)
{
    return pf_equal(v1.x, v2.x) &&
           pf_equal(v1.y, v2.y);
}
 
PM_INLINE pv2 pv2_add(pv2 v1, pv2 v2)
{
    return pv2_make(v1.x + v2.x, v1.y + v2.y);
}
 
PM_INLINE pv2 pv2_sub(pv2 v1, pv2 v2)
{
    return pv2_make(v1.x - v2.x, v1.y - v2.y);
}
 
PM_INLINE pv2 pv2_scale(pv2 v, pfloat c)
{
    return pv2_make(v.x * c, v.y * c);
}
 
PM_INLINE pfloat pv2_dot(pv2 v1, pv2 v2)
{
    return v1.x * v2.x + v1.y * v2.y;
}
 
PM_INLINE pfloat pv2_len2(pv2 v)
{
    return pv2_dot(v, v);
}
 
PM_INLINE pfloat pv2_len(pv2 v)
{
    return pf_sqrt(pv2_len2(v));
}
 
PM_INLINE pv2 pv2_normalize(pv2 v)
{
    pfloat c = pv2_len(v);
 
    if (c < PM_EPSILON)
        return pv2_make(0.0f, 0.0f);
    else
        return pv2_scale(v, 1.0f / c);
}
 
PM_INLINE pv2 pv2_reflect(pv2 v)
{
    return pv2_scale(v, -1.0f);
}
 
PM_INLINE pv2 pv2_perp(pv2 v)
{
    return pv2_make(-v.y, v.x);
}
 
PM_INLINE pfloat pv2_cross(pv2 v1, pv2 v2)
{
    pv2 perp = pv2_perp(v1);
    return pv2_dot(perp, v2);
}
 
PM_INLINE pfloat pv2_angle(pv2 v)
{
    return pf_atan2(v.y, v.x);
}
 
PM_INLINE pv2 pv2_proj(pv2 v1, pv2 v2)
{
    pfloat d = pv2_dot(v1, v2) / pv2_dot(v2, v2);
    return pv2_scale(v2, d);
}
 
PM_INLINE pfloat pv2_dist(pv2 v1, pv2 v2)
{
    pv2 v = pv2_sub(v1, v2);
    return pv2_len(v);
}
 
PM_INLINE pv2 pv2_lerp(pv2 v1, pv2 v2, pfloat alpha)
{
    pv2 out;
    out.x = pf_lerp(v1.x, v2.x, alpha);
    out.y = pf_lerp(v1.y, v2.y, alpha);
    return out;
}
 
#define pv2_zero() (pv2_make(0.0f, 0.0f))
 
PM_INLINE pv2 pv2_polar(pfloat angle, pfloat len)
{
    return pv2_make(len * pf_cos(angle), len * pf_sin(angle));
}
 
PM_INLINE pv2 pv2_min(pv2 v1, pv2 v2)
{
    return pv2_make(pf_min(v1.x, v2.x), pf_min(v1.y, v2.y));
}
 
PM_INLINE pv2 pv2_max(pv2 v1, pv2 v2)
{
    return pv2_make(pf_max(v1.x, v2.x), pf_max(v1.y, v2.y));
}
 
PM_INLINE pv2 pv2_floor(pv2 v)
{
    return pv2_make(pf_floor(v.x), pf_floor(v.y));
}
 
PM_INLINE pv2 pv2_ceil(pv2 v)
{
    return pv2_make(pf_ceil(v.x), pf_ceil(v.y));
}
 
/*==============================================================================
 * 2D Affine Transforms
 *============================================================================*/
 
#define pt2_make(t00, t01, tx, t10, t11, ty) ((const pt2){ t00, t10, t01, t11, tx, ty })
 
PM_INLINE pt2 pt2_identity(void)
{
    return pt2_make(1.0f, 0.0f, 0.0f,
                    0.0f, 1.0f, 0.0f);
}
 
bool pt2_equal(const pt2* t1, const pt2* t2);
 
PM_INLINE pv2 pt2_get_pos(const pt2* t)
{
    return pv2_make(t->tx, t->ty);
}
 
PM_INLINE void pt2_set_pos(pt2* t, pv2 pos)
{
    t->tx = pos.x;
    t->ty = pos.y;
}
 
PM_INLINE pfloat pt2_get_angle(const pt2* t)
{
    return pf_normalize_angle(pf_atan2(t->t10, t->t00));
}
 
void pt2_set_scale(pt2* t, pv2 scale);
 
pv2 pt2_get_scale(const pt2* t);
 
void pt2_set_angle(pt2* t, pfloat angle);
 
PM_INLINE pv2 pt2_map(const pt2* t, pv2 v)
{
    pv2 out;
    out.x = t->t00 * v.x + t->t01 * v.y + t->tx;
    out.y = t->t10 * v.x + t->t11 * v.y + t->ty;
    return out;
}
 
PM_INLINE pfloat pt2_det(const pt2* t)
{
    return t->t00 * t->t11 - t->t01 * t->t10;
}
 
pt2 pt2_inv(const pt2* t);
 
pt2 pt2_mult(const pt2* t1, const pt2* t2);
 
pt2 pt2_lerp(const pt2* t1, const pt2* t2, pfloat alpha);
 
PM_INLINE pt2 pt2_scaling(pv2 scale)
{
     return pt2_make(scale.x, 0.0f,    0.0f,
                     0.0f,    scale.y, 0.0f);
}
 
PM_INLINE pt2 pt2_rotation(pfloat angle)
{
    pfloat c = pf_cos(angle);
    pfloat s = pf_sin(angle);
 
    return pt2_make(c, -s, 0.0f,
                    s,  c, 0.0f);
}
 
PM_INLINE pt2 pt2_translation(pv2 pos)
{
    return pt2_make(1.0f, 0.0f, pos.x,
                    0.0f, 1.0f, pos.y);
}
 
PM_INLINE void pt2_scale(pt2* t, pv2 scale)
{
    pt2 scaling = pt2_scaling(scale);
    *t = pt2_mult(&scaling, t);
}
 
PM_INLINE void pt2_rotate(pt2* t, pfloat angle)
{
    pt2 rotation = pt2_rotation(angle);
    *t = pt2_mult(&rotation, t);
}
 
PM_INLINE void pt2_translate(pt2* t, pv2 pos)
{
    pt2 translation = pt2_translation(pos);
    *t = pt2_mult(&translation, t);
}
 
/*==============================================================================
 * 2D Box (AABB)
 *============================================================================*/
 
#define pb2_make_minmax(min, max) ((const pb2){ min, max })
 
#define pb2_make(x, y, w, h) ((const pb2){ { x, y }, { x + w, y + h } })
 
#define pb2_zero() (pb2_make(0.0f, 0.0f, 0.0f, 0.0f))
 
PM_INLINE pv2 pb2_get_pos(const pb2* b)
{
    return b->min;
}
 
PM_INLINE pv2 pb2_get_size(const pb2* b)
{
    return pv2_sub(b->max, b->min);
}
 
PM_INLINE void pb2_set_pos(pb2* b, pv2 pos)
{
    pv2 size = pb2_get_size(b);
    *b = pb2_make(pos.x, pos.y, size.x, size.y);
}
 
PM_INLINE void pb2_set_size(pb2* b, pv2 size)
{
    pv2 pos = pb2_get_pos(b);
    *b = pb2_make(pos.x, pos.y, size.x, size.y);
}
 
bool pb2_equal(const pb2* b1, const pb2* b2);
 
pb2 pb2_combine(const pb2* b1, const pb2* b2);
 
pb2 pb2_overlap(const pb2* b1, const pb2* b2);
 
PM_INLINE bool pb2_overlaps(const pb2* b1, const pb2* b2)
{
    return b1->max.x >= b2->min.x &&
           b1->max.y >= b2->min.y &&
           b2->max.x >= b1->min.x &&
           b2->max.y >= b1->min.y;
}
 
PM_INLINE bool pb2_contains(const pb2* b1, const pb2* b2)
{
    return b2->min.x >= b1->min.x &&
           b2->min.y >= b1->min.y &&
           b2->max.x <= b1->max.x &&
           b2->max.y <= b1->max.y;
}
 
PM_INLINE bool pb2_contains_point(const pb2* b, pv2 v)
{
    pfloat x = v.x;
    pfloat y = v.y;
 
    return x > b->min.x &&
           y > b->min.y &&
           x < b->max.x &&
           y < b->max.y;
}
 
PM_INLINE pfloat pb2_area(const pb2* b)
{
    return (b->max.x - b->min.x) * (b->max.y - b->min.y);
}
 
PM_INLINE pv2 pb2_center(const pb2* b)
{
    pv2 offset = pv2_scale(pv2_sub(b->max, b->min), 1.0f / 2.0f);
    return pv2_add(offset, b->min);
}
 
pb2 pb2_enclosing(const pv2 verts[], int count);
 
pb2 pb2_transform(const pt2* t, const pb2* b);
 
typedef struct prng_t
{
    uint32_t s[4];
} prng_t;
 
void prng_seed(prng_t* rng, uint64_t seed);
 
uint32_t prng_random(prng_t* rng);
 
pfloat pf_random(prng_t* rng);
 
#ifdef __cplusplus
}
#endif
 
#endif // PICO_MATH_H
 
#ifdef PICO_MATH_IMPLEMENTATION
 
pfloat pf_lerp_angle(pfloat angle1, pfloat angle2, pfloat alpha)
{
    const pv2 v1 = pv2_make(pf_cos(angle1), pf_sin(angle1));
    const pv2 v2 = pv2_make(pf_cos(angle2), pf_sin(angle2));
 
    // Calculuate cosine of angle between the two vectors
    pfloat dot = pf_clamp(pv2_dot(v1, v2), -1.0f, 1.0f);
 
    // LERP if the cosine is too close to its limits
    if (pf_equal(dot, 1.0f) || pf_equal(dot, -1.0f))
    {
        pv2 tmp = pv2_lerp(v1, v2, alpha);
        return pf_normalize_angle(pf_atan2(tmp.y, tmp.x));
    }
 
    // Calculate angle
    pfloat angle = pf_acos(dot) * alpha;
 
    // Gram-Schmidt(construct a new vector 'v0' that is orthogonal to 'v1')
    pv2 v0  = pv2_sub(v2, pv2_scale(v1, dot));
    v0 = pv2_normalize(v0);
 
    // Calcuate vector in new coordinate system
    pv2 tmp1 = pv2_scale(v1, pf_cos(angle));
    pv2 tmp2 = pv2_scale(v0, pf_sin(angle));
 
    pv2 tmp = pv2_add(tmp1, tmp2);
 
    // Calculate new angle
    return pf_normalize_angle(pf_atan2(tmp.y, tmp.x));
}
 
bool pt2_equal(const pt2* t1, const pt2* t2)
{
    return pf_equal(t1->t00, t2->t00) &&
           pf_equal(t1->t10, t2->t10) &&
           pf_equal(t1->t01, t2->t01) &&
           pf_equal(t1->t11, t2->t11) &&
           pf_equal(t1->tx,  t2->tx)  &&
           pf_equal(t1->ty,  t2->ty);
}
 
void pt2_set_scale(pt2* t, pv2 scale)
{
    pfloat angle = pt2_get_angle(t);
 
    pfloat c = pf_cos(angle);
    pfloat s = pf_sin(angle);
 
    pfloat sx = scale.x;
    pfloat sy = scale.y;
 
    t->t00 = sx * c; t->t01 = sy * -s;
    t->t10 = sx * s; t->t11 = sy *  c;
}
 
pv2 pt2_get_scale(const pt2* t)
{
    pfloat angle = pt2_get_angle(t);
    pfloat cos_sign = pf_sign(pf_cos(angle));
 
    pv2 out;
 
    if (0.0f == cos_sign) //TODO: pf_equal?
    {
        out.x =  t->t10;
        out.y = -t->t01;
        return out;
    }
 
    pv2 v1 = pv2_make(t->t00, t->t10);
    pv2 v2 = pv2_make(t->t01, t->t11);
 
    out.x = pf_sign(t->t00) * cos_sign * pv2_len(v1);
    out.y = pf_sign(t->t11) * cos_sign * pv2_len(v2);
 
    return out;
}
 
void pt2_set_angle(pt2* t, pfloat angle)
{
    pfloat c = pf_cos(angle);
    pfloat s = pf_sin(angle);
 
    pv2 scale = pt2_get_scale(t);
 
    pfloat sx = scale.x;
    pfloat sy = scale.y;
 
    t->t00 = sx * c; t->t01 = sy * -s;
    t->t10 = sx * s; t->t11 = sy *  c;
}
 
pt2 pt2_inv(const pt2* t)
{
    pfloat det = pt2_det(t);
 
    if (0.0f == det) // Intentionally not using epsilon because determinants
    {                // can be really small and still be valid
        return pt2_identity();
    }
 
    pfloat inv_det = 1.0f / det;
 
    pt2 out;
 
    out.t00 =  t->t11 * inv_det; out.t01 = -t->t01 * inv_det;
    out.t10 = -t->t10 * inv_det; out.t11 =  t->t00 * inv_det;
 
    out.tx = (t->t01 * t->ty - t->t11 * t->tx) * inv_det;
    out.ty = (t->t10 * t->tx - t->t00 * t->ty) * inv_det;
 
    return out;
}
 
pt2 pt2_mult(const pt2* t1, const pt2* t2)
{
    pt2 out;
 
    out.t00 = t1->t00 * t2->t00 + t1->t01 * t2->t10;
    out.t10 = t1->t10 * t2->t00 + t1->t11 * t2->t10;
 
    out.t01 = t1->t00 * t2->t01 + t1->t01 * t2->t11;
    out.t11 = t1->t10 * t2->t01 + t1->t11 * t2->t11;
 
    out.tx = t1->t00 * t2->tx + t1->t01 * t2->ty + t1->tx;
    out.ty = t1->t10 * t2->tx + t1->t11 * t2->ty + t1->ty;
 
    return out;
}
 
pt2 pt2_lerp(const pt2* t1, const pt2* t2, pfloat alpha)
{
    pv2 scale1 = pt2_get_scale(t1);
    pv2 scale2 = pt2_get_scale(t2);
 
    pfloat angle1 = pt2_get_angle(t1);
    pfloat angle2 = pt2_get_angle(t2);
 
    pv2 pos1 = pt2_get_pos(t1);
    pv2 pos2 = pt2_get_pos(t2);
 
    pv2 scale = pv2_lerp(scale1, scale2, alpha);
    pv2 pos = pv2_lerp(pos1, pos2, alpha);
    pfloat angle = pf_lerp_angle(angle1, angle2, alpha);
 
    pfloat c = pf_cos(angle);
    pfloat s = pf_sin(angle);
 
    pfloat sx = scale.x;
    pfloat sy = scale.y;
 
    pfloat tx = pos.x;
    pfloat ty = pos.y;
 
    pt2 out;
 
    out.t00 = sx * c; out.t01 = sy * -s; out.tx = tx;
    out.t10 = sx * s; out.t11 = sy *  c; out.ty = ty;
 
    return out;
}
 
bool pb2_equal(const pb2* b1, const pb2* b2)
{
    return pv2_equal(b1->min, b2->min) && pv2_equal(b1->max, b2->max);
}
 
pb2 pb2_combine(const pb2* b1, const pb2* b2)
{
    pv2 min = pv2_min(b1->min, b2->min);
    pv2 max = pv2_max(b1->max, b2->max);
    return pb2_make_minmax(min, max);
}
 
pb2 pb2_overlap(const pb2* b1, const pb2* b2)
{
    if (!pb2_overlaps(b1, b2))
        return pb2_make(0.0f, 0.0f, 0.0f, 0.0f);
 
    pv2 min = pv2_max(b1->min, b2->min);
    pv2 max = pv2_min(b1->max, b2->max);
    return pb2_make_minmax(min, max);
}
 
pb2 pb2_enclosing(const pv2 verts[], int count)
{
    if (0 == count)
        return pb2_make(0.0f, 0.0f, 0.0f, 0.0f);
 
    pv2 min = verts[0];
    pv2 max = verts[0];
 
    for (int i = 1; i < count; i++)
    {
        min = pv2_min(min, verts[i]);
        max = pv2_max(max, verts[i]);
    }
 
    return pb2_make_minmax(min, max);
}
 
pb2 pb2_transform(const pt2* t, const pb2* b)
{
    pv2 pos  = pb2_get_pos(b);
    pv2 size = pb2_get_size(b);
 
    pfloat w = size.x;
    pfloat h = size.y;
 
    pv2 verts[4];
 
    verts[0] = pos;
    verts[1] = pv2_make(pos.x,     pos.y + h);
    verts[2] = pv2_make(pos.x + w, pos.y + h);
    verts[3] = pv2_make(pos.x + w, pos.y);
 
    verts[0] = pt2_map(t, verts[0]);
    verts[1] = pt2_map(t, verts[1]);
    verts[2] = pt2_map(t, verts[2]);
    verts[3] = pt2_map(t, verts[3]);
 
    return pb2_enclosing(verts, 4);
}
 
/*
 * Implementation of the xoshiro128** algorithm
 * https://en.wikipedia.org/wiki/Xorshift
 */
 
void prng_seed(prng_t* rng, uint64_t seed)
{
    for (int i = 0; i < 2; i++)
    {
        uint64_t result  = (seed += 0x9E3779B97f4A7C15);
        result = (result ^ (result >> 30)) * 0xBF58476D1CE4E5B9;
        result = (result ^ (result >> 27)) * 0x94D049BB133111EB;
        result = result  ^ (result >> 31);
 
        int j = i * 2;
 
        rng->s[j]     = (uint32_t)result;
        rng->s[j + 1] = (uint32_t)(result >> 32);
    }
}
 
static uint32_t rng_rol32(uint32_t x, int k)
{
    return (x << k) | (x >> (32 - k));
}
 
uint32_t prng_random(prng_t* rng)
{
    uint32_t *s = rng->s;
    uint32_t const result = rng_rol32(s[1] * 5, 7) * 9;
    uint32_t const t = s[1] << 17;
 
    s[2] ^= s[0];
    s[3] ^= s[1];
    s[1] ^= s[2];
    s[0] ^= s[3];
 
    s[2] ^= t;
    s[3] = rng_rol32(s[3], 45);
 
    return result;
}
 
pfloat pf_random(prng_t* rng)
{
    return (pfloat)prng_random(rng) / (pfloat)UINT32_MAX;
}
 
#endif // PICO_MATH_IMPLEMENTATION
 
/*
    ----------------------------------------------------------------------------
    This software is available under two licenses (A) or (B). You may choose
    either one as you wish:
    ----------------------------------------------------------------------------
 
    (A) The zlib License
 
    Copyright (c) 2021 James McLean
 
    This software is provided 'as-is', without any express or implied warranty.
    In no event will the authors be held liable for any damages arising from the
    use of this software.
 
    Permission is granted to anyone to use this software for any purpose,
    including commercial applications, and to alter it and redistribute it
    freely, subject to the following restrictions:
 
    1. The origin of this software must not be misrepresented; you must not
    claim that you wrote the original software. If you use this software in a
    product, an acknowledgment in the product documentation would be appreciated
    but is not required.
 
    2. Altered source versions must be plainly marked as such, and must not be
    misrepresented as being the original software.
 
    3. This notice may not be removed or altered from any source distribution.
 
    ----------------------------------------------------------------------------
 
    (B) Public Domain (www.unlicense.org)
 
    This is free and unencumbered software released into the public domain.
 
    Anyone is free to copy, modify, publish, use, compile, sell, or distribute
    this software, either in source code form or as a compiled binary, for any
    purpose, commercial or non-commercial, and by any means.
 
    In jurisdictions that recognize copyright laws, the author or authors of
    this software dedicate any and all copyright interest in the software to the
    public domain. We make this dedication for the benefit of the public at
    large and to the detriment of our heirs and successors. We intend this
    dedication to be an overt act of relinquishment in perpetuity of all present
    and future rights to this software under copyright law.
 
    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 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.
*/