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::{Isometry, Point3, Quaternion, Transducer, UnitQuaternion, UnitVector3, Vector3};
10
11/// An AUTD device unit.
12#[derive(Getters, Deref, IntoIterator)]
13pub struct Device {
14    pub(crate) idx: u16,
15    #[deref]
16    #[into_iterator(ref)]
17    pub(crate) transducers: Vec<Transducer>,
18    /// enable flag
19    pub enable: bool,
20    /// speed of sound
21    pub sound_speed: f32,
22    #[getset(get = "pub")]
23    /// The rotation of the device.
24    rotation: UnitQuaternion,
25    #[getset(get = "pub")]
26    /// The center of the device.
27    center: Point3,
28    #[getset(get = "pub")]
29    /// The x-direction of the device.
30    x_direction: UnitVector3,
31    #[getset(get = "pub")]
32    /// The y-direction of the device.
33    y_direction: UnitVector3,
34    #[getset(get = "pub")]
35    /// The axial direction of the device.
36    axial_direction: UnitVector3,
37    #[doc(hidden)]
38    #[getset(get = "pub")]
39    inv: Isometry,
40    #[getset(get = "pub")]
41    /// The Axis Aligned Bounding Box of the device.
42    aabb: Aabb<f32, 3>,
43}
44
45impl Device {
46    fn init(&mut self) {
47        self.center = Point3::from(
48            self.transducers
49                .iter()
50                .map(|tr| tr.position().coords)
51                .sum::<Vector3>()
52                / self.transducers.len() as f32,
53        );
54        self.x_direction = Self::get_direction(Vector3::x(), &self.rotation);
55        self.y_direction = Self::get_direction(Vector3::y(), &self.rotation);
56        self.axial_direction = if cfg!(feature = "left_handed") {
57            Self::get_direction(-Vector3::z(), &self.rotation) // GRCOV_EXCL_LINE
58        } else {
59            Self::get_direction(Vector3::z(), &self.rotation)
60        };
61        self.inv = (nalgebra::Translation3::<f32>::from(*self.transducers[0].position())
62            * self.rotation)
63            .inverse();
64        self.aabb = self
65            .transducers
66            .iter()
67            .fold(Aabb::empty(), |aabb, tr| aabb.grow(tr.position()));
68    }
69
70    #[doc(hidden)]
71    #[must_use]
72    pub fn new(rot: UnitQuaternion, transducers: Vec<Transducer>) -> Self {
73        let mut transducers = transducers;
74        transducers.iter_mut().enumerate().for_each(|(tr_idx, tr)| {
75            tr.idx = tr_idx as _;
76        });
77        let mut dev = Self {
78            idx: 0,
79            transducers,
80            enable: true,
81            sound_speed: 340.0 * METER,
82            rotation: rot,
83            center: Point3::origin(),
84            x_direction: Vector3::x_axis(),
85            y_direction: Vector3::y_axis(),
86            axial_direction: Vector3::z_axis(),
87            inv: nalgebra::Isometry3::identity(),
88            aabb: Aabb::empty(),
89        };
90        dev.init();
91        dev
92    }
93
94    /// Gets the index of the device.
95    #[must_use]
96    pub const fn idx(&self) -> usize {
97        self.idx as _
98    }
99
100    /// Gets the number of transducers of the device.
101    #[must_use]
102    pub fn num_transducers(&self) -> usize {
103        self.transducers.len()
104    }
105
106    /// Sets the sound speed of enabled devices from the temperature `t`.
107    ///
108    /// This is equivalent to `Self::set_sound_speed_from_temp_with(t, 1.4, 8.314_463, 28.9647e-3)`.
109    pub fn set_sound_speed_from_temp(&mut self, temp: f32) {
110        self.set_sound_speed_from_temp_with(temp, 1.4, 8.314_463, 28.9647e-3);
111    }
112
113    /// Sets the sound speed of enabled devices from the temperature `t`, heat capacity ratio `k`, gas constant `r`, and molar mass `m` [kg/mol].
114    pub fn set_sound_speed_from_temp_with(&mut self, temp: f32, k: f32, r: f32, m: f32) {
115        self.sound_speed = (k * r * (273.15 + temp) / m).sqrt() * METER;
116    }
117
118    /// Gets the wavelength of the ultrasound.
119    #[must_use]
120    pub const fn wavelength(&self) -> f32 {
121        self.sound_speed / ULTRASOUND_FREQ.hz() as f32
122    }
123
124    /// Gets the wavenumber of the ultrasound.
125    #[must_use]
126    pub const fn wavenumber(&self) -> f32 {
127        2.0 * PI * ULTRASOUND_FREQ.hz() as f32 / self.sound_speed
128    }
129
130    #[must_use]
131    fn get_direction(dir: Vector3, rotation: &UnitQuaternion) -> UnitVector3 {
132        let dir: UnitQuaternion = UnitQuaternion::from_quaternion(Quaternion::from_imag(dir));
133        UnitVector3::new_normalize((rotation * dir * rotation.conjugate()).imag())
134    }
135}
136
137#[cfg(test)]
138pub(crate) mod tests {
139    use super::*;
140    use crate::{
141        defined::{PI, mm},
142        geometry::tests::{TestDevice, create_device},
143    };
144
145    macro_rules! assert_approx_eq_vec3 {
146        ($a:expr, $b:expr) => {
147            approx::assert_abs_diff_eq!($a.x, $b.x, epsilon = 1e-3);
148            approx::assert_abs_diff_eq!($a.y, $b.y, epsilon = 1e-3);
149            approx::assert_abs_diff_eq!($a.z, $b.z, epsilon = 1e-3);
150        };
151    }
152
153    #[test]
154    fn idx() {
155        assert_eq!(0, create_device(249).idx());
156    }
157
158    #[rstest::rstest]
159    #[test]
160    #[case(1)]
161    #[case(249)]
162    fn num_transducers(#[case] n: u8) {
163        assert_eq!(n, create_device(n).num_transducers() as u8);
164    }
165
166    #[test]
167    fn center() {
168        let device: Device = TestDevice::new_autd3(Point3::origin()).into();
169        let expected =
170            device.iter().map(|t| t.position().coords).sum::<Vector3>() / device.len() as f32;
171        assert_approx_eq_vec3!(expected, device.center());
172    }
173
174    #[rstest::rstest]
175    #[test]
176    #[case(
177        Vector3::new(10., 20., 30.),
178        Vector3::new(10., 20., 30.),
179        Point3::origin(),
180        UnitQuaternion::identity()
181    )]
182    #[case(
183        Vector3::zeros(),
184        Vector3::new(10., 20., 30.),
185        Point3::new(10., 20., 30.),
186        UnitQuaternion::identity()
187    )]
188    #[case(
189        Vector3::new(20., -10., 30.),
190        Vector3::new(10., 20., 30.),
191        Point3::origin(),
192        UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.)
193    )]
194    #[case(
195        Vector3::new(30., 30., -30.),
196        Vector3::new(40., 50., 60.),
197        Point3::new(10., 20., 30.),
198        UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.)
199    )]
200    fn inv(
201        #[case] expected: Vector3,
202        #[case] target: Vector3,
203        #[case] origin: Point3,
204        #[case] rot: UnitQuaternion,
205    ) {
206        let device: Device = TestDevice::new_autd3_with_rot(origin, rot).into();
207        assert_approx_eq_vec3!(expected, device.inv.transform_point(&Point3::from(target)));
208    }
209
210    #[rstest::rstest]
211    #[test]
212    #[case(340.29525e3, 15.)]
213    #[case(343.23497e3, 20.)]
214    #[case(349.04013e3, 30.)]
215    fn set_sound_speed_from_temp(#[case] expected: f32, #[case] temp: f32) {
216        let mut device = create_device(249);
217        device.set_sound_speed_from_temp(temp);
218        approx::assert_abs_diff_eq!(expected * mm, device.sound_speed, epsilon = 1e-3);
219    }
220
221    #[rstest::rstest]
222    #[test]
223    #[case(8.5, 340e3)]
224    #[case(10., 400e3)]
225    fn wavelength(#[case] expect: f32, #[case] c: f32) {
226        let mut device = create_device(249);
227        device.sound_speed = c;
228        approx::assert_abs_diff_eq!(expect, device.wavelength());
229    }
230
231    #[rstest::rstest]
232    #[test]
233    #[case(0.739_198_27, 340e3)]
234    #[case(0.628_318_55, 400e3)]
235    fn wavenumber(#[case] expect: f32, #[case] c: f32) {
236        let mut device = create_device(249);
237        device.sound_speed = c;
238        approx::assert_abs_diff_eq!(expect, device.wavenumber());
239    }
240}
autd3_core/geometry/device.rs

autd3_core/geometry/
device.rs
