jacob/power_play
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
3049e02b57
power_play/src/math.h

1057 lines
26 KiB
C
Raw Blame History

#ifndef MATH_H
#define MATH_H
/* Math functions are default 32 bit (f32, i32, etc) unless specified */
#include "intrinsics.h"
INLINE f32 math_sqrt(f32 x);
/* ========================== *
* Float rounding
* ========================== */
/* TODO: Don't use intrinsics for these. */
/* Round */
INLINE f32 math_round(f32 f)
{
return ix_round_f32_to_f32(f);
}
INLINE f64 math_round64(f64 f)
{
return ix_round_f64_to_f64(f);
}
INLINE i32 math_round_to_int(f32 f)
{
return ix_round_f32_to_i32(f);
}
INLINE i64 math_round_to_int64(f64 f)
{
return ix_round_f64_to_i64(f);
}
/* Floor */
INLINE f32 math_floor(f32 f)
{
return ix_floor_f32_to_f32(f);
}
INLINE f64 math_floor64(f64 f)
{
return ix_floor_f64_to_f64(f);
}
INLINE i32 math_floor_to_int(f32 f)
{
return ix_floor_f32_to_i32(f);
}
INLINE i64 math_floor_to_int64(f64 f)
{
return ix_floor_f64_to_i64(f);
}
/* Ceil */
INLINE f32 math_ceil(f32 f)
{
return ix_ceil_f32_to_f32(f);
}
INLINE f64 math_ceil64(f64 f)
{
return ix_ceil_f64_to_f64(f);
}
INLINE i32 math_ceil_to_int(f32 f)
{
return ix_ceil_f32_to_i32(f);
}
INLINE i64 math_ceil_to_int64(f64 f)
{
return ix_ceil_f64_to_i64(f);
}
/* Truncate */
INLINE f32 math_trunc(f32 f)
{
return ix_trunc_f32_to_f32(f);
}
INLINE f64 math_trunc64(f64 f)
{
return ix_trunc_f64_to_f64(f);
}
/* ========================== *
* Float properties
* ========================== */
INLINE f32 math_fmod(f32 x, f32 m)
{
return x - m * math_trunc(x / m);
}
INLINE f64 math_fmod64(f64 x, f64 m)
{
return x - m * math_trunc64(x / m);
}
INLINE f32 math_fabs(f32 f)
{
u32 truncated = *(u32 *)&f & 0x7FFFFFFF;
return *(f32 *)&truncated;
}
INLINE f64 math_fabs64(f64 f)
{
u64 truncated = *(u64 *)&f & 0x7FFFFFFFFFFFFFFFULL;
return *(f64 *)&truncated;
}
INLINE i32 math_fsign(f32 f)
{
u32 sign_bit = (*(u32 *)&f >> 31) & 1;
return 1 + -((i32)(sign_bit << 1));
}
INLINE i64 math_fsign64(f64 f)
{
u64 sign_bit = (*(u64 *)&f >> 63) & 1;
return 1 + -((i64)(sign_bit << 1));
}
/* ========================== *
* Exponential
* ========================== */
/* Taken from https://gist.github.com/orlp/3551590 */
INLINE u64 math_pow_u64(u64 base, u8 exp)
{
LOCAL_PERSIST const u8 highest_bit_set[] = {
0, 1, 2, 2, 3, 3, 3, 3,
4, 4, 4, 4, 4, 4, 4, 4,
5, 5, 5, 5, 5, 5, 5, 5,
5, 5, 5, 5, 5, 5, 5, 5,
6, 6, 6, 6, 6, 6, 6, 6,
6, 6, 6, 6, 6, 6, 6, 6,
6, 6, 6, 6, 6, 6, 6, 6,
6, 6, 6, 6, 6, 6, 6, 255, /* Anything past 63 is a guaranteed overflow with base > 1 */
255, 255, 255, 255, 255, 255, 255, 255,
255, 255, 255, 255, 255, 255, 255, 255,
255, 255, 255, 255, 255, 255, 255, 255,
255, 255, 255, 255, 255, 255, 255, 255,
255, 255, 255, 255, 255, 255, 255, 255,
255, 255, 255, 255, 255, 255, 255, 255,
255, 255, 255, 255, 255, 255, 255, 255,
255, 255, 255, 255, 255, 255, 255, 255,
255, 255, 255, 255, 255, 255, 255, 255,
255, 255, 255, 255, 255, 255, 255, 255,
255, 255, 255, 255, 255, 255, 255, 255,
255, 255, 255, 255, 255, 255, 255, 255,
255, 255, 255, 255, 255, 255, 255, 255,
255, 255, 255, 255, 255, 255, 255, 255,
255, 255, 255, 255, 255, 255, 255, 255,
255, 255, 255, 255, 255, 255, 255, 255,
255, 255, 255, 255, 255, 255, 255, 255,
255, 255, 255, 255, 255, 255, 255, 255,
255, 255, 255, 255, 255, 255, 255, 255,
255, 255, 255, 255, 255, 255, 255, 255,
255, 255, 255, 255, 255, 255, 255, 255,
255, 255, 255, 255, 255, 255, 255, 255,
255, 255, 255, 255, 255, 255, 255, 255,
255, 255, 255, 255, 255, 255, 255, 255,
};
u64 result = 1;
switch (highest_bit_set[exp]) {
case 255: {
/* 255 = overflow, return 0 */
if (base == 1) {
return 1;
}
// if (base == -1) {
// return 1 - 2 * (exp & 1);
// }
return 0;
} break;
case 6: {
if (exp & 1) result *= base;
exp >>= 1;
base *= base;
} FALLTHROUGH;
case 5: {
if (exp & 1) result *= base;
exp >>= 1;
base *= base;
} FALLTHROUGH;
case 4: {
if (exp & 1) result *= base;
exp >>= 1;
base *= base;
} FALLTHROUGH;
case 3: {
if (exp & 1) result *= base;
exp >>= 1;
base *= base;
} FALLTHROUGH;
case 2: {
if (exp & 1) result *= base;
exp >>= 1;
base *= base;
} FALLTHROUGH;
case 1: {
if (exp & 1) result *= base;
} FALLTHROUGH;
default: return result;
}
}
/* Based on FreeBSD's implementation
* https://github.com/freebsd/freebsd-src/blob/main/lib/msun/src/e_logf.c */
INLINE f32 math_ln(f32 x)
{
LOCAL_PERSIST const f32 ln2_hi = 6.9313812256e-01f;
LOCAL_PERSIST const f32 ln2_lo = 9.0580006145e-06f;
i32 x_int = *(u32 *)&x;
i32 k = 0;
if (x_int < 0x00800000) {
f32 two_p25 = 3.3554432000e+07;
if ((x_int & 0x7fffffff) == 0) {
/* Return -inf if x is 0 */
return -two_p25 / 0;
} else if (x_int < 0) {
/* Return NaN if x is negative */
return (x - x) / 0;
}
k -= 25;
x *= two_p25;
x_int = *(u32 *)&x;
} else if (x_int >= 0x7f800000) {
return x + x;
}
k += (x_int >> 23) - 127;
x_int &= 0x007fffff;
i32 i = (x_int + (0x95f64 << 3)) & 0x800000;
i32 x_int_normalized = x_int | (i ^ 0x3f800000);
x = *(f32 *)&x_int_normalized - 1.0f;
k += (i >> 23);
if ((0x007fffff & (0x8000 + x_int)) < 0xc000) {
if (x == 0.0f) {
if (k == 0) {
return 0;
} else {
return (f32)k * ln2_hi + (f32)k * ln2_lo;
}
}
f32 r = x * x * (0.5f - 0.33333333333333333f * x);
if (k == 0) {
return x - r;
} else {
return (f32)k * ln2_hi - ((r - (f32)k * ln2_lo) - x);
}
}
f32 s = x / (2.0f + x);
f32 z = s * s;
f32 w = z * z;
f32 r = (z * (0.66666662693f + w * 0.28498786688f)) + (w * (0.40000972152f + w * 0.24279078841f));
if (((x_int - (0x6147a << 3)) | ((0x6b851 << 3) - x_int)) > 0) {
f32 hfsq = 0.5f * x * x;
if (k == 0) {
return x - (hfsq - s * (hfsq + r));
} else {
return (f32)k * ln2_hi - ((hfsq - (s * (hfsq + r) + (f32)k * ln2_lo)) - x);
}
} else {
if (k == 0) {
return x - s * (x - r);
} else {
return (f32)k * ln2_hi - ((s * (x - r) - (f32)k * ln2_lo) - x);
}
}
}
/* Based on FreeBSD's implementation
* https://github.com/freebsd/freebsd-src/blob/main/lib/msun/src/e_expf.c */
INLINE f32 math_exp(f32 x)
{
LOCAL_PERSIST const f32 half[2] = { 0.5, -0.5 };
LOCAL_PERSIST const f32 o_threshold = 8.8721679688e+01f;
LOCAL_PERSIST const f32 u_threshold = -1.0397208405e+02f;
LOCAL_PERSIST const f32 ln2_hi[2] = { 6.9314575195e-01f, -6.9314575195e-01f };
LOCAL_PERSIST const f32 ln2_lo[2] = { 1.4286067653e-06f, -1.4286067653e-06f };
LOCAL_PERSIST const f32 inv_ln2 = 1.4426950216e+00f;
LOCAL_PERSIST const f32 huge = 1.0e+30f;
LOCAL_PERSIST const f32 two_m100 = 7.8886090522e-31f;
u32 x_uint = *(u32 *)&x;
i32 x_sign_bit = (x_uint >> 31) & 1;
x_uint &= 0x7fffffff;
/* Filter out non-finite argument */
if (x_uint >= 0x42b17218) { /* if |x|>=88.721... */
if (x_uint > 0x7f800000) {
return x + x; /* NaN */
} else if (x_uint == 0x7f800000) {
return (x_sign_bit == 0) ? x : 0.0f;
}
if (x > o_threshold) {
/* Overflow */
return huge * huge;
} else if (x < u_threshold) {
/* Underflow */
return two_m100 * two_m100;
}
}
/* Argument reduction */
i32 k = 0;
f32 hi = 0;
f32 lo = 0;
if (x_uint > 0x3eb17218) {
if (x_uint < 0x3F851592) {
hi = x - ln2_hi[x_sign_bit];
lo = ln2_lo[x_sign_bit];
k = 1 - x_sign_bit - x_sign_bit;
} else {
k = (i32)(inv_ln2 * x + half[x_sign_bit]);
hi = x - (f32)k * ln2_hi[0];
lo = (f32)k * ln2_lo[0];
}
x = hi - lo;
} else if (x_uint < 0x39000000) {
if (huge + x > 1.0f) {
return 1.0f + x;
}
} else {
k = 0;
}
f32 two_pk;
if (k >= -125) {
u32 temp = ((u32)(0x7f + k)) << 23;
two_pk = *(f32 *)&temp;
} else {
u32 temp = ((u32)(0x7f + (k + 100))) << 23;
two_pk = *(f32 *)&temp;
}
f32 t = x * x;
f32 c = x - t * (1.6666625440e-1f + t * -2.7667332906e-3f);
if (k == 0) {
return 1.0f - ((x * c) / (c - 2.0f) - x);
} else {
f32 y = 1.0f - ((lo - (x * c)/(2.0f - c))-hi);
if (k >= -125) {
if (k==128) {
u32 temp = 0x7f800000;
return y * 2.0f * (*(f32 *)&temp);
}
return y * two_pk;
} else {
return y * two_pk * two_m100;
}
}
}
INLINE f32 math_pow(f32 a, f32 b)
{
if (a >= 0) {
/* a is positive */
return math_exp(math_ln(a) * b);
} else {
/* a is negative */
i32 res_sign = math_round_to_int(b) % 2 == 0 ? 1 : -1;
return math_exp(math_ln(-a) * b) * res_sign;
}
}
INLINE f32 math_sqrt(f32 x)
{
return ix_sqrt_f32(x);
}
INLINE f32 math_rsqrt(f32 x)
{
return ix_rsqrt_f32(x);
}
/* From Quake III -
* https://github.com/id-Software/Quake-III-Arena/blob/dbe4ddb10315479fc00086f08e25d968b4b43c49/code/game/q_math.c#L552
*/
INLINE f32 math_rsqrt_fast(f32 x)
{
const f32 three_halfs = 1.5f;
f32 x2 = x * 0.5f;
f32 y = x;
i32 i = *(i32 *)&y;
i = 0x5f3759df - (i >> 1);
y = *(f32 *)&i;
y *= three_halfs - (x2 * y * y);
return y;
}
/* ========================== *
* Trig
* ========================== */
/* Sine approximation using a parabola adjusted to minimize error, as described in
* https://web.archive.org/web/20080228213915/http://www.devmaster.net/forums/showthread.php?t=5784
*
* https://www.desmos.com/calculator/gbtjvt2we8
* c: adjustment weight
* f(x): original parabola
* g(x): adjusted parabola
* h(x): error
*/
INLINE f32 math_sin(f32 x)
{
const f32 c = 0.225f;
x -= (TAU * math_trunc(x / TAU)); /* [0, TAU] */
x += (TAU * (x < -PI)) - (TAU * (x > PI)); /* [-PI, PI] */
f32 y = (4.0f/PI) * x + (-4.0f/(PI*PI)) * x * math_fabs(x);
y = c * (y * math_fabs(y) - y) + y;
return y;
}
INLINE f32 math_cos(f32 x)
{
return math_sin(x + (PI / 2.0f));
}
/* https://mazzo.li/posts/vectorized-atan2.html */
INLINE f32 math_atan2(f32 y, f32 x) {
const f32 a1 = 0.99997726f;
const f32 a3 = -0.33262347f;
const f32 a5 = 0.19354346f;
const f32 a7 = -0.11643287f;
const f32 a9 = 0.05265332f;
const f32 a11 = -0.01172120f;
/* Ensure input is in [-1, +1] */
b32 swap = math_fabs(x) < math_fabs(y);
f32 s = (swap ? x : y) / (swap ? y : x);
/* Approximate atan */
f32 s_sq = s*s;
f32 res = s * (a1 + s_sq * (a3 + s_sq * (a5 + s_sq * (a7 + s_sq * (a9 + s_sq * a11)))));
res = swap ? (s >= 0.0f ? (PI / 2.f) : -(PI / 2.f)) - res : res;
/* Adjust quadrants */
if (x < 0.0f && y >= 0.0f) { res = PI + res; } /* 2nd quadrant */
else if (x <= 0.0f && y < 0.0f) { res = -PI + res; } /* 3rd quadrant */
else if (x == 0.0f && y == 0.0f) { res = 0; } /* NaN -> 0 */
return res;
}
INLINE f32 math_asin(f32 x)
{
/* TODO: Dedicated arcsin approximation */
return math_atan2(x, math_sqrt(1.0f - (x * x)));
}
INLINE f32 math_acos(f32 x)
{
/* TODO: Dedicated arccos approximation */
return (PI / 2.0f) - math_atan2(x, math_sqrt(1.0f - (x * x)));
}
/* ========================== *
* Lerp
* ========================== */
INLINE f32 math_lerp(f32 val0, f32 val1, f32 t)
{
return val0 + ((val1 - val0) * t);
}
INLINE f64 math_lerp64(f64 val0, f64 val1, f64 t)
{
return val0 + ((val1 - val0) * t);
}
INLINE f32 math_lerp_angle(f32 a, f32 b, f32 t) {
f32 diff = math_fmod(b - a, TAU);
diff = math_fmod(2.0f * diff, TAU) - diff;
return a + diff * t;
}
/* ========================== *
* V2
* ========================== */
INLINE struct v2 v2_mul(struct v2 a, f32 s)
{
return V2(a.x * s, a.y * s);
}
INLINE struct v2 v2_mul_v2(struct v2 a, struct v2 b)
{
return V2(a.x * b.x, a.y * b.y);
}
INLINE struct v2 v2_div(struct v2 a, f32 s)
{
f32 d = 1 / s;
return V2(a.x * d, a.y * d);
}
INLINE struct v2 v2_div_v2(struct v2 a, struct v2 b)
{
return V2(a.x * (1 / b.x), a.y * (1 / b.y));
}
INLINE struct v2 v2_neg(struct v2 a)
{
return V2(-a.x, -a.y);
}
INLINE struct v2 v2_add(struct v2 a, struct v2 b)
{
return V2(a.x + b.x, a.y + b.y);
}
INLINE struct v2 v2_sub(struct v2 a, struct v2 b)
{
return V2(a.x - b.x, a.y - b.y);
}
INLINE f32 v2_len(struct v2 a)
{
return math_sqrt(a.x * a.x + a.y * a.y);
}
INLINE f32 v2_len_squared(struct v2 a)
{
return a.x * a.x + a.y * a.y;
}
INLINE struct v2 v2_perp(struct v2 a)
{
return V2(-a.y, a.x);
}
INLINE struct v2 v2_norm(struct v2 a)
{
f32 l = v2_len_squared(a);
if (l != 0) {
l = math_sqrt(l);
a.x /= l;
a.y /= l;
}
return a;
}
INLINE struct v2 v2_norm_fast(struct v2 a)
{
f32 l = v2_len_squared(a);
f32 r_sqrt = math_rsqrt_fast(l);
a.x *= r_sqrt;
a.y *= r_sqrt;
return a;
}
INLINE struct v2 v2_round(struct v2 a)
{
return V2(
math_round(a.x),
math_round(a.y)
);
}
INLINE struct v2 v2_floor(struct v2 a)
{
return V2(
math_floor(a.x),
math_floor(a.y)
);
}
INLINE struct v2 v2_ceil(struct v2 a)
{
return V2(
math_ceil(a.x),
math_ceil(a.y)
);
}
INLINE f32 v2_dot(struct v2 a, struct v2 b)
{
return a.x * b.x + a.y * b.y;
}
INLINE f32 v2_wedge(struct v2 a, struct v2 b)
{
return a.x * b.y - a.y * b.x;
}
INLINE f32 v2_distance(struct v2 a, struct v2 b)
{
f32 dx = b.x - a.x;
f32 dy = b.y - a.y;
return math_sqrt(dx * dx + dy * dy);
}
INLINE b32 v2_eq(struct v2 a, struct v2 b)
{
return a.x == b.x && a.y == b.y;
}
INLINE struct v2 v2_lerp(struct v2 val0, struct v2 val1, f32 t)
{
struct v2 res;
res.x = math_lerp(val0.x, val1.x, t);
res.y = math_lerp(val0.y, val1.y, t);
return res;
}
INLINE struct v2 v2_from_angle(f32 a)
{
return V2(math_cos(a), math_sin(a));
}
INLINE f32 v2_angle(struct v2 v)
{
return math_atan2(v.y, v.x);
}
INLINE f32 v2_angle_from_dirs(struct v2 dir1, struct v2 dir2)
{
return math_atan2(v2_wedge(dir1, dir2), v2_dot(dir1, dir2));
}
INLINE f32 v2_angle_from_points(struct v2 pt1, struct v2 pt2)
{
return v2_angle(v2_sub(pt2, pt1));
}
INLINE struct v2 v2_closest_point_ray(struct v2 ray_pos, struct v2 ray_dir_norm, struct v2 p)
{
struct v2 ray_p_dir = v2_sub(p, ray_pos);
f32 dot = v2_dot(ray_dir_norm, ray_p_dir);
struct v2 ray_dir_closest = v2_mul(ray_dir_norm, dot);
return v2_add(ray_pos, ray_dir_closest);
}
/* ========================== *
* Mat4x4
* ========================== */
INLINE struct mat4x4 mat4x4_from_xform(struct xform xf)
{
return (struct mat4x4) {
.e = {
{xf.bx.x, xf.bx.y, 0, 0},
{xf.by.x, xf.by.y, 0, 0},
{0, 0, 1, 0},
{xf.og.x, xf.og.y, 0, 1},
}
};
}
INLINE struct mat4x4 mat4x4_from_ortho(f32 left, f32 right, f32 bottom, f32 top, f32 near_z, f32 far_z)
{
struct mat4x4 m = {0};
f32 rl = 1.0f / (right - left);
f32 tb = 1.0f / (top - bottom);
f32 fn = -1.0f / (far_z - near_z);
m.e[0][0] = 2.0f * rl;
m.e[1][1] = 2.0f * tb;
m.e[2][2] = 2.0f * fn;
m.e[3][0] = -(right + left) * rl;
m.e[3][1] = -(top + bottom) * tb;
m.e[3][2] = (far_z + near_z) * fn;
m.e[3][3] = 1.0f;
return m;
}
INLINE struct mat4x4 mat4x4_mul(struct mat4x4 m1, struct mat4x4 m2)
{
f32 a00 = m1.e[0][0], a01 = m1.e[0][1], a02 = m1.e[0][2], a03 = m1.e[0][3],
a10 = m1.e[1][0], a11 = m1.e[1][1], a12 = m1.e[1][2], a13 = m1.e[1][3],
a20 = m1.e[2][0], a21 = m1.e[2][1], a22 = m1.e[2][2], a23 = m1.e[2][3],
a30 = m1.e[3][0], a31 = m1.e[3][1], a32 = m1.e[3][2], a33 = m1.e[3][3],
b00 = m2.e[0][0], b01 = m2.e[0][1], b02 = m2.e[0][2], b03 = m2.e[0][3],
b10 = m2.e[1][0], b11 = m2.e[1][1], b12 = m2.e[1][2], b13 = m2.e[1][3],
b20 = m2.e[2][0], b21 = m2.e[2][1], b22 = m2.e[2][2], b23 = m2.e[2][3],
b30 = m2.e[3][0], b31 = m2.e[3][1], b32 = m2.e[3][2], b33 = m2.e[3][3];
struct mat4x4 res;
res.e[0][0] = a00 * b00 + a10 * b01 + a20 * b02 + a30 * b03;
res.e[0][1] = a01 * b00 + a11 * b01 + a21 * b02 + a31 * b03;
res.e[0][2] = a02 * b00 + a12 * b01 + a22 * b02 + a32 * b03;
res.e[0][3] = a03 * b00 + a13 * b01 + a23 * b02 + a33 * b03;
res.e[1][0] = a00 * b10 + a10 * b11 + a20 * b12 + a30 * b13;
res.e[1][1] = a01 * b10 + a11 * b11 + a21 * b12 + a31 * b13;
res.e[1][2] = a02 * b10 + a12 * b11 + a22 * b12 + a32 * b13;
res.e[1][3] = a03 * b10 + a13 * b11 + a23 * b12 + a33 * b13;
res.e[2][0] = a00 * b20 + a10 * b21 + a20 * b22 + a30 * b23;
res.e[2][1] = a01 * b20 + a11 * b21 + a21 * b22 + a31 * b23;
res.e[2][2] = a02 * b20 + a12 * b21 + a22 * b22 + a32 * b23;
res.e[2][3] = a03 * b20 + a13 * b21 + a23 * b22 + a33 * b23;
res.e[3][0] = a00 * b30 + a10 * b31 + a20 * b32 + a30 * b33;
res.e[3][1] = a01 * b30 + a11 * b31 + a21 * b32 + a31 * b33;
res.e[3][2] = a02 * b30 + a12 * b31 + a22 * b32 + a32 * b33;
res.e[3][3] = a03 * b30 + a13 * b31 + a23 * b32 + a33 * b33;
return res;
}
/* ========================== *
* Xform
* ========================== */
/* Construct identity xform */
#define XFORM_IDENT ((struct xform) { .bx.x = 1, .by.y = 1 })
#define XFORM_IDENT_NOCAST { .bx.x = 1, .by.y = 1 }
#define XFORM_POS(p) ((struct xform) { .bx.x = 1, .by.y = 1, .og = (p) })
/* Takes a translation, rotation, and scale as optional parameters for constructing an xform */
#define XFORM_TRS(...) xform_from_trs((struct trs) { .t = V2(0,0), .s = V2(1, 1), .r = 0, __VA_ARGS__ })
INLINE struct xform xform_rotate(struct xform xf, f32 angle);
INLINE struct xform xform_scale(struct xform xf, struct v2 v);
INLINE struct v2 xform_basis_mul_v2(struct xform xf, struct v2 v);
INLINE struct v2 xform_mul_v2(struct xform xf, struct v2 v);
INLINE struct xform xform_with_scale(struct xform xf, struct v2 s);
INLINE f32 xform_get_determinant(struct xform xf);
INLINE struct v2 xform_get_scale(struct xform xf);
INLINE struct trs trs_from_xform(struct xform m);
INLINE struct trs trs_lerp(struct trs a, struct trs b, f32 t);
INLINE struct xform xform_from_pos(struct v2 v)
{
return (struct xform) {
.bx = {1, 0},
.by = {0, 1},
.og = {v.x, v.y}
};
}
INLINE struct xform xform_from_trs(struct trs trs)
{
struct xform xf = XFORM_POS(trs.t);
xf = xform_rotate(xf, trs.r);
xf = xform_scale(xf, trs.s);
return xf;
}
INLINE struct xform xform_translate(struct xform xf, struct v2 v)
{
xf.og = V2(
xf.bx.x * v.x + xf.by.x * v.y + xf.og.x,
xf.bx.y * v.x + xf.by.y * v.y + xf.og.y
);
return xf;
}
INLINE struct xform xform_rotate(struct xform xf, f32 angle)
{
f32 c = math_cos(angle);
f32 s = math_sin(angle);
struct xform res = xf;
res.bx.x = xf.bx.x * c + xf.by.x * s;
res.bx.y = xf.bx.y * c + xf.by.y * s;
res.by.x = xf.bx.x * -s + xf.by.x * c;
res.by.y = xf.bx.y * -s + xf.by.y * c;
return res;
}
INLINE struct xform xform_scale(struct xform xf, struct v2 v)
{
xf.bx = v2_mul(xf.bx, v.x);
xf.by = v2_mul(xf.by, v.y);
return xf;
}
INLINE struct xform xform_with_rotation(struct xform xf, f32 r)
{
struct v2 scale = xform_get_scale(xf);
f32 c = math_cos(r);
f32 s = math_sin(r);
xf.bx = V2(c, s);
xf.by = V2(-s, c);
xf = xform_with_scale(xf, scale);
return xf;
}
INLINE struct xform xform_with_scale(struct xform xf, struct v2 s)
{
xf.bx = v2_mul(v2_norm(xf.bx), s.x);
xf.by = v2_mul(v2_norm(xf.by), s.y);
return xf;
}
INLINE struct xform xform_trs(struct xform xf, struct trs trs)
{
xf = xform_translate(xf, trs.t);
xf = xform_rotate(xf, trs.r);
xf = xform_scale(xf, trs.s);
return xf;
}
INLINE struct xform xform_trs_pivot_r(struct xform xf, struct trs trs, struct v2 pivot)
{
xf = xform_translate(xf, trs.t);
xf = xform_rotate(xf, trs.r);
xf = xform_translate(xf, v2_neg(pivot));
xf = xform_scale(xf, trs.s);
return xf;
}
INLINE struct xform xform_trs_pivot_rs(struct xform xf, struct trs trs, struct v2 pivot)
{
xf = xform_translate(xf, trs.t);
xf = xform_rotate(xf, trs.r);
xf = xform_scale(xf, trs.s);
xf = xform_translate(xf, v2_neg(pivot));
return xf;
}
INLINE struct xform xform_lerp(struct xform a, struct xform b, f32 t)
{
struct trs trs_a = trs_from_xform(a);
struct trs trs_b = trs_from_xform(b);
struct trs trs = trs_lerp(trs_a, trs_b, t);
return xform_from_trs(trs);
}
INLINE struct xform xform_invert(struct xform xf)
{
f32 det = xform_get_determinant(xf);
f32 inv_det = 1.0f / det;
f32 old_bx_x = xf.bx.x;
xf.bx.x = xf.by.y;
xf.by.y = old_bx_x;
xf.bx = v2_mul_v2(xf.bx, V2(inv_det, -inv_det));
xf.by = v2_mul_v2(xf.by, V2(-inv_det, inv_det));
xf.og = xform_basis_mul_v2(xf, v2_neg(xf.og));
return xf;
}
INLINE struct xform xform_mul(struct xform a, struct xform b)
{
struct xform res;
res.bx.x = a.bx.x * b.bx.x + a.by.x * b.bx.y;
res.bx.y = a.bx.y * b.bx.x + a.by.y * b.bx.y;
res.by.x = a.bx.x * b.by.x + a.by.x * b.by.y;
res.by.y = a.bx.y * b.by.x + a.by.y * b.by.y;
res.og = xform_mul_v2(a, b.og);
return res;
}
INLINE struct v2 xform_basis_mul_v2(struct xform xf, struct v2 v)
{
return V2(
xf.bx.x * v.x + xf.by.x * v.y,
xf.bx.y * v.x + xf.by.y * v.y
);
}
INLINE struct v2 xform_mul_v2(struct xform xf, struct v2 v)
{
struct v2 res = xform_basis_mul_v2(xf, v);
res = v2_add(res, xf.og);
return res;
}
INLINE struct v2 xform_basis_invert_mul_v2(struct xform xf, struct v2 v)
{
struct xform inv = xform_invert(xf);
struct v2 res = xform_basis_mul_v2(inv, v);
return res;
}
INLINE struct v2 xform_invert_mul_v2(struct xform xf, struct v2 v)
{
struct xform inv = xform_invert(xf);
struct v2 res = xform_basis_mul_v2(inv, v);
res = v2_add(res, inv.og);
return res;
}
INLINE f32 xform_get_determinant(struct xform xf)
{
return v2_wedge(xf.bx, xf.by);
}
INLINE f32 xform_get_skew(struct xform xf)
{
f32 det = xform_get_determinant(xf);
i32 det_sign = math_fsign(det);
struct v2 bx_norm = v2_norm(xf.bx);
struct v2 by_norm = v2_norm(xf.by);
by_norm = v2_mul(by_norm, det_sign);
f32 dot = v2_dot(bx_norm, by_norm);
return math_acos(dot) - (PI / 2.0f);
}
INLINE struct v2 xform_get_right(struct xform xf)
{
return xf.bx;
}
INLINE struct v2 xform_get_left(struct xform xf)
{
return v2_neg(xf.bx);
}
INLINE struct v2 xform_get_up(struct xform xf)
{
return v2_neg(xf.by);
}
INLINE struct v2 xform_get_down(struct xform xf)
{
return xf.by;
}
INLINE f32 xform_get_rotation(struct xform xf)
{
return v2_angle(xf.bx);
}
INLINE struct v2 xform_get_scale(struct xform xf)
{
f32 det_sign = math_fsign(xform_get_determinant(xf));
return V2(v2_len(xf.bx), det_sign * v2_len(xf.by));
}
/* ========================== *
* Trs
* ========================== */
INLINE struct trs trs_lerp(struct trs a, struct trs b, f32 t)
{
struct trs res;
res.t = v2_lerp(a.t, b.t, t);
res.r = math_lerp_angle(a.r, b.r, t);
res.s = v2_lerp(a.s, b.s, t);
return res;
}
INLINE struct trs trs_from_xform(struct xform xf)
{
struct trs trs = { 0 };
trs.t = xf.og;
trs.r = xform_get_rotation(xf);
trs.s = xform_get_scale(xf);
return trs;
}
/* ========================== *
* Quad
* ========================== */
INLINE struct quad quad_from_rect(struct rect rect)
{
return (struct quad) {
(struct v2) { rect.x, rect.y }, /* Top left */
(struct v2) { rect.x + rect.width, rect.y }, /* Top right */
(struct v2) { rect.x + rect.width, rect.y + rect.height }, /* Bottom right */
(struct v2) { rect.x, rect.y + rect.height }, /* Bottom left */
};
}
INLINE struct quad quad_from_line(struct v2 start, struct v2 end, f32 thickness)
{
f32 width = thickness / 2.f;
struct v2 rel = v2_sub(end, start);
struct v2 dir = v2_norm(rel);
struct v2 dir_perp = v2_perp(dir);
struct v2 left = v2_mul(dir_perp, -width);
struct v2 right = v2_mul(dir_perp, width);
return (struct quad) {
.p1 = v2_add(start, left),
.p2 = v2_add(start, right),
.p3 = v2_add(end, right),
.p4 = v2_add(end, left)
};
}
INLINE struct quad quad_from_ray(struct v2 pos, struct v2 rel, f32 thickness)
{
struct v2 end = v2_add(pos, rel);
return quad_from_line(pos, end, thickness);
}
INLINE struct quad quad_scale(struct quad q, f32 s)
{
q.p1 = v2_mul(q.p1, s);
q.p2 = v2_mul(q.p2, s);
q.p3 = v2_mul(q.p3, s);
q.p4 = v2_mul(q.p4, s);
return q;
}
INLINE struct quad quad_mul_xform(struct quad quad, struct xform m)
{
return (struct quad) {
xform_mul_v2(m, quad.p1),
xform_mul_v2(m, quad.p2),
xform_mul_v2(m, quad.p3),
xform_mul_v2(m, quad.p4)
};
}
INLINE struct quad quad_round(struct quad quad)
{
return (struct quad) {
v2_round(quad.p1),
v2_round(quad.p2),
v2_round(quad.p3),
v2_round(quad.p4)
};
}
INLINE struct quad quad_floor(struct quad quad)
{
return (struct quad) {
v2_floor(quad.p1),
v2_round(quad.p2),
v2_round(quad.p3),
v2_round(quad.p4)
};
}
#endif
Reference in New Issue View Git Blame Copy Permalink