autd3_core/geometry/
device.rs

1use super::{
2    Isometry3, Point3, Quaternion, Transducer, Translation3, UnitQuaternion, UnitVector3, Vector3,
3};
4
5/// An AUTD device unit.
6pub struct Device {
7    pub(crate) idx: u16,
8    pub(crate) transducers: Vec<Transducer>,
9    rotation: UnitQuaternion,
10    center: Point3,
11    x_direction: UnitVector3,
12    y_direction: UnitVector3,
13    axial_direction: UnitVector3,
14    inv: Isometry3,
15}
16
17impl Device {
18    #[doc(hidden)]
19    #[must_use]
20    pub fn new(rot: UnitQuaternion, transducers: Vec<Transducer>) -> Self {
21        let mut transducers = transducers;
22        transducers.iter_mut().enumerate().for_each(|(tr_idx, tr)| {
23            tr.idx = tr_idx as _;
24        });
25        let mut dev = Self {
26            idx: 0,
27            transducers,
28            rotation: rot,
29            center: Point3::origin(),
30            x_direction: Vector3::x_axis(),
31            y_direction: Vector3::y_axis(),
32            axial_direction: Vector3::z_axis(),
33            inv: Isometry3::identity(),
34        };
35        dev.init();
36        dev
37    }
38
39    fn init(&mut self) {
40        self.center = Point3::from(
41            self.transducers
42                .iter()
43                .map(|tr| tr.position().coords)
44                .sum::<Vector3>()
45                / self.transducers.len() as f32,
46        );
47        self.x_direction = Self::get_direction(Vector3::x(), &self.rotation);
48        self.y_direction = Self::get_direction(Vector3::y(), &self.rotation);
49        #[cfg(feature = "left_handed")]
50        {
51            self.axial_direction = Self::get_direction(-Vector3::z(), &self.rotation);
52        }
53        #[cfg(not(feature = "left_handed"))]
54        {
55            self.axial_direction = Self::get_direction(Vector3::z(), &self.rotation);
56        }
57        self.inv = (Translation3::from(self.transducers[0].position()) * self.rotation).inverse();
58    }
59
60    /// Gets the index of the device.
61    #[must_use]
62    pub const fn idx(&self) -> usize {
63        self.idx as _
64    }
65
66    /// Gets the number of transducers of the device.
67    #[must_use]
68    pub const fn num_transducers(&self) -> usize {
69        self.transducers.len()
70    }
71
72    /// Gets the rotation of the device.
73    #[must_use]
74    pub const fn rotation(&self) -> UnitQuaternion {
75        self.rotation
76    }
77
78    /// Gets the center of the device.
79    #[must_use]
80    pub const fn center(&self) -> Point3 {
81        self.center
82    }
83
84    /// Gets the x-direction of the device.
85    #[must_use]
86    pub const fn x_direction(&self) -> UnitVector3 {
87        self.x_direction
88    }
89
90    /// Gets the y-direction of the device.
91    #[must_use]
92    pub const fn y_direction(&self) -> UnitVector3 {
93        self.y_direction
94    }
95
96    /// Gets the axial direction of the device.
97    #[must_use]
98    pub const fn axial_direction(&self) -> UnitVector3 {
99        self.axial_direction
100    }
101
102    #[doc(hidden)]
103    #[must_use]
104    pub const fn inv(&self) -> &Isometry3 {
105        &self.inv
106    }
107
108    #[must_use]
109    fn get_direction(dir: Vector3, rotation: &UnitQuaternion) -> UnitVector3 {
110        let dir: UnitQuaternion = UnitQuaternion::from_quaternion(Quaternion::from_imag(dir));
111        UnitVector3::new_normalize((rotation * dir * rotation.conjugate()).imag())
112    }
113}
114
115impl core::ops::Deref for Device {
116    type Target = [Transducer];
117
118    fn deref(&self) -> &Self::Target {
119        &self.transducers
120    }
121}
122
123impl core::iter::IntoIterator for Device {
124    type Item = Transducer;
125    type IntoIter = std::vec::IntoIter<Transducer>;
126
127    fn into_iter(self) -> Self::IntoIter {
128        self.transducers.into_iter()
129    }
130}
131
132impl<'a> IntoIterator for &'a Device {
133    type Item = &'a Transducer;
134    type IntoIter = core::slice::Iter<'a, Transducer>;
135
136    fn into_iter(self) -> Self::IntoIter {
137        self.transducers.iter()
138    }
139}
140
141#[cfg(test)]
142mod tests {
143    use super::*;
144    use crate::{
145        common::PI,
146        geometry::tests::{TestDevice, create_device},
147    };
148
149    macro_rules! assert_approx_eq_vec3 {
150        ($a:expr, $b:expr) => {
151            approx::assert_abs_diff_eq!($a.x, $b.x, epsilon = 1e-3);
152            approx::assert_abs_diff_eq!($a.y, $b.y, epsilon = 1e-3);
153            approx::assert_abs_diff_eq!($a.z, $b.z, epsilon = 1e-3);
154        };
155    }
156
157    #[test]
158    fn into_iter() {
159        let device = create_device(249);
160        let mut count = 0;
161        for tr in &device {
162            assert_eq!(count, tr.idx());
163            count += 1;
164        }
165        assert_eq!(249, count);
166
167        let mut count = 0;
168        device.into_iter().for_each(|tr| {
169            assert_eq!(count, tr.idx());
170            count += 1;
171        });
172        assert_eq!(249, count);
173    }
174
175    #[test]
176    fn idx() {
177        assert_eq!(0, create_device(249).idx());
178    }
179
180    #[rstest::rstest]
181    #[case(1)]
182    #[case(249)]
183    fn num_transducers(#[case] n: u8) {
184        assert_eq!(n, create_device(n).num_transducers() as u8);
185    }
186
187    #[test]
188    fn center() {
189        let device: Device = TestDevice::new_autd3(Point3::origin()).into();
190        let expected =
191            device.iter().map(|t| t.position().coords).sum::<Vector3>() / device.len() as f32;
192        assert_approx_eq_vec3!(expected, device.center().coords);
193    }
194
195    #[test]
196    fn direction() {
197        let device: Device = TestDevice::new_autd3(Point3::origin()).into();
198        assert_approx_eq_vec3!(Vector3::x(), device.x_direction().into_inner());
199        assert_approx_eq_vec3!(Vector3::y(), device.y_direction().into_inner());
200        #[cfg(feature = "left_handed")]
201        {
202            assert_approx_eq_vec3!(-Vector3::z(), device.axial_direction().into_inner());
203        }
204        #[cfg(not(feature = "left_handed"))]
205        {
206            assert_approx_eq_vec3!(Vector3::z(), device.axial_direction().into_inner());
207        }
208    }
209
210    #[rstest::rstest]
211    #[case(
212        Vector3::new(10., 20., 30.),
213        Vector3::new(10., 20., 30.),
214        Point3::origin(),
215        UnitQuaternion::identity()
216    )]
217    #[case(
218        Vector3::zeros(),
219        Vector3::new(10., 20., 30.),
220        Point3::new(10., 20., 30.),
221        UnitQuaternion::identity()
222    )]
223    #[case(
224        Vector3::new(20., -10., 30.),
225        Vector3::new(10., 20., 30.),
226        Point3::origin(),
227        UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.)
228    )]
229    #[case(
230        Vector3::new(30., 30., -30.),
231        Vector3::new(40., 50., 60.),
232        Point3::new(10., 20., 30.),
233        UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.)
234    )]
235    fn inv(
236        #[case] expected: Vector3,
237        #[case] target: Vector3,
238        #[case] origin: Point3,
239        #[case] rot: UnitQuaternion,
240    ) {
241        let device: Device = TestDevice::new_autd3_with_rot(origin, rot).into();
242        assert_approx_eq_vec3!(expected, device.inv * Point3::from(target));
243    }
244}
autd3_core/geometry/device.rs

autd3_core/geometry/
device.rs
