math.c (26016B)
1 /* See LICENSE for license details. */ 2 #include "external/cephes.c" 3 4 function void 5 fill_kronecker_sub_matrix_f16(f16 *out, i32 out_stride, f16 scale, const f16 *b, iv2 b_dim) 6 { 7 for (i32 i = 0; i < b_dim.y; i++) { 8 for (i32 j = 0; j < b_dim.x; j += 4, b += 4) { 9 out[j + 0] = scale * b[0]; 10 out[j + 1] = scale * b[1]; 11 out[j + 2] = scale * b[2]; 12 out[j + 3] = scale * b[3]; 13 } 14 out += out_stride; 15 } 16 } 17 18 /* NOTE: this won't check for valid space/etc and assumes row major order */ 19 function void 20 kronecker_product_f16(f16 *out, const f16 *a, iv2 a_dim, const f16 *b, iv2 b_dim) 21 { 22 iv2 out_dim = {{a_dim.x * b_dim.x, a_dim.y * b_dim.y}}; 23 assert(out_dim.y % 4 == 0); 24 for (i32 i = 0; i < a_dim.y; i++) { 25 f16 *vout = out; 26 for (i32 j = 0; j < a_dim.x; j++, a++) { 27 fill_kronecker_sub_matrix_f16(vout, out_dim.y, *a, b, b_dim); 28 vout += b_dim.y; 29 } 30 out += out_dim.y * b_dim.x; 31 } 32 } 33 34 /* NOTE/TODO: to support even more hadamard sizes use the Paley construction */ 35 function f16 * 36 make_hadamard_transpose(Arena *arena, i32 dim, b32 row_major) 37 { 38 read_only f16 hadamard_12_12_transpose[] = { 39 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 40 1, -1, -1, 1, -1, -1, -1, 1, 1, 1, -1, 1, 41 1, 1, -1, -1, 1, -1, -1, -1, 1, 1, 1, -1, 42 1, -1, 1, -1, -1, 1, -1, -1, -1, 1, 1, 1, 43 1, 1, -1, 1, -1, -1, 1, -1, -1, -1, 1, 1, 44 1, 1, 1, -1, 1, -1, -1, 1, -1, -1, -1, 1, 45 1, 1, 1, 1, -1, 1, -1, -1, 1, -1, -1, -1, 46 1, -1, 1, 1, 1, -1, 1, -1, -1, 1, -1, -1, 47 1, -1, -1, 1, 1, 1, -1, 1, -1, -1, 1, -1, 48 1, -1, -1, -1, 1, 1, 1, -1, 1, -1, -1, 1, 49 1, 1, -1, -1, -1, 1, 1, 1, -1, 1, -1, -1, 50 1, -1, 1, -1, -1, -1, 1, 1, 1, -1, 1, -1, 51 }; 52 53 read_only f16 hadamard_20_20_transpose[] = { 54 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 55 1, -1, -1, 1, 1, -1, -1, -1, -1, 1, -1, 1, -1, 1, 1, 1, 1, -1, -1, 1, 56 1, -1, 1, 1, -1, -1, -1, -1, 1, -1, 1, -1, 1, 1, 1, 1, -1, -1, 1, -1, 57 1, 1, 1, -1, -1, -1, -1, 1, -1, 1, -1, 1, 1, 1, 1, -1, -1, 1, -1, -1, 58 1, 1, -1, -1, -1, -1, 1, -1, 1, -1, 1, 1, 1, 1, -1, -1, 1, -1, -1, 1, 59 1, -1, -1, -1, -1, 1, -1, 1, -1, 1, 1, 1, 1, -1, -1, 1, -1, -1, 1, 1, 60 1, -1, -1, -1, 1, -1, 1, -1, 1, 1, 1, 1, -1, -1, 1, -1, -1, 1, 1, -1, 61 1, -1, -1, 1, -1, 1, -1, 1, 1, 1, 1, -1, -1, 1, -1, -1, 1, 1, -1, -1, 62 1, -1, 1, -1, 1, -1, 1, 1, 1, 1, -1, -1, 1, -1, -1, 1, 1, -1, -1, -1, 63 1, 1, -1, 1, -1, 1, 1, 1, 1, -1, -1, 1, -1, -1, 1, 1, -1, -1, -1, -1, 64 1, -1, 1, -1, 1, 1, 1, 1, -1, -1, 1, -1, -1, 1, 1, -1, -1, -1, -1, 1, 65 1, 1, -1, 1, 1, 1, 1, -1, -1, 1, -1, -1, 1, 1, -1, -1, -1, -1, 1, -1, 66 1, -1, 1, 1, 1, 1, -1, -1, 1, -1, -1, 1, 1, -1, -1, -1, -1, 1, -1, 1, 67 1, 1, 1, 1, 1, -1, -1, 1, -1, -1, 1, 1, -1, -1, -1, -1, 1, -1, 1, -1, 68 1, 1, 1, 1, -1, -1, 1, -1, -1, 1, 1, -1, -1, -1, -1, 1, -1, 1, -1, 1, 69 1, 1, 1, -1, -1, 1, -1, -1, 1, 1, -1, -1, -1, -1, 1, -1, 1, -1, 1, 1, 70 1, 1, -1, -1, 1, -1, -1, 1, 1, -1, -1, -1, -1, 1, -1, 1, -1, 1, 1, 1, 71 1, -1, -1, 1, -1, -1, 1, 1, -1, -1, -1, -1, 1, -1, 1, -1, 1, 1, 1, 1, 72 1, -1, 1, -1, -1, 1, 1, -1, -1, -1, -1, 1, -1, 1, -1, 1, 1, 1, 1, -1, 73 1, 1, -1, -1, 1, 1, -1, -1, -1, -1, 1, -1, 1, -1, 1, 1, 1, 1, -1, -1, 74 }; 75 76 f16 *result = 0; 77 78 i32 order = dim; 79 b32 power_of_2 = IsPowerOfTwo(dim); 80 b32 multiple_of_12 = dim % 12 == 0; 81 b32 multiple_of_20 = dim % 20 == 0; 82 i64 elements = dim * dim; 83 84 i32 base_dim = 0; 85 if (power_of_2) { 86 base_dim = dim; 87 } else if (multiple_of_20 && IsPowerOfTwo(dim / 20)) { 88 base_dim = 20; 89 dim /= 20; 90 } else if (multiple_of_12 && IsPowerOfTwo(dim / 12)) { 91 base_dim = 12; 92 dim /= 12; 93 } 94 95 if (power_of_2 && base_dim) { 96 result = push_array(arena, f16, elements); 97 98 Temp scratch = temp_begin(arena); 99 f16 *m = dim == base_dim ? result : push_array(arena, f16, elements); 100 101 #define IND(i, j) ((i) * dim + (j)) 102 m[0] = 1; 103 for (i32 k = 1; k < dim; k *= 2) { 104 for (i32 i = 0; i < k; i++) { 105 for (i32 j = 0; j < k; j++) { 106 f16 val = m[IND(i, j)]; 107 m[IND(i + k, j)] = val; 108 m[IND(i, j + k)] = val; 109 m[IND(i + k, j + k)] = -val; 110 } 111 } 112 } 113 #undef IND 114 115 const f16 *m2 = 0; 116 iv2 m2_dim; 117 switch (base_dim) { 118 case 12:{ m2 = hadamard_12_12_transpose; m2_dim = (iv2){{12, 12}}; }break; 119 case 20:{ m2 = hadamard_20_20_transpose; m2_dim = (iv2){{20, 20}}; }break; 120 } 121 if (m2) kronecker_product_f16(result, m, (iv2){{dim, dim}}, m2, m2_dim); 122 123 temp_end(scratch); 124 } 125 126 if (result && row_major) { 127 for (i32 r = 0; r < order; r++) 128 for (i32 c = 0; c < order; c++) 129 swap(result[r * order + c], result[c * order + r]); 130 } 131 132 return result; 133 } 134 135 function b32 136 u128_equal(u128 a, u128 b) 137 { 138 b32 result = a.U64[0] == b.U64[0] && a.U64[1] == b.U64[1]; 139 return result; 140 } 141 142 function RangeU64 143 subrange_n_from_n_m_count(u64 n, u64 n_count, u64 m) 144 { 145 assert(n < n_count); 146 147 u64 per_lane = m / n_count; 148 u64 leftover = m - per_lane * n_count; 149 u64 leftovers_before_n = Min(leftover, n); 150 u64 base_index = n * per_lane + leftovers_before_n; 151 u64 one_past_last_index = base_index + per_lane + ((n < leftover) ? 1 : 0); 152 153 RangeU64 result = {base_index, one_past_last_index}; 154 return result; 155 } 156 157 function i32 158 iv3_dimension(iv3 points) 159 { 160 i32 result = (points.x > 1) + (points.y > 1) + (points.z > 1); 161 return result; 162 } 163 164 function bv3 165 iv3_equal(iv3 a, iv3 b) 166 { 167 bv3 result; 168 result.x = a.x == b.x; 169 result.y = a.y == b.y; 170 result.z = a.z == b.z; 171 return result; 172 } 173 174 function b32 175 bv3_all(bv3 a) 176 { 177 b32 result = a.x != 0 && a.y != 0 && a.z != 0; 178 return result; 179 } 180 181 function b32 182 bv3_any(bv3 a) 183 { 184 b32 result = a.x != 0 || a.y != 0 || a.z != 0; 185 return result; 186 } 187 188 function v2 189 clamp_v2_rect(v2 v, Rect r) 190 { 191 v2 result = v; 192 result.x = Clamp(v.x, r.pos.x, r.pos.x + r.size.x); 193 result.y = Clamp(v.y, r.pos.y, r.pos.y + r.size.y); 194 return result; 195 } 196 197 function v2 198 v2_from_iv2(iv2 v) 199 { 200 v2 result; 201 result.E[0] = (f32)v.E[0]; 202 result.E[1] = (f32)v.E[1]; 203 return result; 204 } 205 206 function v2 207 v2_abs(v2 a) 208 { 209 v2 result; 210 result.x = Abs(a.x); 211 result.y = Abs(a.y); 212 return result; 213 } 214 215 function v2 216 v2_scale(v2 a, f32 scale) 217 { 218 v2 result; 219 result.x = a.x * scale; 220 result.y = a.y * scale; 221 return result; 222 } 223 224 function v2 225 v2_add(v2 a, v2 b) 226 { 227 v2 result; 228 result.x = a.x + b.x; 229 result.y = a.y + b.y; 230 return result; 231 } 232 233 function v2 234 v2_sub(v2 a, v2 b) 235 { 236 v2 result = v2_add(a, v2_scale(b, -1.0f)); 237 return result; 238 } 239 240 function v2 241 v2_mul(v2 a, v2 b) 242 { 243 v2 result; 244 result.x = a.x * b.x; 245 result.y = a.y * b.y; 246 return result; 247 } 248 249 function v2 250 v2_div(v2 a, v2 b) 251 { 252 v2 result; 253 result.x = a.x / b.x; 254 result.y = a.y / b.y; 255 return result; 256 } 257 258 function v2 259 v2_floor(v2 a) 260 { 261 v2 result; 262 result.x = (f32)((i32)a.x); 263 result.y = (f32)((i32)a.y); 264 return result; 265 } 266 267 function f32 268 v2_magnitude_squared(v2 a) 269 { 270 f32 result = a.x * a.x + a.y * a.y; 271 return result; 272 } 273 274 function f32 275 v2_magnitude(v2 a) 276 { 277 f32 result = sqrt_f32(a.x * a.x + a.y * a.y); 278 return result; 279 } 280 281 function v3 282 cross(v3 a, v3 b) 283 { 284 v3 result; 285 result.x = a.y * b.z - a.z * b.y; 286 result.y = a.z * b.x - a.x * b.z; 287 result.z = a.x * b.y - a.y * b.x; 288 return result; 289 } 290 291 function v3 292 v3_from_iv3(iv3 v) 293 { 294 v3 result; 295 result.E[0] = (f32)v.E[0]; 296 result.E[1] = (f32)v.E[1]; 297 result.E[2] = (f32)v.E[2]; 298 return result; 299 } 300 301 function v3 302 v3_abs(v3 a) 303 { 304 v3 result; 305 result.x = Abs(a.x); 306 result.y = Abs(a.y); 307 result.z = Abs(a.z); 308 return result; 309 } 310 311 function v3 312 v3_scale(v3 a, f32 scale) 313 { 314 v3 result; 315 result.x = scale * a.x; 316 result.y = scale * a.y; 317 result.z = scale * a.z; 318 return result; 319 } 320 321 function v3 322 v3_add(v3 a, v3 b) 323 { 324 v3 result; 325 result.x = a.x + b.x; 326 result.y = a.y + b.y; 327 result.z = a.z + b.z; 328 return result; 329 } 330 331 function v3 332 v3_sub(v3 a, v3 b) 333 { 334 v3 result = v3_add(a, v3_scale(b, -1.0f)); 335 return result; 336 } 337 338 function v3 339 v3_div(v3 a, v3 b) 340 { 341 v3 result; 342 result.x = a.x / b.x; 343 result.y = a.y / b.y; 344 result.z = a.z / b.z; 345 return result; 346 } 347 348 function f32 349 v3_dot(v3 a, v3 b) 350 { 351 f32 result = a.x * b.x + a.y * b.y + a.z * b.z; 352 return result; 353 } 354 355 function f32 356 v3_magnitude_squared(v3 a) 357 { 358 f32 result = v3_dot(a, a); 359 return result; 360 } 361 362 function f32 363 v3_magnitude(v3 a) 364 { 365 f32 result = sqrt_f32(v3_dot(a, a)); 366 return result; 367 } 368 369 function v3 370 v3_normalize(v3 a) 371 { 372 v3 result = v3_scale(a, 1.0f / v3_magnitude(a)); 373 return result; 374 } 375 376 function v4 377 v4_scale(v4 a, f32 scale) 378 { 379 v4 result; 380 result.x = scale * a.x; 381 result.y = scale * a.y; 382 result.z = scale * a.z; 383 result.w = scale * a.w; 384 return result; 385 } 386 387 function v4 388 v4_add(v4 a, v4 b) 389 { 390 v4 result; 391 result.x = a.x + b.x; 392 result.y = a.y + b.y; 393 result.z = a.z + b.z; 394 result.w = a.w + b.w; 395 return result; 396 } 397 398 function v4 399 v4_sub(v4 a, v4 b) 400 { 401 v4 result = v4_add(a, v4_scale(b, -1)); 402 return result; 403 } 404 405 function f32 406 v4_dot(v4 a, v4 b) 407 { 408 f32 result = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w; 409 return result; 410 } 411 412 function v4 413 v4_lerp(v4 a, v4 b, f32 t) 414 { 415 v4 result = v4_add(a, v4_scale(v4_sub(b, a), t)); 416 return result; 417 } 418 419 function b32 420 m4_equal(m4 a, m4 b) 421 { 422 b32 result = 1; 423 #ifdef __AVX512F__ 424 __m512 one = _mm512_set1_ps(1.f); 425 __m512 eps = _mm512_set1_ps(F32_EPSILON); 426 __m512 av = _mm512_loadu_ps(&a); 427 __m512 bv = _mm512_loadu_ps(&b); 428 __m512 delta = _mm512_abs_ps(_mm512_sub_ps(av, bv)); 429 __m512 test = _mm512_mul_ps(eps, _mm512_max_ps(one, _mm512_max_ps(_mm512_abs_ps(av), _mm512_abs_ps(bv)))); 430 result = _mm512_cmp_ps_mask(delta, test, _CMP_GT_OS) == 0; 431 #else 432 for EachElement(a.E, it) 433 result &= f32_equal(a.E[it], b.E[it]); 434 #endif 435 return result; 436 } 437 438 #define m4_identity() \ 439 (m4){.E = { \ 440 1, 0, 0, 0, \ 441 0, 1, 0, 0, \ 442 0, 0, 1, 0, \ 443 0, 0, 0, 1, \ 444 }} 445 446 function v4 447 m4_row(m4 a, u32 row) 448 { 449 v4 result; 450 result.E[0] = a.c[0].E[row]; 451 result.E[1] = a.c[1].E[row]; 452 result.E[2] = a.c[2].E[row]; 453 result.E[3] = a.c[3].E[row]; 454 return result; 455 } 456 457 function m4 458 m4_mul(m4 a, m4 b) 459 { 460 m4 result; 461 for (u32 i = 0; i < 4; i++) { 462 for (u32 j = 0; j < 4; j++) { 463 result.c[i].E[j] = v4_dot(m4_row(a, j), b.c[i]); 464 } 465 } 466 return result; 467 } 468 469 /* NOTE(rnp): based on: 470 * https://web.archive.org/web/20131215123403/ftp://download.intel.com/design/PentiumIII/sml/24504301.pdf 471 * TODO(rnp): redo with SIMD as given in the link (but need to rewrite for column-major) 472 */ 473 function m4 474 m4_inverse(m4 m) 475 { 476 m4 result; 477 result.E[ 0] = m.E[5] * m.E[10] * m.E[15] - m.E[5] * m.E[11] * m.E[14] - m.E[9] * m.E[6] * m.E[15] + m.E[9] * m.E[7] * m.E[14] + m.E[13] * m.E[6] * m.E[11] - m.E[13] * m.E[7] * m.E[10]; 478 result.E[ 4] = -m.E[4] * m.E[10] * m.E[15] + m.E[4] * m.E[11] * m.E[14] + m.E[8] * m.E[6] * m.E[15] - m.E[8] * m.E[7] * m.E[14] - m.E[12] * m.E[6] * m.E[11] + m.E[12] * m.E[7] * m.E[10]; 479 result.E[ 8] = m.E[4] * m.E[ 9] * m.E[15] - m.E[4] * m.E[11] * m.E[13] - m.E[8] * m.E[5] * m.E[15] + m.E[8] * m.E[7] * m.E[13] + m.E[12] * m.E[5] * m.E[11] - m.E[12] * m.E[7] * m.E[ 9]; 480 result.E[12] = -m.E[4] * m.E[ 9] * m.E[14] + m.E[4] * m.E[10] * m.E[13] + m.E[8] * m.E[5] * m.E[14] - m.E[8] * m.E[6] * m.E[13] - m.E[12] * m.E[5] * m.E[10] + m.E[12] * m.E[6] * m.E[ 9]; 481 result.E[ 1] = -m.E[1] * m.E[10] * m.E[15] + m.E[1] * m.E[11] * m.E[14] + m.E[9] * m.E[2] * m.E[15] - m.E[9] * m.E[3] * m.E[14] - m.E[13] * m.E[2] * m.E[11] + m.E[13] * m.E[3] * m.E[10]; 482 result.E[ 5] = m.E[0] * m.E[10] * m.E[15] - m.E[0] * m.E[11] * m.E[14] - m.E[8] * m.E[2] * m.E[15] + m.E[8] * m.E[3] * m.E[14] + m.E[12] * m.E[2] * m.E[11] - m.E[12] * m.E[3] * m.E[10]; 483 result.E[ 9] = -m.E[0] * m.E[ 9] * m.E[15] + m.E[0] * m.E[11] * m.E[13] + m.E[8] * m.E[1] * m.E[15] - m.E[8] * m.E[3] * m.E[13] - m.E[12] * m.E[1] * m.E[11] + m.E[12] * m.E[3] * m.E[ 9]; 484 result.E[13] = m.E[0] * m.E[ 9] * m.E[14] - m.E[0] * m.E[10] * m.E[13] - m.E[8] * m.E[1] * m.E[14] + m.E[8] * m.E[2] * m.E[13] + m.E[12] * m.E[1] * m.E[10] - m.E[12] * m.E[2] * m.E[ 9]; 485 result.E[ 2] = m.E[1] * m.E[ 6] * m.E[15] - m.E[1] * m.E[ 7] * m.E[14] - m.E[5] * m.E[2] * m.E[15] + m.E[5] * m.E[3] * m.E[14] + m.E[13] * m.E[2] * m.E[ 7] - m.E[13] * m.E[3] * m.E[ 6]; 486 result.E[ 6] = -m.E[0] * m.E[ 6] * m.E[15] + m.E[0] * m.E[ 7] * m.E[14] + m.E[4] * m.E[2] * m.E[15] - m.E[4] * m.E[3] * m.E[14] - m.E[12] * m.E[2] * m.E[ 7] + m.E[12] * m.E[3] * m.E[ 6]; 487 result.E[10] = m.E[0] * m.E[ 5] * m.E[15] - m.E[0] * m.E[ 7] * m.E[13] - m.E[4] * m.E[1] * m.E[15] + m.E[4] * m.E[3] * m.E[13] + m.E[12] * m.E[1] * m.E[ 7] - m.E[12] * m.E[3] * m.E[ 5]; 488 result.E[14] = -m.E[0] * m.E[ 5] * m.E[14] + m.E[0] * m.E[ 6] * m.E[13] + m.E[4] * m.E[1] * m.E[14] - m.E[4] * m.E[2] * m.E[13] - m.E[12] * m.E[1] * m.E[ 6] + m.E[12] * m.E[2] * m.E[ 5]; 489 result.E[ 3] = -m.E[1] * m.E[ 6] * m.E[11] + m.E[1] * m.E[ 7] * m.E[10] + m.E[5] * m.E[2] * m.E[11] - m.E[5] * m.E[3] * m.E[10] - m.E[ 9] * m.E[2] * m.E[ 7] + m.E[ 9] * m.E[3] * m.E[ 6]; 490 result.E[ 7] = m.E[0] * m.E[ 6] * m.E[11] - m.E[0] * m.E[ 7] * m.E[10] - m.E[4] * m.E[2] * m.E[11] + m.E[4] * m.E[3] * m.E[10] + m.E[ 8] * m.E[2] * m.E[ 7] - m.E[ 8] * m.E[3] * m.E[ 6]; 491 result.E[11] = -m.E[0] * m.E[ 5] * m.E[11] + m.E[0] * m.E[ 7] * m.E[ 9] + m.E[4] * m.E[1] * m.E[11] - m.E[4] * m.E[3] * m.E[ 9] - m.E[ 8] * m.E[1] * m.E[ 7] + m.E[ 8] * m.E[3] * m.E[ 5]; 492 result.E[15] = m.E[0] * m.E[ 5] * m.E[10] - m.E[0] * m.E[ 6] * m.E[ 9] - m.E[4] * m.E[1] * m.E[10] + m.E[4] * m.E[2] * m.E[ 9] + m.E[ 8] * m.E[1] * m.E[ 6] - m.E[ 8] * m.E[2] * m.E[ 5]; 493 494 f32 determinant = m.E[0] * result.E[0] + m.E[1] * result.E[4] + m.E[2] * result.E[8] + m.E[3] * result.E[12]; 495 determinant = 1.0f / determinant; 496 for(i32 i = 0; i < 16; i++) 497 result.E[i] *= determinant; 498 return result; 499 } 500 501 function m4 502 m4_translation(v3 delta) 503 { 504 m4 result; 505 result.c[0] = (v4){{1, 0, 0, 0}}; 506 result.c[1] = (v4){{0, 1, 0, 0}}; 507 result.c[2] = (v4){{0, 0, 1, 0}}; 508 result.c[3] = (v4){{delta.x, delta.y, delta.z, 1}}; 509 return result; 510 } 511 512 function m4 513 m4_scale(v3 scale) 514 { 515 m4 result; 516 result.c[0] = (v4){{scale.x, 0, 0, 0}}; 517 result.c[1] = (v4){{0, scale.y, 0, 0}}; 518 result.c[2] = (v4){{0, 0, scale.z, 0}}; 519 result.c[3] = (v4){{0, 0, 0, 1}}; 520 return result; 521 } 522 523 function m4 524 m4_rotation_about_axis(v3 axis, f32 turns) 525 { 526 assert(f32_equal(v3_magnitude_squared(axis), 1.0f)); 527 f32 sa = sin_f32(turns * 2 * PI); 528 f32 ca = cos_f32(turns * 2 * PI); 529 f32 mca = 1.0f - ca; 530 531 f32 x = axis.x, x2 = x * x; 532 f32 y = axis.y, y2 = y * y; 533 f32 z = axis.z, z2 = z * z; 534 535 m4 result; 536 result.c[0] = (v4){{ca + mca * x2, mca * x * y - sa * z, mca * x * z + sa * y, 0}}; 537 result.c[1] = (v4){{mca * x * y + sa * z, ca + mca * y2, mca * y * z - sa * x, 0}}; 538 result.c[2] = (v4){{mca * x * z - sa * y, mca * y * z + sa * x, ca + mca * z2, 0}}; 539 result.c[3] = (v4){{0, 0, 0, 1}}; 540 return result; 541 } 542 543 function m4 544 m4_rotation_about_y(f32 turns) 545 { 546 m4 result = m4_rotation_about_axis((v3){.y = 1.0f}, turns); 547 return result; 548 } 549 550 function m4 551 y_aligned_volume_transform(v3 extent, v3 translation, f32 rotation_turns) 552 { 553 m4 T = m4_translation(translation); 554 m4 R = m4_rotation_about_axis((v3){.y = 1.0f}, rotation_turns); 555 m4 S = m4_scale(extent); 556 m4 result = m4_mul(T, m4_mul(R, S)); 557 return result; 558 } 559 560 function v4 561 m4_mul_v4(m4 a, v4 v) 562 { 563 v4 result; 564 result.x = v4_dot(m4_row(a, 0), v); 565 result.y = v4_dot(m4_row(a, 1), v); 566 result.z = v4_dot(m4_row(a, 2), v); 567 result.w = v4_dot(m4_row(a, 3), v); 568 return result; 569 } 570 571 function v3 572 m4_mul_v3(m4 a, v3 v) 573 { 574 v3 result = m4_mul_v4(a, (v4){{v.x, v.y, v.z, 1.0f}}).xyz; 575 return result; 576 } 577 578 function v2 579 rect_uv(v2 p, Rect r) 580 { 581 v2 result = v2_div(v2_sub(p, r.pos), r.size); 582 return result; 583 } 584 585 function v2 586 rect_uv_ndc(v2 p, Rect r) 587 { 588 v2 uv = rect_uv(p, r); 589 v2 result = v2_sub(v2_scale(uv, 2.f), (v2){{1.f, 1.f}}); 590 return result; 591 } 592 593 function Rect 594 rect_intersect(Rect a, Rect b) 595 { 596 v2 ae = v2_add(a.pos, a.size); 597 v2 be = v2_add(b.pos, b.size); 598 599 Rect result = {0}; 600 result.pos.x = Max(a.pos.x, b.pos.x); 601 result.pos.y = Max(a.pos.y, b.pos.y); 602 result.size.x = Min(ae.x, be.x) - result.pos.x; 603 result.size.y = Min(ae.y, be.y) - result.pos.y; 604 return result; 605 } 606 607 function Rect 608 rect_squish_centered(Rect a, v2 pct) 609 { 610 v2 delta_size = v2_mul(a.size, pct); 611 Rect result; 612 result.pos = v2_add(a.pos, v2_scale(delta_size, 0.5f)); 613 result.size = v2_add(a.size, v2_scale(delta_size, -1.f)); 614 return result; 615 } 616 617 function Rect 618 rect_shrink_centered(Rect a, v2 px) 619 { 620 Rect result; 621 result.pos = v2_add(a.pos, v2_scale(px, 0.5f)); 622 result.size = v2_add(a.size, v2_scale(px, -1.f)); 623 return result; 624 } 625 626 function m4 627 orthographic_projection(f32 n, f32 f, f32 t, f32 r) 628 { 629 m4 result; 630 f32 a = -2 / (f - n); 631 f32 b = - (f + n) / (f - n); 632 result.c[0] = (v4){{1 / r, 0, 0, 0}}; 633 result.c[1] = (v4){{0, 1 / t, 0, 0}}; 634 result.c[2] = (v4){{0, 0, a, 0}}; 635 result.c[3] = (v4){{0, 0, b, 1}}; 636 return result; 637 } 638 639 function m4 640 perspective_projection(f32 n, f32 f, f32 fov, f32 aspect) 641 { 642 m4 result; 643 f32 t = n * tan_f32(fov / 2.0f); 644 f32 r = t * aspect; 645 f32 a = -(f + n) / (f - n); 646 f32 b = -2 * f * n / (f - n); 647 result.c[0] = (v4){{n / r, 0, 0, 0}}; 648 result.c[1] = (v4){{0, n / t, 0, 0}}; 649 result.c[2] = (v4){{0, 0, a, -1}}; 650 result.c[3] = (v4){{0, 0, b, 0}}; 651 return result; 652 } 653 654 function m4 655 camera_look_at(v3 camera, v3 point) 656 { 657 v3 orthogonal = {{0, 1.0f, 0}}; 658 v3 normal = v3_normalize(v3_sub(camera, point)); 659 v3 right = cross(orthogonal, normal); 660 v3 up = cross(normal, right); 661 662 v3 translate; 663 camera = v3_sub((v3){0}, camera); 664 translate.x = v3_dot(camera, right); 665 translate.y = v3_dot(camera, up); 666 translate.z = v3_dot(camera, normal); 667 668 m4 result; 669 result.c[0] = (v4){{right.x, up.x, normal.x, 0}}; 670 result.c[1] = (v4){{right.y, up.y, normal.y, 0}}; 671 result.c[2] = (v4){{right.z, up.z, normal.z, 0}}; 672 result.c[3] = (v4){{translate.x, translate.y, translate.z, 1}}; 673 return result; 674 } 675 676 /* NOTE(rnp): adapted from "Essential Mathematics for Games and Interactive Applications" (Verth, Bishop) */ 677 function f32 678 obb_raycast(m4 obb_orientation, v3 obb_size, v3 obb_center, ray r) 679 { 680 v3 p = v3_sub(obb_center, r.origin); 681 v3 X = obb_orientation.c[0].xyz; 682 v3 Y = obb_orientation.c[1].xyz; 683 v3 Z = obb_orientation.c[2].xyz; 684 685 /* NOTE(rnp): projects direction vector onto OBB axis */ 686 v3 f; 687 f.x = v3_dot(X, r.direction); 688 f.y = v3_dot(Y, r.direction); 689 f.z = v3_dot(Z, r.direction); 690 691 /* NOTE(rnp): projects relative vector onto OBB axis */ 692 v3 e; 693 e.x = v3_dot(X, p); 694 e.y = v3_dot(Y, p); 695 e.z = v3_dot(Z, p); 696 697 f32 result = 0; 698 f32 t[6] = {0}; 699 for (i32 i = 0; i < 3; i++) { 700 if (f32_equal(f.E[i], 0)) { 701 if (-e.E[i] - obb_size.E[i] > 0 || -e.E[i] + obb_size.E[i] < 0) 702 result = -1.0f; 703 f.E[i] = F32_EPSILON; 704 } 705 t[i * 2 + 0] = (e.E[i] + obb_size.E[i]) / f.E[i]; 706 t[i * 2 + 1] = (e.E[i] - obb_size.E[i]) / f.E[i]; 707 } 708 709 if (result != -1) { 710 f32 tmin = Max(Max(Min(t[0], t[1]), Min(t[2], t[3])), Min(t[4], t[5])); 711 f32 tmax = Min(Min(Max(t[0], t[1]), Max(t[2], t[3])), Max(t[4], t[5])); 712 if (tmax >= 0 && tmin <= tmax) { 713 result = tmin > 0 ? tmin : tmax; 714 } else { 715 result = -1; 716 } 717 } 718 719 return result; 720 } 721 722 function f32 723 complex_filter_first_moment(v2 *filter, i32 length, f32 sampling_frequency) 724 { 725 f32 n = 0, d = 0; 726 for (i32 i = 0; i < length; i++) { 727 f32 t = v2_magnitude_squared(filter[i]); 728 n += (f32)i * t; 729 d += t; 730 } 731 f32 result = n / d / sampling_frequency; 732 return result; 733 } 734 735 function f32 736 real_filter_first_moment(f32 *filter, i32 length, f32 sampling_frequency) 737 { 738 f32 n = 0, d = 0; 739 for (i32 i = 0; i < length; i++) { 740 f32 t = filter[i] * filter[i]; 741 n += (f32)i * t; 742 d += t; 743 } 744 f32 result = n / d / sampling_frequency; 745 return result; 746 } 747 748 function f32 749 tukey_window(f32 t, f32 tapering) 750 { 751 f32 r = tapering; 752 f32 result = 1; 753 if (t < r / 2) result = 0.5f * (1 + cos_f32(2 * PI * (t - r / 2) / r)); 754 if (t >= 1 - r / 2) result = 0.5f * (1 + cos_f32(2 * PI * (t - 1 + r / 2) / r)); 755 return result; 756 } 757 758 /* NOTE(rnp): adapted from "Discrete Time Signal Processing" (Oppenheim) */ 759 function f32 * 760 kaiser_low_pass_filter(Arena *arena, f32 cutoff_frequency, f32 sampling_frequency, f32 beta, i32 length) 761 { 762 f32 *result = push_array(arena, f32, length); 763 f32 wc = 2 * PI * cutoff_frequency / sampling_frequency; 764 f32 a = (f32)length / 2.0f; 765 f32 pi_i0_b = PI * (f32)cephes_i0(beta); 766 767 for (i32 n = 0; n < length; n++) { 768 f32 t = (f32)n - a; 769 f32 impulse = !f32_equal(t, 0) ? sin_f32(wc * t) / t : wc; 770 t = t / a; 771 f32 window = (f32)cephes_i0(beta * sqrt_f32(1 - t * t)) / pi_i0_b; 772 result[n] = impulse * window; 773 } 774 775 return result; 776 } 777 778 function f32 * 779 rf_chirp(Arena *arena, f32 min_frequency, f32 max_frequency, f32 sampling_frequency, 780 i32 length, b32 reverse) 781 { 782 f32 *result = push_array(arena, f32, length); 783 for (i32 i = 0; i < length; i++) { 784 i32 index = reverse? length - 1 - i : i; 785 f32 fc = min_frequency + (f32)i * (max_frequency - min_frequency) / (2 * (f32)length); 786 f32 arg = 2 * PI * fc * (f32)i / sampling_frequency; 787 result[index] = sin_f32(arg) * tukey_window((f32)i / (f32)length, 0.2f); 788 } 789 return result; 790 } 791 792 function v2 * 793 baseband_chirp(Arena *arena, f32 min_frequency, f32 max_frequency, f32 sampling_frequency, 794 i32 length, b32 reverse, f32 scale) 795 { 796 v2 *result = push_array(arena, v2, length); 797 f32 conjugate = reverse ? -1 : 1; 798 for (i32 i = 0; i < length; i++) { 799 i32 index = reverse? length - 1 - i : i; 800 f32 fc = min_frequency + (f32)i * (max_frequency - min_frequency) / (2 * (f32)length); 801 f32 arg = 2 * PI * fc * (f32)i / sampling_frequency; 802 v2 sample = {{scale * cos_f32(arg), conjugate * scale * sin_f32(arg)}}; 803 result[index] = v2_scale(sample, tukey_window((f32)i / (f32)length, 0.2f)); 804 } 805 return result; 806 } 807 808 function m4 809 das_transform_1d(v3 p1, v3 p2) 810 { 811 v3 extent = v3_sub(p2, p1); 812 m4 result = { 813 .c[0] = (v4){{extent.x, extent.y, extent.z, 0.0f}}, 814 .c[1] = (v4){{0.0f, 0.0f, 0.0f, 0.0f}}, 815 .c[2] = (v4){{0.0f, 0.0f, 0.0f, 0.0f}}, 816 .c[3] = (v4){{p1.x, p1.y, p1.z, 1.0f}}, 817 }; 818 return result; 819 } 820 821 function m4 822 das_transform_2d_with_normal(v3 normal, v2 min_coordinate, v2 max_coordinate, f32 offset) 823 { 824 v3 U = {{0, 1.0f, 0}}; 825 if (f32_equal(v3_dot(U, normal), 1.0f)) 826 U = (v3){{1.0f, 0, 0}}; 827 828 v3 N = normal; 829 v3 V = cross(U, N); 830 831 v3 min = v3_add(v3_scale(U, min_coordinate.x), v3_scale(V, min_coordinate.y)); 832 v3 max = v3_add(v3_scale(U, max_coordinate.x), v3_scale(V, max_coordinate.y)); 833 834 v3 extent = v3_sub(max, min); 835 U = v3_scale(U, v3_dot(U, extent)); 836 V = v3_scale(V, v3_dot(V, extent)); 837 838 v3 t = v3_add(v3_scale(N, offset), min); 839 840 m4 result; 841 result.c[0] = (v4){{U.x, U.y, U.z, 0.0f}}; 842 result.c[1] = (v4){{V.x, V.y, V.z, 0.0f}}; 843 result.c[2] = (v4){{N.x, N.y, N.z, 0.0f}}; 844 result.c[3] = (v4){{t.x, t.y, t.z, 1.0f}}; 845 846 return result; 847 } 848 849 function m4 850 das_transform_2d_xz(v2 min_coordinate, v2 max_coordinate, f32 y_off) 851 { 852 m4 result = das_transform_2d_with_normal((v3){.y = 1.0f}, min_coordinate, max_coordinate, y_off); 853 return result; 854 } 855 856 function m4 857 das_transform_2d_yz(v2 min_coordinate, v2 max_coordinate, f32 x_off) 858 { 859 // NOTE(rnp): flip so that region extends in correct direction 860 m4 result = das_transform_2d_with_normal((v3){.x = -1.0f}, min_coordinate, max_coordinate, x_off); 861 return result; 862 } 863 864 function m4 865 das_transform_2d_xy(v2 min_coordinate, v2 max_coordinate, f32 z_off) 866 { 867 m4 result = das_transform_2d_with_normal((v3){.z = 1.0f}, min_coordinate, max_coordinate, z_off); 868 return result; 869 } 870 871 function m4 872 das_transform_3d(v3 min_coordinate, v3 max_coordinate) 873 { 874 v3 extent = v3_sub(max_coordinate, min_coordinate); 875 m4 result; 876 result.c[0] = (v4){{extent.x, 0.0f, 0.0f, 0.0f}}; 877 result.c[1] = (v4){{0.0f, extent.y, 0.0f, 0.0f}}; 878 result.c[2] = (v4){{0.0f, 0.0f, extent.z, 0.0f}}; 879 result.c[3] = (v4){{min_coordinate.x, min_coordinate.y, min_coordinate.z, 1.0f}}; 880 return result; 881 } 882 883 function v3 884 plane_normal_from_transform(m4 transform) 885 { 886 v3 U = v3_normalize(transform.c[0].xyz); 887 v3 V = v3_normalize(transform.c[1].xyz); 888 v3 result = cross(V, U); 889 return result; 890 } 891 892 function f32 893 plane_offset_from_transform(m4 transform) 894 { 895 f32 result = v3_dot(plane_normal_from_transform(transform), transform.c[3].xyz); 896 return result; 897 } 898 899 function void 900 plane_corners_from_transform(m4 transform, v2 *min, v2 *max) 901 { 902 v3 U = v3_normalize(transform.c[0].xyz); 903 v3 V = v3_normalize(transform.c[1].xyz); 904 905 v3 min_3d = m4_mul_v3(transform, (v3){{0.f, 0.f, 0.f}}); 906 v3 max_3d = m4_mul_v3(transform, (v3){{1.f, 1.f, 1.f}}); 907 908 if (min) *min = (v2){{v3_dot(U, min_3d), v3_dot(V, min_3d)}}; 909 if (max) *max = (v2){{v3_dot(U, max_3d), v3_dot(V, max_3d)}}; 910 } 911 912 function v2 913 plane_uv(v3 point, v3 U, v3 V) 914 { 915 v2 result; 916 result.x = v3_dot(U, point) / v3_dot(U, U); 917 result.y = v3_dot(V, point) / v3_dot(V, V); 918 return result; 919 } 920 921 function v4 922 hsv_to_rgb(v4 hsv) 923 { 924 /* f(k(n)) = V - V*S*max(0, min(k, min(4 - k, 1))) 925 * k(n) = fmod((n + H * 6), 6) 926 * (R, G, B) = (f(n = 5), f(n = 3), f(n = 1)) 927 */ 928 alignas(16) f32 nval[4] = {5.0f, 3.0f, 1.0f, 0.0f}; 929 f32x4 n = load_f32x4(nval); 930 f32x4 H = dup_f32x4(hsv.x); 931 f32x4 S = dup_f32x4(hsv.y); 932 f32x4 V = dup_f32x4(hsv.z); 933 f32x4 six = dup_f32x4(6); 934 935 f32x4 t = add_f32x4(n, mul_f32x4(six, H)); 936 f32x4 rem = floor_f32x4(div_f32x4(t, six)); 937 f32x4 k = sub_f32x4(t, mul_f32x4(rem, six)); 938 939 t = min_f32x4(sub_f32x4(dup_f32x4(4), k), dup_f32x4(1)); 940 t = max_f32x4(dup_f32x4(0), min_f32x4(k, t)); 941 t = mul_f32x4(t, mul_f32x4(S, V)); 942 943 v4 rgba; 944 store_f32x4(rgba.E, sub_f32x4(V, t)); 945 rgba.a = hsv.a; 946 return rgba; 947 } 948 949 function f32 950 ease_in_out_cubic(f32 t) 951 { 952 f32 result; 953 if (t < 0.5f) { 954 result = 4.0f * t * t * t; 955 } else { 956 t = -2.0f * t + 2.0f; 957 result = 1.0f - t * t * t / 2.0f; 958 } 959 return result; 960 } 961 962 function f32 963 ease_in_out_quartic(f32 t) 964 { 965 f32 result; 966 if (t < 0.5f) { 967 result = 8.0f * t * t * t * t; 968 } else { 969 t = -2.0f * t + 2.0f; 970 result = 1.0f - t * t * t * t / 2.0f; 971 } 972 return result; 973 }