Skip to content

Commit

Permalink
Fix singularities in Quat::to_euler()
Browse files Browse the repository at this point in the history
  • Loading branch information
@oisyn
oisyn committed Sep 2, 2023
1 parent abf1a3c commit 2b0b483
Show file tree
Hide file tree
Showing 3 changed files with 154 additions and 23 deletions.
157 changes: 134 additions & 23 deletions src/euler.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,9 +46,10 @@ pub(crate) trait EulerFromQuaternion<Q: Copy>: Sized + Copy {
fn third(self, q: Q) -> Self::Output;

/// Compute all angles of a rotation in the notation order
fn convert_quat(self, q: Q) -> (Self::Output, Self::Output, Self::Output) {
(self.first(q), self.second(q), self.third(q))
}
fn convert_quat(self, q: Q) -> (Self::Output, Self::Output, Self::Output);

#[doc(hidden)]
fn sine_theta(self, q: Q) -> Self::Output;
}

/// Conversion from euler angles to quaternion.
Expand All @@ -62,10 +63,135 @@ macro_rules! impl_from_quat {
($t:ident, $quat:ident) => {
impl EulerFromQuaternion<$quat> for EulerRot {
type Output = $t;

fn sine_theta(self, q: $quat) -> $t {
use EulerRot::*;
match self {
ZYX => -2.0 * (q.x * q.z - q.w * q.y),
ZXY => 2.0 * (q.y * q.z + q.w * q.x),
YXZ => -2.0 * (q.y * q.z - q.w * q.x),
YZX => 2.0 * (q.x * q.y + q.w * q.z),
XYZ => 2.0 * (q.x * q.z + q.w * q.y),
XZY => -2.0 * (q.x * q.y - q.w * q.z),
}
.clamp(-1.0, 1.0)
}

fn first(self, q: $quat) -> $t {
use crate::$t::math;
use EulerRot::*;

let sine_theta = self.sine_theta(q);
if math::abs(sine_theta) > 0.99999 {
let scale = 2.0 * math::signum(sine_theta.signum());

match self {
ZYX => scale * math::atan2(-q.x, q.w),
ZXY => scale * math::atan2(q.y, q.w),
YXZ => scale * math::atan2(-q.z, q.w),
YZX => scale * math::atan2(q.x, q.w),
XYZ => scale * math::atan2(q.z, q.w),
XZY => scale * math::atan2(-q.y, q.w),
}
} else {
match self {
ZYX => math::atan2(
2.0 * (q.x * q.y + q.w * q.z),
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
),
ZXY => math::atan2(
-2.0 * (q.x * q.y - q.w * q.z),
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
),
YXZ => math::atan2(
2.0 * (q.x * q.z + q.w * q.y),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
),
YZX => math::atan2(
-2.0 * (q.x * q.z - q.w * q.y),
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
),
XYZ => math::atan2(
-2.0 * (q.y * q.z - q.w * q.x),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
),
XZY => math::atan2(
2.0 * (q.y * q.z + q.w * q.x),
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
),
}
}
}

fn second(self, q: $quat) -> $t {
use crate::$t::math;
use EulerRot::*;
match self {
ZYX => math::asin_clamped(-2.0 * (q.x * q.z - q.w * q.y)),
ZXY => math::asin_clamped(2.0 * (q.y * q.z + q.w * q.x)),
YXZ => math::asin_clamped(-2.0 * (q.y * q.z - q.w * q.x)),
YZX => math::asin_clamped(2.0 * (q.x * q.y + q.w * q.z)),
XYZ => math::asin_clamped(2.0 * (q.x * q.z + q.w * q.y)),
XZY => math::asin_clamped(-2.0 * (q.x * q.y - q.w * q.z)),
}
}

fn third(self, q: $quat) -> $t {
use crate::$t::math;
use EulerRot::*;
if math::abs(self.sine_theta(q)) > 0.99999 {
0.0
} else {
match self {
ZYX => math::atan2(
2.0 * (q.y * q.z + q.w * q.x),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
),
ZXY => math::atan2(
-2.0 * (q.x * q.z - q.w * q.y),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
),
YXZ => math::atan2(
2.0 * (q.x * q.y + q.w * q.z),
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
),
YZX => math::atan2(
-2.0 * (q.y * q.z - q.w * q.x),
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
),
XYZ => math::atan2(
-2.0 * (q.x * q.y - q.w * q.z),
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
),
XZY => math::atan2(
2.0 * (q.x * q.z + q.w * q.y),
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
),
}
}
}

fn convert_quat(self, q: $quat) -> ($t, $t, $t) {
use crate::$t::math;
use EulerRot::*;

let sine_theta = self.sine_theta(q);
let second = math::asin(sine_theta);

if math::abs(sine_theta) > 0.99999 {
let scale = 2.0 * math::signum(sine_theta);

return match self {
ZYX => (scale * math::atan2(-q.x, q.w), second, 0.0),
ZXY => (scale * math::atan2(q.y, q.w), second, 0.0),
YXZ => (scale * math::atan2(-q.z, q.w), second, 0.0),
YZX => (scale * math::atan2(q.x, q.w), second, 0.0),
XYZ => (scale * math::atan2(q.z, q.w), second, 0.0),
XZY => (scale * math::atan2(-q.y, q.w), second, 0.0),
};
}

let first = match self {
ZYX => math::atan2(
2.0 * (q.x * q.y + q.w * q.z),
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
Expand All @@ -90,26 +216,9 @@ macro_rules! impl_from_quat {
2.0 * (q.y * q.z + q.w * q.x),
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
),
}
}
};

fn second(self, q: $quat) -> $t {
use crate::$t::math;
use EulerRot::*;
match self {
ZYX => math::asin_clamped(-2.0 * (q.x * q.z - q.w * q.y)),
ZXY => math::asin_clamped(2.0 * (q.y * q.z + q.w * q.x)),
YXZ => math::asin_clamped(-2.0 * (q.y * q.z - q.w * q.x)),
YZX => math::asin_clamped(2.0 * (q.x * q.y + q.w * q.z)),
XYZ => math::asin_clamped(2.0 * (q.x * q.z + q.w * q.y)),
XZY => math::asin_clamped(-2.0 * (q.x * q.y - q.w * q.z)),
}
}

fn third(self, q: $quat) -> $t {
use crate::$t::math;
use EulerRot::*;
match self {
let third = match self {
ZYX => math::atan2(
2.0 * (q.y * q.z + q.w * q.x),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
Expand All @@ -134,7 +243,9 @@ macro_rules! impl_from_quat {
2.0 * (q.x * q.z + q.w * q.y),
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
),
}
};

(first, second, third)
}
}
// End - impl EulerFromQuaternion
Expand Down
10 changes: 10 additions & 0 deletions src/f32/math.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,11 @@ mod libm_math {
super::acos_approx_f32(f)
}

#[inline(always)]
pub(crate) fn asin(f: f32) -> f32 {
libm::asinf(f)
}

#[inline(always)]
pub(crate) fn asin_clamped(f: f32) -> f32 {
libm::asinf(f.clamp(-1.0, 1.0))
Expand Down Expand Up @@ -137,6 +142,11 @@ mod std_math {
super::acos_approx_f32(f)
}

#[inline(always)]
pub(crate) fn asin(f: f32) -> f32 {
f32::asin(f)
}

#[inline(always)]
pub(crate) fn asin_clamped(f: f32) -> f32 {
f32::asin(f32::clamp(f, -1.0, 1.0))
Expand Down
10 changes: 10 additions & 0 deletions src/f64/math.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,11 @@ mod libm_math {
libm::acos(f.clamp(-1.0, 1.0))
}

#[inline(always)]
pub(crate) fn asin(f: f64) -> f64 {
libm::asin(f)
}

#[inline(always)]
pub(crate) fn asin_clamped(f: f64) -> f64 {
libm::asin(f.clamp(-1.0, 1.0))
Expand Down Expand Up @@ -102,6 +107,11 @@ mod std_math {
f64::acos(f64::clamp(f, -1.0, 1.0))
}

#[inline(always)]
pub(crate) fn asin(f: f64) -> f64 {
f64::asin(f)
}

#[inline(always)]
pub(crate) fn asin_clamped(f: f64) -> f64 {
f64::asin(f64::clamp(f, -1.0, 1.0))
Expand Down

0 comments on commit 2b0b483

Please sign in to comment.