test_common.c (12757B)
1 /* See LICENSE for license details. */ 2 #define BASE_EXPORT function 3 #define BASE_IMPORT function 4 #define BEAMFORMER_LIB_EXPORT function 5 #include "base_platform.h" 6 #include "ogl_beamformer_lib.c" 7 8 #include <signal.h> 9 #include <stdarg.h> 10 #include <stdio.h> 11 #include <stdlib.h> 12 #include <zstd.h> 13 14 #include "external/zemp_bp.h" 15 16 typedef struct { 17 ZBP_DataKind kind; 18 ZBP_DataCompressionKind compression_kind; 19 str8 bytes; 20 } ZBP_Data; 21 22 #define shift_n(v, c, n) v += n, c -= n 23 #define shift(v, c) shift_n(v, c, 1) 24 25 #define die(...) die_((char *)__func__, __VA_ARGS__) 26 function no_return void 27 die_(char *function_name, char *format, ...) 28 { 29 if (function_name) 30 fprintf(stderr, "%s: ", function_name); 31 32 va_list ap; 33 34 va_start(ap, format); 35 vfprintf(stderr, format, ap); 36 va_end(ap); 37 38 os_exit(1); 39 } 40 41 function b32 42 beamformer_simple_parameters_from_zbp_file(Arena *arena, BeamformerSimpleParameters *bp, char *path, ZBP_Data *raw_data, u32 *data_frame_count) 43 { 44 str8 raw = os_read_entire_file(arena, path); 45 if (raw.length < (i64)sizeof(ZBP_BaseHeader) || ((ZBP_BaseHeader *)raw.data)->magic != ZBP_HeaderMagic) 46 return 0; 47 48 switch (((ZBP_BaseHeader *)raw.data)->major) { 49 50 case 1:{ 51 ZBP_HeaderV1 *header = (ZBP_HeaderV1 *)raw.data; 52 53 bp->sample_count = header->sample_count; 54 bp->channel_count = header->channel_count; 55 bp->acquisition_count = header->receive_event_count; 56 57 bp->sampling_mode = BeamformerSamplingMode_4X; 58 bp->acquisition_kind = header->beamform_mode; 59 bp->decode_mode = header->decode_mode; 60 bp->sampling_frequency = header->sampling_frequency; 61 bp->demodulation_frequency = header->sampling_frequency / 4; 62 bp->speed_of_sound = header->speed_of_sound; 63 bp->time_offset = header->time_offset; 64 65 memory_copy(bp->channel_mapping, header->channel_mapping, sizeof(*bp->channel_mapping) * bp->channel_count); 66 memory_copy(bp->xdc_transform.E, header->transducer_transform_matrix, sizeof(bp->xdc_transform)); 67 memory_copy(bp->xdc_element_pitch.E, header->transducer_element_pitch, sizeof(bp->xdc_element_pitch)); 68 // NOTE(rnp): ignores emission count and ensemble count 69 memory_copy(bp->raw_data_dimensions.E, header->raw_data_dimension, sizeof(bp->raw_data_dimensions)); 70 if (data_frame_count) *data_frame_count = header->raw_data_dimension[2]; 71 //if (data_frame_count) *data_frame_count = header->raw_data_dimension[3]; 72 73 bp->data_kind = (BeamformerDataKind)ZBP_DataKind_Int16; 74 raw_data->kind = ZBP_DataKind_Int16; 75 raw_data->compression_kind = ZBP_DataCompressionKind_ZSTD; 76 77 read_only u8 transmit_mode_to_orientation[] = { 78 [0] = (ZBP_RCAOrientation_Rows << 4) | ZBP_RCAOrientation_Rows, 79 [1] = (ZBP_RCAOrientation_Rows << 4) | ZBP_RCAOrientation_Columns, 80 [2] = (ZBP_RCAOrientation_Columns << 4) | ZBP_RCAOrientation_Rows, 81 [3] = (ZBP_RCAOrientation_Columns << 4) | ZBP_RCAOrientation_Columns, 82 }; 83 if (header->transmit_mode >= countof(transmit_mode_to_orientation)) 84 return 0; 85 86 bp->transmit_receive_orientation = transmit_mode_to_orientation[header->transmit_mode]; 87 88 ZBP_AcquisitionKind acquisition_kind = header->beamform_mode; 89 if (acquisition_kind == ZBP_AcquisitionKind_FORCES || 90 acquisition_kind == ZBP_AcquisitionKind_HERCULES || 91 acquisition_kind == ZBP_AcquisitionKind_UFORCES || 92 acquisition_kind == ZBP_AcquisitionKind_UHERCULES) 93 { 94 bp->single_focus = 1; 95 bp->single_orientation = 1; 96 bp->focal_vector.E[0] = header->steering_angles[0]; 97 bp->focal_vector.E[1] = header->focal_depths[0]; 98 } 99 100 if (acquisition_kind == ZBP_AcquisitionKind_UFORCES || 101 acquisition_kind == ZBP_AcquisitionKind_UHERCULES) 102 { 103 memory_copy(bp->sparse_elements, header->sparse_elements, sizeof(*bp->sparse_elements) * bp->acquisition_count); 104 } 105 106 if (acquisition_kind == ZBP_AcquisitionKind_RCA_TPW || 107 acquisition_kind == ZBP_AcquisitionKind_RCA_VLS) 108 { 109 memory_copy(bp->focal_depths, header->focal_depths, sizeof(*bp->focal_depths) * bp->acquisition_count); 110 memory_copy(bp->steering_angles, header->steering_angles, sizeof(*bp->steering_angles) * bp->acquisition_count); 111 for EachIndex(bp->acquisition_count, it) 112 bp->transmit_receive_orientations[it] = bp->transmit_receive_orientation; 113 } 114 115 bp->emission_parameters.kind = BeamformerEmissionKind_Sine; 116 bp->emission_parameters.sine.cycles = 2; 117 bp->emission_parameters.sine.frequency = bp->demodulation_frequency; 118 }break; 119 120 case 2:{ 121 ZBP_HeaderV2 *header = (ZBP_HeaderV2 *)raw.data; 122 123 bp->sample_count = header->sample_count; 124 bp->channel_count = header->channel_count; 125 bp->acquisition_count = header->receive_event_count; 126 127 read_only BeamformerSamplingMode zbp_sampling_mode_to_beamformer[] = { 128 [ZBP_SamplingMode_Standard] = BeamformerSamplingMode_4X, 129 [ZBP_SamplingMode_Bandpass] = BeamformerSamplingMode_2X, 130 }; 131 bp->sampling_mode = zbp_sampling_mode_to_beamformer[header->sampling_mode]; 132 133 bp->acquisition_kind = (BeamformerAcquisitionKind)header->acquisition_mode; 134 bp->decode_mode = (BeamformerDecodeMode)header->decode_mode; 135 bp->sampling_frequency = header->sampling_frequency; 136 bp->demodulation_frequency = header->demodulation_frequency; 137 bp->speed_of_sound = header->speed_of_sound; 138 bp->time_offset = header->time_offset; 139 140 bp->contrast_mode = (BeamformerContrastMode)header->contrast_mode; 141 142 if (header->channel_mapping_offset != -1) { 143 memory_copy(bp->channel_mapping, raw.data + header->channel_mapping_offset, 144 sizeof(*bp->channel_mapping) * bp->channel_count); 145 } else { 146 for EachIndex(bp->channel_count, it) 147 bp->channel_mapping[it] = it; 148 } 149 150 memory_copy(bp->xdc_transform.E, header->transducer_transform_matrix, sizeof(bp->xdc_transform)); 151 memory_copy(bp->xdc_element_pitch.E, header->transducer_element_pitch, sizeof(bp->xdc_element_pitch)); 152 // NOTE(rnp): ignores group count and ensemble count 153 memory_copy(bp->raw_data_dimensions.E, header->raw_data_dimension, sizeof(bp->raw_data_dimensions)); 154 if (data_frame_count) *data_frame_count = header->raw_data_dimension[2]; 155 //if (data_frame_count) *data_frame_count = header->raw_data_dimension[3]; 156 157 bp->data_kind = (BeamformerDataKind)header->raw_data_kind; 158 raw_data->kind = header->raw_data_kind; 159 raw_data->compression_kind = header->raw_data_compression_kind; 160 161 if (header->raw_data_offset != -1) { 162 raw_data->bytes.data = raw.data + header->raw_data_offset; 163 if (raw_data->compression_kind == ZBP_DataCompressionKind_ZSTD) { 164 // NOTE(rnp): limitation in the header format 165 raw_data->bytes.length = raw.length - header->raw_data_offset; 166 } else { 167 raw_data->bytes.length = header->raw_data_dimension[0] * header->raw_data_dimension[1] * 168 header->raw_data_dimension[2] * header->raw_data_dimension[3]; 169 raw_data->bytes.length *= beamformer_data_kind_byte_size[header->raw_data_kind]; 170 } 171 } 172 173 // NOTE(rnp): only look at the first emission descriptor, other cases aren't currently relevant 174 { 175 ZBP_EmissionDescriptor *ed = (ZBP_EmissionDescriptor *)(raw.data + header->emission_descriptors_offset); 176 switch (ed->emission_kind) { 177 178 case ZBP_EmissionKind_Sine:{ 179 ZBP_EmissionSineParameters *ep = (ZBP_EmissionSineParameters *)(raw.data + ed->parameters_offset); 180 bp->emission_parameters.kind = BeamformerEmissionKind_Sine; 181 bp->emission_parameters.sine.cycles = ep->cycles; 182 bp->emission_parameters.sine.frequency = ep->frequency; 183 }break; 184 185 case ZBP_EmissionKind_Chirp:{ 186 ZBP_EmissionChirpParameters *ep = (ZBP_EmissionChirpParameters *)(raw.data + ed->parameters_offset); 187 bp->emission_parameters.kind = BeamformerEmissionKind_Chirp; 188 bp->emission_parameters.chirp.duration = ep->duration; 189 bp->emission_parameters.chirp.min_frequency = ep->min_frequency; 190 bp->emission_parameters.chirp.max_frequency = ep->max_frequency; 191 }break; 192 193 InvalidDefaultCase; 194 static_assert(ZBP_EmissionKind_Count == (ZBP_EmissionKind_Chirp + 1), ""); 195 } 196 } 197 198 switch (header->acquisition_mode) { 199 case ZBP_AcquisitionKind_FORCES:{}break; 200 201 case ZBP_AcquisitionKind_HERCULES:{ 202 ZBP_HERCULESParameters *p = (ZBP_HERCULESParameters *)(raw.data + header->acquisition_parameters_offset); 203 bp->transmit_receive_orientation = p->transmit_focus.transmit_receive_orientation; 204 bp->focal_vector.E[0] = p->transmit_focus.steering_angle; 205 bp->focal_vector.E[1] = p->transmit_focus.focal_depth; 206 207 bp->single_focus = 1; 208 bp->single_orientation = 1; 209 }break; 210 211 case ZBP_AcquisitionKind_UFORCES:{ 212 ZBP_uFORCESParameters *p = (ZBP_uFORCESParameters *)(raw.data + header->acquisition_parameters_offset); 213 memory_copy(bp->sparse_elements, raw.data + p->sparse_elements_offset, 214 sizeof(*bp->sparse_elements) * bp->acquisition_count); 215 }break; 216 217 case ZBP_AcquisitionKind_UHERCULES:{ 218 ZBP_uHERCULESParameters *p = (ZBP_uHERCULESParameters *)(raw.data + header->acquisition_parameters_offset); 219 bp->transmit_receive_orientation = p->transmit_focus.transmit_receive_orientation; 220 bp->focal_vector.E[0] = p->transmit_focus.steering_angle; 221 bp->focal_vector.E[1] = p->transmit_focus.focal_depth; 222 223 bp->single_focus = 1; 224 bp->single_orientation = 1; 225 226 memory_copy(bp->sparse_elements, raw.data + p->sparse_elements_offset, 227 sizeof(*bp->sparse_elements) * bp->acquisition_count); 228 }break; 229 230 case ZBP_AcquisitionKind_RCA_TPW:{ 231 ZBP_TPWParameters *p = (ZBP_TPWParameters *)(raw.data + header->acquisition_parameters_offset); 232 233 memory_copy(bp->transmit_receive_orientations, raw.data + p->transmit_receive_orientations_offset, 234 sizeof(*bp->transmit_receive_orientations) * bp->acquisition_count); 235 memory_copy(bp->steering_angles, raw.data + p->tilting_angles_offset, 236 sizeof(*bp->steering_angles) * bp->acquisition_count); 237 238 for EachIndex(bp->acquisition_count, it) 239 bp->focal_depths[it] = inf32(); 240 }break; 241 242 case ZBP_AcquisitionKind_RCA_VLS:{ 243 ZBP_VLSParameters *p = (ZBP_VLSParameters *)(raw.data + header->acquisition_parameters_offset); 244 245 memory_copy(bp->transmit_receive_orientations, raw.data + p->transmit_receive_orientations_offset, 246 sizeof(*bp->transmit_receive_orientations) * bp->acquisition_count); 247 248 f32 *focal_depths = (f32 *)(raw.data + p->focal_depths_offset); 249 f32 *origin_offsets = (f32 *)(raw.data + p->origin_offsets_offset); 250 251 for EachIndex(bp->acquisition_count, it) { 252 f32 sign = Sign(focal_depths[it]); 253 f32 depth = focal_depths[it]; 254 f32 origin = origin_offsets[it]; 255 bp->steering_angles[it] = atan2_f32(origin, -depth) * 180.0f / PI; 256 bp->focal_depths[it] = sign * sqrt_f32(depth * depth + origin * origin); 257 } 258 }break; 259 260 InvalidDefaultCase; 261 } 262 263 }break; 264 265 default:{return 0;}break; 266 } 267 268 return 1; 269 } 270 271 function void 272 stream_ensure_termination(Stream *s, u8 byte) 273 { 274 b32 found = 0; 275 if (!s->errors && s->widx > 0) 276 found = s->data[s->widx - 1] == byte; 277 if (!found) { 278 s->errors |= s->cap - 1 < s->widx; 279 if (!s->errors) 280 s->data[s->widx++] = byte; 281 } 282 } 283 284 function str8 285 zstd_decompress_data(Arena *arena, str8 raw) 286 { 287 u64 requested_size = ZSTD_getFrameContentSize(raw.data, (u64)raw.length); 288 void *out = push_array_no_zero(arena, u8, requested_size); 289 str8 result = {.data = out}; 290 u64 decompressed = ZSTD_decompress(out, requested_size, raw.data, (u64)raw.length); 291 if (decompressed == requested_size) result.length = requested_size; 292 return result; 293 } 294 295 function void * 296 zbp_data_pointer(Arena *arena, ZBP_Data *raw, Stream path, u32 frame_number) 297 { 298 void *result = 0; 299 if (raw->bytes.length == 0) { 300 // NOTE(rnp): strip ".bp" 301 stream_reset(&path, path.widx - 3); 302 303 b32 compressed = raw->compression_kind == ZBP_DataCompressionKind_ZSTD; 304 stream_append_byte(&path, '_'); 305 stream_append_u64_width(&path, frame_number, 2); 306 stream_append_str8(&path, compressed ? str8(".zst") : str8(".bin")); 307 stream_ensure_termination(&path, 0); 308 str8 compressed_data = os_read_entire_file(arena, (char *)path.data); 309 310 str8 bytes = compressed_data; 311 if (compressed) { 312 bytes = zstd_decompress_data(arena, compressed_data); 313 if (!bytes.length) 314 die("failed to decompress data: %s\n", path.data); 315 } 316 result = bytes.data; 317 } else { 318 if (raw->compression_kind == ZBP_DataCompressionKind_ZSTD) { 319 str8 bytes = zstd_decompress_data(arena, raw->bytes); 320 result = bytes.data; 321 } else { 322 result = raw->bytes.data; 323 } 324 } 325 return result; 326 }