ogl_beamforming

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

util.c (14486B)


      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 u8 *
     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 - 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 /* NOTE(rnp): FNV-1a hash */
    441 function u64
    442 s8_hash(s8 v)
    443 {
    444 	u64 h = 0x3243f6a8885a308d; /* digits of pi */
    445 	for (; v.len; v.len--) {
    446 		h ^= v.data[v.len - 1] & 0xFF;
    447 		h *= 1111111111111111111; /* random prime */
    448 	}
    449 	return h;
    450 }
    451 
    452 function s8
    453 c_str_to_s8(char *cstr)
    454 {
    455 	s8 result = {.data = (u8 *)cstr};
    456 	if (cstr) { while (*cstr) { result.len++; cstr++; } }
    457 	return result;
    458 }
    459 
    460 /* NOTE(rnp): returns < 0 if byte is not found */
    461 function iz
    462 s8_scan_backwards(s8 s, u8 byte)
    463 {
    464 	iz result = s.len;
    465 	while (result && s.data[result - 1] != byte) result--;
    466 	result--;
    467 	return result;
    468 }
    469 
    470 function s8
    471 s8_cut_head(s8 s, iz cut)
    472 {
    473 	s8 result = s;
    474 	if (cut > 0) {
    475 		result.data += cut;
    476 		result.len  -= cut;
    477 	}
    478 	return result;
    479 }
    480 
    481 function s8
    482 s8_alloc(Arena *a, iz len)
    483 {
    484 	s8 result = {.data = push_array(a, u8, len), .len = len};
    485 	return result;
    486 }
    487 
    488 function s8
    489 s16_to_s8(Arena *a, s16 in)
    490 {
    491 	s8 result = s8("");
    492 	if (in.len) {
    493 		iz commit = in.len * 4;
    494 		iz length = 0;
    495 		u8 *data = arena_commit(a, commit + 1);
    496 		u16 *beg = in.data;
    497 		u16 *end = in.data + in.len;
    498 		while (beg < end) {
    499 			UnicodeDecode decode = utf16_decode(beg, end - beg);
    500 			length += utf8_encode(data + length, decode.cp);
    501 			beg    += decode.consumed;
    502 		}
    503 		data[length] = 0;
    504 		result = (s8){.len = length, .data = data};
    505 		arena_pop(a, commit - length);
    506 	}
    507 	return result;
    508 }
    509 
    510 function s16
    511 s8_to_s16(Arena *a, s8 in)
    512 {
    513 	s16 result = {0};
    514 	if (in.len) {
    515 		iz required = 2 * in.len + 1;
    516 		u16 *data   = push_array(a, u16, required);
    517 		iz length   = 0;
    518 		/* TODO(rnp): utf8_decode */
    519 		for (iz i = 0; i < in.len; i++) {
    520 			u32 cp  = in.data[i];
    521 			length += utf16_encode(data + length, cp);
    522 		}
    523 		result = (s16){.len = length, .data = data};
    524 		arena_pop(a, required - length);
    525 	}
    526 	return result;
    527 }
    528 
    529 #define push_s8_from_parts(a, j, ...) push_s8_from_parts_((a), (j), arg_list(s8, __VA_ARGS__))
    530 function s8
    531 push_s8_from_parts_(Arena *arena, s8 joiner, s8 *parts, iz count)
    532 {
    533 	iz length = joiner.len * (count - 1);
    534 	for (iz i = 0; i < count; i++)
    535 		length += parts[i].len;
    536 
    537 	s8 result = {.len = length, .data = arena_commit(arena, length + 1)};
    538 
    539 	iz offset = 0;
    540 	for (iz i = 0; i < count; i++) {
    541 		if (i != 0) {
    542 			mem_copy(result.data + offset, joiner.data, (uz)joiner.len);
    543 			offset += joiner.len;
    544 		}
    545 		mem_copy(result.data + offset, parts[i].data, (uz)parts[i].len);
    546 		offset += parts[i].len;
    547 	}
    548 	result.data[result.len] = 0;
    549 
    550 	return result;
    551 }
    552 
    553 function s8
    554 push_s8(Arena *a, s8 str)
    555 {
    556 	s8 result   = s8_alloc(a, str.len + 1);
    557 	result.len -= 1;
    558 	mem_copy(result.data, str.data, (uz)result.len);
    559 	return result;
    560 }
    561 
    562 function force_inline u32
    563 round_down_power_of_2(u32 a)
    564 {
    565 	u32 result = 0x80000000UL >> clz_u32(a);
    566 	return result;
    567 }
    568 
    569 function force_inline u32
    570 round_up_power_of_2(u32 a)
    571 {
    572 	u32 result = 0x80000000UL >> (clz_u32(a - 1) - 1);
    573 	return result;
    574 }
    575 
    576 function force_inline iz
    577 round_up_to(iz value, iz multiple)
    578 {
    579 	iz result = value;
    580 	if (value % multiple != 0)
    581 		result += multiple - value % multiple;
    582 	return result;
    583 }
    584 
    585 function void
    586 split_rect_horizontal(Rect rect, f32 fraction, Rect *left, Rect *right)
    587 {
    588 	if (left) {
    589 		left->pos    = rect.pos;
    590 		left->size.h = rect.size.h;
    591 		left->size.w = rect.size.w * fraction;
    592 	}
    593 	if (right) {
    594 		right->pos    = rect.pos;
    595 		right->pos.x += rect.size.w * fraction;
    596 		right->size.h = rect.size.h;
    597 		right->size.w = rect.size.w * (1.0f - fraction);
    598 	}
    599 }
    600 
    601 function void
    602 split_rect_vertical(Rect rect, f32 fraction, Rect *top, Rect *bot)
    603 {
    604 	if (top) {
    605 		top->pos    = rect.pos;
    606 		top->size.w = rect.size.w;
    607 		top->size.h = rect.size.h * fraction;
    608 	}
    609 	if (bot) {
    610 		bot->pos    = rect.pos;
    611 		bot->pos.y += rect.size.h * fraction;
    612 		bot->size.w = rect.size.w;
    613 		bot->size.h = rect.size.h * (1.0f - fraction);
    614 	}
    615 }
    616 
    617 function void
    618 cut_rect_horizontal(Rect rect, f32 at, Rect *left, Rect *right)
    619 {
    620 	at = MIN(at, rect.size.w);
    621 	if (left) {
    622 		*left = rect;
    623 		left->size.w = at;
    624 	}
    625 	if (right) {
    626 		*right = rect;
    627 		right->pos.x  += at;
    628 		right->size.w -= at;
    629 	}
    630 }
    631 
    632 function void
    633 cut_rect_vertical(Rect rect, f32 at, Rect *top, Rect *bot)
    634 {
    635 	at = MIN(at, rect.size.h);
    636 	if (top) {
    637 		*top = rect;
    638 		top->size.h = at;
    639 	}
    640 	if (bot) {
    641 		*bot = rect;
    642 		bot->pos.y  += at;
    643 		bot->size.h -= at;
    644 	}
    645 }
    646 
    647 function f64
    648 parse_f64(s8 s)
    649 {
    650 	f64 integral = 0, fractional = 0, sign = 1;
    651 
    652 	if (s.len > 0 && *s.data == '-') {
    653 		sign = -1;
    654 		s.data++;
    655 		s.len--;
    656 	}
    657 
    658 	while (s.len > 0 && *s.data != '.') {
    659 		integral *= 10;
    660 		integral += *s.data - '0';
    661 		s.data++;
    662 		s.len--;
    663 	}
    664 
    665 	if (*s.data == '.') { s.data++; s.len--; }
    666 
    667 	while (s.len > 0) {
    668 		ASSERT(s.data[s.len - 1] != '.');
    669 		fractional /= 10;
    670 		fractional += (f64)(s.data[--s.len] - '0') / 10.0;
    671 	}
    672 	f64 result = sign * (integral + fractional);
    673 	return result;
    674 }
    675 
    676 function FileWatchDirectory *
    677 lookup_file_watch_directory(FileWatchContext *ctx, u64 hash)
    678 {
    679 	FileWatchDirectory *result = 0;
    680 	for (u32 i = 0; i < ctx->count; i++) {
    681 		FileWatchDirectory *test = ctx->data + i;
    682 		if (test->hash == hash) {
    683 			result = test;
    684 			break;
    685 		}
    686 	}
    687 	return result;
    688 }