Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion applications/autoware/common_types/src/lib.rs
Original file line number Diff line number Diff line change
Expand Up @@ -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};
40 changes: 38 additions & 2 deletions applications/autoware/common_types/src/transform.rs
Original file line number Diff line number Diff line change
@@ -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]
Expand Down Expand Up @@ -104,6 +103,11 @@ impl Transform {
pub fn translate(&self, vec: &Vector3<f64>) -> Vector3<f64> {
vec + self.translation
}

#[inline]
pub fn to_isometry(&self) -> Isometry3<f64> {
Isometry3::from_parts(self.translation.into(), self.rotation)
}
}

impl Default for Transform {
Expand Down Expand Up @@ -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);
}
}
}
Loading