41 lines
990 B
Rust
41 lines
990 B
Rust
use glam::{Mat3, Vec3};
|
|
|
|
pub fn ypr_to_mat(ypr: Vec3) -> Mat3 {
|
|
glam::Mat3::from_euler(glam::EulerRot::ZXZEx, ypr.x, ypr.y, ypr.z)
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use std::f32::consts::PI;
|
|
|
|
use approx::assert_ulps_eq;
|
|
use glam::vec3;
|
|
|
|
use super::*;
|
|
|
|
#[test]
|
|
fn test_ypr_to_mat() {
|
|
assert_ulps_eq!(ypr_to_mat(vec3(0., 0., 0.)), Mat3::IDENTITY, max_ulps = 3);
|
|
assert_ulps_eq!(
|
|
ypr_to_mat(vec3(PI / 2., 0., 0.)),
|
|
Mat3::from_cols_array_2d(&[[0., 1., 0.], [-1., 0., 0.], [0., 0., 1.]]),
|
|
max_ulps = 3,
|
|
);
|
|
assert_ulps_eq!(
|
|
ypr_to_mat(vec3(0., 0., PI / 2.)),
|
|
Mat3::from_cols_array_2d(&[[0., 1., 0.], [-1., 0., 0.], [0., 0., 1.]]),
|
|
max_ulps = 3,
|
|
);
|
|
assert_ulps_eq!(
|
|
ypr_to_mat(vec3(0., PI / 2., 0.)),
|
|
Mat3::from_cols_array_2d(&[[1., 0., 0.], [0., 0., 1.], [0., -1., 0.]]),
|
|
max_ulps = 3,
|
|
);
|
|
assert_ulps_eq!(
|
|
ypr_to_mat(vec3(PI / 2., PI / 2., 0.)),
|
|
Mat3::from_cols_array_2d(&[[0., 0., 1.], [-1., 0., 0.], [0., -1., 0.]]),
|
|
max_ulps = 3,
|
|
);
|
|
}
|
|
}
|