Fix quaternion polar decomposition.
This commit is contained in:
parent
9c4bff1f85
commit
c4728a33d4
|
@ -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
|
||||
|
|
|
@ -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
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -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)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
|
|
@ -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)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
|
@ -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>;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
@ -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))
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue