1use bincode::{Decode, Encode};
2use core::fmt::Debug;
3use serde::{Deserialize, Serialize};
4use uom::si::angle::radian;
5use uom::si::length::meter;
6pub type Pose<T> = Transform3D<T>;
7use uom::si::f32::Angle as Angle32;
8use uom::si::f32::Length as Length32;
9use uom::si::f64::Angle as Angle64;
10use uom::si::f64::Length as Length64;
11
12#[derive(Debug, Clone, Encode, Decode, Serialize, Deserialize)]
13pub struct Transform3D<T: Copy + Debug + 'static> {
14 pub mat: [[T; 4]; 4],
15}
16
17impl Transform3D<f64> {
18 pub fn translation(&self) -> [Length64; 3] {
19 [
20 Length64::new::<meter>(self.mat[0][3]),
21 Length64::new::<meter>(self.mat[1][3]),
22 Length64::new::<meter>(self.mat[2][3]),
23 ]
24 }
25
26 pub fn rotation(&self) -> [[Angle64; 3]; 3] {
27 [
28 [
29 Angle64::new::<radian>(self.mat[0][0]),
30 Angle64::new::<radian>(self.mat[0][1]),
31 Angle64::new::<radian>(self.mat[0][2]),
32 ],
33 [
34 Angle64::new::<radian>(self.mat[1][0]),
35 Angle64::new::<radian>(self.mat[1][1]),
36 Angle64::new::<radian>(self.mat[1][2]),
37 ],
38 [
39 Angle64::new::<radian>(self.mat[2][0]),
40 Angle64::new::<radian>(self.mat[2][1]),
41 Angle64::new::<radian>(self.mat[2][2]),
42 ],
43 ]
44 }
45}
46
47impl Transform3D<f32> {
48 pub fn translation(&self) -> [Length32; 3] {
49 [
50 Length32::new::<meter>(self.mat[0][3]),
51 Length32::new::<meter>(self.mat[1][3]),
52 Length32::new::<meter>(self.mat[2][3]),
53 ]
54 }
55
56 pub fn rotation(&self) -> [[Angle32; 3]; 3] {
57 [
58 [
59 Angle32::new::<radian>(self.mat[0][0]),
60 Angle32::new::<radian>(self.mat[0][1]),
61 Angle32::new::<radian>(self.mat[0][2]),
62 ],
63 [
64 Angle32::new::<radian>(self.mat[1][0]),
65 Angle32::new::<radian>(self.mat[1][1]),
66 Angle32::new::<radian>(self.mat[1][2]),
67 ],
68 [
69 Angle32::new::<radian>(self.mat[2][0]),
70 Angle32::new::<radian>(self.mat[2][1]),
71 Angle32::new::<radian>(self.mat[2][2]),
72 ],
73 ]
74 }
75}
76
77impl<T: Copy + Debug + Default> Default for Transform3D<T> {
78 fn default() -> Self {
79 Self {
80 mat: [[T::default(); 4]; 4],
81 }
82 }
83}
84
85#[cfg(feature = "faer")]
86mod faer_integration {
87 use super::Transform3D;
88 use faer::prelude::*;
89
90 impl From<&Transform3D<f64>> for Mat<f64> {
91 fn from(p: &Transform3D<f64>) -> Self {
92 let mut mat: Mat<f64> = Mat::zeros(4, 4);
93 for r in 0..4 {
94 for c in 0..4 {
95 *mat.get_mut(r, c) = p.mat[r][c];
96 }
97 }
98 mat
99 }
100 }
101
102 impl From<Mat<f64>> for Transform3D<f64> {
103 fn from(mat: Mat<f64>) -> Self {
104 assert_eq!(mat.nrows(), 4);
105 assert_eq!(mat.ncols(), 4);
106 let mut transform = [[0.0; 4]; 4];
107 for (r, row) in transform.iter_mut().enumerate() {
108 for (c, val) in row.iter_mut().enumerate() {
109 *val = *mat.get(r, c);
110 }
111 }
112 Self { mat: transform }
113 }
114 }
115}
116
117#[cfg(feature = "nalgebra")]
119mod nalgebra_integration {
120 use super::Transform3D;
121 use nalgebra::{Isometry3, Matrix3, Matrix4, Rotation3, Translation3, Vector3};
122
123 impl From<&Transform3D<f64>> for Isometry3<f64> {
124 fn from(pose: &Transform3D<f64>) -> Self {
125 let flat_transform: [f64; 16] = std::array::from_fn(|i| pose.mat[i / 4][i % 4]);
126 let matrix = Matrix4::from_row_slice(&flat_transform);
127
128 let rotation_matrix: Matrix3<f64> = matrix.fixed_view::<3, 3>(0, 0).into();
129 let rotation = Rotation3::from_matrix_unchecked(rotation_matrix);
130
131 let translation_vector: Vector3<f64> = matrix.fixed_view::<3, 1>(0, 3).into();
132 let translation = Translation3::from(translation_vector);
133
134 Isometry3::from_parts(translation, rotation.into())
135 }
136 }
137
138 impl From<Isometry3<f64>> for Transform3D<f64> {
139 fn from(iso: Isometry3<f64>) -> Self {
140 let matrix = iso.to_homogeneous();
141 let transform = std::array::from_fn(|r| std::array::from_fn(|c| matrix[(r, c)]));
142 Transform3D { mat: transform }
143 }
144 }
145}
146
147#[cfg(feature = "glam")]
148mod glam_integration {
149 use super::Transform3D;
150 use glam::DAffine3;
151
152 impl From<Transform3D<f64>> for DAffine3 {
153 fn from(p: Transform3D<f64>) -> Self {
154 let mut aff = DAffine3::IDENTITY;
155 aff.matrix3.x_axis.x = p.mat[0][0];
156 aff.matrix3.x_axis.y = p.mat[0][1];
157 aff.matrix3.x_axis.z = p.mat[0][2];
158
159 aff.matrix3.y_axis.x = p.mat[1][0];
160 aff.matrix3.y_axis.y = p.mat[1][1];
161 aff.matrix3.y_axis.z = p.mat[1][2];
162
163 aff.matrix3.z_axis.x = p.mat[2][0];
164 aff.matrix3.z_axis.y = p.mat[2][1];
165 aff.matrix3.z_axis.z = p.mat[2][2];
166
167 aff.translation.x = p.mat[0][3];
168 aff.translation.y = p.mat[1][3];
169 aff.translation.z = p.mat[2][3];
170
171 aff
172 }
173 }
174
175 impl From<DAffine3> for Transform3D<f64> {
176 fn from(aff: DAffine3) -> Self {
177 let mut transform = [[0.0f64; 4]; 4];
178
179 transform[0][0] = aff.matrix3.x_axis.x;
180 transform[0][1] = aff.matrix3.x_axis.y;
181 transform[0][2] = aff.matrix3.x_axis.z;
182
183 transform[1][0] = aff.matrix3.y_axis.x;
184 transform[1][1] = aff.matrix3.y_axis.y;
185 transform[1][2] = aff.matrix3.y_axis.z;
186
187 transform[2][0] = aff.matrix3.z_axis.x;
188 transform[2][1] = aff.matrix3.z_axis.y;
189 transform[2][2] = aff.matrix3.z_axis.z;
190
191 transform[0][3] = aff.translation.x;
192 transform[1][3] = aff.translation.y;
193 transform[2][3] = aff.translation.z;
194 transform[3][3] = 1.0;
195
196 Transform3D { mat: transform }
197 }
198 }
199}
200
201#[cfg(feature = "nalgebra")]
202#[allow(unused_imports)]
203pub use nalgebra_integration::*;
204
205#[cfg(feature = "faer")]
206#[allow(unused_imports)]
207pub use faer_integration::*;
208
209#[cfg(test)]
210mod tests {
211 use super::*;
212
213 #[test]
214 fn test_pose_default() {
215 let pose: Transform3D<f32> = Transform3D::default();
216 assert_eq!(
217 pose.mat, [[0.0; 4]; 4],
218 "Default pose should be a zero matrix"
219 );
220 }
221
222 #[cfg(feature = "faer")]
223 #[test]
224 fn test_pose_faer_conversion() {
225 use faer::prelude::*;
226
227 let pose = Transform3D {
228 mat: [
229 [1.0, 2.0, 3.0, 4.0],
230 [5.0, 6.0, 7.0, 8.0],
231 [9.0, 10.0, 11.0, 12.0],
232 [13.0, 14.0, 15.0, 16.0],
233 ],
234 };
235
236 let mat: Mat<f64> = (&pose).into();
237 let pose_from_mat = Transform3D::from(mat);
238
239 assert_eq!(
240 pose.mat, pose_from_mat.mat,
241 "Faer conversion should be lossless"
242 );
243 }
244
245 #[cfg(feature = "nalgebra")]
246 #[test]
247 fn test_pose_nalgebra_conversion() {
248 use nalgebra::Isometry3;
249
250 let pose = Transform3D {
251 mat: [
252 [1.0, 0.0, 0.0, 2.0],
253 [0.0, 1.0, 0.0, 3.0],
254 [0.0, 0.0, 1.0, 4.0],
255 [0.0, 0.0, 0.0, 1.0],
256 ],
257 };
258
259 let iso: Isometry3<f64> = (&pose.clone()).into();
260 let pose_from_iso: Transform3D<f64> = iso.into();
261
262 assert_eq!(
263 pose.mat, pose_from_iso.mat,
264 "Nalgebra conversion should be lossless"
265 );
266 }
267
268 #[cfg(feature = "glam")]
269 #[test]
270 fn test_pose_glam_conversion() {
271 use glam::DAffine3;
272
273 let orig_pose = Transform3D {
274 mat: [
275 [1.0, 0.0, 0.0, 5.0],
276 [0.0, 1.0, 0.0, 6.0],
277 [0.0, 0.0, 1.0, 7.0],
278 [0.0, 0.0, 0.0, 1.0],
279 ],
280 };
281 let pose = orig_pose.clone();
282 assert_eq!(pose.mat, orig_pose.mat);
283 let aff: DAffine3 = pose.into();
284 assert_eq!(aff.translation[0], 5.0);
285 let pose_from_aff: Transform3D<f64> = aff.into();
286
287 assert_eq!(
288 orig_pose.mat, pose_from_aff.mat,
289 "Glam conversion should be lossless"
290 );
291 }
292}