ogl_beamforming

Ultrasound Beamforming Implemented with OpenGL
git clone anongit@rnpnr.xyz:ogl_beamforming.git
Log | Files | Refs | Feed | Submodules | README | LICENSE

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 }