From 889cf2f71d37b11140ac629f6999a6a8b74a536b Mon Sep 17 00:00:00 2001 From: Zach Kozar Date: Sat, 22 Apr 2023 11:00:29 -0400 Subject: [PATCH 1/2] Add euler_angles_ordered function on Rotation --- src/geometry/rotation_specialization.rs | 107 ++++++++++++++++++++++++ 1 file changed, 107 insertions(+) diff --git a/src/geometry/rotation_specialization.rs b/src/geometry/rotation_specialization.rs index 1ea5cd92..b1ee8c82 100644 --- a/src/geometry/rotation_specialization.rs +++ b/src/geometry/rotation_specialization.rs @@ -979,6 +979,113 @@ impl Rotation3 { ) } } + + /// Represent this rotation as Euler angles. + /// + /// Returns the angles produced in the order provided by seq parameter, along with the + /// observability flag. If the rotation is gimbal locked, then the observability flag is false. + /// + /// Algorithm based on: + /// Malcolm D. Shuster, F. Landis Markley, “General formula for extraction the Euler + /// angles”, Journal of guidance, control, and dynamics, vol. 29.1, pp. 215-221. 2006, + /// and modified to be able to produce extrinsic rotations. + #[must_use] + pub fn euler_angles_ordered( + &self, + mut seq: [Unit>; 3], + extrinsic: bool, + ) -> (Vector3, bool) + where + T: RealField + Copy, + { + let mut angles = Vector3::zeros(); + let eps = T::from_subset(&1e-7); + let _2 = T::from_subset(&2.0); + + if extrinsic { + seq.reverse(); + } + + let [n1, n2, n3] = &seq; + + let n1_c_n2 = n1.cross(n2); + let s1 = n1_c_n2.dot(n3); + let c1 = n1.dot(n3); + let lambda = s1.atan2(c1); + + let mut c = Matrix3::zeros(); + c.column_mut(0).copy_from(n2); + c.column_mut(1).copy_from(&n1_c_n2); + c.column_mut(2).copy_from(n1); + c.transpose_mut(); + + let r1l = Matrix3::new( + T::one(), T::zero(), T::zero(), + T::zero(), c1, s1, + T::zero(), -s1, c1, + ); + let o_t = &c * self.matrix() * (c.transpose() * r1l); + angles.y = o_t.m33.acos(); + + let safe1 = angles.y.abs() >= eps; + let safe2 = (angles.y - T::pi()).abs() >= eps; + let observable = safe1 && safe2; + angles.y += lambda; + + if observable { + angles.x = o_t.m13.atan2(-o_t.m23); + angles.z = o_t.m31.atan2(o_t.m32); + } else { + // gimbal lock detected + if extrinsic { + // angle1 is initialized to zero + if !safe1 { + angles.z = (o_t.m12 - o_t.m21).atan2(o_t.m11 + o_t.m22); + } else { + angles.z = -(o_t.m12 + o_t.m21).atan2(o_t.m11 - o_t.m22); + }; + } else { + // angle3 is initialized to zero + if !safe1 { + angles.x = (o_t.m12 - o_t.m21).atan2(o_t.m11 + o_t.m22); + } else { + angles.x = (o_t.m12 + o_t.m21).atan2(o_t.m11 - o_t.m22); + }; + }; + }; + + let adjust = if seq[0] == seq[2] { + // lambda = 0, so ensure angle2 -> [0, pi] + angles.y < T::zero() || angles.y > T::pi() + } else { + // lamda = + or - pi/2, so ensure angle2 -> [-pi/2, pi/2] + angles.y < -T::frac_pi_2() || angles.y > T::frac_pi_2() + }; + + // dont adjust gimbal locked rotation + if adjust && observable { + angles.x += T::pi(); + angles.y = _2 * lambda - angles.y; + angles.z -= T::pi(); + } + + // ensure all angles are within [-pi, pi] + for angle in angles.as_mut_slice().iter_mut() { + if *angle < -T::pi() { + *angle += T::two_pi(); + } else if *angle > T::pi() { + *angle -= T::two_pi(); + } + } + + if extrinsic { + let tmp = angles.x; + angles.x = angles.z; + angles.z = tmp; + } + + (angles, observable) + } } #[cfg(feature = "rand-no-std")] From 033f722d4f8c931688cfeee024ba49ac69a7093d Mon Sep 17 00:00:00 2001 From: Zach Kozar Date: Fri, 5 May 2023 00:00:25 -0400 Subject: [PATCH 2/2] Return angles as array, add requirements and examples to doc --- src/geometry/rotation_specialization.rs | 111 +++++++++++++++++++----- 1 file changed, 87 insertions(+), 24 deletions(-) diff --git a/src/geometry/rotation_specialization.rs b/src/geometry/rotation_specialization.rs index b1ee8c82..ae954dd2 100644 --- a/src/geometry/rotation_specialization.rs +++ b/src/geometry/rotation_specialization.rs @@ -983,7 +983,64 @@ impl Rotation3 { /// Represent this rotation as Euler angles. /// /// Returns the angles produced in the order provided by seq parameter, along with the - /// observability flag. If the rotation is gimbal locked, then the observability flag is false. + /// observability flag. The Euler axes passed to seq must form an orthonormal basis. If the + /// rotation is gimbal locked, then the observability flag is false. + /// + /// # Panics + /// + /// Panics if the Euler axes in `seq` are not orthonormal. + /// + /// # Example 1: + /// ``` + /// use std::f64::consts::PI; + /// use approx::assert_relative_eq; + /// use nalgebra::{Matrix3, Rotation3, Unit, Vector3}; + /// + /// // 3-1-2 + /// let n = [ + /// Unit::new_unchecked(Vector3::new(0.0, 0.0, 1.0)), + /// Unit::new_unchecked(Vector3::new(1.0, 0.0, 0.0)), + /// Unit::new_unchecked(Vector3::new(0.0, 1.0, 0.0)), + /// ]; + /// + /// let r1 = Rotation3::from_axis_angle(&n[2], 20.0 * PI / 180.0); + /// let r2 = Rotation3::from_axis_angle(&n[1], 30.0 * PI / 180.0); + /// let r3 = Rotation3::from_axis_angle(&n[0], 45.0 * PI / 180.0); + /// + /// let d = r3 * r2 * r1; + /// + /// let (angles, observable) = d.euler_angles_ordered(n, false); + /// assert!(observable); + /// assert_relative_eq!(angles[0] * 180.0 / PI, 45.0, epsilon = 1e-12); + /// assert_relative_eq!(angles[1] * 180.0 / PI, 30.0, epsilon = 1e-12); + /// assert_relative_eq!(angles[2] * 180.0 / PI, 20.0, epsilon = 1e-12); + /// ``` + /// + /// # Example 2: + /// ``` + /// use std::f64::consts::PI; + /// use approx::assert_relative_eq; + /// use nalgebra::{Matrix3, Rotation3, Unit, Vector3}; + /// + /// let sqrt_2 = 2.0_f64.sqrt(); + /// let n = [ + /// Unit::new_unchecked(Vector3::new(1.0 / sqrt_2, 1.0 / sqrt_2, 0.0)), + /// Unit::new_unchecked(Vector3::new(1.0 / sqrt_2, -1.0 / sqrt_2, 0.0)), + /// Unit::new_unchecked(Vector3::new(0.0, 0.0, 1.0)), + /// ]; + /// + /// let r1 = Rotation3::from_axis_angle(&n[2], 20.0 * PI / 180.0); + /// let r2 = Rotation3::from_axis_angle(&n[1], 30.0 * PI / 180.0); + /// let r3 = Rotation3::from_axis_angle(&n[0], 45.0 * PI / 180.0); + /// + /// let d = r3 * r2 * r1; + /// + /// let (angles, observable) = d.euler_angles_ordered(n, false); + /// assert!(observable); + /// assert_relative_eq!(angles[0] * 180.0 / PI, 45.0, epsilon = 1e-12); + /// assert_relative_eq!(angles[1] * 180.0 / PI, 30.0, epsilon = 1e-12); + /// assert_relative_eq!(angles[2] * 180.0 / PI, 20.0, epsilon = 1e-12); + /// ``` /// /// Algorithm based on: /// Malcolm D. Shuster, F. Landis Markley, “General formula for extraction the Euler @@ -994,11 +1051,11 @@ impl Rotation3 { &self, mut seq: [Unit>; 3], extrinsic: bool, - ) -> (Vector3, bool) + ) -> ([T; 3], bool) where T: RealField + Copy, { - let mut angles = Vector3::zeros(); + let mut angles = [T::zero(); 3]; let eps = T::from_subset(&1e-7); let _2 = T::from_subset(&2.0); @@ -1007,6 +1064,8 @@ impl Rotation3 { } let [n1, n2, n3] = &seq; + assert_relative_eq!(n1.dot(n2), T::zero(), epsilon = eps); + assert_relative_eq!(n3.dot(n1), T::zero(), epsilon = eps); let n1_c_n2 = n1.cross(n2); let s1 = n1_c_n2.dot(n3); @@ -1020,53 +1079,59 @@ impl Rotation3 { c.transpose_mut(); let r1l = Matrix3::new( - T::one(), T::zero(), T::zero(), - T::zero(), c1, s1, - T::zero(), -s1, c1, + T::one(), + T::zero(), + T::zero(), + T::zero(), + c1, + s1, + T::zero(), + -s1, + c1, ); let o_t = &c * self.matrix() * (c.transpose() * r1l); - angles.y = o_t.m33.acos(); + angles[1] = o_t.m33.acos(); - let safe1 = angles.y.abs() >= eps; - let safe2 = (angles.y - T::pi()).abs() >= eps; + let safe1 = angles[1].abs() >= eps; + let safe2 = (angles[1] - T::pi()).abs() >= eps; let observable = safe1 && safe2; - angles.y += lambda; + angles[1] += lambda; if observable { - angles.x = o_t.m13.atan2(-o_t.m23); - angles.z = o_t.m31.atan2(o_t.m32); + angles[0] = o_t.m13.atan2(-o_t.m23); + angles[2] = o_t.m31.atan2(o_t.m32); } else { // gimbal lock detected if extrinsic { // angle1 is initialized to zero if !safe1 { - angles.z = (o_t.m12 - o_t.m21).atan2(o_t.m11 + o_t.m22); + angles[2] = (o_t.m12 - o_t.m21).atan2(o_t.m11 + o_t.m22); } else { - angles.z = -(o_t.m12 + o_t.m21).atan2(o_t.m11 - o_t.m22); + angles[2] = -(o_t.m12 + o_t.m21).atan2(o_t.m11 - o_t.m22); }; } else { // angle3 is initialized to zero if !safe1 { - angles.x = (o_t.m12 - o_t.m21).atan2(o_t.m11 + o_t.m22); + angles[0] = (o_t.m12 - o_t.m21).atan2(o_t.m11 + o_t.m22); } else { - angles.x = (o_t.m12 + o_t.m21).atan2(o_t.m11 - o_t.m22); + angles[0] = (o_t.m12 + o_t.m21).atan2(o_t.m11 - o_t.m22); }; }; }; let adjust = if seq[0] == seq[2] { // lambda = 0, so ensure angle2 -> [0, pi] - angles.y < T::zero() || angles.y > T::pi() + angles[1] < T::zero() || angles[1] > T::pi() } else { // lamda = + or - pi/2, so ensure angle2 -> [-pi/2, pi/2] - angles.y < -T::frac_pi_2() || angles.y > T::frac_pi_2() + angles[1] < -T::frac_pi_2() || angles[1] > T::frac_pi_2() }; // dont adjust gimbal locked rotation if adjust && observable { - angles.x += T::pi(); - angles.y = _2 * lambda - angles.y; - angles.z -= T::pi(); + angles[0] += T::pi(); + angles[1] = _2 * lambda - angles[1]; + angles[2] -= T::pi(); } // ensure all angles are within [-pi, pi] @@ -1079,9 +1144,7 @@ impl Rotation3 { } if extrinsic { - let tmp = angles.x; - angles.x = angles.z; - angles.z = tmp; + angles.reverse(); } (angles, observable)