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
90
use autd3_driver::{
is_missing_transducer, NUM_TRANS_IN_UNIT, NUM_TRANS_X, NUM_TRANS_Y, TRANS_SPACING_MM,
};
use super::{Matrix3, Matrix4, Quaternion, Transducer, UnitQuaternion, Vector3, Vector4};
pub struct Device<T: Transducer> {
transducers: Vec<T>,
origin: Vector3,
trans_inv: Matrix3,
}
impl<T: Transducer> Device<T> {
fn get_direction(dir: Vector3, rotation: UnitQuaternion) -> Vector3 {
let dir: UnitQuaternion = UnitQuaternion::from_quaternion(Quaternion::from_imag(dir));
(rotation * dir * rotation.conjugate()).imag().normalize()
}
pub fn local_position(&self, global_position: &Vector3) -> Vector3 {
self.trans_inv * (global_position - self.origin)
}
pub fn transducers(&self) -> &[T] {
&self.transducers
}
pub fn transducers_mut(&mut self) -> &mut [T] {
&mut self.transducers
}
pub fn center(&self) -> Vector3 {
let sum: Vector3 = self.transducers().iter().map(|t| t.position()).sum();
sum / self.transducers.len() as f64
}
}
impl<T: Transducer> Device<T> {
pub fn new(id: usize, position: Vector3, rotation: UnitQuaternion) -> Self {
let rot_mat: Matrix4 = From::from(rotation);
let trans_mat = rot_mat.append_translation(&position);
let x_direction = Self::get_direction(Vector3::x(), rotation);
let y_direction = Self::get_direction(Vector3::y(), rotation);
let z_direction = Self::get_direction(Vector3::z(), rotation);
let transducers: Vec<T> = itertools::iproduct!((0..NUM_TRANS_Y), (0..NUM_TRANS_X))
.filter(|&(y, x)| !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(id * NUM_TRANS_IN_UNIT..)
.map(|(p, i)| {
T::new(
i,
Vector3::new(p.x, p.y, p.z),
x_direction,
y_direction,
z_direction,
)
})
.collect();
let origin = *transducers[0].position();
let trans_inv = Matrix3::from_columns(&[x_direction, y_direction, z_direction]).transpose();
Self {
transducers,
origin,
trans_inv,
}
}
}