1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
use autd_geometry::consts::NUM_TRANS_IN_UNIT;
use autd_geometry::Geometry;
use autd_geometry::Vector3;
use crate::consts::ULTRASOUND_WAVELENGTH;
use crate::convert_to_pwm_params;
use crate::Gain;
pub struct FocalPointGain {
point: Vector3,
amp: u8,
data: Option<Vec<u8>>,
}
impl FocalPointGain {
pub fn create(point: Vector3) -> Box<FocalPointGain> {
FocalPointGain::create_with_amp(point, 0xff)
}
pub fn create_with_amp(point: Vector3, amp: u8) -> Box<FocalPointGain> {
Box::new(FocalPointGain {
point,
amp,
data: None,
})
}
}
impl Gain for FocalPointGain {
fn get_data(&self) -> &[u8] {
assert!(self.data.is_some());
match &self.data {
Some(data) => data,
None => panic!(),
}
}
fn build(&mut self, geometry: &Geometry) {
if self.data.is_some() {
return;
}
let num_devices = geometry.num_devices();
let num_trans = NUM_TRANS_IN_UNIT * num_devices;
let mut data = Vec::with_capacity(num_trans * 2);
let point = self.point;
let amp = self.amp;
for i in 0..num_trans {
let trp = geometry.position(i);
let dist = (trp - point).norm();
let phase = (dist % ULTRASOUND_WAVELENGTH) / ULTRASOUND_WAVELENGTH;
let phase = (255.0 * (1.0 - phase)) as u8;
let amp: u8 = amp;
let (d, s) = convert_to_pwm_params(amp, phase);
data.push(s);
data.push(d);
}
self.data = Some(data);
}
}