autd3_driver/firmware/operation/stm/
control_point.rs

1use crate::{
2    firmware::fpga::{EmitIntensity, Phase},
3    geometry::{Isometry, Point3},
4};
5
6use derive_more::{Deref, DerefMut};
7
8/// A pair of a focal point and a phase offset.
9#[derive(Clone, Copy, PartialEq, Debug, Default)]
10#[repr(C)]
11pub struct ControlPoint {
12    /// The focal point.
13    pub point: Point3,
14    /// The phase offset of the control point.
15    pub phase_offset: Phase,
16}
17
18impl ControlPoint {
19    /// Create a new [`ControlPoint`].
20    pub const fn new(point: Point3, phase_offset: Phase) -> Self {
21        Self {
22            point,
23            phase_offset,
24        }
25    }
26
27    pub(crate) fn transform(&self, iso: &Isometry) -> Self {
28        Self {
29            point: iso.transform_point(&self.point),
30            phase_offset: self.phase_offset,
31        }
32    }
33}
34
35impl From<Point3> for ControlPoint {
36    fn from(point: Point3) -> Self {
37        Self {
38            point,
39            ..Default::default()
40        }
41    }
42}
43
44impl From<&Point3> for ControlPoint {
45    fn from(point: &Point3) -> Self {
46        Self {
47            point: *point,
48            ..Default::default()
49        }
50    }
51}
52
53/// A collection of control points and the intensity of all control points.
54#[derive(Clone, PartialEq, Debug, Deref, DerefMut)]
55#[repr(C)]
56pub struct ControlPoints<const N: usize> {
57    #[deref]
58    #[deref_mut]
59    /// The control points.
60    pub points: [ControlPoint; N],
61    /// The intensity of all control points.
62    pub intensity: EmitIntensity,
63}
64
65impl<const N: usize> Default for ControlPoints<N> {
66    fn default() -> Self {
67        Self {
68            points: [Default::default(); N],
69            intensity: EmitIntensity::MAX,
70        }
71    }
72}
73
74impl<const N: usize> ControlPoints<N> {
75    /// Create a new [`ControlPoints`].
76    pub const fn new(points: [ControlPoint; N], intensity: EmitIntensity) -> Self {
77        Self { points, intensity }
78    }
79
80    pub(crate) fn transform(&self, iso: &nalgebra::Isometry3<f32>) -> Self {
81        Self {
82            points: self.points.map(|p| p.transform(iso)),
83            intensity: self.intensity,
84        }
85    }
86}
87
88impl<C> From<C> for ControlPoints<1>
89where
90    ControlPoint: From<C>,
91{
92    fn from(point: C) -> Self {
93        Self {
94            points: [point.into()],
95            ..Default::default()
96        }
97    }
98}
99
100impl<C, const N: usize> From<[C; N]> for ControlPoints<N>
101where
102    ControlPoint: From<C>,
103{
104    fn from(points: [C; N]) -> Self {
105        Self {
106            points: points.map(ControlPoint::from),
107            ..Default::default()
108        }
109    }
110}
111
112#[cfg(test)]
113mod tests {
114    use super::*;
115
116    #[test]
117    fn from_vector3() {
118        let v = Point3::new(1.0, 2.0, 3.0);
119        let cp = ControlPoint::from(v);
120        assert_eq!(v, cp.point);
121    }
122
123    #[test]
124    fn from_vector3_ref() {
125        let v = Point3::new(1.0, 2.0, 3.0);
126        let cp = ControlPoint::from(&v);
127        assert_eq!(v, cp.point);
128    }
129
130    #[test]
131    fn from_control_point() {
132        let v1 = Point3::new(1.0, 2.0, 3.0);
133        let v2 = Point3::new(4.0, 5.0, 6.0);
134        let cp = ControlPoints::from([v1, v2]);
135        assert_eq!(EmitIntensity::MAX, cp.intensity);
136        assert_eq!(v1, cp[0].point);
137        assert_eq!(v2, cp[1].point);
138    }
139}
autd3_driver/firmware/operation/stm/control_point.rs

autd3_driver/firmware/operation/stm/
control_point.rs
