1449 lines
30 KiB
C
1449 lines
30 KiB
C
/* Math functions are default 32 bit (f32, i32, etc) unless specified */
|
|
|
|
////////////////////////////////
|
|
//~ Min / max
|
|
|
|
//- Min
|
|
u8 MinU8(u8 a, u8 b) { return a <= b ? a : b; }
|
|
i8 MinI8(i8 a, i8 b) { return a <= b ? a : b; }
|
|
u32 MinU32(u32 a, u32 b) { return a <= b ? a : b; }
|
|
i32 MinI32(i32 a, i32 b) { return a <= b ? a : b; }
|
|
f32 MinF32(f32 a, f32 b) { return a <= b ? a : b; }
|
|
u64 MinU64(u64 a, u64 b) { return a <= b ? a : b; }
|
|
i64 MinI64(i64 a, i64 b) { return a <= b ? a : b; }
|
|
f64 MinF64(f64 a, f64 b) { return a <= b ? a : b; }
|
|
|
|
//- Max
|
|
u8 MaxU8(u8 a, u8 b) { return a >= b ? a : b; }
|
|
i8 MaxI8(i8 a, i8 b) { return a >= b ? a : b; }
|
|
u32 MaxU32(u32 a, u32 b) { return a >= b ? a : b; }
|
|
i32 MaxI32(i32 a, i32 b) { return a >= b ? a : b; }
|
|
f32 MaxF32(f32 a, f32 b) { return a >= b ? a : b; }
|
|
u64 MaxU64(u64 a, u64 b) { return a >= b ? a : b; }
|
|
i64 MaxI64(i64 a, i64 b) { return a >= b ? a : b; }
|
|
f64 MaxF64(f64 a, f64 b) { return a >= b ? a : b; }
|
|
|
|
//- Clamp
|
|
u32 ClampU32(u32 v, u32 min, u32 max) { return v < min ? min : v > max ? max : v; }
|
|
i32 ClampI32(i32 v, i32 min, i32 max) { return v < min ? min : v > max ? max : v; }
|
|
f32 ClampF32(f32 v, f32 min, f32 max) { return v < min ? min : v > max ? max : v; }
|
|
u64 ClampU64(u64 v, u64 min, u64 max) { return v < min ? min : v > max ? max : v; }
|
|
i64 ClampI64(i64 v, i64 min, i64 max) { return v < min ? min : v > max ? max : v; }
|
|
f64 ClampF64(f64 v, f64 min, f64 max) { return v < min ? min : v > max ? max : v; }
|
|
|
|
////////////////////////////////
|
|
//~ Rounding ops
|
|
|
|
//- Round
|
|
f32 RoundF32(f32 f)
|
|
{
|
|
return IxRoundF32ToF32(f);
|
|
}
|
|
|
|
f64 RoundF64(f64 f)
|
|
{
|
|
return IxRoundF64ToF64(f);
|
|
}
|
|
|
|
i32 RoundF32ToI32(f32 f)
|
|
{
|
|
return IxRoundF32ToI32(f);
|
|
}
|
|
|
|
i64 RoundF64ToI64(f64 f)
|
|
{
|
|
return IxRoundF64ToI64(f);
|
|
}
|
|
|
|
//- Floor
|
|
f32 FloorF32(f32 f)
|
|
{
|
|
return IxFloorF32ToF32(f);
|
|
}
|
|
|
|
f64 FloorF64(f64 f)
|
|
{
|
|
return IxFloorF64ToF64(f);
|
|
}
|
|
|
|
i32 FloorF32ToI32(f32 f)
|
|
{
|
|
return IxFloorF32ToI32(f);
|
|
}
|
|
|
|
i64 FloorF64ToI64(f64 f)
|
|
{
|
|
return IxFloorF64ToI64(f);
|
|
}
|
|
|
|
//- Ceil
|
|
f32 CeilF32(f32 f)
|
|
{
|
|
return IxCeilF32ToF32(f);
|
|
}
|
|
|
|
f64 CeilF64(f64 f)
|
|
{
|
|
return IxCeilF64ToF64(f);
|
|
}
|
|
|
|
i32 CeilF32ToI32(f32 f)
|
|
{
|
|
return IxCeilF32ToI32(f);
|
|
}
|
|
|
|
i64 CeilF64ToI64(f64 f)
|
|
{
|
|
return IxCeilF64ToI64(f);
|
|
}
|
|
|
|
//- Trunc
|
|
f32 TruncF32(f32 f)
|
|
{
|
|
return IxTruncF32ToF32(f);
|
|
}
|
|
|
|
f64 TruncF64(f64 f)
|
|
{
|
|
return IxTruncF64ToF64(f);
|
|
}
|
|
|
|
////////////////////////////////
|
|
//~ Fmod
|
|
|
|
f32 ModF32(f32 x, f32 m)
|
|
{
|
|
return x - m * TruncF32(x / m);
|
|
}
|
|
|
|
f64 ModF64(f64 x, f64 m)
|
|
{
|
|
return x - m * TruncF64(x / m);
|
|
}
|
|
|
|
////////////////////////////////
|
|
//~ Floating point sign
|
|
|
|
f32 AbsF32(f32 f)
|
|
{
|
|
u32 truncated = *(u32 *)&f & 0x7FFFFFFF;
|
|
return *(f32 *)&truncated;
|
|
}
|
|
|
|
f64 AbsF64(f64 f)
|
|
{
|
|
u64 truncated = *(u64 *)&f & 0x7FFFFFFFFFFFFFFFULL;
|
|
return *(f64 *)&truncated;
|
|
}
|
|
|
|
u32 AbsI32(i32 v)
|
|
{
|
|
return (u32)(v * ((v >= 0) - (v < 0)));
|
|
}
|
|
|
|
u64 AbsI64(i64 v)
|
|
{
|
|
return (u64)(v * ((v >= 0) - (v < 0)));
|
|
}
|
|
|
|
i32 SignF32(f32 f)
|
|
{
|
|
u32 sign_bit = (*(u32 *)&f >> 31) & 1;
|
|
return 1 + -((i32)(sign_bit << 1));
|
|
}
|
|
|
|
i64 SignF64(f64 f)
|
|
{
|
|
u64 sign_bit = (*(u64 *)&f >> 63) & 1;
|
|
return 1 + -((i64)(sign_bit << 1));
|
|
}
|
|
|
|
////////////////////////////////
|
|
//~ U64 pow
|
|
|
|
/* Taken from https://gist.github.com/orlp/3551590 */
|
|
u64 PowU64(u64 base, u8 exp)
|
|
{
|
|
LocalPersist 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;
|
|
}
|
|
}
|
|
|
|
////////////////////////////////
|
|
//~ Align up
|
|
|
|
u64 AlignU64Pow2(u64 x)
|
|
{
|
|
u64 result = 0;
|
|
if (x > 0)
|
|
{
|
|
result = x - 1;
|
|
result |= result >> 1;
|
|
result |= result >> 2;
|
|
result |= result >> 4;
|
|
result |= result >> 8;
|
|
result |= result >> 16;
|
|
result |= result >> 32;
|
|
++result;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
////////////////////////////////
|
|
//~ Logn
|
|
|
|
/* Based on FreeBSD's implementation
|
|
* https://github.com/freebsd/freebsd-src/blob/main/lib/msun/src/e_logf.c */
|
|
f32 LnF32(f32 x)
|
|
{
|
|
LocalPersist const f32 ln2_hi = 6.9313812256e-01f;
|
|
LocalPersist const f32 ln2_lo = 9.0580006145e-06f;
|
|
|
|
i32 x_int = *(i32 *)&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 = *(i32 *)&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)
|
|
{
|
|
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);
|
|
}
|
|
}
|
|
}
|
|
|
|
////////////////////////////////
|
|
//~ Exp
|
|
|
|
/* Based on FreeBSD's implementation
|
|
* https://github.com/freebsd/freebsd-src/blob/main/lib/msun/src/e_expf.c */
|
|
f32 ExpF32(f32 x)
|
|
{
|
|
LocalPersist const f32 half[2] = { 0.5, -0.5 };
|
|
LocalPersist const f32 o_threshold = 8.8721679688e+01f;
|
|
LocalPersist const f32 u_threshold = -1.0397208405e+02f;
|
|
LocalPersist const f32 ln2_hi[2] = { 6.9314575195e-01f, -6.9314575195e-01f };
|
|
LocalPersist const f32 ln2_lo[2] = { 1.4286067653e-06f, -1.4286067653e-06f };
|
|
LocalPersist const f32 inv_ln2 = 1.4426950216e+00f;
|
|
LocalPersist const f32 huge = 1.0e+30f;
|
|
LocalPersist const f32 two_m100 = 7.8886090522e-31f;
|
|
|
|
u32 x_uint = *(u32 *)&x;
|
|
i32 x_sign_bit = (i32)((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;
|
|
}
|
|
if (x > o_threshold)
|
|
{
|
|
/* Overflow */
|
|
return F32Infinity;
|
|
}
|
|
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;
|
|
}
|
|
}
|
|
}
|
|
|
|
////////////////////////////////
|
|
//~ Pow
|
|
|
|
f32 PowF32(f32 a, f32 b)
|
|
{
|
|
if (a >= 0)
|
|
{
|
|
/* a is positive */
|
|
return ExpF32(LnF32(a) * b);
|
|
}
|
|
else
|
|
{
|
|
/* a is negative */
|
|
f32 res_sign = RoundF32ToI32(b) % 2 == 0 ? 1 : -1;
|
|
return ExpF32(LnF32(-a) * b) * res_sign;
|
|
}
|
|
}
|
|
|
|
////////////////////////////////
|
|
//~ Sqrt
|
|
|
|
f32 SqrtF32(f32 x)
|
|
{
|
|
return IxSqrtF32(x);
|
|
}
|
|
|
|
f64 SqrtF64(f64 x)
|
|
{
|
|
return IxSqrtF64(x);
|
|
}
|
|
|
|
f32 RSqrtF32(f32 x)
|
|
{
|
|
return IxRsqrtF32(x);
|
|
}
|
|
|
|
////////////////////////////////
|
|
//~ Trig
|
|
|
|
/* Functions based on Cephes implementation (https://www.netlib.org/cephes/):
|
|
* - SinApproxF32
|
|
* - CosApproxF32
|
|
* - ReduceToPio4
|
|
* - ArcTanF32
|
|
*/
|
|
|
|
//- Reduce
|
|
/* Reduce postive x to range [0, Pi/4] (Cody-Waite argument reduction).
|
|
* Returns 0 if x > (2^24 - 1);
|
|
* Sets octant_out=-1 on error. */
|
|
f32 ReduceToPio4(f32 x, i32 *octant_out)
|
|
{
|
|
i32 octant = -1;
|
|
|
|
if (x <= ((1 << 24) - 1))
|
|
{
|
|
octant = x * (4 / Pi); /* Integer part of x/(Pi/4) */
|
|
f32 y = octant;
|
|
if (octant & 1)
|
|
{
|
|
octant += 1;
|
|
y += 1.0;
|
|
}
|
|
|
|
/* Modulo 360 degrees */
|
|
octant &= 7;
|
|
|
|
if (x > 8192)
|
|
{
|
|
x = x - y * (Pi / 4);
|
|
}
|
|
else
|
|
{
|
|
/* Extended precision modular arithmetic */
|
|
x = ((x - y * 0.78515625) - y * 2.4187564849853515625e-4) - y * 3.77489497744594108e-8;
|
|
}
|
|
}
|
|
|
|
*octant_out = octant;
|
|
return x;
|
|
}
|
|
|
|
//- Sin approximation
|
|
/* Approximate sin in range [0, Pi/4] */
|
|
f32 SinApproxF32(f32 x)
|
|
{
|
|
f32 x_sq = x * x;
|
|
return (((-1.9515295891E-4 * x_sq + 8.3321608736E-3) * x_sq - 1.6666654611E-1) * x_sq * x) + x;
|
|
}
|
|
|
|
//- Cos approximation
|
|
f32 CosApproxF32(f32 x)
|
|
{
|
|
f32 x_sq = x * x;
|
|
return (((2.443315711809948E-005 * x_sq - 1.388731625493765E-003) * x_sq + 4.166664568298827E-002) * x_sq * x_sq) - (x_sq * 0.5) + 1;
|
|
}
|
|
|
|
//- Sin
|
|
f32 SinF32(f32 x)
|
|
{
|
|
f32 sign = 1;
|
|
|
|
if (IsF32Nan(x))
|
|
{
|
|
return 0;
|
|
}
|
|
else if (x < 0)
|
|
{
|
|
sign = -1;
|
|
x = -x;
|
|
}
|
|
|
|
i32 octant;
|
|
x = ReduceToPio4(x, &octant);
|
|
|
|
/* Reflect in x axis */
|
|
if (octant > 3)
|
|
{
|
|
sign = -sign;
|
|
octant -= 4;
|
|
}
|
|
|
|
switch (octant)
|
|
{
|
|
case -1: return 0;
|
|
case 1: case 2: return CosApproxF32(x) * sign;
|
|
default: return SinApproxF32(x) * sign;
|
|
}
|
|
}
|
|
|
|
//- Cos
|
|
f32 CosF32(f32 x)
|
|
{
|
|
f32 sign = 1;
|
|
|
|
if (IsF32Nan(x))
|
|
{
|
|
return 0;
|
|
}
|
|
else if (x < 0)
|
|
{
|
|
x = -x;
|
|
}
|
|
|
|
i32 octant;
|
|
x = ReduceToPio4(x, &octant);
|
|
|
|
/* Reflect in x axis */
|
|
if (octant > 3)
|
|
{
|
|
sign = -sign;
|
|
octant -= 4;
|
|
}
|
|
|
|
if (octant > 1)
|
|
{
|
|
sign = -sign;
|
|
}
|
|
|
|
switch (octant)
|
|
{
|
|
case -1: return 0;
|
|
case 1: case 2: return SinApproxF32(x) * sign;
|
|
default: return CosApproxF32(x) * sign;
|
|
}
|
|
}
|
|
|
|
//- ArcTan
|
|
f32 ArcTanF32(f32 x)
|
|
{
|
|
f32 sign = 1;
|
|
if (x < 0)
|
|
{
|
|
sign = -1;
|
|
x = -x;
|
|
}
|
|
|
|
/* Reduce range */
|
|
f32 y;
|
|
if (x > 2.414213562373095)
|
|
{ /* tan((Pi / 8) * 3) */
|
|
y = Pi / 2;
|
|
x = -(1.f / x);
|
|
}
|
|
else if (x > 0.4142135623730950)
|
|
{ /* tan(Pi / 8) */
|
|
y = Pi / 4;
|
|
x = (x - 1.f) / (x + 1.f);
|
|
}
|
|
else
|
|
{
|
|
y = 0;
|
|
}
|
|
|
|
f32 x_sq = x * x;
|
|
y += ((((8.05374449538e-2 * x_sq - 1.38776856032E-1) * x_sq + 1.99777106478E-1) * x_sq - 3.33329491539E-1) * x_sq * x + x);
|
|
|
|
return y * sign;
|
|
}
|
|
|
|
//- ArcTan2
|
|
f32 ArcTan2F32(f32 y, f32 x)
|
|
{
|
|
f32 result;
|
|
if (x == 0)
|
|
{
|
|
if (y < 0)
|
|
{
|
|
result = 3 * Pi / 2;
|
|
}
|
|
else if (y == 0)
|
|
{
|
|
result = 0;
|
|
}
|
|
else
|
|
{
|
|
result = Pi / 2;
|
|
}
|
|
}
|
|
else if (y == 0)
|
|
{
|
|
if (x < 0)
|
|
{
|
|
result = Pi;
|
|
}
|
|
else
|
|
{
|
|
result = 0;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
f32 offset;
|
|
if (x < 0)
|
|
{
|
|
offset = Pi;
|
|
}
|
|
else if (y < 0)
|
|
{
|
|
offset = Pi * 2;
|
|
}
|
|
else
|
|
{
|
|
offset = 0;
|
|
}
|
|
result = ArcTanF32(y / x) + offset;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
//- ArcSin
|
|
f32 ArcSinF32(f32 x)
|
|
{
|
|
/* TODO: Dedicated arcsin approximation */
|
|
return ArcTan2F32(x, SqrtF32(1.0f - (x * x)));
|
|
}
|
|
|
|
//- ArcCos
|
|
f32 ArcCosF32(f32 x)
|
|
{
|
|
/* TODO: Dedicated arccos approximation */
|
|
return (Pi / 2.0f) - ArcTan2F32(x, SqrtF32(1.0f - (x * x)));
|
|
}
|
|
|
|
////////////////////////////////
|
|
//~ Angle unwind
|
|
|
|
/* Returns angle in range [-Pi, Pi] */
|
|
f32 UnwindAngleF32(f32 a)
|
|
{
|
|
f32 d = ModF32(a, Tau);
|
|
return ModF32(2.0f * d, Tau) - d;
|
|
}
|
|
|
|
////////////////////////////////
|
|
//~ Float lerp
|
|
|
|
f32 LerpF32(f32 val0, f32 val1, f32 t)
|
|
{
|
|
return val0 + ((val1 - val0) * t);
|
|
}
|
|
|
|
f64 LerpF64(f64 val0, f64 val1, f64 t)
|
|
{
|
|
return val0 + ((val1 - val0) * t);
|
|
}
|
|
|
|
f32 LerpAngleF32(f32 a, f32 b, f32 t)
|
|
{
|
|
f32 diff = UnwindAngleF32(b - a);
|
|
return a + diff * t;
|
|
}
|
|
|
|
////////////////////////////////
|
|
//~ Int lerp
|
|
|
|
i32 LerpI32(i32 val0, i32 val1, f32 t)
|
|
{
|
|
return val0 + RoundF32ToI32(((f32)(val1 - val0) * t));
|
|
}
|
|
|
|
|
|
//- Lerp i64
|
|
i64 LerpI64(i64 val0, i64 val1, f64 t)
|
|
{
|
|
return val0 + RoundF64ToI64(((f64)(val1 - val0) * t));
|
|
}
|
|
|
|
////////////////////////////////
|
|
//~ Vec2 operations
|
|
|
|
b32 IsVec2Zero(Vec2 a)
|
|
{
|
|
return a.x == 0 && a.y == 0;
|
|
}
|
|
|
|
b32 EqVec2(Vec2 a, Vec2 b)
|
|
{
|
|
return a.x == b.x && a.y == b.y;
|
|
}
|
|
|
|
//- Mul
|
|
|
|
Vec2 MulVec2(Vec2 a, f32 s)
|
|
{
|
|
return VEC2(a.x * s, a.y * s);
|
|
}
|
|
|
|
Vec2 MulVec2Vec2(Vec2 a, Vec2 b)
|
|
{
|
|
return VEC2(a.x * b.x, a.y * b.y);
|
|
}
|
|
|
|
Vec2 NegVec2(Vec2 a)
|
|
{
|
|
return VEC2(-a.x, -a.y);
|
|
}
|
|
|
|
//- Div
|
|
|
|
Vec2 DivVec2(Vec2 a, f32 s)
|
|
{
|
|
f32 d = 1 / s;
|
|
return VEC2(a.x * d, a.y * d);
|
|
}
|
|
|
|
Vec2 DivVec2Vec2(Vec2 a, Vec2 b)
|
|
{
|
|
return VEC2(a.x * (1 / b.x), a.y * (1 / b.y));
|
|
}
|
|
|
|
//- Add
|
|
|
|
Vec2 AddVec2(Vec2 a, Vec2 b)
|
|
{
|
|
return VEC2(a.x + b.x, a.y + b.y);
|
|
}
|
|
|
|
Vec2 SubVec2(Vec2 a, Vec2 b)
|
|
{
|
|
return VEC2(a.x - b.x, a.y - b.y);
|
|
}
|
|
|
|
//- Len
|
|
|
|
f32 Vec2Len(Vec2 a)
|
|
{
|
|
return SqrtF32(a.x * a.x + a.y * a.y);
|
|
}
|
|
|
|
f32 Vec2LenSq(Vec2 a)
|
|
{
|
|
return a.x * a.x + a.y * a.y;
|
|
}
|
|
|
|
Vec2 Vec2WithLen(Vec2 a, f32 len)
|
|
{
|
|
f32 l_sq = a.x * a.x + a.y * a.y;
|
|
if (l_sq != 0)
|
|
{
|
|
f32 denom = len / SqrtF32(l_sq);
|
|
a.x *= denom;
|
|
a.y *= denom;
|
|
}
|
|
return a;
|
|
}
|
|
|
|
Vec2 ClampVec2Len(Vec2 a, f32 max)
|
|
{
|
|
f32 l_sq = a.x * a.x + a.y * a.y;
|
|
if (l_sq > max * max)
|
|
{
|
|
f32 denom = max / SqrtF32(l_sq);
|
|
a.x *= denom;
|
|
a.y *= denom;
|
|
}
|
|
return a;
|
|
}
|
|
|
|
f32 Vec2Distance(Vec2 a, Vec2 b)
|
|
{
|
|
f32 dx = b.x - a.x;
|
|
f32 dy = b.y - a.y;
|
|
return SqrtF32(dx * dx + dy * dy);
|
|
}
|
|
|
|
Vec2 NormVec2(Vec2 a)
|
|
{
|
|
return Vec2WithLen(a, 1.f);
|
|
}
|
|
|
|
|
|
//- Dot
|
|
|
|
f32 DotVec2(Vec2 a, Vec2 b)
|
|
{
|
|
return a.x * b.x + a.y * b.y;
|
|
}
|
|
|
|
/* Returns signed area between vectors (positive in clockwise direction)
|
|
* AKA perpendicular dot product
|
|
* AKA 2d cross-product */
|
|
f32 WedgeVec2(Vec2 a, Vec2 b)
|
|
{
|
|
return a.x * b.y - a.y * b.x;
|
|
}
|
|
|
|
/* Clockwise (right) perpendicular vector */
|
|
Vec2 PerpVec2(Vec2 a)
|
|
{
|
|
return VEC2(-a.y, a.x);
|
|
}
|
|
|
|
/* Clockwise (right) perpendicular vector scaled by s */
|
|
Vec2 MulPerpVec2(Vec2 a, f32 s)
|
|
{
|
|
return VEC2(s * -a.y, s * a.x);
|
|
}
|
|
|
|
Vec2 PerpVec2TowardsDir(Vec2 v, Vec2 dir)
|
|
{
|
|
f32 wedge = WedgeVec2(v, dir);
|
|
return MulPerpVec2(v, (wedge >= 0) - (wedge < 0));
|
|
}
|
|
|
|
//- Round / floor / ceil
|
|
|
|
Vec2 RoundVec2(Vec2 a)
|
|
{
|
|
return VEC2(RoundF32(a.x), RoundF32(a.y));
|
|
}
|
|
|
|
Vec2I32 RoundVec2ToVec2I32(Vec2 a)
|
|
{
|
|
return VEC2I32(RoundF32ToI32(a.x), RoundF32ToI32(a.y));
|
|
}
|
|
|
|
Vec2 FloorVec2(Vec2 a)
|
|
{
|
|
return VEC2(FloorF32(a.x), FloorF32(a.y));
|
|
}
|
|
|
|
Vec2 CeilVec2(Vec2 a)
|
|
{
|
|
return VEC2(CeilF32(a.x), CeilF32(a.y));
|
|
}
|
|
|
|
//- Angle
|
|
|
|
/* Returns 1 if winding between vectors a & b is clockwise or straight, -1 if counter-clockwise */
|
|
i32 WindingFromVec2(Vec2 a, Vec2 b)
|
|
{
|
|
f32 w = WedgeVec2(a, b);
|
|
return (w >= 0) - (w < 0);
|
|
}
|
|
|
|
Vec2 RotateVec2(Vec2 v, f32 a)
|
|
{
|
|
f32 c = CosF32(a);
|
|
f32 s = SinF32(a);
|
|
return VEC2(v.x * c - v.y * s, v.x * s + v.y * c);
|
|
}
|
|
|
|
Vec2 Vec2FromAngle(f32 a)
|
|
{
|
|
return VEC2(CosF32(a), SinF32(a));
|
|
}
|
|
|
|
f32 AngleFromVec2(Vec2 v)
|
|
{
|
|
return ArcTan2F32(v.y, v.x);
|
|
}
|
|
|
|
f32 AngleFromVec2Dirs(Vec2 dir1, Vec2 dir2)
|
|
{
|
|
return ArcTan2F32(WedgeVec2(dir1, dir2), DotVec2(dir1, dir2));
|
|
}
|
|
|
|
f32 AngleFromVec2Points(Vec2 pt1, Vec2 pt2)
|
|
{
|
|
return AngleFromVec2(SubVec2(pt2, pt1));
|
|
}
|
|
|
|
//- Closest point
|
|
|
|
Vec2 ClosestPointFromRay(Vec2 ray_pos, Vec2 ray_dir_norm, Vec2 p)
|
|
{
|
|
Vec2 ray_p_dir = SubVec2(p, ray_pos);
|
|
f32 dot = DotVec2(ray_dir_norm, ray_p_dir);
|
|
Vec2 ray_dir_closest = MulVec2(ray_dir_norm, dot);
|
|
return AddVec2(ray_pos, ray_dir_closest);
|
|
}
|
|
|
|
//- Lerp
|
|
|
|
/* Interpolate position vectors */
|
|
Vec2 LerpVec2(Vec2 val0, Vec2 val1, f32 t)
|
|
{
|
|
return VEC2(LerpF32(val0.x, val1.x, t), LerpF32(val0.y, val1.y, t));
|
|
}
|
|
|
|
/* Interpolate direction vectors (spherical lerp) */
|
|
Vec2 SlerpVec2(Vec2 val0, Vec2 val1, f32 t)
|
|
{
|
|
f32 rot = LerpAngleF32(AngleFromVec2(val0), AngleFromVec2(val1), t);
|
|
f32 len = LerpF32(Vec2Len(val0), Vec2Len(val1), t);
|
|
return MulVec2(Vec2FromAngle(rot), len);
|
|
}
|
|
|
|
////////////////////////////////
|
|
//~ Vec2I32 Operations
|
|
|
|
b32 EqVec2I32(Vec2I32 a, Vec2I32 b)
|
|
{
|
|
return a.x == b.x && a.y == b.y;
|
|
}
|
|
|
|
Vec2I32 NegVec2I32(Vec2I32 a)
|
|
{
|
|
return VEC2I32(-a.x, -a.y);
|
|
}
|
|
|
|
Vec2I32 AddVec2I32(Vec2I32 a, Vec2I32 b)
|
|
{
|
|
return VEC2I32(a.x + b.x, a.y + b.y);
|
|
}
|
|
|
|
Vec2I32 SubVec2I32(Vec2I32 a, Vec2I32 b)
|
|
{
|
|
return VEC2I32(a.x - b.x, a.y - b.y);
|
|
}
|
|
|
|
////////////////////////////////
|
|
//~ Xform operations
|
|
|
|
b32 EqXform(Xform xf1, Xform xf2)
|
|
{
|
|
return EqVec2(xf1.og, xf2.og) && EqVec2(xf1.bx, xf2.bx) && EqVec2(xf1.by, xf2.by);
|
|
}
|
|
|
|
//- Initialization
|
|
Xform XformFromPos(Vec2 v)
|
|
{
|
|
Xform xf;
|
|
xf.bx = VEC2(1, 0);
|
|
xf.by = VEC2(0, 1);
|
|
xf.og = v;
|
|
return xf;
|
|
}
|
|
|
|
Xform XformFromRot(f32 r)
|
|
{
|
|
Xform result;
|
|
f32 c = CosF32(r);
|
|
f32 s = SinF32(r);
|
|
result.bx = VEC2(c, s);
|
|
result.by = VEC2(-s, c);
|
|
result.og = VEC2(0, 0);
|
|
return result;
|
|
}
|
|
|
|
Xform XformFromScale(Vec2 scale)
|
|
{
|
|
Xform result;
|
|
result.bx = VEC2(scale.x, 0);
|
|
result.by = VEC2(0, scale.y);
|
|
result.og = VEC2(0, 0);
|
|
return result;
|
|
}
|
|
|
|
Xform XformFromTrs(Trs trs)
|
|
{
|
|
Xform xf = XformFromPos(trs.t);
|
|
xf = RotateXform(xf, trs.r);
|
|
xf = ScaleXform(xf, trs.s);
|
|
return xf;
|
|
}
|
|
|
|
Xform XformFromRect(Rect rect)
|
|
{
|
|
Vec2 half_size = MulVec2(rect.size, 0.5);
|
|
Xform xf = ZI;
|
|
xf.bx = VEC2(rect.size.x, 0);
|
|
xf.by = VEC2(0, rect.size.y);
|
|
xf.og = AddVec2(rect.pos, half_size);
|
|
return xf;
|
|
}
|
|
|
|
//- Translation
|
|
Xform TranslateXform(Xform xf, Vec2 v)
|
|
{
|
|
xf.og = VEC2(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;
|
|
}
|
|
|
|
Xform WorldTranslateXform(Xform xf, Vec2 v)
|
|
{
|
|
xf.og = AddVec2(xf.og, v);
|
|
return xf;
|
|
}
|
|
|
|
//- Rotation
|
|
Xform RotateXform(Xform xf, f32 r)
|
|
{
|
|
return MulXform(xf, XformFromRot(r));
|
|
}
|
|
|
|
Xform WorldRotateXform(Xform xf, f32 r)
|
|
{
|
|
return MulXform(XformFromRot(r), xf);
|
|
}
|
|
|
|
Xform WorldRotateXformBasis(Xform xf, f32 r)
|
|
{
|
|
f32 diff = r;
|
|
f32 c = CosF32(diff);
|
|
f32 s = SinF32(diff);
|
|
xf.bx = VEC2(xf.bx.x * c - xf.bx.y * s, xf.bx.x * s + xf.bx.y * c);
|
|
xf.by = VEC2(xf.by.x * c - xf.by.y * s, xf.by.x * s + xf.by.y * c);
|
|
return xf;
|
|
}
|
|
|
|
Xform XformWIthWorldRotation(Xform xf, f32 r)
|
|
{
|
|
return WorldRotateXformBasis(xf, r - RotationFromXform(xf));
|
|
}
|
|
|
|
//- Scale
|
|
Xform ScaleXform(Xform xf, Vec2 scale)
|
|
{
|
|
xf.bx = MulVec2(xf.bx, scale.x);
|
|
xf.by = MulVec2(xf.by, scale.y);
|
|
return xf;
|
|
}
|
|
|
|
Xform WorldScaleXform(Xform xf, Vec2 scale)
|
|
{
|
|
Xform result;
|
|
result.bx = MulVec2Vec2(xf.bx, scale);
|
|
result.by = MulVec2Vec2(xf.by, scale);
|
|
result.og = MulVec2Vec2(xf.og, scale);
|
|
return result;
|
|
}
|
|
|
|
//- Lerp
|
|
Xform LerpXform(Xform a, Xform b, f32 t)
|
|
{
|
|
Xform result;
|
|
result.bx = SlerpVec2(a.bx, b.bx, t);
|
|
result.by = SlerpVec2(a.by, b.by, t);
|
|
result.og = LerpVec2(a.og, b.og, t);
|
|
return result;
|
|
}
|
|
|
|
//- Invert
|
|
Xform InvertXform(Xform xf)
|
|
{
|
|
f32 det = DeterminantFromXform(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 = MulVec2Vec2(xf.bx, VEC2(inv_det, -inv_det));
|
|
xf.by = MulVec2Vec2(xf.by, VEC2(-inv_det, inv_det));
|
|
|
|
xf.og = MulXformBasisV2(xf, NegVec2(xf.og));
|
|
|
|
return xf;
|
|
}
|
|
|
|
//- Mul
|
|
Xform MulXform(Xform a, Xform b)
|
|
{
|
|
Xform result;
|
|
result.bx.x = a.bx.x * b.bx.x + a.by.x * b.bx.y;
|
|
result.bx.y = a.bx.y * b.bx.x + a.by.y * b.bx.y;
|
|
result.by.x = a.bx.x * b.by.x + a.by.x * b.by.y;
|
|
result.by.y = a.bx.y * b.by.x + a.by.y * b.by.y;
|
|
result.og = MulXformV2(a, b.og);
|
|
return result;
|
|
}
|
|
|
|
Vec2 MulXformBasisV2(Xform xf, Vec2 v)
|
|
{
|
|
return VEC2(
|
|
xf.bx.x * v.x + xf.by.x * v.y,
|
|
xf.bx.y * v.x + xf.by.y * v.y
|
|
);
|
|
}
|
|
|
|
Vec2 MulXformV2(Xform xf, Vec2 v)
|
|
{
|
|
Vec2 result = MulXformBasisV2(xf, v);
|
|
result = AddVec2(result, xf.og);
|
|
return result;
|
|
}
|
|
|
|
|
|
Quad MulXformQuad(Xform xf, Quad quad)
|
|
{
|
|
Quad result;
|
|
result.p0 = MulXformV2(xf, quad.p0);
|
|
result.p1 = MulXformV2(xf, quad.p1);
|
|
result.p2 = MulXformV2(xf, quad.p2);
|
|
result.p3 = MulXformV2(xf, quad.p3);
|
|
return result;
|
|
}
|
|
|
|
Vec2 InvertXformBasisMulV2(Xform xf, Vec2 v)
|
|
{
|
|
Xform inv = InvertXform(xf);
|
|
Vec2 result = MulXformBasisV2(inv, v);
|
|
return result;
|
|
}
|
|
|
|
/* TODO: Get rid of this? Just force caller to use invert manually since it's expensive. */
|
|
Vec2 InvertXformMulV2(Xform xf, Vec2 v)
|
|
{
|
|
Xform inv = InvertXform(xf);
|
|
return MulXformV2(inv, v);
|
|
}
|
|
|
|
//- Helpers
|
|
Xform BasisFromXform(Xform xf)
|
|
{
|
|
Xform result = ZI;
|
|
result.bx = xf.bx;
|
|
result.by = xf.by;
|
|
return result;
|
|
}
|
|
|
|
f32 DeterminantFromXform(Xform xf)
|
|
{
|
|
return WedgeVec2(xf.bx, xf.by);
|
|
}
|
|
|
|
Vec2 RightFromXform(Xform xf)
|
|
{
|
|
return xf.bx;
|
|
}
|
|
|
|
Vec2 LeftFromXform(Xform xf)
|
|
{
|
|
return NegVec2(xf.bx);
|
|
}
|
|
|
|
Vec2 UpFromXform(Xform xf)
|
|
{
|
|
return NegVec2(xf.by);
|
|
}
|
|
|
|
Vec2 DownFromXform(Xform xf)
|
|
{
|
|
return xf.by;
|
|
}
|
|
|
|
f32 RotationFromXform(Xform xf)
|
|
{
|
|
return AngleFromVec2(xf.bx);
|
|
}
|
|
|
|
Vec2 ScaleFromXform(Xform xf)
|
|
{
|
|
f32 det_sign = SignF32(DeterminantFromXform(xf));
|
|
return VEC2(Vec2Len(xf.bx), det_sign * Vec2Len(xf.by));
|
|
}
|
|
|
|
////////////////////////////////
|
|
//~ Quad operations
|
|
|
|
Quad QuadFromRect(Rect rect)
|
|
{
|
|
Quad result;
|
|
result.p0 = VEC2(rect.x, rect.y); /* Top left */
|
|
result.p1 = VEC2(rect.x + rect.width, rect.y); /* Top right */
|
|
result.p2 = VEC2(rect.x + rect.width, rect.y + rect.height); /* Bottom right */
|
|
result.p3 = VEC2(rect.x, rect.y + rect.height); /* Bottom left */
|
|
return result;
|
|
}
|
|
|
|
Quad QuadFromAabb(Aabb aabb)
|
|
{
|
|
Quad result;
|
|
result.p0 = VEC2(aabb.p0.x, aabb.p0.y); /* Top left */
|
|
result.p1 = VEC2(aabb.p1.x, aabb.p0.y); /* Top right */
|
|
result.p2 = VEC2(aabb.p1.x, aabb.p1.y); /* Bottom right */
|
|
result.p3 = VEC2(aabb.p0.x, aabb.p1.y); /* Bottom left */
|
|
return result;
|
|
}
|
|
|
|
Quad QuadFromLine(Vec2 start, Vec2 end, f32 thickness)
|
|
{
|
|
f32 width = thickness / 2.f;
|
|
|
|
Vec2 dir = NormVec2(SubVec2(end, start));
|
|
Vec2 dir_perp = PerpVec2(dir);
|
|
|
|
Vec2 left = MulVec2(dir_perp, -width);
|
|
Vec2 right = MulVec2(dir_perp, width);
|
|
|
|
Quad result;
|
|
result.p0 = AddVec2(start, left);
|
|
result.p1 = AddVec2(start, right);
|
|
result.p2 = AddVec2(end, right);
|
|
result.p3 = AddVec2(end, left);
|
|
return result;
|
|
}
|
|
|
|
Quad QuadFromRay(Vec2 pos, Vec2 rel, f32 thickness)
|
|
{
|
|
Vec2 end = AddVec2(pos, rel);
|
|
return QuadFromLine(pos, end, thickness);
|
|
}
|
|
|
|
Quad ScaleQuad(Quad q, f32 s)
|
|
{
|
|
q.p0 = MulVec2(q.p0, s);
|
|
q.p1 = MulVec2(q.p1, s);
|
|
q.p2 = MulVec2(q.p2, s);
|
|
q.p3 = MulVec2(q.p3, s);
|
|
return q;
|
|
}
|
|
|
|
Quad RoundQuad(Quad quad)
|
|
{
|
|
Quad result;
|
|
result.p0 = RoundVec2(quad.p0);
|
|
result.p1 = RoundVec2(quad.p1);
|
|
result.p2 = RoundVec2(quad.p2);
|
|
result.p3 = RoundVec2(quad.p3);
|
|
return result;
|
|
}
|
|
|
|
Quad FloorQuad(Quad quad)
|
|
{
|
|
Quad result;
|
|
result.p0 = FloorVec2(quad.p0);
|
|
result.p1 = FloorVec2(quad.p1);
|
|
result.p2 = FloorVec2(quad.p2);
|
|
result.p3 = FloorVec2(quad.p3);
|
|
return result;
|
|
}
|
|
|
|
////////////////////////////////
|
|
//~ Spring operations
|
|
|
|
/* https://box2d.org/files/ErinCatto_SoftConstraints_GDC2011.pdf */
|
|
SoftSpring MakeSpring(f32 hertz, f32 damping_ratio, f32 dt)
|
|
{
|
|
SoftSpring result;
|
|
if (hertz == 0.0f)
|
|
{
|
|
result.bias_rate = 0;
|
|
result.mass_scale = 1;
|
|
result.impulse_scale = 0;
|
|
}
|
|
else
|
|
{
|
|
f32 angular_frequency = Tau * hertz;
|
|
f32 a = 2 * damping_ratio + angular_frequency * dt;
|
|
f32 b = angular_frequency * a * dt;
|
|
f32 c = 1 / (b + 1);
|
|
result.bias_rate = angular_frequency / a;
|
|
result.mass_scale = b * c;
|
|
result.impulse_scale = c;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
////////////////////////////////
|
|
//~ Mat4x4 operations
|
|
|
|
Mat4x4 Mat4x4FromXform(Xform xf)
|
|
{
|
|
return (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},
|
|
}
|
|
};
|
|
}
|
|
|
|
Mat4x4 Mat4x4FromOrtho(f32 left, f32 right, f32 bottom, f32 top, f32 near_z, f32 far_z)
|
|
{
|
|
Mat4x4 m = ZI;
|
|
|
|
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;
|
|
}
|
|
|
|
Mat4x4 MulMat4x4(Mat4x4 m1, 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];
|
|
|
|
Mat4x4 result;
|
|
result.e[0][0] = a00 * b00 + a10 * b01 + a20 * b02 + a30 * b03;
|
|
result.e[0][1] = a01 * b00 + a11 * b01 + a21 * b02 + a31 * b03;
|
|
result.e[0][2] = a02 * b00 + a12 * b01 + a22 * b02 + a32 * b03;
|
|
result.e[0][3] = a03 * b00 + a13 * b01 + a23 * b02 + a33 * b03;
|
|
result.e[1][0] = a00 * b10 + a10 * b11 + a20 * b12 + a30 * b13;
|
|
result.e[1][1] = a01 * b10 + a11 * b11 + a21 * b12 + a31 * b13;
|
|
result.e[1][2] = a02 * b10 + a12 * b11 + a22 * b12 + a32 * b13;
|
|
result.e[1][3] = a03 * b10 + a13 * b11 + a23 * b12 + a33 * b13;
|
|
result.e[2][0] = a00 * b20 + a10 * b21 + a20 * b22 + a30 * b23;
|
|
result.e[2][1] = a01 * b20 + a11 * b21 + a21 * b22 + a31 * b23;
|
|
result.e[2][2] = a02 * b20 + a12 * b21 + a22 * b22 + a32 * b23;
|
|
result.e[2][3] = a03 * b20 + a13 * b21 + a23 * b22 + a33 * b23;
|
|
result.e[3][0] = a00 * b30 + a10 * b31 + a20 * b32 + a30 * b33;
|
|
result.e[3][1] = a01 * b30 + a11 * b31 + a21 * b32 + a31 * b33;
|
|
result.e[3][2] = a02 * b30 + a12 * b31 + a22 * b32 + a32 * b33;
|
|
result.e[3][3] = a03 * b30 + a13 * b31 + a23 * b32 + a33 * b33;
|
|
|
|
return result;
|
|
}
|
|
|
|
Mat4x4 ProjectMat4x4View(Xform view, f32 viewport_width, f32 viewport_height)
|
|
{
|
|
Mat4x4 projection = Mat4x4FromOrtho(0.0, viewport_width, viewport_height, 0.0, -1.0, 1.0);
|
|
Mat4x4 view4x4 = Mat4x4FromXform(view);
|
|
return MulMat4x4(projection, view4x4);
|
|
}
|