fast_gicp/
transform.rs

1//! 3D transformation utilities
2
3use fast_gicp_sys::ffi;
4use nalgebra::{Isometry3, Matrix3, Matrix4, Vector3};
5
6/// 3D transformation matrix (4x4 homogeneous transformation)
7#[derive(Debug, Clone, Copy, PartialEq)]
8pub struct Transform3f {
9    /// 4x4 transformation matrix in row-major order
10    pub matrix: [[f32; 4]; 4],
11}
12
13impl Transform3f {
14    /// Create an identity transformation
15    ///
16    /// # Examples
17    #[cfg_attr(feature = "docs-only", doc = "```no_run")]
18    #[cfg_attr(not(feature = "docs-only"), doc = "```")]
19    /// use fast_gicp::Transform3f;
20    ///
21    /// let transform = Transform3f::identity();
22    /// assert_eq!(transform.matrix[0][0], 1.0);
23    /// assert_eq!(transform.matrix[3][3], 1.0);
24    /// ```
25    pub fn identity() -> Self {
26        let ffi_transform = ffi::transform_identity();
27        Self::from_transform4f(&ffi_transform)
28    }
29
30    /// Create transformation from flat array (row-major order)
31    ///
32    /// # Examples
33    #[cfg_attr(feature = "docs-only", doc = "```no_run")]
34    #[cfg_attr(not(feature = "docs-only"), doc = "```")]
35    /// use fast_gicp::Transform3f;
36    ///
37    /// let data = [
38    ///     1.0, 0.0, 0.0, 1.0,
39    ///     0.0, 1.0, 0.0, 2.0,
40    ///     0.0, 0.0, 1.0, 3.0,
41    ///     0.0, 0.0, 0.0, 1.0,
42    /// ];
43    /// let transform = Transform3f::from_flat(&data);
44    /// assert_eq!(transform.matrix[0][3], 1.0); // x translation
45    /// assert_eq!(transform.matrix[1][3], 2.0); // y translation
46    /// assert_eq!(transform.matrix[2][3], 3.0); // z translation
47    /// ```
48    pub fn from_flat(data: &[f32; 16]) -> Self {
49        let mut matrix = [[0.0; 4]; 4];
50        for i in 0..4 {
51            for j in 0..4 {
52                matrix[i][j] = data[i * 4 + j];
53            }
54        }
55        Self { matrix }
56    }
57
58    /// Convert to flat array (row-major order)
59    pub fn to_flat(&self) -> [f32; 16] {
60        let mut data = [0.0; 16];
61        for i in 0..4 {
62            for j in 0..4 {
63                data[i * 4 + j] = self.matrix[i][j];
64            }
65        }
66        data
67    }
68
69    /// Create transformation from translation
70    pub fn from_translation(x: f32, y: f32, z: f32) -> Self {
71        let transform4f = ffi::transform_from_translation(x, y, z);
72        Self::from_transform4f(&transform4f)
73    }
74
75    /// Creates a transformation from a translation vector.
76    pub fn from_translation_array(translation: [f32; 3]) -> Self {
77        Self::from_translation(translation[0], translation[1], translation[2])
78    }
79
80    /// Get translation component
81    pub fn translation(&self) -> Vector3<f32> {
82        Vector3::new(self.matrix[0][3], self.matrix[1][3], self.matrix[2][3])
83    }
84
85    /// Set translation component
86    pub fn set_translation(&mut self, x: f32, y: f32, z: f32) {
87        self.matrix[0][3] = x;
88        self.matrix[1][3] = y;
89        self.matrix[2][3] = z;
90    }
91
92    /// Get rotation matrix (3x3 upper-left block)
93    pub fn rotation(&self) -> Matrix3<f32> {
94        Matrix3::new(
95            self.matrix[0][0],
96            self.matrix[0][1],
97            self.matrix[0][2],
98            self.matrix[1][0],
99            self.matrix[1][1],
100            self.matrix[1][2],
101            self.matrix[2][0],
102            self.matrix[2][1],
103            self.matrix[2][2],
104        )
105    }
106
107    /// Set rotation matrix (3x3 upper-left block)
108    pub fn set_rotation(&mut self, rotation: [[f32; 3]; 3]) {
109        for (i, row) in rotation.iter().enumerate() {
110            for (j, &val) in row.iter().enumerate() {
111                self.matrix[i][j] = val;
112            }
113        }
114    }
115
116    /// Create transformation from rotation matrix and translation
117    pub fn from_rotation_translation(rotation: [[f32; 3]; 3], translation: [f32; 3]) -> Self {
118        let mut transform = Self::identity();
119        transform.set_rotation(rotation);
120        transform.set_translation(translation[0], translation[1], translation[2]);
121        transform
122    }
123
124    /// Create transformation from nalgebra translation and rotation
125    pub fn from_parts(translation: Vector3<f32>, rotation: Matrix3<f32>) -> Self {
126        let mut transform = Self::identity();
127
128        // Set rotation from Matrix3
129        for i in 0..3 {
130            for j in 0..3 {
131                transform.matrix[i][j] = rotation[(i, j)];
132            }
133        }
134
135        // Set translation
136        transform.matrix[0][3] = translation.x;
137        transform.matrix[1][3] = translation.y;
138        transform.matrix[2][3] = translation.z;
139
140        transform
141    }
142
143    /// Create transformation from a rotation matrix
144    pub fn from_rotation_matrix(rotation: &Matrix3<f32>) -> Self {
145        Self::from_parts(Vector3::zeros(), *rotation)
146    }
147
148    /// Multiply two transformations
149    pub fn multiply(&self, other: &Transform3f) -> Self {
150        let a = self.as_transform4f();
151        let b = other.as_transform4f();
152        let result = ffi::transform_multiply(&a, &b);
153        Self::from_transform4f(&result)
154    }
155
156    /// Composes this transformation with another (self * other).
157    pub fn compose(&self, other: &Transform3f) -> Transform3f {
158        self.multiply(other)
159    }
160
161    /// Compute inverse transformation
162    pub fn inverse(&self) -> Self {
163        let transform4f = self.as_transform4f();
164        let result = ffi::transform_inverse(&transform4f);
165        Self::from_transform4f(&result)
166    }
167
168    /// Transform a 3D point
169    pub fn transform_point(&self, point: &Vector3<f32>) -> Vector3<f32> {
170        let x = point.x;
171        let y = point.y;
172        let z = point.z;
173        let tx = self.matrix[0][0] * x
174            + self.matrix[0][1] * y
175            + self.matrix[0][2] * z
176            + self.matrix[0][3];
177        let ty = self.matrix[1][0] * x
178            + self.matrix[1][1] * y
179            + self.matrix[1][2] * z
180            + self.matrix[1][3];
181        let tz = self.matrix[2][0] * x
182            + self.matrix[2][1] * y
183            + self.matrix[2][2] * z
184            + self.matrix[2][3];
185        Vector3::new(tx, ty, tz)
186    }
187
188    /// Transform a 3D point array
189    pub fn transform_point_array(&self, point: [f32; 3]) -> [f32; 3] {
190        let v = self.transform_point(&Vector3::new(point[0], point[1], point[2]));
191        [v.x, v.y, v.z]
192    }
193
194    /// Create from nalgebra Isometry3
195    pub fn from_isometry(isometry: &Isometry3<f32>) -> Self {
196        let matrix = isometry.to_homogeneous();
197        let mut flat = [0.0f32; 16];
198
199        // Convert column-major nalgebra matrix to row-major
200        for i in 0..4 {
201            for j in 0..4 {
202                flat[i * 4 + j] = matrix[(i, j)];
203            }
204        }
205
206        Self::from_flat(&flat)
207    }
208
209    /// Convert to nalgebra Isometry3
210    pub fn to_isometry(&self) -> Isometry3<f32> {
211        let mut matrix = Matrix4::<f32>::zeros();
212
213        // Convert row-major to column-major
214        for i in 0..4 {
215            for j in 0..4 {
216                matrix[(i, j)] = self.matrix[i][j];
217            }
218        }
219
220        // Extract rotation and translation from the matrix
221        let rotation = matrix.fixed_view::<3, 3>(0, 0);
222        let translation = matrix.fixed_view::<3, 1>(0, 3);
223
224        Isometry3::from_parts(
225            nalgebra::Translation3::from(nalgebra::Vector3::new(
226                translation[(0, 0)],
227                translation[(1, 0)],
228                translation[(2, 0)],
229            )),
230            nalgebra::UnitQuaternion::from_matrix(&rotation.into()),
231        )
232    }
233
234    /// Get the raw 4x4 matrix in row-major order
235    pub fn matrix_data(&self) -> &[[f32; 4]; 4] {
236        &self.matrix
237    }
238
239    /// Convert to nalgebra Matrix4
240    pub fn to_matrix(&self) -> Matrix4<f32> {
241        Matrix4::from_fn(|i, j| self.matrix[i][j])
242    }
243
244    /// Create from nalgebra Matrix4
245    pub fn from_matrix(matrix: &Matrix4<f32>) -> Self {
246        let mut transform = Self::identity();
247        for i in 0..4 {
248            for j in 0..4 {
249                transform.matrix[i][j] = matrix[(i, j)];
250            }
251        }
252        transform
253    }
254
255    /// Convert to FFI Transform4f
256    pub(crate) fn as_transform4f(&self) -> ffi::Transform4f {
257        ffi::Transform4f {
258            data: self.to_flat(),
259        }
260    }
261
262    /// Create from FFI Transform4f
263    pub(crate) fn from_transform4f(transform: &ffi::Transform4f) -> Self {
264        Self::from_flat(&transform.data)
265    }
266}
267
268impl Default for Transform3f {
269    fn default() -> Self {
270        Self::identity()
271    }
272}
273
274impl std::ops::Mul for Transform3f {
275    type Output = Transform3f;
276
277    fn mul(self, rhs: Transform3f) -> Self::Output {
278        self.multiply(&rhs)
279    }
280}
281
282impl std::ops::MulAssign for Transform3f {
283    fn mul_assign(&mut self, rhs: Transform3f) {
284        *self = self.multiply(&rhs);
285    }
286}
287
288// Conversion for nalgebra interop
289impl From<nalgebra::Matrix4<f32>> for Transform3f {
290    fn from(m: nalgebra::Matrix4<f32>) -> Self {
291        let mut flat = [0.0f32; 16];
292        for i in 0..4 {
293            for j in 0..4 {
294                flat[i * 4 + j] = m[(i, j)];
295            }
296        }
297        Self::from_flat(&flat)
298    }
299}
300
301impl From<Transform3f> for nalgebra::Matrix4<f32> {
302    fn from(t: Transform3f) -> Self {
303        let mut matrix = nalgebra::Matrix4::zeros();
304        for i in 0..4 {
305            for j in 0..4 {
306                matrix[(i, j)] = t.matrix[i][j];
307            }
308        }
309        matrix
310    }
311}
312
313impl From<Isometry3<f32>> for Transform3f {
314    fn from(isometry: Isometry3<f32>) -> Self {
315        Self::from_isometry(&isometry)
316    }
317}
318
319impl From<&Transform3f> for Isometry3<f32> {
320    fn from(transform: &Transform3f) -> Self {
321        transform.to_isometry()
322    }
323}
324
325impl From<Transform3f> for Isometry3<f32> {
326    fn from(transform: Transform3f) -> Self {
327        transform.to_isometry()
328    }
329}
330
331#[cfg(test)]
332mod tests {
333    use super::*;
334
335    #[test]
336    fn test_identity() {
337        let identity = Transform3f::identity();
338        assert_eq!(identity.translation(), Vector3::new(0.0, 0.0, 0.0));
339
340        let point = Vector3::new(1.0, 2.0, 3.0);
341        let transformed = identity.transform_point(&point);
342        assert_eq!(transformed, point);
343    }
344
345    #[test]
346    fn test_translation() {
347        let transform = Transform3f::from_translation(1.0, 2.0, 3.0);
348        assert_eq!(transform.translation(), Vector3::new(1.0, 2.0, 3.0));
349
350        let point = Vector3::new(0.0, 0.0, 0.0);
351        let transformed = transform.transform_point(&point);
352        assert_eq!(transformed, Vector3::new(1.0, 2.0, 3.0));
353    }
354
355    #[test]
356    fn test_translation_array() {
357        let translation = [1.0, 2.0, 3.0];
358        let transform = Transform3f::from_translation_array(translation);
359        assert_eq!(transform.translation(), Vector3::new(1.0, 2.0, 3.0));
360
361        let rotation = transform.rotation();
362        let expected_rotation = Matrix3::identity();
363        assert_eq!(rotation, expected_rotation);
364    }
365
366    #[test]
367    fn test_inverse() {
368        let transform = Transform3f::from_translation(1.0, 2.0, 3.0);
369        let inverse = transform.inverse();
370        let identity = transform.multiply(&inverse);
371
372        // Check if result is approximately identity
373        let point = Vector3::new(5.0, 6.0, 7.0);
374        let original = identity.transform_point(&point);
375        assert!((original.x - point.x).abs() < 1e-6);
376        assert!((original.y - point.y).abs() < 1e-6);
377        assert!((original.z - point.z).abs() < 1e-6);
378    }
379
380    #[test]
381    fn test_flat_conversion() {
382        let identity = Transform3f::identity();
383        let flat = identity.to_flat();
384        let restored = Transform3f::from_flat(&flat);
385        assert_eq!(identity, restored);
386    }
387
388    #[test]
389    fn test_nalgebra_conversion() {
390        let translation = Vector3::new(1.0, 2.0, 3.0);
391        let rotation = nalgebra::UnitQuaternion::from_euler_angles(0.1, 0.2, 0.3);
392        let isometry = Isometry3::from_parts(translation.into(), rotation);
393
394        let transform = Transform3f::from_isometry(&isometry);
395        let back_to_isometry = transform.to_isometry();
396
397        // Check that the round-trip preserves the transformation (within floating point precision)
398        let diff = (isometry.to_homogeneous() - back_to_isometry.to_homogeneous()).abs();
399        for i in 0..4 {
400            for j in 0..4 {
401                assert!(
402                    diff[(i, j)] < 1e-6,
403                    "Matrix element ({}, {}) differs by {}",
404                    i,
405                    j,
406                    diff[(i, j)]
407                );
408            }
409        }
410    }
411
412    #[test]
413    fn test_transform_composition() {
414        let t1 = Transform3f::from_translation(1.0, 0.0, 0.0);
415        let t2 = Transform3f::from_translation(0.0, 1.0, 0.0);
416        let composed = t1.compose(&t2);
417
418        let expected_translation = Vector3::new(1.0, 1.0, 0.0);
419        let actual_translation = composed.translation();
420
421        assert!((actual_translation - expected_translation).norm() < 1e-6);
422    }
423
424    #[test]
425    fn test_transform_inverse() {
426        let translation = [1.0, 2.0, 3.0];
427        let transform = Transform3f::from_translation_array(translation);
428        let inverse = transform.inverse();
429        let identity = transform.compose(&inverse);
430
431        let result_translation = identity.translation();
432        assert!(
433            result_translation.norm() < 1e-6,
434            "Translation should be near zero, got {result_translation:?}"
435        );
436    }
437}
fast_gicp/transform.rs

fast_gicp/
transform.rs
