autd3_rs_core/geometry/
autd3.rs

1use nalgebra::{Point3, UnitQuaternion, Vector3};
2
3use super::Device;
4
5#[derive(Clone, Copy, Debug, PartialEq)]
6pub struct Autd3 {
7    pub origin: Point3<f32>,
8    pub rotation: UnitQuaternion<f32>,
9}
10
11impl Autd3 {
12    pub const NUM_TRANSDUCERS: usize = autd3_cpu_wire::params::NUM_TRANSDUCERS;
13    pub const GRID_X: u32 = 18;
14    pub const GRID_Y: u32 = 14;
15    pub const PITCH_MM: f32 = 10.16;
16    pub const DEVICE_WIDTH: f32 = 192.0;
17    pub const DEVICE_HEIGHT: f32 = 151.4;
18
19    #[must_use]
20    pub fn new(origin: Point3<f32>, rotation: UnitQuaternion<f32>) -> Self {
21        Self { origin, rotation }
22    }
23}
24
25pub(crate) const fn is_missing_transducer(x: u32, y: u32) -> bool {
26    y == 1 && (x == 1 || x == 2 || x == 16)
27}
28
29impl Default for Autd3 {
30    fn default() -> Self {
31        Self::new(Point3::origin(), UnitQuaternion::identity())
32    }
33}
34
35impl From<Autd3> for Device {
36    fn from(a: Autd3) -> Device {
37        let direction = a.rotation * Vector3::z_axis();
38        let (positions, directions): (Vec<_>, Vec<_>) = (0..Autd3::GRID_Y)
39            .flat_map(|y| (0..Autd3::GRID_X).map(move |x| (x, y)))
40            .filter(|&(x, y)| !is_missing_transducer(x, y))
41            .map(|(x, y)| {
42                let position = a.origin
43                    + a.rotation
44                        * Vector3::new(x as f32 * Autd3::PITCH_MM, y as f32 * Autd3::PITCH_MM, 0.0);
45                (position, direction)
46            })
47            .unzip();
48        debug_assert_eq!(positions.len(), Autd3::NUM_TRANSDUCERS);
49        Device::new(a.rotation, positions, directions)
50    }
51}
52
53#[cfg(test)]
54mod tests {
55    use super::*;
56
57    #[test]
58    fn autd3_has_249_transducers_in_fpga_order() {
59        let dev: Device = Autd3::new(Point3::origin(), UnitQuaternion::identity()).into();
60        assert_eq!(dev.num_transducers(), Autd3::NUM_TRANSDUCERS);
61
62        assert_eq!(dev.position(0), Point3::origin());
63        assert_eq!(
64            dev.position(17),
65            Point3::new(17.0 * Autd3::PITCH_MM, 0.0, 0.0)
66        );
67        assert_eq!(dev.position(18), Point3::new(0.0, Autd3::PITCH_MM, 0.0));
68        assert_eq!(
69            dev.position(19),
70            Point3::new(3.0 * Autd3::PITCH_MM, Autd3::PITCH_MM, 0.0)
71        );
72    }
73
74    #[test]
75    fn autd3_translates_by_origin() {
76        let origin = Point3::new(100.0, 200.0, 300.0);
77        let dev: Device = Autd3::new(origin, UnitQuaternion::identity()).into();
78        assert_eq!(dev.position(0), origin);
79    }
80
81    #[test]
82    fn autd3_default_direction_is_z() {
83        let dev: Device = Autd3::new(Point3::origin(), UnitQuaternion::identity()).into();
84        let z = Vector3::z_axis();
85        for &dir in dev.directions() {
86            assert_eq!(dir, z);
87        }
88    }
89
90    #[test]
91    fn autd3_rotates_positions_and_direction() {
92        let rot = UnitQuaternion::from_axis_angle(&Vector3::x_axis(), std::f32::consts::FRAC_PI_2);
93        let dev: Device = Autd3 {
94            origin: Point3::origin(),
95            rotation: rot,
96        }
97        .into();
98
99        let p1 = dev.position(1);
100        assert!((p1.x - Autd3::PITCH_MM).abs() < 1e-4);
101        assert!(p1.y.abs() < 1e-4);
102        assert!(p1.z.abs() < 1e-4);
103
104        let dir = dev.direction(0);
105        assert!(dir.x.abs() < 1e-4);
106        assert!((dir.y + 1.0).abs() < 1e-4, "y={}", dir.y);
107        assert!(dir.z.abs() < 1e-4);
108    }
109
110    #[test]
111    fn device_center_is_array_center() {
112        let dev: Device = Autd3::new(Point3::origin(), UnitQuaternion::identity()).into();
113        let c = dev.center();
114        assert!((c.x - 86.36).abs() < 1.0, "x center ≈ 86.36, got {}", c.x);
115        assert!((c.y - 66.04).abs() < 1.0, "y center ≈ 66.04, got {}", c.y);
116        assert!(c.z.abs() < f32::EPSILON);
117    }
118
119    #[test]
120    fn autd3_default_is_origin_no_rotation() {
121        let dev: Device = Autd3::default().into();
122        assert_eq!(dev.position(0), Point3::origin());
123        assert_eq!(dev.direction(0), Vector3::z_axis());
124    }
125}
autd3_rs_core/geometry/autd3.rs

autd3_rs_core/geometry/
autd3.rs
