Fix quaternion polar decomposition.

This commit is contained in:
Sébastien Crozet 2016-08-16 12:26:36 +02:00
parent 9c4bff1f85
commit c4728a33d4
6 changed files with 88 additions and 37 deletions

View File

@ -24,7 +24,7 @@ generic_sizes = [ "generic-array", "typenum" ]
rustc-serialize = "0.3.*"
rand = "0.3.*"
num = "0.1.*"
algebra = "*"
# algebra = "*"
[dependencies.generic-array]
optional = true

View File

@ -70,10 +70,11 @@ impl<N: BaseFloat> Quaternion<N> {
/// Creates a new quaternion from its polar decomposition.
///
/// Note that `axis` is assumed to be a unit vector.
pub fn from_polar_decomposition(scalar: N, theta: N, axis: Unit<Vector3<N>>) -> Quaternion<N> {
pub fn from_polar_decomposition(scale: N, theta: N, axis: Unit<Vector3<N>>)
-> Quaternion<N> {
let rot = UnitQuaternion::from_axisangle(axis, theta * ::cast(2.0));
rot.unwrap() * scalar
rot.unwrap() * scale
}
/// The polar decomposition of this quaternion.
@ -81,29 +82,20 @@ impl<N: BaseFloat> Quaternion<N> {
/// Returns, from left to right: the quaternion norm, the half rotation angle, the rotation
/// axis. If the rotation angle is zero, the rotation axis is set to the `y` axis.
pub fn polar_decomposition(&self) -> (N, N, Unit<Vector3<N>>) {
let nn = ::norm_squared(self);
let default_axis = Unit::from_unit_value_unchecked(Vector3::y());
if ApproxEq::approx_eq(&nn, &::zero()) {
(::zero(), ::zero(), default_axis)
}
else {
let n = nn.sqrt();
let nq = *self / n;
let v = *self.vector();
let vnn = ::norm_squared(&v);
if ApproxEq::approx_eq(&vnn, &::zero()) {
(n, ::zero(), default_axis)
}
else {
let angle = self.scalar().acos();
let vn = n.sqrt();
let axis = Unit::from_unit_value_unchecked(v / vn);
if let Some((q, n)) = Unit::try_new_and_get(*self, ::zero()) {
if let Some(axis) = Unit::try_new(self.vector(), ::zero()) {
let angle = q.angle() / ::cast(2.0);
(n, angle, axis)
}
else {
(n, ::zero(), default_axis)
}
}
else {
(::zero(), ::zero(), default_axis)
}
}
}
@ -147,7 +139,7 @@ impl<N: BaseFloat> Norm for Quaternion<N> {
#[inline]
fn normalize(&self) -> Quaternion<N> {
let n = self.norm();
let n = ::norm(self);
*self / n
}
@ -170,6 +162,19 @@ impl<N: BaseFloat> Norm for Quaternion<N> {
Some(*self / n)
}
}
#[inline]
fn try_normalize_mut(&mut self, min_norm: N) -> Option<N> {
let n = ::norm(self);
if n <= min_norm {
None
}
else {
*self /= n;
Some(n)
}
}
}
impl<N> Mul<Quaternion<N>> for Quaternion<N>
@ -260,12 +265,15 @@ impl<N: fmt::Display> fmt::Display for Quaternion<N> {
/// A unit quaternions. May be used to represent a rotation.
pub type UnitQuaternion<N> = Unit<Quaternion<N>>;
// /// A unit quaternion that can represent a 3D rotation.
// #[repr(C)]
// #[derive(Eq, PartialEq, RustcEncodable, RustcDecodable, Clone, Hash, Debug, Copy)]
// pub struct UnitQuaternion<N> {
// q: Quaternion<N>
// }
impl<N> UnitQuaternion<N> {
/// The underlying quaternion.
///
/// Same as `self.as_ref()`.
#[inline]
pub fn quaternion(&self) -> &Quaternion<N> {
self.as_ref()
}
}
impl<N: BaseFloat> UnitQuaternion<N> {
/// Creates a new quaternion from a unit vector (the rotation axis) and an angle
@ -282,7 +290,7 @@ impl<N: BaseFloat> UnitQuaternion<N> {
#[inline]
pub fn from_scaled_axis(axis: Vector3<N>) -> UnitQuaternion<N> {
let two: N = ::cast(2.0);
let q = Quaternion::from_parts(::zero(), axis * two).exp();
let q = Quaternion::from_parts(::zero(), axis / two).exp();
UnitQuaternion::from_unit_value_unchecked(q)
}
@ -315,7 +323,7 @@ impl<N: BaseFloat> UnitQuaternion<N> {
/// The rotation angle of this unit quaternion.
#[inline]
pub fn angle(&self) -> N {
self.as_ref().scalar().acos()
self.as_ref().scalar().acos() * ::cast(2.0)
}
/// The rotation axis of this unit quaternion or `None` if the rotation is zero.
@ -439,9 +447,9 @@ impl<N: BaseNum> Mul<Vector3<N>> for UnitQuaternion<N> {
#[inline]
fn mul(self, right: Vector3<N>) -> Vector3<N> {
let two: N = ::one::<N>() + ::one();
let t = ::cross(self.as_ref().vector(), &right);
let t = ::cross(self.as_ref().vector(), &right) * two;
t * (two * self.as_ref().w) + ::cross(self.as_ref().vector(), &t) + right
t * self.as_ref().w + ::cross(self.as_ref().vector(), &t) + right
}
}

View File

@ -333,6 +333,19 @@ macro_rules! vector_impl(
Some(*self / n)
}
}
#[inline]
fn try_normalize_mut(&mut self, min_norm: N) -> Option<N> {
let n = ::norm(self);
if n <= min_norm {
None
}
else {
*self /= n;
Some(n)
}
}
}

View File

@ -538,6 +538,19 @@ macro_rules! vecn_dvec_common_impl(
Some(self / n)
}
}
#[inline]
fn try_normalize_mut(&mut self, min_norm: N) -> Option<N> {
let n = ::norm(self);
if n <= min_norm {
None
}
else {
*self /= n;
Some(n)
}
}
}
/*

View File

@ -254,6 +254,12 @@ pub trait Norm: Sized {
/// or equal to `min_norm`. In particular, `.try_normalize(0.0)` returns `None` if the norm is
/// exactly zero.
fn try_normalize(&self, min_norm: Self::NormType) -> Option<Self>;
/// Normalized `v` or does nothing if the vector has a norm smaller
/// or equal to `min_norm`.
///
/// Returns the old norm or `None` if the normalization failed.
fn try_normalize_mut(&mut self, min_norm: Self::NormType) -> Option<Self::NormType>;
}
/**

View File

@ -17,16 +17,16 @@ fn test_quaternion_as_matrix() {
fn test_quaternion_mul_vec_or_point_as_matrix() {
for _ in 0usize .. 10000 {
let axis_angle: Vector3<f64> = random();
let vector: Vector3<f64> = random();
let point: Point3<f64> = random();
let vector: Vector3<f64> = random();
let point: Point3<f64> = random();
let matrix = Rotation3::new(axis_angle);
let matrix = Rotation3::new(axis_angle);
let quaternion = UnitQuaternion::from_scaled_axis(axis_angle);
assert!(na::approx_eq(&(matrix * vector), &(quaternion * vector)));
assert!(na::approx_eq(&(matrix * point), &(quaternion * point)));
assert!(na::approx_eq(&(matrix * point), &(quaternion * point)));
assert!(na::approx_eq(&(vector * matrix), &(vector * quaternion)));
assert!(na::approx_eq(&(point * matrix), &(point * quaternion)));
assert!(na::approx_eq(&(point * matrix), &(point * quaternion)));
}
}
@ -108,3 +108,14 @@ fn test_quaternion_neutral() {
assert!(na::approx_eq(&q1, &q2) && na::approx_eq(&q2, &q3))
}
}
#[test]
fn test_quaternion_polar_decomposition() {
for _ in 0 .. 10000 {
let q1: Quaternion<f32> = random();
let decomp = q1.polar_decomposition();
let q2 = Quaternion::from_polar_decomposition(decomp.0, decomp.1, decomp.2);
assert!(na::approx_eq(&q1, &q2))
}
}