ogl_beamforming

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

util.c (14741B)


      1 /* See LICENSE for license details. */
      2 #define zero_struct(s) mem_clear(s, 0, sizeof(*s))
      3 function void *
      4 mem_clear(void *restrict p_, u8 c, iz size)
      5 {
      6 	u8 *p = p_;
      7 	while (size > 0) p[--size] = c;
      8 	return p;
      9 }
     10 
     11 function void
     12 mem_copy(void *restrict dest, void *restrict src, uz n)
     13 {
     14 	u8 *s = src, *d = dest;
     15 	for (; n; n--) *d++ = *s++;
     16 }
     17 
     18 function void
     19 mem_move(u8 *dest, u8 *src, uz n)
     20 {
     21 	if (dest < src) mem_copy(dest, src, n);
     22 	else            while (n) { n--; dest[n] = src[n]; }
     23 }
     24 
     25 function void *
     26 arena_aligned_start(Arena a, uz alignment)
     27 {
     28 	uz padding = -(uintptr_t)a.beg & (alignment - 1);
     29 	u8 *result = a.beg + padding;
     30 	return result;
     31 }
     32 
     33 #define arena_capacity(a, t) arena_capacity_(a, sizeof(t), alignof(t))
     34 function iz
     35 arena_capacity_(Arena *a, iz size, uz alignment)
     36 {
     37 	iz available = a->end - (u8 *)arena_aligned_start(*a, alignment);
     38 	iz result    = available / size;
     39 	return result;
     40 }
     41 
     42 function u8 *
     43 arena_commit(Arena *a, iz size)
     44 {
     45 	assert(a->end - a->beg >= size);
     46 	u8 *result = a->beg;
     47 	a->beg += size;
     48 	return result;
     49 }
     50 
     51 function void
     52 arena_pop(Arena *a, iz length)
     53 {
     54 	a->beg -= length;
     55 }
     56 
     57 #define push_array(a, t, n) (t *)arena_alloc(a, sizeof(t), _Alignof(t), n)
     58 #define push_struct(a, t)   (t *)arena_alloc(a, sizeof(t), _Alignof(t), 1)
     59 function void *
     60 arena_alloc(Arena *a, iz len, uz align, iz count)
     61 {
     62 	void *result = 0;
     63 	if (a->beg) {
     64 		u8 *start = arena_aligned_start(*a, align);
     65 		iz available = a->end - start;
     66 		assert((available >= 0 && count <= available / len));
     67 		asan_unpoison_region(start, count * len);
     68 		a->beg = start + count * len;
     69 		/* TODO: Performance? */
     70 		result = mem_clear(start, 0, count * len);
     71 	}
     72 	return result;
     73 }
     74 
     75 enum { DA_INITIAL_CAP = 16 };
     76 
     77 #define DA_STRUCT(kind, name) typedef struct { \
     78 	kind *data;     \
     79 	iz    count;    \
     80 	iz    capacity; \
     81 } name ##List;
     82 
     83 #define da_index(it, s) ((it) - (s)->data)
     84 #define da_reserve(a, s, n) \
     85   (s)->data = da_reserve_((a), (s)->data, &(s)->capacity, (s)->count + n, \
     86                           _Alignof(typeof(*(s)->data)), sizeof(*(s)->data))
     87 
     88 #define da_append_count(a, s, items, item_count) do { \
     89 	da_reserve((a), (s), (item_count));                                             \
     90 	mem_copy((s)->data + (s)->count, (items), sizeof(*(items)) * (uz)(item_count)); \
     91 	(s)->count += (item_count);                                                     \
     92 } while (0)
     93 
     94 #define da_push(a, s) \
     95   ((s)->count == (s)->capacity  \
     96     ? da_reserve(a, s, 1),      \
     97       (s)->data + (s)->count++  \
     98     : (s)->data + (s)->count++)
     99 
    100 function void *
    101 da_reserve_(Arena *a, void *data, iz *capacity, iz needed, uz align, iz size)
    102 {
    103 	iz cap = *capacity;
    104 
    105 	/* NOTE(rnp): handle both 0 initialized DAs and DAs that need to be moved (they started
    106 	 * on the stack or someone allocated something in the middle of the arena during usage) */
    107 	if (!data || a->beg != (u8 *)data + cap * size) {
    108 		void *copy = arena_alloc(a, size, align, cap);
    109 		if (data) mem_copy(copy, data, (uz)(cap * size));
    110 		data = copy;
    111 	}
    112 
    113 	if (!cap) cap = DA_INITIAL_CAP;
    114 	while (cap < needed) cap *= 2;
    115 	arena_alloc(a, size, align, cap - *capacity);
    116 	*capacity = cap;
    117 	return data;
    118 }
    119 
    120 function Arena
    121 sub_arena(Arena *a, iz len, uz align)
    122 {
    123 	Arena result = {0};
    124 
    125 	uz padding = -(uintptr_t)a->beg & (align - 1);
    126 	result.beg   = a->beg + padding;
    127 	result.end   = result.beg + len;
    128 	arena_commit(a, len + (iz)padding);
    129 
    130 	return result;
    131 }
    132 
    133 function TempArena
    134 begin_temp_arena(Arena *a)
    135 {
    136 	TempArena result = {.arena = a, .old_beg = a->beg};
    137 	return result;
    138 }
    139 
    140 function void
    141 end_temp_arena(TempArena ta)
    142 {
    143 	Arena *a = ta.arena;
    144 	if (a) {
    145 		assert(a->beg >= ta.old_beg);
    146 		a->beg = ta.old_beg;
    147 	}
    148 }
    149 
    150 function u32
    151 utf8_encode(u8 *out, u32 cp)
    152 {
    153 	u32 result = 1;
    154 	if (cp <= 0x7F) {
    155 		out[0] = cp & 0x7F;
    156 	} else if (cp <= 0x7FF) {
    157 		result = 2;
    158 		out[0] = ((cp >>  6) & 0x1F) | 0xC0;
    159 		out[1] = ((cp >>  0) & 0x3F) | 0x80;
    160 	} else if (cp <= 0xFFFF) {
    161 		result = 3;
    162 		out[0] = ((cp >> 12) & 0x0F) | 0xE0;
    163 		out[1] = ((cp >>  6) & 0x3F) | 0x80;
    164 		out[2] = ((cp >>  0) & 0x3F) | 0x80;
    165 	} else if (cp <= 0x10FFFF) {
    166 		result = 4;
    167 		out[0] = ((cp >> 18) & 0x07) | 0xF0;
    168 		out[1] = ((cp >> 12) & 0x3F) | 0x80;
    169 		out[2] = ((cp >>  6) & 0x3F) | 0x80;
    170 		out[3] = ((cp >>  0) & 0x3F) | 0x80;
    171 	} else {
    172 		out[0] = '?';
    173 	}
    174 	return result;
    175 }
    176 
    177 function UnicodeDecode
    178 utf16_decode(u16 *data, iz length)
    179 {
    180 	UnicodeDecode result = {.cp = U32_MAX};
    181 	if (length) {
    182 		result.consumed = 1;
    183 		result.cp = data[0];
    184 		if (length > 1 && BETWEEN(data[0], 0xD800u, 0xDBFFu)
    185 		               && BETWEEN(data[1], 0xDC00u, 0xDFFFu))
    186 		{
    187 			result.consumed = 2;
    188 			result.cp = ((data[0] - 0xD800u) << 10u) | ((data[1] - 0xDC00u) + 0x10000u);
    189 		}
    190 	}
    191 	return result;
    192 }
    193 
    194 function u32
    195 utf16_encode(u16 *out, u32 cp)
    196 {
    197 	u32 result = 1;
    198 	if (cp == U32_MAX) {
    199 		out[0] = '?';
    200 	} else if (cp < 0x10000u) {
    201 		out[0] = (u16)cp;
    202 	} else {
    203 		u32 value = cp - 0x10000u;
    204 		out[0] = (u16)(0xD800u + (value >> 10u));
    205 		out[1] = (u16)(0xDC00u + (value & 0x3FFu));
    206 		result = 2;
    207 	}
    208 	return result;
    209 }
    210 
    211 function Stream
    212 stream_alloc(Arena *a, i32 cap)
    213 {
    214 	Stream result = {.cap = cap};
    215 	result.data = arena_commit(a, cap);
    216 	return result;
    217 }
    218 
    219 function s8
    220 stream_to_s8(Stream *s)
    221 {
    222 	s8 result = s8("");
    223 	if (!s->errors) result = (s8){.len = s->widx, .data = s->data};
    224 	return result;
    225 }
    226 
    227 function void
    228 stream_reset(Stream *s, i32 index)
    229 {
    230 	s->errors = s->cap <= index;
    231 	if (!s->errors)
    232 		s->widx = index;
    233 }
    234 
    235 function void
    236 stream_commit(Stream *s, i32 count)
    237 {
    238 	s->errors |= !BETWEEN(s->widx + count, 0, s->cap);
    239 	if (!s->errors)
    240 		s->widx += count;
    241 }
    242 
    243 function void
    244 stream_append(Stream *s, void *data, iz count)
    245 {
    246 	s->errors |= (s->cap - s->widx) < count;
    247 	if (!s->errors) {
    248 		mem_copy(s->data + s->widx, data, (uz)count);
    249 		s->widx += (i32)count;
    250 	}
    251 }
    252 
    253 function void
    254 stream_append_byte(Stream *s, u8 b)
    255 {
    256 	stream_append(s, &b, 1);
    257 }
    258 
    259 function void
    260 stream_pad(Stream *s, u8 b, i32 n)
    261 {
    262 	while (n > 0) stream_append_byte(s, b), n--;
    263 }
    264 
    265 function void
    266 stream_append_s8(Stream *s, s8 str)
    267 {
    268 	stream_append(s, str.data, str.len);
    269 }
    270 
    271 #define stream_append_s8s(s, ...) stream_append_s8s_(s, arg_list(s8, ##__VA_ARGS__))
    272 function void
    273 stream_append_s8s_(Stream *s, s8 *strs, iz count)
    274 {
    275 	for (iz i = 0; i < count; i++)
    276 		stream_append(s, strs[i].data, strs[i].len);
    277 }
    278 
    279 function void
    280 stream_append_u64_width(Stream *s, u64 n, u64 min_width)
    281 {
    282 	u8 tmp[64];
    283 	u8 *end = tmp + sizeof(tmp);
    284 	u8 *beg = end;
    285 	min_width = MIN(sizeof(tmp), min_width);
    286 
    287 	do { *--beg = (u8)('0' + (n % 10)); } while (n /= 10);
    288 	while (end - beg > 0 && (uz)(end - beg) < min_width)
    289 		*--beg = '0';
    290 
    291 	stream_append(s, beg, end - beg);
    292 }
    293 
    294 function void
    295 stream_append_u64(Stream *s, u64 n)
    296 {
    297 	stream_append_u64_width(s, n, 0);
    298 }
    299 
    300 function void
    301 stream_append_hex_u64(Stream *s, u64 n)
    302 {
    303 	if (!s->errors) {
    304 		u8  buf[16];
    305 		u8 *end = buf + sizeof(buf);
    306 		u8 *beg = end;
    307 		while (n) {
    308 			*--beg = (u8)"0123456789abcdef"[n & 0x0F];
    309 			n >>= 4;
    310 		}
    311 		while (end - beg < 2)
    312 			*--beg = '0';
    313 		stream_append(s, beg, end - beg);
    314 	}
    315 }
    316 
    317 function void
    318 stream_append_i64(Stream *s, i64 n)
    319 {
    320 	if (n < 0) {
    321 		stream_append_byte(s, '-');
    322 		n *= -1;
    323 	}
    324 	stream_append_u64(s, (u64)n);
    325 }
    326 
    327 function void
    328 stream_append_f64(Stream *s, f64 f, u64 prec)
    329 {
    330 	if (f < 0) {
    331 		stream_append_byte(s, '-');
    332 		f *= -1;
    333 	}
    334 
    335 	/* NOTE: round last digit */
    336 	f += 0.5f / (f64)prec;
    337 
    338 	if (f >= (f64)(-1UL >> 1)) {
    339 		stream_append_s8(s, s8("inf"));
    340 	} else {
    341 		u64 integral = (u64)f;
    342 		u64 fraction = (u64)((f - (f64)integral) * (f64)prec);
    343 		stream_append_u64(s, integral);
    344 		stream_append_byte(s, '.');
    345 		for (u64 i = prec / 10; i > 1; i /= 10) {
    346 			if (i > fraction)
    347 				stream_append_byte(s, '0');
    348 		}
    349 		stream_append_u64(s, fraction);
    350 	}
    351 }
    352 
    353 function void
    354 stream_append_f64_e(Stream *s, f64 f)
    355 {
    356 	/* TODO: there should be a better way of doing this */
    357 	#if 0
    358 	/* NOTE: we ignore subnormal numbers for now */
    359 	union { f64 f; u64 u; } u = {.f = f};
    360 	i32 exponent = ((u.u >> 52) & 0x7ff) - 1023;
    361 	f32 log_10_of_2 = 0.301f;
    362 	i32 scale       = (exponent * log_10_of_2);
    363 	/* NOTE: normalize f */
    364 	for (i32 i = ABS(scale); i > 0; i--)
    365 		f *= (scale > 0)? 0.1f : 10.0f;
    366 	#else
    367 	i32 scale = 0;
    368 	if (f != 0) {
    369 		while (f > 1) {
    370 			f *= 0.1f;
    371 			scale++;
    372 		}
    373 		while (f < 1) {
    374 			f *= 10.0f;
    375 			scale--;
    376 		}
    377 	}
    378 	#endif
    379 
    380 	u32 prec = 100;
    381 	stream_append_f64(s, f, prec);
    382 	stream_append_byte(s, 'e');
    383 	stream_append_byte(s, scale >= 0? '+' : '-');
    384 	for (u32 i = prec / 10; i > 1; i /= 10)
    385 		stream_append_byte(s, '0');
    386 	stream_append_u64(s, (u64)ABS(scale));
    387 }
    388 
    389 function void
    390 stream_append_v2(Stream *s, v2 v)
    391 {
    392 	stream_append_byte(s, '{');
    393 	stream_append_f64(s, v.x, 100);
    394 	stream_append_s8(s, s8(", "));
    395 	stream_append_f64(s, v.y, 100);
    396 	stream_append_byte(s, '}');
    397 }
    398 
    399 function Stream
    400 arena_stream(Arena a)
    401 {
    402 	Stream result = {0};
    403 	result.data   = a.beg;
    404 	result.cap    = (i32)(a.end - a.beg);
    405 
    406 	/* TODO(rnp): no idea what to do here if we want to maintain the ergonomics */
    407 	asan_unpoison_region(result.data, result.cap);
    408 
    409 	return result;
    410 }
    411 
    412 function s8
    413 arena_stream_commit(Arena *a, Stream *s)
    414 {
    415 	ASSERT(s->data == a->beg);
    416 	s8 result = stream_to_s8(s);
    417 	arena_commit(a, result.len);
    418 	return result;
    419 }
    420 
    421 function s8
    422 arena_stream_commit_zero(Arena *a, Stream *s)
    423 {
    424 	b32 error = s->errors || s->widx == s->cap;
    425 	if (!error)
    426 		s->data[s->widx] = 0;
    427 	s8 result = stream_to_s8(s);
    428 	arena_commit(a, result.len + 1);
    429 	return result;
    430 }
    431 
    432 function s8
    433 arena_stream_commit_and_reset(Arena *arena, Stream *s)
    434 {
    435 	s8 result = arena_stream_commit_zero(arena, s);
    436 	*s = arena_stream(*arena);
    437 	return result;
    438 }
    439 
    440 #if !defined(XXH_IMPLEMENTATION)
    441 # define XXH_INLINE_ALL
    442 # define XXH_IMPLEMENTATION
    443 # define XXH_STATIC_LINKING_ONLY
    444 # include "external/xxhash.h"
    445 #endif
    446 
    447 function u128
    448 u128_hash_from_data(void *data, uz size)
    449 {
    450 	u128 result = {0};
    451 	XXH128_hash_t hash = XXH3_128bits_withSeed(data, size, 4969);
    452 	mem_copy(&result, &hash, sizeof(result));
    453 	return result;
    454 }
    455 
    456 function u64
    457 u64_hash_from_s8(s8 v)
    458 {
    459 	u64 result = XXH3_64bits_withSeed(v.data, (uz)v.len, 4969);
    460 	return result;
    461 }
    462 
    463 function s8
    464 c_str_to_s8(char *cstr)
    465 {
    466 	s8 result = {.data = (u8 *)cstr};
    467 	if (cstr) { while (*cstr) { result.len++; cstr++; } }
    468 	return result;
    469 }
    470 
    471 /* NOTE(rnp): returns < 0 if byte is not found */
    472 function iz
    473 s8_scan_backwards(s8 s, u8 byte)
    474 {
    475 	iz result = s.len;
    476 	while (result && s.data[result - 1] != byte) result--;
    477 	result--;
    478 	return result;
    479 }
    480 
    481 function s8
    482 s8_cut_head(s8 s, iz cut)
    483 {
    484 	s8 result = s;
    485 	if (cut > 0) {
    486 		result.data += cut;
    487 		result.len  -= cut;
    488 	}
    489 	return result;
    490 }
    491 
    492 function s8
    493 s8_alloc(Arena *a, iz len)
    494 {
    495 	s8 result = {.data = push_array(a, u8, len), .len = len};
    496 	return result;
    497 }
    498 
    499 function s8
    500 s16_to_s8(Arena *a, s16 in)
    501 {
    502 	s8 result = s8("");
    503 	if (in.len) {
    504 		iz commit = in.len * 4;
    505 		iz length = 0;
    506 		u8 *data = arena_commit(a, commit + 1);
    507 		u16 *beg = in.data;
    508 		u16 *end = in.data + in.len;
    509 		while (beg < end) {
    510 			UnicodeDecode decode = utf16_decode(beg, end - beg);
    511 			length += utf8_encode(data + length, decode.cp);
    512 			beg    += decode.consumed;
    513 		}
    514 		data[length] = 0;
    515 		result = (s8){.len = length, .data = data};
    516 		arena_pop(a, commit - length);
    517 	}
    518 	return result;
    519 }
    520 
    521 function s16
    522 s8_to_s16(Arena *a, s8 in)
    523 {
    524 	s16 result = {0};
    525 	if (in.len) {
    526 		iz required = 2 * in.len + 1;
    527 		u16 *data   = push_array(a, u16, required);
    528 		iz length   = 0;
    529 		/* TODO(rnp): utf8_decode */
    530 		for (iz i = 0; i < in.len; i++) {
    531 			u32 cp  = in.data[i];
    532 			length += utf16_encode(data + length, cp);
    533 		}
    534 		result = (s16){.len = length, .data = data};
    535 		arena_pop(a, required - length);
    536 	}
    537 	return result;
    538 }
    539 
    540 #define push_s8_from_parts(a, j, ...) push_s8_from_parts_((a), (j), arg_list(s8, __VA_ARGS__))
    541 function s8
    542 push_s8_from_parts_(Arena *arena, s8 joiner, s8 *parts, iz count)
    543 {
    544 	iz length = joiner.len * (count - 1);
    545 	for (iz i = 0; i < count; i++)
    546 		length += parts[i].len;
    547 
    548 	s8 result = {.len = length, .data = arena_commit(arena, length + 1)};
    549 
    550 	iz offset = 0;
    551 	for (iz i = 0; i < count; i++) {
    552 		if (i != 0) {
    553 			mem_copy(result.data + offset, joiner.data, (uz)joiner.len);
    554 			offset += joiner.len;
    555 		}
    556 		mem_copy(result.data + offset, parts[i].data, (uz)parts[i].len);
    557 		offset += parts[i].len;
    558 	}
    559 	result.data[result.len] = 0;
    560 
    561 	return result;
    562 }
    563 
    564 function s8
    565 push_s8(Arena *a, s8 str)
    566 {
    567 	s8 result   = s8_alloc(a, str.len + 1);
    568 	result.len -= 1;
    569 	mem_copy(result.data, str.data, (uz)result.len);
    570 	return result;
    571 }
    572 
    573 function force_inline u32
    574 round_down_power_of_2(u32 a)
    575 {
    576 	u32 result = 0x80000000UL >> clz_u32(a);
    577 	return result;
    578 }
    579 
    580 function force_inline u32
    581 round_up_power_of_2(u32 a)
    582 {
    583 	u32 result = 0x80000000UL >> (clz_u32(a - 1) - 1);
    584 	return result;
    585 }
    586 
    587 function force_inline iz
    588 round_up_to(iz value, iz multiple)
    589 {
    590 	iz result = value;
    591 	if (value % multiple != 0)
    592 		result += multiple - value % multiple;
    593 	return result;
    594 }
    595 
    596 function void
    597 split_rect_horizontal(Rect rect, f32 fraction, Rect *left, Rect *right)
    598 {
    599 	if (left) {
    600 		left->pos    = rect.pos;
    601 		left->size.h = rect.size.h;
    602 		left->size.w = rect.size.w * fraction;
    603 	}
    604 	if (right) {
    605 		right->pos    = rect.pos;
    606 		right->pos.x += rect.size.w * fraction;
    607 		right->size.h = rect.size.h;
    608 		right->size.w = rect.size.w * (1.0f - fraction);
    609 	}
    610 }
    611 
    612 function void
    613 split_rect_vertical(Rect rect, f32 fraction, Rect *top, Rect *bot)
    614 {
    615 	if (top) {
    616 		top->pos    = rect.pos;
    617 		top->size.w = rect.size.w;
    618 		top->size.h = rect.size.h * fraction;
    619 	}
    620 	if (bot) {
    621 		bot->pos    = rect.pos;
    622 		bot->pos.y += rect.size.h * fraction;
    623 		bot->size.w = rect.size.w;
    624 		bot->size.h = rect.size.h * (1.0f - fraction);
    625 	}
    626 }
    627 
    628 function void
    629 cut_rect_horizontal(Rect rect, f32 at, Rect *left, Rect *right)
    630 {
    631 	at = MIN(at, rect.size.w);
    632 	if (left) {
    633 		*left = rect;
    634 		left->size.w = at;
    635 	}
    636 	if (right) {
    637 		*right = rect;
    638 		right->pos.x  += at;
    639 		right->size.w -= at;
    640 	}
    641 }
    642 
    643 function void
    644 cut_rect_vertical(Rect rect, f32 at, Rect *top, Rect *bot)
    645 {
    646 	at = MIN(at, rect.size.h);
    647 	if (top) {
    648 		*top = rect;
    649 		top->size.h = at;
    650 	}
    651 	if (bot) {
    652 		*bot = rect;
    653 		bot->pos.y  += at;
    654 		bot->size.h -= at;
    655 	}
    656 }
    657 
    658 function f64
    659 parse_f64(s8 s)
    660 {
    661 	f64 integral = 0, fractional = 0, sign = 1;
    662 
    663 	if (s.len > 0 && *s.data == '-') {
    664 		sign = -1;
    665 		s.data++;
    666 		s.len--;
    667 	}
    668 
    669 	while (s.len > 0 && *s.data != '.') {
    670 		integral *= 10;
    671 		integral += *s.data - '0';
    672 		s.data++;
    673 		s.len--;
    674 	}
    675 
    676 	if (*s.data == '.') { s.data++; s.len--; }
    677 
    678 	while (s.len > 0) {
    679 		ASSERT(s.data[s.len - 1] != '.');
    680 		fractional /= 10;
    681 		fractional += (f64)(s.data[--s.len] - '0') / 10.0;
    682 	}
    683 	f64 result = sign * (integral + fractional);
    684 	return result;
    685 }
    686 
    687 function FileWatchDirectory *
    688 lookup_file_watch_directory(FileWatchContext *ctx, u64 hash)
    689 {
    690 	FileWatchDirectory *result = 0;
    691 	for (u32 i = 0; i < ctx->count; i++) {
    692 		FileWatchDirectory *test = ctx->data + i;
    693 		if (test->hash == hash) {
    694 			result = test;
    695 			break;
    696 		}
    697 	}
    698 	return result;
    699 }