autd3_core/geometry/
mod.rs

1pub(crate) mod device;
2mod rotation;
3mod transducer;
4
5/// a complex number
6pub type Complex = nalgebra::Complex<f32>;
7/// 3-dimensional column vector.
8pub type Vector3 = nalgebra::Vector3<f32>;
9/// 3-dimensional unit vector.
10pub type UnitVector3 = nalgebra::UnitVector3<f32>;
11/// 3-dimensional point.
12pub type Point3 = nalgebra::Point3<f32>;
13/// A quaternion.
14pub type Quaternion = nalgebra::Quaternion<f32>;
15/// A unit quaternion.
16pub type UnitQuaternion = nalgebra::UnitQuaternion<f32>;
17/// A 3-dimensional translation.
18pub type Translation = nalgebra::Translation3<f32>;
19/// A 3-dimensional isometry.
20pub type Isometry = nalgebra::Isometry3<f32>;
21
22pub use bvh::aabb::Aabb;
23pub use device::*;
24use getset::CopyGetters;
25pub use rotation::*;
26pub use transducer::*;
27
28use derive_more::{Deref, IntoIterator};
29
30/// Geometry of the devices.
31#[derive(Deref, CopyGetters, IntoIterator)]
32pub struct Geometry {
33    #[deref]
34    #[into_iterator(ref)]
35    pub(crate) devices: Vec<Device>,
36    #[doc(hidden)]
37    #[getset(get_copy = "pub")]
38    version: usize,
39}
40
41impl Geometry {
42    /// Creates a new [`Geometry`].
43    #[must_use]
44    pub fn new(devices: Vec<Device>) -> Self {
45        let mut geometry = Self {
46            devices,
47            version: 0,
48        };
49        geometry.assign_idx();
50        geometry
51    }
52
53    fn assign_idx(&mut self) {
54        self.devices
55            .iter_mut()
56            .enumerate()
57            .for_each(|(dev_idx, dev)| {
58                dev.idx = dev_idx as _;
59                dev.transducers.iter_mut().for_each(|tr| {
60                    tr.dev_idx = dev_idx as _;
61                });
62            });
63    }
64
65    /// Gets the number of enabled devices.
66    #[must_use]
67    pub fn num_devices(&self) -> usize {
68        self.devices().count()
69    }
70
71    /// Gets the number of enabled transducers.
72    #[must_use]
73    pub fn num_transducers(&self) -> usize {
74        self.devices().map(|dev| dev.num_transducers()).sum()
75    }
76
77    /// Gets the center of the enabled transducers.
78    #[must_use]
79    pub fn center(&self) -> Point3 {
80        Point3::from(
81            self.devices().map(|d| d.center().coords).sum::<Vector3>() / self.devices.len() as f32,
82        )
83    }
84
85    /// Gets the iterator of enabled devices.
86    pub fn devices(&self) -> impl Iterator<Item = &Device> {
87        self.iter().filter(|dev| dev.enable)
88    }
89
90    /// Gets the mutable iterator of enabled devices.
91    pub fn devices_mut(&mut self) -> impl Iterator<Item = &mut Device> {
92        self.iter_mut().filter(|dev| dev.enable)
93    }
94
95    /// Sets the sound speed of enabled devices.
96    pub fn set_sound_speed(&mut self, c: f32) {
97        self.devices_mut().for_each(|dev| dev.sound_speed = c);
98    }
99
100    /// Sets the sound speed of enabled devices from the temperature `t`.
101    ///
102    /// This is equivalent to `Self::set_sound_speed_from_temp_with(t, 1.4, 8.314_463, 28.9647e-3)`.
103    pub fn set_sound_speed_from_temp(&mut self, t: f32) {
104        self.set_sound_speed_from_temp_with(t, 1.4, 8.314_463, 28.9647e-3);
105    }
106
107    /// Sets the sound speed of enabled devices from the temperature `t`, heat capacity ratio `k`, gas constant `r`, and molar mass `m` [kg/mol].
108    pub fn set_sound_speed_from_temp_with(&mut self, t: f32, k: f32, r: f32, m: f32) {
109        self.devices_mut()
110            .for_each(|dev| dev.set_sound_speed_from_temp_with(t, k, r, m));
111    }
112
113    /// Axis Aligned Bounding Box of enabled devices.
114    #[must_use]
115    pub fn aabb(&self) -> Aabb<f32, 3> {
116        self.devices()
117            .fold(Aabb::empty(), |aabb, dev| aabb.join(dev.aabb()))
118    }
119
120    /// Reconfigure the geometry.
121    pub fn reconfigure<D: Into<Device>, F: Fn(&Device) -> D>(&mut self, f: F) {
122        self.devices.iter_mut().for_each(|dev| {
123            let enable = dev.enable;
124            let sound_speed = dev.sound_speed;
125            *dev = f(dev).into();
126            dev.enable = enable;
127            dev.sound_speed = sound_speed;
128        });
129        self.assign_idx();
130        self.version += 1;
131    }
132}
133
134impl<'a> IntoIterator for &'a mut Geometry {
135    type Item = &'a mut Device;
136    type IntoIter = std::slice::IterMut<'a, Device>;
137
138    fn into_iter(self) -> Self::IntoIter {
139        self.version += 1;
140        self.devices.iter_mut()
141    }
142}
143
144impl std::ops::DerefMut for Geometry {
145    fn deref_mut(&mut self) -> &mut Self::Target {
146        self.version += 1;
147        &mut self.devices
148    }
149}
150
151#[cfg(test)]
152pub(crate) mod tests {
153    use rand::Rng;
154
155    use crate::common::{deg, mm};
156
157    use super::*;
158
159    macro_rules! assert_approx_eq_vec3 {
160        ($a:expr, $b:expr) => {
161            approx::assert_abs_diff_eq!($a.x, $b.x, epsilon = 1e-3);
162            approx::assert_abs_diff_eq!($a.y, $b.y, epsilon = 1e-3);
163            approx::assert_abs_diff_eq!($a.z, $b.z, epsilon = 1e-3);
164        };
165    }
166
167    pub struct TestDevice {
168        pub rotation: UnitQuaternion,
169        pub transducers: Vec<Transducer>,
170    }
171
172    impl TestDevice {
173        pub fn new_autd3(pos: Point3) -> Self {
174            Self::new_autd3_with_rot(pos, UnitQuaternion::identity())
175        }
176
177        pub fn new_autd3_with_rot(pos: Point3, rot: impl Into<UnitQuaternion>) -> Self {
178            let rotation = rot.into();
179            let isometry = Isometry {
180                rotation,
181                translation: Translation::from(pos),
182            };
183            Self {
184                rotation,
185                transducers: itertools::iproduct!(0..14, 0..18)
186                    .map(|(y, x)| {
187                        Transducer::new(
188                            (isometry * (10.16 * mm * Point3::new(x as f32, y as f32, 0.))).xyz(),
189                        )
190                    })
191                    .collect(),
192            }
193        }
194    }
195
196    impl From<TestDevice> for Device {
197        fn from(dev: TestDevice) -> Self {
198            Self::new(dev.rotation, dev.transducers)
199        }
200    }
201
202    pub fn create_device(n: u8) -> Device {
203        Device::new(
204            UnitQuaternion::identity(),
205            (0..n).map(|_| Transducer::new(Point3::origin())).collect(),
206        )
207    }
208
209    pub fn create_geometry(n: u16, num_trans_in_unit: u8) -> Geometry {
210        Geometry::new((0..n).map(|_| create_device(num_trans_in_unit)).collect())
211    }
212
213    #[rstest::rstest]
214    #[test]
215    #[case(1, vec![create_device(249)])]
216    #[case(2, vec![create_device(249), create_device(249)])]
217    fn test_num_devices(#[case] expected: usize, #[case] devices: Vec<Device>) {
218        let geometry = Geometry::new(devices);
219        assert_eq!(0, geometry.version());
220        assert_eq!(expected, geometry.num_devices());
221        assert_eq!(0, geometry.version());
222    }
223
224    #[rstest::rstest]
225    #[test]
226    #[case(249, vec![create_device(249)])]
227    #[case(498, vec![create_device(249), create_device(249)])]
228    fn test_num_transducers(#[case] expected: usize, #[case] devices: Vec<Device>) {
229        let geometry = Geometry::new(devices);
230        assert_eq!(0, geometry.version());
231        assert_eq!(expected, geometry.num_transducers());
232        assert_eq!(0, geometry.version());
233    }
234
235    #[test]
236    fn test_center() {
237        let geometry = Geometry::new(vec![
238            TestDevice::new_autd3(Point3::origin()).into(),
239            TestDevice::new_autd3(Point3::new(10., 20., 30.)).into(),
240        ]);
241        let expect = geometry
242            .iter()
243            .map(|dev| dev.center().coords)
244            .sum::<Vector3>()
245            / geometry.num_devices() as f32;
246        assert_eq!(0, geometry.version());
247        assert_approx_eq_vec3!(expect, geometry.center());
248        assert_eq!(0, geometry.version());
249    }
250
251    #[rstest::rstest]
252    #[test]
253    #[case(340.29525e3, 15.)]
254    #[case(343.23497e3, 20.)]
255    #[case(349.04013e3, 30.)]
256    fn test_set_sound_speed_from_temp(#[case] expected: f32, #[case] temp: f32) {
257        let mut geometry = create_geometry(2, 1);
258        assert_eq!(0, geometry.version());
259        geometry.set_sound_speed_from_temp(temp);
260        assert_eq!(1, geometry.version());
261        geometry.iter().for_each(|dev| {
262            approx::assert_abs_diff_eq!(expected * mm, dev.sound_speed, epsilon = 1e-3);
263        });
264    }
265
266    #[rstest::rstest]
267    #[test]
268    #[case(3.402_952_8e5)]
269    #[case(3.432_35e5)]
270    #[case(3.490_401_6e5)]
271    fn test_set_sound_speed(#[case] temp: f32) {
272        let mut geometry = create_geometry(2, 1);
273        assert_eq!(0, geometry.version());
274        geometry.set_sound_speed(temp * mm);
275        assert_eq!(1, geometry.version());
276        geometry.iter().for_each(|dev| {
277            assert_eq!(dev.sound_speed, temp * mm);
278        });
279    }
280
281    #[test]
282    fn into_iter() {
283        let mut geometry = create_geometry(1, 1);
284        assert_eq!(0, geometry.version());
285        for dev in &mut geometry {
286            dev.enable = true;
287        }
288        assert_eq!(1, geometry.version());
289    }
290
291    #[rstest::rstest]
292    #[test]
293    #[case(Aabb{min: Point3::origin(), max: Point3::new(172.72 * mm, 132.08 * mm, 0.)}, vec![TestDevice::new_autd3(Point3::origin())])]
294    #[case(Aabb{min: Point3::new(10. * mm, 20. * mm, 30. * mm), max: Point3::new(182.72 * mm, 152.08 * mm, 30. * mm)}, vec![TestDevice::new_autd3(Point3::new(10. * mm, 20. * mm, 30. * mm))])]
295    #[case(Aabb{min: Point3::new(-132.08 * mm, 0., 0.), max: Point3::new(0., 172.72 * mm, 0.)}, vec![TestDevice::new_autd3_with_rot(Point3::origin(), EulerAngle::ZYZ(90. * deg, 0. * deg, 0. * deg))])]
296    #[case(Aabb{min: Point3::new(-132.08 * mm, -10. * mm, 0.), max: Point3::new(172.72 * mm, 162.72 * mm, 10. * mm)}, vec![
297        TestDevice::new_autd3(Point3::origin()),
298        TestDevice::new_autd3_with_rot(Point3::new(0., -10. * mm, 10. * mm), EulerAngle::ZYZ(90. * deg, 0. * deg, 0. * deg))
299    ])]
300    fn aabb(#[case] expect: Aabb<f32, 3>, #[case] dev: Vec<TestDevice>) {
301        let geometry = Geometry::new(dev.into_iter().map(|d| d.into()).collect());
302        assert_approx_eq_vec3!(expect.min, geometry.aabb().min);
303        assert_approx_eq_vec3!(expect.max, geometry.aabb().max);
304    }
305
306    #[test]
307    fn idx() {
308        let geometry = Geometry::new(vec![
309            TestDevice::new_autd3_with_rot(Point3::origin(), UnitQuaternion::identity()).into(),
310            TestDevice::new_autd3_with_rot(Point3::origin(), UnitQuaternion::identity()).into(),
311        ]);
312        (0..2).for_each(|dev_idx| {
313            assert_eq!(dev_idx, geometry[dev_idx].idx());
314            (0..14 * 18).for_each(|tr_idx| {
315                assert_eq!(tr_idx, geometry[dev_idx][tr_idx].idx());
316                assert_eq!(dev_idx, geometry[dev_idx][tr_idx].dev_idx());
317            });
318        });
319    }
320
321    #[test]
322    fn reconfigure() {
323        let mut geometry = Geometry::new(vec![
324            TestDevice::new_autd3_with_rot(Point3::origin(), UnitQuaternion::identity()).into(),
325            TestDevice::new_autd3_with_rot(Point3::origin(), UnitQuaternion::identity()).into(),
326        ]);
327
328        let mut rng = rand::rng();
329        let t = Point3::new(rng.random(), rng.random(), rng.random());
330        let rot = UnitQuaternion::new_normalize(Quaternion::new(
331            rng.random(),
332            rng.random(),
333            rng.random(),
334            rng.random(),
335        ));
336
337        geometry.reconfigure(|dev| match dev.idx() {
338            0 => TestDevice::new_autd3_with_rot(t, rot),
339            _ => TestDevice::new_autd3_with_rot(*dev[0].position(), *dev.rotation()),
340        });
341
342        assert_eq!(1, geometry.version());
343        assert_eq!(t, *geometry[0][0].position());
344        assert_eq!(rot, *geometry[0].rotation());
345        assert_eq!(Point3::origin(), *geometry[1][0].position());
346        assert_eq!(UnitQuaternion::identity(), *geometry[1].rotation());
347    }
348}
autd3_core/geometry/mod.rs

autd3_core/geometry/
mod.rs
