1use nalgebra::{Point3, Quaternion, UnitQuaternion};
2use serde::de::Error as _;
3use serde::{Deserialize, Deserializer, Serialize, Serializer};
4use thiserror::Error;
5
6use super::{Autd3, Device, Geometry};
7
8const ROTATION_NORM_TOLERANCE: f32 = 1e-3;
9
10#[derive(Debug, Error)]
11#[error("failed to convert the geometry layout: {0}")]
12pub struct LayoutError(#[from] serde_json::Error);
13
14const fn identity_rotation() -> [f32; 4] {
15 [1.0, 0.0, 0.0, 0.0]
16}
17
18#[derive(Serialize, Deserialize)]
19#[serde(deny_unknown_fields)]
20struct Autd3Repr {
21 origin: [f32; 3],
22 #[serde(default = "identity_rotation")]
23 rotation: [f32; 4],
24}
25
26impl Serialize for Autd3 {
27 fn serialize<S: Serializer>(&self, serializer: S) -> Result<S::Ok, S::Error> {
28 Autd3Repr {
29 origin: [self.origin.x, self.origin.y, self.origin.z],
30 rotation: [
31 self.rotation.w,
32 self.rotation.i,
33 self.rotation.j,
34 self.rotation.k,
35 ],
36 }
37 .serialize(serializer)
38 }
39}
40
41impl<'de> Deserialize<'de> for Autd3 {
42 fn deserialize<D: Deserializer<'de>>(deserializer: D) -> Result<Self, D::Error> {
43 let repr = Autd3Repr::deserialize(deserializer)?;
44 let rotation = Quaternion::new(
45 repr.rotation[0],
46 repr.rotation[1],
47 repr.rotation[2],
48 repr.rotation[3],
49 );
50 let norm = rotation.norm();
51 if (norm - 1.0).abs() > ROTATION_NORM_TOLERANCE {
52 return Err(D::Error::custom(format!(
53 "`rotation` must be a unit quaternion [w, x, y, z], but its norm is {norm}"
54 )));
55 }
56 Ok(Self::new(
57 Point3::new(repr.origin[0], repr.origin[1], repr.origin[2]),
58 UnitQuaternion::from_quaternion(rotation),
59 ))
60 }
61}
62
63impl From<&Device> for Autd3 {
64 fn from(device: &Device) -> Self {
65 Self::new(device.position(0), device.rotation())
66 }
67}
68
69impl Geometry {
70 pub fn from_json(json: &str) -> Result<Self, LayoutError> {
71 Ok(Self::new(serde_json::from_str::<Vec<Autd3>>(json)?))
72 }
73
74 pub fn to_json(&self) -> Result<String, LayoutError> {
75 let devices: Vec<Autd3> = self.iter().map(Autd3::from).collect();
76 Ok(serde_json::to_string_pretty(&devices)?)
77 }
78}
79
80#[cfg(test)]
81mod tests {
82 use super::*;
83
84 const CROSS_LANGUAGE_FIXTURE: &str = r#"[
85 { "origin": [0.0, 0.0, 0.0] },
86 { "origin": [192.0, 0.0, 0.0], "rotation": [0.7071068, 0.0, 0.7071068, 0.0] }
87]"#;
88
89 fn sample() -> Geometry {
90 Geometry::new(vec![
91 Autd3::default(),
92 Autd3::new(
93 Point3::new(192.0, 0.0, 0.0),
94 UnitQuaternion::from_axis_angle(
95 &nalgebra::Vector3::y_axis(),
96 core::f32::consts::FRAC_PI_2,
97 ),
98 ),
99 ])
100 }
101
102 fn assert_geometry_eq(expected: &Geometry, actual: &Geometry) {
103 assert_eq!(expected.num_devices(), actual.num_devices());
104 for (expected, actual) in expected.iter().zip(actual.iter()) {
105 approx::assert_abs_diff_eq!(expected.position(0), actual.position(0), epsilon = 1e-6);
106 approx::assert_abs_diff_eq!(
107 expected.rotation().angle_to(&actual.rotation()),
108 0.0,
109 epsilon = 1e-6
110 );
111 }
112 }
113
114 #[test]
115 fn a_geometry_serializes_to_the_documented_shape() {
116 let geometry = Geometry::new(vec![Autd3::new(
117 Point3::new(1.0, 2.0, 3.0),
118 UnitQuaternion::identity(),
119 )]);
120 assert_eq!(
121 serde_json::json!([
122 { "origin": [1.0, 2.0, 3.0], "rotation": [1.0, 0.0, 0.0, 0.0] }
123 ]),
124 serde_json::from_str::<serde_json::Value>(&geometry.to_json().unwrap()).unwrap()
125 );
126 }
127
128 #[test]
129 fn json_round_trip_preserves_the_placement() {
130 let geometry = sample();
131 let restored = Geometry::from_json(&geometry.to_json().unwrap()).unwrap();
132 assert_geometry_eq(&geometry, &restored);
133 }
134
135 #[test]
136 fn rotation_defaults_to_the_identity_quaternion() {
137 let geometry = Geometry::from_json(r#"[{"origin":[0,0,0]}]"#).unwrap();
138 assert_eq!(UnitQuaternion::identity(), geometry[0].rotation());
139 }
140
141 #[test]
142 fn the_cross_language_fixture_places_the_devices_identically() {
143 let geometry = Geometry::from_json(CROSS_LANGUAGE_FIXTURE).unwrap();
144 assert_eq!(2, geometry.num_devices());
145 assert_eq!(0, geometry[0].idx());
146 assert_eq!(1, geometry[1].idx());
147 assert_eq!(Point3::origin(), geometry[0].position(0));
148 assert_eq!(Point3::new(192.0, 0.0, 0.0), geometry[1].position(0));
149 approx::assert_abs_diff_eq!(
150 Point3::new(192.0, 0.0, -Autd3::PITCH_MM),
151 geometry[1].position(1),
152 epsilon = 1e-3
153 );
154 approx::assert_abs_diff_eq!(
155 nalgebra::Vector3::x(),
156 geometry[1].axial_direction().into_inner(),
157 epsilon = 1e-3
158 );
159 }
160
161 #[test]
162 fn an_unknown_field_is_rejected() {
163 let err = Geometry::from_json(r#"[{"origin":[0,0,0],"rotaton":[1,0,0,0]}]"#)
164 .expect_err("must fail");
165 assert!(err.to_string().contains("rotaton"), "{err}");
166 }
167
168 #[test]
169 fn a_non_normalized_rotation_is_rejected() {
170 let err = Geometry::from_json(r#"[{"origin":[0,0,0],"rotation":[1,1,0,0]}]"#)
171 .expect_err("must fail");
172 assert!(err.to_string().contains("unit quaternion"), "{err}");
173 }
174
175 #[test]
176 fn a_zero_rotation_is_rejected() {
177 let err = Geometry::from_json(r#"[{"origin":[0,0,0],"rotation":[0,0,0,0]}]"#)
178 .expect_err("must fail");
179 assert!(err.to_string().contains("unit quaternion"), "{err}");
180 }
181
182 #[test]
183 fn a_wrong_length_origin_is_rejected() {
184 let err = Geometry::from_json(r#"[{"origin":[0,0]}]"#).expect_err("must fail");
185 assert!(err.to_string().contains("length"), "{err}");
186 }
187
188 #[test]
189 fn a_wrong_length_rotation_is_rejected() {
190 let err = Geometry::from_json(r#"[{"origin":[0,0,0],"rotation":[1,0,0]}]"#)
191 .expect_err("must fail");
192 assert!(err.to_string().contains("length"), "{err}");
193 }
194}