diff --git a/src/geometry/rotation_specialization.rs b/src/geometry/rotation_specialization.rs index bba92512..1ea5cd92 100644 --- a/src/geometry/rotation_specialization.rs +++ b/src/geometry/rotation_specialization.rs @@ -956,13 +956,15 @@ impl Rotation3 { { // 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()),