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,
		);
	}
}