Skip to content

Commit 2b187c2

Browse files
jnisesjnises
authored
Calculate Quat::to_axis_angle in a more numerically stable way (#387)
1 parent 38345b9 commit 2b187c2

File tree

7 files changed

+89
-63
lines changed

7 files changed

+89
-63
lines changed

‎codegen/templates/quat.rs.tera

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -456,18 +456,18 @@ impl {{ self_t }} {
456456
}
457457
}
458458

459-
/// Returns the rotation axis and angle (in radians) of `self`.
459+
/// Returns the rotation axis (normalized) and angle (in radians) of `self`.
460460
#[inline]
461461
pub fn to_axis_angle(self) -> ({{ vec3_t }}, {{ scalar_t }}) {
462462
const EPSILON: {{ scalar_t }} = 1.0e-8;
463-
const EPSILON_SQUARED: {{ scalar_t }} = EPSILON * EPSILON;
464-
let w = self.w;
465-
let angle = w.acos_approx() * 2.0;
466-
let scale_sq = {{ scalar_t }}::max(1.0 - w * w, 0.0);
467-
if scale_sq >= EPSILON_SQUARED {
468-
({{ vec3_t }}::new(self.x, self.y, self.z) * scale_sq.sqrt().recip(), angle)
463+
let v = {{ vec3_t }}::new(self.x, self.y, self.z);
464+
let length = v.length();
465+
if length >= EPSILON {
466+
let angle = 2.0 * {{ scalar_t }}::atan2(length, self.w);
467+
let axis = v / length;
468+
(axis, angle)
469469
} else {
470-
({{ vec3_t }}::X, angle)
470+
({{ vec3_t }}::X, 0.0)
471471
}
472472
}
473473

‎src/f32/coresimd/quat.rs

Lines changed: 8 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -343,21 +343,18 @@ impl Quat {
343343
}
344344
}
345345

346-
/// Returns the rotation axis and angle (in radians) of `self`.
346+
/// Returns the rotation axis (normalized) and angle (in radians) of `self`.
347347
#[inline]
348348
pub fn to_axis_angle(self) -> (Vec3, f32) {
349349
const EPSILON: f32 = 1.0e-8;
350-
const EPSILON_SQUARED: f32 = EPSILON * EPSILON;
351-
let w = self.w;
352-
let angle = w.acos_approx() * 2.0;
353-
let scale_sq = f32::max(1.0 - w * w, 0.0);
354-
if scale_sq >= EPSILON_SQUARED {
355-
(
356-
Vec3::new(self.x, self.y, self.z) * scale_sq.sqrt().recip(),
357-
angle,
358-
)
350+
let v = Vec3::new(self.x, self.y, self.z);
351+
let length = v.length();
352+
if length >= EPSILON {
353+
let angle = 2.0 * f32::atan2(length, self.w);
354+
let axis = v / length;
355+
(axis, angle)
359356
} else {
360-
(Vec3::X, angle)
357+
(Vec3::X, 0.0)
361358
}
362359
}
363360

‎src/f32/scalar/quat.rs

Lines changed: 8 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -351,21 +351,18 @@ impl Quat {
351351
}
352352
}
353353

354-
/// Returns the rotation axis and angle (in radians) of `self`.
354+
/// Returns the rotation axis (normalized) and angle (in radians) of `self`.
355355
#[inline]
356356
pub fn to_axis_angle(self) -> (Vec3, f32) {
357357
const EPSILON: f32 = 1.0e-8;
358-
const EPSILON_SQUARED: f32 = EPSILON * EPSILON;
359-
let w = self.w;
360-
let angle = w.acos_approx() * 2.0;
361-
let scale_sq = f32::max(1.0 - w * w, 0.0);
362-
if scale_sq >= EPSILON_SQUARED {
363-
(
364-
Vec3::new(self.x, self.y, self.z) * scale_sq.sqrt().recip(),
365-
angle,
366-
)
358+
let v = Vec3::new(self.x, self.y, self.z);
359+
let length = v.length();
360+
if length >= EPSILON {
361+
let angle = 2.0 * f32::atan2(length, self.w);
362+
let axis = v / length;
363+
(axis, angle)
367364
} else {
368-
(Vec3::X, angle)
365+
(Vec3::X, 0.0)
369366
}
370367
}
371368

‎src/f32/sse2/quat.rs

Lines changed: 8 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -350,21 +350,18 @@ impl Quat {
350350
}
351351
}
352352

353-
/// Returns the rotation axis and angle (in radians) of `self`.
353+
/// Returns the rotation axis (normalized) and angle (in radians) of `self`.
354354
#[inline]
355355
pub fn to_axis_angle(self) -> (Vec3, f32) {
356356
const EPSILON: f32 = 1.0e-8;
357-
const EPSILON_SQUARED: f32 = EPSILON * EPSILON;
358-
let w = self.w;
359-
let angle = w.acos_approx() * 2.0;
360-
let scale_sq = f32::max(1.0 - w * w, 0.0);
361-
if scale_sq >= EPSILON_SQUARED {
362-
(
363-
Vec3::new(self.x, self.y, self.z) * scale_sq.sqrt().recip(),
364-
angle,
365-
)
357+
let v = Vec3::new(self.x, self.y, self.z);
358+
let length = v.length();
359+
if length >= EPSILON {
360+
let angle = 2.0 * f32::atan2(length, self.w);
361+
let axis = v / length;
362+
(axis, angle)
366363
} else {
367-
(Vec3::X, angle)
364+
(Vec3::X, 0.0)
368365
}
369366
}
370367

‎src/f32/wasm32/quat.rs

Lines changed: 8 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -343,21 +343,18 @@ impl Quat {
343343
}
344344
}
345345

346-
/// Returns the rotation axis and angle (in radians) of `self`.
346+
/// Returns the rotation axis (normalized) and angle (in radians) of `self`.
347347
#[inline]
348348
pub fn to_axis_angle(self) -> (Vec3, f32) {
349349
const EPSILON: f32 = 1.0e-8;
350-
const EPSILON_SQUARED: f32 = EPSILON * EPSILON;
351-
let w = self.w;
352-
let angle = w.acos_approx() * 2.0;
353-
let scale_sq = f32::max(1.0 - w * w, 0.0);
354-
if scale_sq >= EPSILON_SQUARED {
355-
(
356-
Vec3::new(self.x, self.y, self.z) * scale_sq.sqrt().recip(),
357-
angle,
358-
)
350+
let v = Vec3::new(self.x, self.y, self.z);
351+
let length = v.length();
352+
if length >= EPSILON {
353+
let angle = 2.0 * f32::atan2(length, self.w);
354+
let axis = v / length;
355+
(axis, angle)
359356
} else {
360-
(Vec3::X, angle)
357+
(Vec3::X, 0.0)
361358
}
362359
}
363360

‎src/f64/dquat.rs

Lines changed: 8 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -341,21 +341,18 @@ impl DQuat {
341341
}
342342
}
343343

344-
/// Returns the rotation axis and angle (in radians) of `self`.
344+
/// Returns the rotation axis (normalized) and angle (in radians) of `self`.
345345
#[inline]
346346
pub fn to_axis_angle(self) -> (DVec3, f64) {
347347
const EPSILON: f64 = 1.0e-8;
348-
const EPSILON_SQUARED: f64 = EPSILON * EPSILON;
349-
let w = self.w;
350-
let angle = w.acos_approx() * 2.0;
351-
let scale_sq = f64::max(1.0 - w * w, 0.0);
352-
if scale_sq >= EPSILON_SQUARED {
353-
(
354-
DVec3::new(self.x, self.y, self.z) * scale_sq.sqrt().recip(),
355-
angle,
356-
)
348+
let v = DVec3::new(self.x, self.y, self.z);
349+
let length = v.length();
350+
if length >= EPSILON {
351+
let angle = 2.0 * f64::atan2(length, self.w);
352+
let axis = v / length;
353+
(axis, angle)
357354
} else {
358-
(DVec3::X, angle)
355+
(DVec3::X, 0.0)
359356
}
360357
}
361358

‎tests/quat.rs

Lines changed: 41 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -507,6 +507,47 @@ macro_rules! impl_quat_tests {
507507
glam_test!(test_to_array, {
508508
assert!($new(1.0, 2.0, 3.0, 4.0).to_array() == [1.0, 2.0, 3.0, 4.0]);
509509
});
510+
511+
glam_test!(test_to_axis_angle, {
512+
{
513+
let q = $quat::from_xyzw(
514+
5.28124762e-08,
515+
-5.12559303e-03,
516+
8.29266140e-08,
517+
9.99986828e-01,
518+
);
519+
assert!(q.is_normalized());
520+
let (axis, angle) = q.to_axis_angle();
521+
assert!(axis.is_normalized());
522+
let q2 = $quat::from_axis_angle(axis, angle);
523+
assert!((q.dot(q2) - 1.0).abs() < 1e-6);
524+
}
525+
{
526+
let q = $quat::IDENTITY;
527+
let (axis, angle) = q.to_axis_angle();
528+
assert!(axis.is_normalized());
529+
let q2 = $quat::from_axis_angle(axis, angle);
530+
assert!((q.dot(q2) - 1.0).abs() < 1e-6);
531+
}
532+
{
533+
let q = $quat::from_xyzw(0.0, 1.0, 0.0, 0.0);
534+
assert!(q.is_normalized());
535+
let (axis, angle) = q.to_axis_angle();
536+
assert!(axis.is_normalized());
537+
let q2 = $quat::from_axis_angle(axis, angle);
538+
assert!((q.dot(q2) - 1.0).abs() < 1e-6);
539+
}
540+
{
541+
let axis = $vec3::Z;
542+
let angle = core::$t::consts::PI * 0.25;
543+
let q = $quat::from_axis_angle(axis, angle);
544+
assert!(q.is_normalized());
545+
let (axis2, angle2) = q.to_axis_angle();
546+
assert!(axis.is_normalized());
547+
assert_approx_eq!(axis, axis2);
548+
assert_approx_eq!(angle, angle2);
549+
}
550+
});
510551
};
511552
}
512553

0 commit comments

Comments
 (0)