autd3_protobuf/traits/driver/geometry/
mod.rs

1use crate::{AUTDProtoBufError, pb::*, traits::FromMessage};
2
3impl From<autd3_core::geometry::UnitVector3> for UnitVector3 {
4    fn from(value: autd3_core::geometry::UnitVector3) -> Self {
5        Self {
6            x: value.x,
7            y: value.y,
8            z: value.z,
9        }
10    }
11}
12
13impl From<autd3_core::geometry::Point3> for Point3 {
14    fn from(value: autd3_core::geometry::Point3) -> Self {
15        Self {
16            x: value.x,
17            y: value.y,
18            z: value.z,
19        }
20    }
21}
22
23impl From<autd3_core::geometry::UnitQuaternion> for Quaternion {
24    fn from(value: autd3_core::geometry::UnitQuaternion) -> Self {
25        Self {
26            w: value.w,
27            x: value.i,
28            y: value.j,
29            z: value.k,
30        }
31    }
32}
33
34impl From<&autd3_core::geometry::Geometry> for Geometry {
35    fn from(value: &autd3_core::geometry::Geometry) -> Self {
36        Self {
37            devices: value
38                .iter()
39                .map(|dev| geometry::Autd3 {
40                    pos: Some(dev[0].position().into()),
41                    rot: Some(dev.rotation().into()),
42                })
43                .collect(),
44        }
45    }
46}
47
48impl FromMessage<UnitVector3> for autd3_core::geometry::UnitVector3 {
49    fn from_msg(msg: UnitVector3) -> Result<Self, AUTDProtoBufError> {
50        Ok(autd3_core::geometry::UnitVector3::new_normalize(
51            autd3_core::geometry::Vector3::new(msg.x as _, msg.y as _, msg.z as _),
52        ))
53    }
54}
55
56impl FromMessage<Point3> for autd3_core::geometry::Point3 {
57    fn from_msg(msg: Point3) -> Result<Self, AUTDProtoBufError> {
58        Ok(autd3_core::geometry::Point3::new(
59            msg.x as _, msg.y as _, msg.z as _,
60        ))
61    }
62}
63
64impl FromMessage<Quaternion> for autd3_core::geometry::UnitQuaternion {
65    fn from_msg(msg: Quaternion) -> Result<Self, AUTDProtoBufError> {
66        Ok(autd3_core::geometry::UnitQuaternion::from_quaternion(
67            autd3_core::geometry::Quaternion::new(msg.w as _, msg.x as _, msg.y as _, msg.z as _),
68        ))
69    }
70}
71
72impl FromMessage<Geometry> for autd3_core::geometry::Geometry {
73    fn from_msg(msg: Geometry) -> Result<Self, AUTDProtoBufError> {
74        Ok(autd3_core::geometry::Geometry::new(
75            msg.devices
76                .into_iter()
77                .map(|dev_msg| {
78                    let pos = dev_msg
79                        .pos
80                        .map(autd3_core::geometry::Point3::from_msg)
81                        .transpose()?
82                        .unwrap_or(autd3_core::geometry::Point3::origin());
83                    let rot = dev_msg
84                        .rot
85                        .map(autd3_core::geometry::UnitQuaternion::from_msg)
86                        .transpose()?
87                        .unwrap_or(autd3_core::geometry::UnitQuaternion::identity());
88                    Ok(autd3_driver::autd3_device::AUTD3 { pos, rot }.into())
89                })
90                .collect::<Result<Vec<_>, AUTDProtoBufError>>()?,
91        ))
92    }
93}
94
95#[cfg(test)]
96mod tests {
97    use super::*;
98    use autd3_driver::{
99        autd3_device::AUTD3,
100        geometry::{Device, Geometry, Point3, Quaternion, UnitQuaternion, UnitVector3, Vector3},
101    };
102    use rand::Rng;
103
104    #[test]
105    fn point3() {
106        let mut rng = rand::rng();
107        let v = Point3::new(rng.random(), rng.random(), rng.random());
108        let msg = v.into();
109        let v2 = Point3::from_msg(msg).unwrap();
110        approx::assert_abs_diff_eq!(v.x, v2.x);
111        approx::assert_abs_diff_eq!(v.y, v2.y);
112        approx::assert_abs_diff_eq!(v.z, v2.z);
113    }
114
115    #[test]
116    fn unitvector3() {
117        let mut rng = rand::rng();
118        let v = UnitVector3::new_normalize(Vector3::new(rng.random(), rng.random(), rng.random()));
119        let msg = v.into();
120        let v2 = UnitVector3::from_msg(msg).unwrap();
121        approx::assert_abs_diff_eq!(v.x, v2.x);
122        approx::assert_abs_diff_eq!(v.y, v2.y);
123        approx::assert_abs_diff_eq!(v.z, v2.z);
124    }
125
126    #[test]
127    fn quaternion() {
128        let mut rng = rand::rng();
129        let q = UnitQuaternion::from_quaternion(Quaternion::new(
130            rng.random(),
131            rng.random(),
132            rng.random(),
133            rng.random(),
134        ));
135        let msg = q.into();
136        let q2 = UnitQuaternion::from_msg(msg).unwrap();
137        approx::assert_abs_diff_eq!(q.w, q2.w);
138        approx::assert_abs_diff_eq!(q.i, q2.i);
139        approx::assert_abs_diff_eq!(q.j, q2.j);
140        approx::assert_abs_diff_eq!(q.k, q2.k);
141    }
142
143    #[test]
144    fn geometry() {
145        let mut rng = rand::rng();
146        let dev: Device = AUTD3 {
147            pos: Point3::new(rng.random(), rng.random(), rng.random()),
148            rot: UnitQuaternion::identity(),
149        }
150        .into();
151        let geometry = Geometry::new(vec![dev]);
152        let msg = (&geometry).into();
153        let geometry2 = Geometry::from_msg(msg).unwrap();
154        geometry
155            .into_iter()
156            .zip(geometry2.iter())
157            .for_each(|(dev, dev2)| {
158                approx::assert_abs_diff_eq!(dev.rotation().w, dev2.rotation().w);
159                approx::assert_abs_diff_eq!(dev.rotation().i, dev2.rotation().i);
160                approx::assert_abs_diff_eq!(dev.rotation().j, dev2.rotation().j);
161                approx::assert_abs_diff_eq!(dev.rotation().k, dev2.rotation().k);
162                dev.iter().zip(dev2.iter()).for_each(|(t, t2)| {
163                    approx::assert_abs_diff_eq!(t.position().x, t2.position().x);
164                    approx::assert_abs_diff_eq!(t.position().y, t2.position().y);
165                    approx::assert_abs_diff_eq!(t.position().z, t2.position().z);
166                });
167            });
168    }
169}
autd3_protobuf/traits/driver/geometry/mod.rs

autd3_protobuf/traits/driver/geometry/
mod.rs
