diff --git a/applications/autoware/common_types/src/lib.rs b/applications/autoware/common_types/src/lib.rs index 2f5859597..b4099344d 100644 --- a/applications/autoware/common_types/src/lib.rs +++ b/applications/autoware/common_types/src/lib.rs @@ -24,4 +24,4 @@ pub struct Header { pub timestamp: u64, } -pub use nalgebra::{Quaternion, UnitQuaternion, Vector3}; +pub use nalgebra::{Isometry3, Matrix4, Quaternion, UnitQuaternion, Vector3, Vector4}; diff --git a/applications/autoware/common_types/src/transform.rs b/applications/autoware/common_types/src/transform.rs index 9f2136dc7..e74b4ed87 100644 --- a/applications/autoware/common_types/src/transform.rs +++ b/applications/autoware/common_types/src/transform.rs @@ -1,6 +1,5 @@ -use crate::{Header, Quaternion, UnitQuaternion, Vector3}; +use crate::{Header, Isometry3, Quaternion, UnitQuaternion, Vector3, Vector4}; use alloc::{format, string::String}; -use nalgebra::Vector4; #[derive(Clone, Debug)] #[non_exhaustive] @@ -104,6 +103,11 @@ impl Transform { pub fn translate(&self, vec: &Vector3) -> Vector3 { vec + self.translation } + + #[inline] + pub fn to_isometry(&self) -> Isometry3 { + Isometry3::from_parts(self.translation.into(), self.rotation) + } } impl Default for Transform { @@ -424,4 +428,36 @@ mod tests { assert_almost_eq(transformed.y, 3.0); assert_almost_eq(transformed.z, 3.0); } + + #[test] + fn to_isometry_matches_apply_and_homogeneous_matrix() { + let half_sqrt_two = core::f64::consts::FRAC_1_SQRT_2; + let translation = Vector3::new(1.0, 2.0, 3.0); + let rotation = Quaternion::new(half_sqrt_two, 0.0, 0.0, half_sqrt_two); + let transform = Transform::try_new(translation, rotation).unwrap(); + + let isometry = transform.to_isometry(); + assert_eq!(isometry.translation.vector, translation); + assert_almost_eq(isometry.rotation.angle_to(&transform.rotation), 0.0); + + let point = Vector3::new(1.0, -2.0, 0.5); + let expected = transform.apply(&point); + let transformed = isometry.transform_point(&point.into()); + assert_almost_eq(transformed.x, expected.x); + assert_almost_eq(transformed.y, expected.y); + assert_almost_eq(transformed.z, expected.z); + + // 90 degrees about z: [x, y] -> [-y, x], translation in the last column. + #[rustfmt::skip] + let expected_matrix = nalgebra::Matrix4::new( + 0.0, -1.0, 0.0, 1.0, + 1.0, 0.0, 0.0, 2.0, + 0.0, 0.0, 1.0, 3.0, + 0.0, 0.0, 0.0, 1.0, + ); + let matrix = isometry.to_homogeneous(); + for (actual, expected) in matrix.iter().zip(expected_matrix.iter()) { + assert_almost_eq(*actual, *expected); + } + } }