From dc1b291ece4acf1744b43fa9f242698ed4427ef5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sat, 14 Jan 2023 11:47:17 +0100 Subject: [PATCH] Merge pull request #1179 from timethy/euler-angles-patch Fix typo in `euler_angles()`, and cache cosine computation. --- src/geometry/rotation_specialization.rs | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) 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()),