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
use autd3_derive::Builder;

use super::{Matrix4, UnitQuaternion, Vector3};

use derive_new::new;

#[derive(Clone, Debug, PartialEq, Builder, new)]
#[new(visibility = "pub(crate)")]
pub struct Transducer {
    idx: u8,
    dev_idx: u16,
    #[get(ref)]
    position: Vector3,
}

impl Transducer {
    pub const fn idx(&self) -> usize {
        self.idx as _
    }

    pub const fn dev_idx(&self) -> usize {
        self.dev_idx as _
    }

    pub(super) fn affine(&mut self, t: Vector3, r: UnitQuaternion) {
        self.position = (Matrix4::from(r).append_translation(&t) * self.position.push(1.0)).xyz();
    }
}

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

    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, Vector3::zeros())
    }

    #[rstest::rstest]
    #[test]
    fn affine(mut tr: Transducer) {
        let t = Vector3::new(40., 50., 60.);
        let rot = 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(t, rot);

        let expect_pos = Vector3::zeros() + t;
        assert_vec3_approx_eq!(expect_pos, tr.position());
    }
}