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
use autd3_core::geometry::{Device, Matrix4, Transducer, UnitQuaternion, Vector3, Vector4};
use num::FromPrimitive;
pub const NUM_TRANS_IN_UNIT: usize = 249;
pub const NUM_TRANS_X: usize = 18;
pub const NUM_TRANS_Y: usize = 14;
pub const TRANS_SPACING_MM: f64 = 10.16;
pub const DEVICE_WIDTH: f64 = 192.0;
pub const DEVICE_HEIGHT: f64 = 151.4;
pub struct AUTD3 {
position: Vector3,
rotation: UnitQuaternion,
}
impl AUTD3 {
pub fn new(position: Vector3, euler_angles: Vector3) -> Self {
let q = UnitQuaternion::from_axis_angle(&Vector3::z_axis(), euler_angles.x)
* UnitQuaternion::from_axis_angle(&Vector3::y_axis(), euler_angles.y)
* UnitQuaternion::from_axis_angle(&Vector3::z_axis(), euler_angles.z);
Self::new_with_quaternion(position, q)
}
pub fn new_with_quaternion(position: Vector3, rotation: UnitQuaternion) -> Self {
Self { position, rotation }
}
pub fn is_missing_transducer<T1, T2>(x: T1, y: T2) -> bool
where
T1: FromPrimitive + PartialEq<T1>,
T2: FromPrimitive + PartialEq<T2>,
{
y == FromPrimitive::from_u8(1).unwrap()
&& (x == FromPrimitive::from_u8(1).unwrap()
|| x == FromPrimitive::from_u8(2).unwrap()
|| x == FromPrimitive::from_u8(16).unwrap())
}
}
impl<T: Transducer> Device<T> for AUTD3 {
fn get_transducers(&self, start_id: usize) -> Vec<T> {
let rot_mat: Matrix4 = From::from(self.rotation);
let trans_mat = rot_mat.append_translation(&self.position);
itertools::iproduct!((0..NUM_TRANS_Y), (0..NUM_TRANS_X))
.filter(|&(y, x)| !Self::is_missing_transducer(x, y))
.map(|(y, x)| {
Vector4::new(
x as f64 * TRANS_SPACING_MM,
y as f64 * TRANS_SPACING_MM,
0.,
1.,
)
})
.map(|p| trans_mat * p)
.zip(start_id..)
.map(|(p, i)| T::new(i, Vector3::new(p.x, p.y, p.z), self.rotation))
.collect()
}
}