autd3_core/geometry/
device.rs

1use bvh::aabb::Aabb;
2use derive_more::{Deref, IntoIterator};
3use getset::Getters;
4
5use super::{Isometry, Point3, Quaternion, Transducer, UnitQuaternion, UnitVector3, Vector3};
6
7/// An AUTD device unit.
8#[derive(Getters, Deref, IntoIterator)]
9pub struct Device {
10    pub(crate) idx: u16,
11    #[deref]
12    #[into_iterator(ref)]
13    pub(crate) transducers: Vec<Transducer>,
14    #[getset(get = "pub")]
15    /// The rotation of the device.
16    rotation: UnitQuaternion,
17    #[getset(get = "pub")]
18    /// The center of the device.
19    center: Point3,
20    #[getset(get = "pub")]
21    /// The x-direction of the device.
22    x_direction: UnitVector3,
23    #[getset(get = "pub")]
24    /// The y-direction of the device.
25    y_direction: UnitVector3,
26    #[getset(get = "pub")]
27    /// The axial direction of the device.
28    axial_direction: UnitVector3,
29    #[doc(hidden)]
30    #[getset(get = "pub")]
31    inv: Isometry,
32    #[getset(get = "pub")]
33    /// The Axis Aligned Bounding Box of the device.
34    aabb: Aabb<f32, 3>,
35}
36
37impl Device {
38    fn init(&mut self) {
39        self.center = Point3::from(
40            self.transducers
41                .iter()
42                .map(|tr| tr.position().coords)
43                .sum::<Vector3>()
44                / self.transducers.len() as f32,
45        );
46        self.x_direction = Self::get_direction(Vector3::x(), &self.rotation);
47        self.y_direction = Self::get_direction(Vector3::y(), &self.rotation);
48        self.axial_direction = if cfg!(feature = "left_handed") {
49            Self::get_direction(-Vector3::z(), &self.rotation) // GRCOV_EXCL_LINE
50        } else {
51            Self::get_direction(Vector3::z(), &self.rotation)
52        };
53        self.inv = (nalgebra::Translation3::<f32>::from(*self.transducers[0].position())
54            * self.rotation)
55            .inverse();
56        self.aabb = self
57            .transducers
58            .iter()
59            .fold(Aabb::empty(), |aabb, tr| aabb.grow(tr.position()));
60    }
61
62    #[doc(hidden)]
63    #[must_use]
64    pub fn new(rot: UnitQuaternion, transducers: Vec<Transducer>) -> Self {
65        let mut transducers = transducers;
66        transducers.iter_mut().enumerate().for_each(|(tr_idx, tr)| {
67            tr.idx = tr_idx as _;
68        });
69        let mut dev = Self {
70            idx: 0,
71            transducers,
72            rotation: rot,
73            center: Point3::origin(),
74            x_direction: Vector3::x_axis(),
75            y_direction: Vector3::y_axis(),
76            axial_direction: Vector3::z_axis(),
77            inv: nalgebra::Isometry3::identity(),
78            aabb: Aabb::empty(),
79        };
80        dev.init();
81        dev
82    }
83
84    /// Gets the index of the device.
85    #[must_use]
86    pub const fn idx(&self) -> usize {
87        self.idx as _
88    }
89
90    /// Gets the number of transducers of the device.
91    #[must_use]
92    pub const fn num_transducers(&self) -> usize {
93        self.transducers.len()
94    }
95
96    #[must_use]
97    fn get_direction(dir: Vector3, rotation: &UnitQuaternion) -> UnitVector3 {
98        let dir: UnitQuaternion = UnitQuaternion::from_quaternion(Quaternion::from_imag(dir));
99        UnitVector3::new_normalize((rotation * dir * rotation.conjugate()).imag())
100    }
101}
102
103#[cfg(test)]
104pub(crate) mod tests {
105    use super::*;
106    use crate::{
107        common::PI,
108        geometry::tests::{TestDevice, create_device},
109    };
110
111    macro_rules! assert_approx_eq_vec3 {
112        ($a:expr, $b:expr) => {
113            approx::assert_abs_diff_eq!($a.x, $b.x, epsilon = 1e-3);
114            approx::assert_abs_diff_eq!($a.y, $b.y, epsilon = 1e-3);
115            approx::assert_abs_diff_eq!($a.z, $b.z, epsilon = 1e-3);
116        };
117    }
118
119    #[test]
120    fn idx() {
121        assert_eq!(0, create_device(249).idx());
122    }
123
124    #[rstest::rstest]
125    #[test]
126    #[case(1)]
127    #[case(249)]
128    fn num_transducers(#[case] n: u8) {
129        assert_eq!(n, create_device(n).num_transducers() as u8);
130    }
131
132    #[test]
133    fn center() {
134        let device: Device = TestDevice::new_autd3(Point3::origin()).into();
135        let expected =
136            device.iter().map(|t| t.position().coords).sum::<Vector3>() / device.len() as f32;
137        assert_approx_eq_vec3!(expected, device.center());
138    }
139
140    #[rstest::rstest]
141    #[test]
142    #[case(
143        Vector3::new(10., 20., 30.),
144        Vector3::new(10., 20., 30.),
145        Point3::origin(),
146        UnitQuaternion::identity()
147    )]
148    #[case(
149        Vector3::zeros(),
150        Vector3::new(10., 20., 30.),
151        Point3::new(10., 20., 30.),
152        UnitQuaternion::identity()
153    )]
154    #[case(
155        Vector3::new(20., -10., 30.),
156        Vector3::new(10., 20., 30.),
157        Point3::origin(),
158        UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.)
159    )]
160    #[case(
161        Vector3::new(30., 30., -30.),
162        Vector3::new(40., 50., 60.),
163        Point3::new(10., 20., 30.),
164        UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.)
165    )]
166    fn inv(
167        #[case] expected: Vector3,
168        #[case] target: Vector3,
169        #[case] origin: Point3,
170        #[case] rot: UnitQuaternion,
171    ) {
172        let device: Device = TestDevice::new_autd3_with_rot(origin, rot).into();
173        assert_approx_eq_vec3!(expected, device.inv.transform_point(&Point3::from(target)));
174    }
175}
autd3_core/geometry/device.rs

autd3_core/geometry/
device.rs
