Merge pull request #1179 from timethy/euler-angles-patch

Fix typo in `euler_angles()`, and cache cosine computation.
This commit is contained in:
Sébastien Crozet 2023-01-14 11:47:17 +01:00
parent 0cf79aef0e
commit dc1b291ece
1 changed files with 8 additions and 6 deletions

View File

@ -956,13 +956,15 @@ impl<T: SimdRealField> Rotation3<T> {
{
// Implementation informed by "Computing Euler angles from a rotation matrix", by Gregory G. Slabaugh
// https://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.371.6578
// where roll, pitch, yaw angles are referred to as ψ, θ, ϕ,
if self[(2, 0)].clone().abs() < T::one() {
let yaw = -self[(2, 0)].clone().asin();
let roll = (self[(2, 1)].clone() / yaw.clone().cos())
.atan2(self[(2, 2)].clone() / yaw.clone().cos());
let pitch = (self[(1, 0)].clone() / yaw.clone().cos())
.atan2(self[(0, 0)].clone() / yaw.clone().cos());
(roll, yaw, pitch)
let pitch = -self[(2, 0)].clone().asin();
let theta_cos = pitch.clone().cos();
let roll = (self[(2, 1)].clone() / theta_cos.clone())
.atan2(self[(2, 2)].clone() / theta_cos.clone());
let yaw =
(self[(1, 0)].clone() / theta_cos.clone()).atan2(self[(0, 0)].clone() / theta_cos);
(roll, pitch, yaw)
} else if self[(2, 0)].clone() <= -T::one() {
(
self[(0, 1)].clone().atan2(self[(0, 2)].clone()),