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    #[cfg_attr(not(feature = "dynamic_freq"), const_fn::const_fn)]
121    pub fn wavelength(&self) -> f32 {
122        self.sound_speed / ultrasound_freq().hz() as f32
123    }
124
125    /// Gets the wavenumber of the ultrasound.
126    #[must_use]
127    #[cfg_attr(not(feature = "dynamic_freq"), const_fn::const_fn)]
128    pub fn wavenumber(&self) -> f32 {
129        2.0 * PI * ultrasound_freq().hz() as f32 / self.sound_speed
130    }
131
132    #[must_use]
133    fn get_direction(dir: Vector3, rotation: &UnitQuaternion) -> UnitVector3 {
134        let dir: UnitQuaternion = UnitQuaternion::from_quaternion(Quaternion::from_imag(dir));
135        UnitVector3::new_normalize((rotation * dir * rotation.conjugate()).imag())
136    }
137}
138
139#[cfg(test)]
140pub(crate) mod tests {
141    use super::*;
142    use crate::{
143        defined::{PI, mm},
144        geometry::tests::{TestDevice, create_device},
145    };
146
147    macro_rules! assert_approx_eq_vec3 {
148        ($a:expr, $b:expr) => {
149            approx::assert_abs_diff_eq!($a.x, $b.x, epsilon = 1e-3);
150            approx::assert_abs_diff_eq!($a.y, $b.y, epsilon = 1e-3);
151            approx::assert_abs_diff_eq!($a.z, $b.z, epsilon = 1e-3);
152        };
153    }
154
155    #[test]
156    fn idx() {
157        assert_eq!(0, create_device(249).idx());
158    }
159
160    #[rstest::rstest]
161    #[test]
162    #[case(1)]
163    #[case(249)]
164    fn num_transducers(#[case] n: u8) {
165        assert_eq!(n, create_device(n).num_transducers() as u8);
166    }
167
168    #[test]
169    fn center() {
170        let device: Device = TestDevice::new_autd3(Point3::origin()).into();
171        let expected =
172            device.iter().map(|t| t.position().coords).sum::<Vector3>() / device.len() as f32;
173        assert_approx_eq_vec3!(expected, device.center());
174    }
175
176    #[rstest::rstest]
177    #[test]
178    #[case(
179        Vector3::new(10., 20., 30.),
180        Vector3::new(10., 20., 30.),
181        Point3::origin(),
182        UnitQuaternion::identity()
183    )]
184    #[case(
185        Vector3::zeros(),
186        Vector3::new(10., 20., 30.),
187        Point3::new(10., 20., 30.),
188        UnitQuaternion::identity()
189    )]
190    #[case(
191        Vector3::new(20., -10., 30.),
192        Vector3::new(10., 20., 30.),
193        Point3::origin(),
194        UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.)
195    )]
196    #[case(
197        Vector3::new(30., 30., -30.),
198        Vector3::new(40., 50., 60.),
199        Point3::new(10., 20., 30.),
200        UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.)
201    )]
202    fn inv(
203        #[case] expected: Vector3,
204        #[case] target: Vector3,
205        #[case] origin: Point3,
206        #[case] rot: UnitQuaternion,
207    ) {
208        let device: Device = TestDevice::new_autd3_with_rot(origin, rot).into();
209        assert_approx_eq_vec3!(expected, device.inv.transform_point(&Point3::from(target)));
210    }
211
212    #[rstest::rstest]
213    #[test]
214    #[case(340.29525e3, 15.)]
215    #[case(343.23497e3, 20.)]
216    #[case(349.04013e3, 30.)]
217    fn set_sound_speed_from_temp(#[case] expected: f32, #[case] temp: f32) {
218        let mut device = create_device(249);
219        device.set_sound_speed_from_temp(temp);
220        approx::assert_abs_diff_eq!(expected * mm, device.sound_speed, epsilon = 1e-3);
221    }
222
223    #[rstest::rstest]
224    #[test]
225    #[case(8.5, 340e3)]
226    #[case(10., 400e3)]
227    fn wavelength(#[case] expect: f32, #[case] c: f32) {
228        let mut device = create_device(249);
229        device.sound_speed = c;
230        approx::assert_abs_diff_eq!(expect, device.wavelength());
231    }
232
233    #[rstest::rstest]
234    #[test]
235    #[case(0.739_198_27, 340e3)]
236    #[case(0.628_318_55, 400e3)]
237    fn wavenumber(#[case] expect: f32, #[case] c: f32) {
238        let mut device = create_device(249);
239        device.sound_speed = c;
240        approx::assert_abs_diff_eq!(expect, device.wavenumber());
241    }
242}
autd3_core/geometry/device.rs

autd3_core/geometry/
device.rs
