qualia_core_db/medical/reconstruction/
ultrasound_das.rs1use super::spectral_fft::{analytic_hilbert_envelope_1d, apodization_weight, WindowKind};
5
6#[derive(Debug, Clone, Copy, PartialEq, Eq)]
7pub enum DasError {
8 DimensionMismatch,
9 InvalidConfig(&'static str),
10 NonFiniteInput,
11 OutputTooSmall,
12 Overflow,
13}
14
15impl std::fmt::Display for DasError {
16 fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
17 write!(f, "Ultrasound DAS beamforming error: {self:?}")
18 }
19}
20impl std::error::Error for DasError {}
21
22#[derive(Clone, Copy, Debug)]
23pub enum TxFocusMode {
24 PlaneWave { angle_rad: f64 },
25 VirtualPoint { source_xz_mm: [f64; 2], t0_s: f64 },
26}
27
28#[derive(Clone, Copy, Debug)]
29pub struct UltrasoundArray<'a> {
30 pub elem_x_mm: &'a [f64],
31 pub elem_z_mm: &'a [f64],
32 pub tx_mode: TxFocusMode,
33}
34
35#[derive(Clone, Copy, Debug)]
36pub struct DasConfig<'a> {
37 pub sound_speed_mps: f64,
38 pub sample_rate_hz: f64,
39 pub f_number: f64,
40 pub min_aperture_half_mm: f64,
41 pub apodization: WindowKind,
42 pub dynamic_range_db: f64,
43 pub pixel_x_mm: &'a [f64],
44 pub pixel_z_mm: &'a [f64],
45}
46
47#[derive(Clone, Debug)]
48pub struct DasBeamformResult {
49 pub source_blake3: [u8; 32],
50 pub derivation_blake3: [u8; 32],
51 pub width: usize,
52 pub height: usize,
53 pub rf_beamformed: Vec<f64>,
54 pub envelope: Vec<f64>,
55 pub peak_envelope: f64,
56}
57
58fn sample_rf_cubic(channel: &[f64], frac_sample: f64) -> f64 {
60 if frac_sample < 0.0 || frac_sample >= (channel.len() - 1) as f64 {
61 return 0.0;
62 }
63 let i = frac_sample.floor() as isize;
64 let t = frac_sample - i as f64;
65 let get = |idx: isize| -> f64 {
66 if idx >= 0 && (idx as usize) < channel.len() {
67 channel[idx as usize]
68 } else {
69 0.0
70 }
71 };
72 let p0 = get(i - 1);
73 let p1 = get(i);
74 let p2 = get(i + 1);
75 let p3 = get(i + 2);
76 let t2 = t * t;
77 let t3 = t2 * t;
78 0.5 * ((2.0 * p1)
79 + (-p0 + p2) * t
80 + (2.0 * p0 - 5.0 * p1 + 4.0 * p2 - p3) * t2
81 + (-p0 + 3.0 * p1 - 3.0 * p2 + p3) * t3)
82}
83
84pub fn beamform_ultrasound_das_into(
86 rf_channels: &[f64], n_samples: usize,
88 array: &UltrasoundArray<'_>,
89 cfg: &DasConfig<'_>,
90 source_blake3: [u8; 32],
91 bmode_db_out: &mut [f64],
92) -> Result<DasBeamformResult, DasError> {
93 let n_elem = array.elem_x_mm.len();
94 let nx = cfg.pixel_x_mm.len();
95 let nz = cfg.pixel_z_mm.len();
96 if n_elem == 0 || n_samples < 4 || nx == 0 || nz == 0 || array.elem_z_mm.len() != n_elem {
97 return Err(DasError::DimensionMismatch);
98 }
99 let expect_rf = n_elem.checked_mul(n_samples).ok_or(DasError::Overflow)?;
100 let expect_out = nz.checked_mul(nx).ok_or(DasError::Overflow)?;
101 if rf_channels.len() != expect_rf {
102 return Err(DasError::DimensionMismatch);
103 }
104 if bmode_db_out.len() < expect_out {
105 return Err(DasError::OutputTooSmall);
106 }
107 if cfg.sound_speed_mps <= 0.0
108 || cfg.sample_rate_hz <= 0.0
109 || cfg.f_number <= 0.0
110 || cfg.dynamic_range_db <= 0.0
111 || cfg.min_aperture_half_mm <= 0.0
112 || !cfg.sound_speed_mps.is_finite()
113 || !cfg.sample_rate_hz.is_finite()
114 || !cfg.f_number.is_finite()
115 || !cfg.dynamic_range_db.is_finite()
116 {
117 return Err(DasError::InvalidConfig("acoustic speed, sample rate, f-number, and dynamic range must be positive"));
118 }
119 if rf_channels.iter().any(|v| !v.is_finite())
120 || array.elem_x_mm.iter().any(|v| !v.is_finite())
121 || array.elem_z_mm.iter().any(|v| !v.is_finite())
122 || cfg.pixel_x_mm.iter().any(|v| !v.is_finite())
123 || cfg.pixel_z_mm.iter().any(|v| !v.is_finite())
124 {
125 return Err(DasError::NonFiniteInput);
126 }
127
128 let c_mm_per_s = cfg.sound_speed_mps * 1000.0;
129 let x_ref_min = array.elem_x_mm.iter().copied().fold(f64::INFINITY, f64::min);
130 let x_ref_max = array.elem_x_mm.iter().copied().fold(f64::NEG_INFINITY, f64::max);
131
132 let mut rf_beamformed = vec![0.0f64; expect_out];
133 for iz in 0..nz {
134 let pz = cfg.pixel_z_mm[iz];
135 for ix in 0..nx {
136 let px = cfg.pixel_x_mm[ix];
137 let t_tx = match array.tx_mode {
138 TxFocusMode::PlaneWave { angle_rad } => {
139 let x_orig = if angle_rad >= 0.0 { x_ref_min } else { x_ref_max };
140 ((px - x_orig) * angle_rad.sin() + pz * angle_rad.cos()) / c_mm_per_s
141 }
142 TxFocusMode::VirtualPoint { source_xz_mm, t0_s } => {
143 let sign = if pz >= source_xz_mm[1] { 1.0 } else { -1.0 };
144 let dist = (px - source_xz_mm[0]).hypot(pz - source_xz_mm[1]);
145 t0_s + sign * dist / c_mm_per_s
146 }
147 };
148
149 let half_ap = ((pz.abs()) / (2.0 * cfg.f_number)).max(cfg.min_aperture_half_mm);
150 let mut sum = 0.0;
151 let mut w_sum = 0.0;
152 for e in 0..n_elem {
153 let ex = array.elem_x_mm[e];
154 let ez = array.elem_z_mm[e];
155 let dx_el = (ex - px).abs();
156 let u = dx_el / half_ap;
157 let w = apodization_weight(cfg.apodization, u);
158 if w <= 0.0 {
159 continue;
160 }
161 let t_rx = (px - ex).hypot(pz - ez) / c_mm_per_s;
162 let sample_idx = (t_tx + t_rx) * cfg.sample_rate_hz;
163 let ch = &rf_channels[e * n_samples..(e + 1) * n_samples];
164 sum += w * sample_rf_cubic(ch, sample_idx);
165 w_sum += w;
166 }
167 rf_beamformed[iz * nx + ix] = if w_sum > 1e-12 { sum / w_sum } else { 0.0 };
168 }
169 }
170
171 let mut envelope = vec![0.0f64; expect_out];
173 let mut col_buf = vec![0.0f64; nz];
174 for ix in 0..nx {
175 for iz in 0..nz {
176 col_buf[iz] = rf_beamformed[iz * nx + ix];
177 }
178 let env_col = analytic_hilbert_envelope_1d(&col_buf).map_err(|_| DasError::NonFiniteInput)?;
179 for iz in 0..nz {
180 envelope[iz * nx + ix] = env_col[iz];
181 }
182 }
183
184 let peak_envelope = envelope.iter().copied().fold(0.0f64, f64::max);
185 let ref_peak = peak_envelope.max(1e-15);
186 let mut staged_db = vec![0.0f64; expect_out];
187 for (i, &env) in envelope.iter().enumerate() {
188 let ratio = (env / ref_peak).max(1e-12);
189 staged_db[i] = (20.0 * ratio.log10()).clamp(-cfg.dynamic_range_db, 0.0);
190 }
191
192 let mut hasher = blake3::Hasher::new();
193 hasher.update(&source_blake3);
194 for v in &staged_db {
195 hasher.update(&v.to_le_bytes());
196 }
197 bmode_db_out[..expect_out].copy_from_slice(&staged_db);
198
199 Ok(DasBeamformResult {
200 source_blake3,
201 derivation_blake3: *hasher.finalize().as_bytes(),
202 width: nx,
203 height: nz,
204 rf_beamformed,
205 envelope,
206 peak_envelope,
207 })
208}