autd3_driver/geometry/
transducer.rs

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
use autd3_derive::Builder;

use super::{Isometry, Point3};

use derive_new::new;

/// A ultrasound transducer.
#[derive(Clone, Debug, PartialEq, Builder, new)]
pub struct Transducer {
    idx: u8,
    dev_idx: u16,
    #[get(ref)]
    /// The position of the transducer.
    position: Point3,
}

impl Transducer {
    /// Gets the local index of the transducer.
    pub const fn idx(&self) -> usize {
        self.idx as _
    }

    /// Gets the index of the device to which this transducer belongs.
    pub const fn dev_idx(&self) -> usize {
        self.dev_idx as _
    }

    pub(super) fn affine(&mut self, isometry: &Isometry) {
        self.position = isometry * self.position;
    }
}

#[cfg(test)]
mod tests {
    use std::f32::consts::PI;

    use crate::geometry::{Translation, UnitQuaternion, Vector3};

    use super::*;

    macro_rules! assert_vec3_approx_eq {
        ($a:expr, $b:expr) => {
            approx::assert_abs_diff_eq!($a.x, $b.x, epsilon = 1e-3);
            approx::assert_abs_diff_eq!($a.y, $b.y, epsilon = 1e-3);
            approx::assert_abs_diff_eq!($a.z, $b.z, epsilon = 1e-3);
        };
    }

    #[rstest::fixture]
    fn tr() -> Transducer {
        Transducer::new(0, 0, Point3::origin())
    }

    #[rstest::rstest]
    #[test]
    fn affine(mut tr: Transducer) {
        let vector = Vector3::new(40., 50., 60.);
        let rotation = UnitQuaternion::from_axis_angle(&Vector3::x_axis(), 0.)
            * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), 0.)
            * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.);
        tr.affine(&Isometry {
            translation: Translation { vector },
            rotation,
        });

        let expect_pos = vector;
        assert_vec3_approx_eq!(expect_pos, tr.position());
    }
}