autd3_core/geometry/
device.rs

1use std::f32::consts::PI;
2
3use bvh::aabb::Aabb;
4use derive_more::{Deref, IntoIterator};
5use getset::Getters;
6
7use crate::defined::{METER, ultrasound_freq};
8
9use super::{
10    Isometry, Point3, Quaternion, Transducer, Translation, UnitQuaternion, UnitVector3, Vector3,
11};
12
13/// An AUTD device unit.
14#[derive(Getters, Deref, IntoIterator)]
15pub struct Device {
16    idx: u16,
17    #[deref]
18    #[into_iterator(ref)]
19    transducers: Vec<Transducer>,
20    /// enable flag
21    pub enable: bool,
22    /// speed of sound
23    pub sound_speed: f32,
24    #[getset(get = "pub")]
25    /// The rotation of the device.
26    rotation: UnitQuaternion,
27    #[getset(get = "pub")]
28    /// The center of the device.
29    center: Point3,
30    #[getset(get = "pub")]
31    /// The x-direction of the device.
32    x_direction: UnitVector3,
33    #[getset(get = "pub")]
34    /// The y-direction of the device.
35    y_direction: UnitVector3,
36    #[getset(get = "pub")]
37    /// The axial direction of the device.
38    axial_direction: UnitVector3,
39    #[doc(hidden)]
40    #[getset(get = "pub")]
41    inv: Isometry,
42    #[getset(get = "pub")]
43    /// The Axis Aligned Bounding Box of the device.
44    aabb: Aabb<f32, 3>,
45}
46
47impl Device {
48    fn init(&mut self) {
49        self.center = Point3::from(
50            self.transducers
51                .iter()
52                .map(|tr| tr.position().coords)
53                .sum::<Vector3>()
54                / self.transducers.len() as f32,
55        );
56        self.x_direction = Self::get_direction(Vector3::x(), &self.rotation);
57        self.y_direction = Self::get_direction(Vector3::y(), &self.rotation);
58        self.axial_direction = if cfg!(feature = "left_handed") {
59            Self::get_direction(-Vector3::z(), &self.rotation) // GRCOV_EXCL_LINE
60        } else {
61            Self::get_direction(Vector3::z(), &self.rotation)
62        };
63        self.inv = (nalgebra::Translation3::<f32>::from(*self.transducers[0].position())
64            * self.rotation)
65            .inverse();
66        self.aabb = self
67            .transducers
68            .iter()
69            .fold(Aabb::empty(), |aabb, tr| aabb.grow(tr.position()));
70    }
71
72    #[doc(hidden)]
73    #[must_use]
74    pub fn new(idx: u16, rot: UnitQuaternion, transducers: Vec<Transducer>) -> Self {
75        let mut dev = Self {
76            idx,
77            transducers,
78            enable: true,
79            sound_speed: 340.0 * METER,
80            rotation: rot,
81            center: Point3::origin(),
82            x_direction: Vector3::x_axis(),
83            y_direction: Vector3::y_axis(),
84            axial_direction: Vector3::z_axis(),
85            inv: nalgebra::Isometry3::identity(),
86            aabb: Aabb::empty(),
87        };
88        dev.init();
89        dev
90    }
91
92    /// Gets the index of the device.
93    #[must_use]
94    pub const fn idx(&self) -> usize {
95        self.idx as _
96    }
97
98    /// Gets the number of transducers of the device.
99    #[must_use]
100    pub fn num_transducers(&self) -> usize {
101        self.transducers.len()
102    }
103
104    /// Translates the device to the target position.
105    pub fn translate_to(&mut self, t: Point3) {
106        self.translate(t - self.transducers[0].position());
107    }
108
109    /// Rotates the device to the target rotation.
110    pub fn rotate_to(&mut self, r: UnitQuaternion) {
111        self.rotate(r * self.rotation.conjugate());
112    }
113
114    /// Translates the device.
115    pub fn translate(&mut self, t: Vector3) {
116        self.affine(t, UnitQuaternion::identity());
117    }
118
119    /// Rotates the device.
120    pub fn rotate(&mut self, r: UnitQuaternion) {
121        self.affine(Vector3::zeros(), r);
122    }
123
124    /// Translates and rotates the device.
125    pub fn affine(&mut self, t: Vector3, r: UnitQuaternion) {
126        let isometry = Isometry {
127            translation: Translation::from(t),
128            rotation: r,
129        };
130        self.transducers
131            .iter_mut()
132            .for_each(|tr| tr.affine(&isometry));
133        self.rotation = r * self.rotation;
134        self.init();
135    }
136
137    /// Sets the sound speed of enabled devices from the temperature `t`.
138    ///
139    /// This is equivalent to `Self::set_sound_speed_from_temp_with(t, 1.4, 8.314_463, 28.9647e-3)`.
140    pub fn set_sound_speed_from_temp(&mut self, temp: f32) {
141        self.set_sound_speed_from_temp_with(temp, 1.4, 8.314_463, 28.9647e-3);
142    }
143
144    /// Sets the sound speed of enabled devices from the temperature `t`, heat capacity ratio `k`, gas constant `r`, and molar mass `m` [kg/mol].
145    pub fn set_sound_speed_from_temp_with(&mut self, temp: f32, k: f32, r: f32, m: f32) {
146        self.sound_speed = (k * r * (273.15 + temp) / m).sqrt() * METER;
147    }
148
149    /// Gets the wavelength of the ultrasound.
150    #[must_use]
151    #[cfg_attr(not(feature = "dynamic_freq"), const_fn::const_fn)]
152    pub fn wavelength(&self) -> f32 {
153        self.sound_speed / ultrasound_freq().hz() as f32
154    }
155
156    /// Gets the wavenumber of the ultrasound.
157    #[must_use]
158    #[cfg_attr(not(feature = "dynamic_freq"), const_fn::const_fn)]
159    pub fn wavenumber(&self) -> f32 {
160        2.0 * PI * ultrasound_freq().hz() as f32 / self.sound_speed
161    }
162
163    #[must_use]
164    fn get_direction(dir: Vector3, rotation: &UnitQuaternion) -> UnitVector3 {
165        let dir: UnitQuaternion = UnitQuaternion::from_quaternion(Quaternion::from_imag(dir));
166        UnitVector3::new_normalize((rotation * dir * rotation.conjugate()).imag())
167    }
168}
169
170/// Trait for converting to [`Device`].
171pub trait IntoDevice {
172    /// Converts to [`Device`].
173    #[must_use]
174    fn into_device(self, dev_idx: u16) -> Device;
175}
176
177impl IntoDevice for Device {
178    fn into_device(mut self, dev_idx: u16) -> Device {
179        self.idx = dev_idx;
180        self
181    }
182}
183
184#[cfg(test)]
185pub(crate) mod tests {
186    use rand::Rng;
187
188    use super::*;
189    use crate::{
190        defined::{PI, mm},
191        geometry::tests::{TestDevice, create_device},
192    };
193
194    macro_rules! assert_approx_eq_vec3 {
195        ($a:expr, $b:expr) => {
196            approx::assert_abs_diff_eq!($a.x, $b.x, epsilon = 1e-3);
197            approx::assert_abs_diff_eq!($a.y, $b.y, epsilon = 1e-3);
198            approx::assert_abs_diff_eq!($a.z, $b.z, epsilon = 1e-3);
199        };
200    }
201
202    macro_rules! assert_approx_eq_quat {
203        ($a:expr, $b:expr) => {
204            approx::assert_abs_diff_eq!($a.w, $b.w, epsilon = 1e-3);
205            approx::assert_abs_diff_eq!($a.i, $b.i, epsilon = 1e-3);
206            approx::assert_abs_diff_eq!($a.j, $b.j, epsilon = 1e-3);
207            approx::assert_abs_diff_eq!($a.k, $b.k, epsilon = 1e-3);
208        };
209    }
210
211    #[rstest::rstest]
212    #[test]
213    #[case(0)]
214    #[case(1)]
215    fn idx(#[case] expect: u16) {
216        assert_eq!(expect, create_device(expect, 249).idx() as u16);
217    }
218
219    #[rstest::rstest]
220    #[test]
221    #[case(1)]
222    #[case(249)]
223    fn num_transducers(#[case] n: u8) {
224        assert_eq!(n, create_device(0, n).num_transducers() as u8);
225    }
226
227    #[test]
228    fn center() {
229        let device = TestDevice::new_autd3(Point3::origin()).into_device(0);
230        let expected =
231            device.iter().map(|t| t.position().coords).sum::<Vector3>() / device.len() as f32;
232        assert_approx_eq_vec3!(expected, device.center());
233    }
234
235    #[rstest::rstest]
236    #[test]
237    #[case(
238        Vector3::new(10., 20., 30.),
239        Vector3::new(10., 20., 30.),
240        Point3::origin(),
241        UnitQuaternion::identity()
242    )]
243    #[case(
244        Vector3::zeros(),
245        Vector3::new(10., 20., 30.),
246        Point3::new(10., 20., 30.),
247        UnitQuaternion::identity()
248    )]
249    #[case(
250        Vector3::new(20., -10., 30.),
251        Vector3::new(10., 20., 30.),
252        Point3::origin(),
253        UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.)
254    )]
255    #[case(
256        Vector3::new(30., 30., -30.),
257        Vector3::new(40., 50., 60.),
258        Point3::new(10., 20., 30.),
259        UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.)
260    )]
261    fn inv(
262        #[case] expected: Vector3,
263        #[case] target: Vector3,
264        #[case] origin: Point3,
265        #[case] rot: UnitQuaternion,
266    ) {
267        let device = TestDevice::new_autd3_with_rot(origin, rot).into_device(0);
268        assert_approx_eq_vec3!(expected, device.inv.transform_point(&Point3::from(target)));
269    }
270
271    #[test]
272    fn translate_to() {
273        let mut rng = rand::rng();
274        let origin = Point3::new(rng.random(), rng.random(), rng.random());
275
276        let mut device = TestDevice::new_autd3(origin).into_device(0);
277
278        let t = Point3::new(40., 50., 60.);
279        device.translate_to(t);
280
281        TestDevice::new_autd3(t)
282            .into_device(0)
283            .iter()
284            .zip(device.iter())
285            .for_each(|(expect, tr)| {
286                assert_approx_eq_vec3!(expect.position(), tr.position());
287            });
288    }
289
290    #[test]
291    fn rotate_to() {
292        let mut rng = rand::rng();
293        let mut device = TestDevice::new_autd3_with_rot(
294            Point3::origin(),
295            UnitQuaternion::from_axis_angle(&Vector3::x_axis(), rng.random())
296                * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), rng.random())
297                * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), rng.random()),
298        )
299        .into_device(0);
300
301        let rot = UnitQuaternion::from_axis_angle(&Vector3::x_axis(), 0.)
302            * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), 0.)
303            * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.);
304        device.rotate_to(rot);
305
306        let expect_x = Vector3::new(0., 1., 0.);
307        let expect_y = Vector3::new(-1., 0., 0.);
308        let expect_z = if cfg!(feature = "left_handed") {
309            Vector3::new(0., 0., -1.) // GRCOV_EXCL_LINE
310        } else {
311            Vector3::new(0., 0., 1.)
312        };
313        assert_approx_eq_quat!(rot, device.rotation());
314        assert_approx_eq_vec3!(expect_x, device.x_direction());
315        assert_approx_eq_vec3!(expect_y, device.y_direction());
316        assert_approx_eq_vec3!(expect_z, device.axial_direction());
317        TestDevice::new_autd3_with_rot(Point3::origin(), rot)
318            .into_device(0)
319            .iter()
320            .zip(device.iter())
321            .for_each(|(expect, tr)| {
322                assert_approx_eq_vec3!(expect.position(), tr.position());
323            });
324    }
325
326    #[test]
327    fn translate() {
328        let mut rng = rand::rng();
329        let origin = Point3::new(rng.random(), rng.random(), rng.random());
330
331        let mut device = TestDevice::new_autd3(origin).into_device(0);
332
333        let t = Vector3::new(40., 50., 60.);
334        device.translate(t);
335        TestDevice::new_autd3(origin + t)
336            .into_device(0)
337            .iter()
338            .zip(device.iter())
339            .for_each(|(expect, tr)| {
340                assert_approx_eq_vec3!(expect.position(), tr.position());
341            });
342    }
343
344    #[test]
345    fn rotate() {
346        let mut device = TestDevice::new_autd3(Point3::origin()).into_device(0);
347
348        let rot = UnitQuaternion::from_axis_angle(&Vector3::x_axis(), 0.)
349            * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), 0.)
350            * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.);
351        device.rotate(rot);
352        let expect_x = Vector3::new(0., 1., 0.);
353        let expect_y = Vector3::new(-1., 0., 0.);
354        let expect_z = if cfg!(feature = "left_handed") {
355            Vector3::new(0., 0., -1.) // GRCOV_EXCL_LINE
356        } else {
357            Vector3::new(0., 0., 1.)
358        };
359        assert_approx_eq_quat!(rot, device.rotation());
360        assert_approx_eq_vec3!(expect_x, device.x_direction());
361        assert_approx_eq_vec3!(expect_y, device.y_direction());
362        assert_approx_eq_vec3!(expect_z, device.axial_direction());
363        TestDevice::new_autd3_with_rot(Point3::origin(), rot)
364            .into_device(0)
365            .iter()
366            .zip(device.iter())
367            .for_each(|(expect, tr)| {
368                assert_approx_eq_vec3!(expect.position(), tr.position());
369            });
370    }
371
372    #[test]
373    fn affine() {
374        let mut device = TestDevice::new_autd3(Point3::origin()).into_device(0);
375
376        let t = Vector3::new(40., 50., 60.);
377        let rot = UnitQuaternion::from_axis_angle(&Vector3::x_axis(), 0.)
378            * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), 0.)
379            * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.);
380        device.affine(t, rot);
381
382        let expect_x = Vector3::new(0., 1., 0.);
383        let expect_y = Vector3::new(-1., 0., 0.);
384        let expect_z = if cfg!(feature = "left_handed") {
385            Vector3::new(0., 0., -1.) // GRCOV_EXCL_LINE
386        } else {
387            Vector3::new(0., 0., 1.)
388        };
389        assert_approx_eq_quat!(rot, device.rotation());
390        assert_approx_eq_vec3!(expect_x, device.x_direction());
391        assert_approx_eq_vec3!(expect_y, device.y_direction());
392        assert_approx_eq_vec3!(expect_z, device.axial_direction());
393
394        TestDevice::new_autd3_with_rot(Point3::from(t), rot)
395            .into_device(0)
396            .iter()
397            .zip(device.iter())
398            .for_each(|(expect, tr)| {
399                assert_approx_eq_vec3!(expect.position(), tr.position());
400            });
401    }
402
403    #[rstest::rstest]
404    #[test]
405    #[case(340.29525e3, 15.)]
406    #[case(343.23497e3, 20.)]
407    #[case(349.04013e3, 30.)]
408    fn set_sound_speed_from_temp(#[case] expected: f32, #[case] temp: f32) {
409        let mut device = create_device(0, 249);
410        device.set_sound_speed_from_temp(temp);
411        approx::assert_abs_diff_eq!(expected * mm, device.sound_speed, epsilon = 1e-3);
412    }
413
414    #[rstest::rstest]
415    #[test]
416    #[case(8.5, 340e3)]
417    #[case(10., 400e3)]
418    fn wavelength(#[case] expect: f32, #[case] c: f32) {
419        let mut device = create_device(0, 249);
420        device.sound_speed = c;
421        approx::assert_abs_diff_eq!(expect, device.wavelength());
422    }
423
424    #[rstest::rstest]
425    #[test]
426    #[case(0.739_198_27, 340e3)]
427    #[case(0.628_318_55, 400e3)]
428    fn wavenumber(#[case] expect: f32, #[case] c: f32) {
429        let mut device = create_device(0, 249);
430        device.sound_speed = c;
431        approx::assert_abs_diff_eq!(expect, device.wavenumber());
432    }
433
434    #[test]
435    fn into_device() {
436        let mut rng = rand::rng();
437        let t = Vector3::new(rng.random(), rng.random(), rng.random());
438        let rot = UnitQuaternion::from_axis_angle(&Vector3::x_axis(), rng.random())
439            * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), rng.random())
440            * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), rng.random());
441        let Device {
442            idx: _idx,
443            transducers: expect_transducers,
444            enable: expect_enable,
445            sound_speed: expect_sound_speed,
446            rotation: expect_rotation,
447            center: expect_center,
448            x_direction: expect_x_direction,
449            y_direction: expect_y_direction,
450            axial_direction: expect_axial_direction,
451            inv: expect_inv,
452            aabb: expect_aabb,
453        } = TestDevice::new_autd3_with_rot(Point3::from(t), rot).into_device(0);
454        let dev = TestDevice::new_autd3_with_rot(Point3::from(t), rot)
455            .into_device(0)
456            .into_device(1);
457        assert_eq!(1, dev.idx());
458        assert_eq!(expect_transducers, dev.transducers);
459        assert_eq!(expect_enable, dev.enable);
460        assert_eq!(expect_sound_speed, dev.sound_speed);
461        assert_eq!(expect_rotation, dev.rotation);
462        assert_eq!(expect_center, dev.center);
463        assert_eq!(expect_x_direction, dev.x_direction);
464        assert_eq!(expect_y_direction, dev.y_direction);
465        assert_eq!(expect_axial_direction, dev.axial_direction);
466        assert_eq!(expect_inv, dev.inv);
467        assert_eq!(expect_aabb.min, dev.aabb.min);
468        assert_eq!(expect_aabb.max, dev.aabb.max);
469    }
470}
autd3_core/geometry/device.rs

autd3_core/geometry/
device.rs
