diff --git a/Cargo.toml b/Cargo.toml index fb3c090a..253be7f1 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -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 diff --git a/src/structs/quaternion.rs b/src/structs/quaternion.rs index 2c0128ad..737ec988 100644 --- a/src/structs/quaternion.rs +++ b/src/structs/quaternion.rs @@ -70,10 +70,11 @@ impl Quaternion { /// 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>) -> Quaternion { + pub fn from_polar_decomposition(scale: N, theta: N, axis: Unit>) + -> Quaternion { 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 Quaternion { /// 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>) { - 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 Norm for Quaternion { #[inline] fn normalize(&self) -> Quaternion { - let n = self.norm(); + let n = ::norm(self); *self / n } @@ -170,6 +162,19 @@ impl Norm for Quaternion { Some(*self / n) } } + + #[inline] + fn try_normalize_mut(&mut self, min_norm: N) -> Option { + let n = ::norm(self); + + if n <= min_norm { + None + } + else { + *self /= n; + Some(n) + } + } } impl Mul> for Quaternion @@ -260,12 +265,15 @@ impl fmt::Display for Quaternion { /// A unit quaternions. May be used to represent a rotation. pub type UnitQuaternion = Unit>; -// /// A unit quaternion that can represent a 3D rotation. -// #[repr(C)] -// #[derive(Eq, PartialEq, RustcEncodable, RustcDecodable, Clone, Hash, Debug, Copy)] -// pub struct UnitQuaternion { -// q: Quaternion -// } +impl UnitQuaternion { + /// The underlying quaternion. + /// + /// Same as `self.as_ref()`. + #[inline] + pub fn quaternion(&self) -> &Quaternion { + self.as_ref() + } +} impl UnitQuaternion { /// Creates a new quaternion from a unit vector (the rotation axis) and an angle @@ -282,7 +290,7 @@ impl UnitQuaternion { #[inline] pub fn from_scaled_axis(axis: Vector3) -> UnitQuaternion { 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 UnitQuaternion { /// 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 Mul> for UnitQuaternion { #[inline] fn mul(self, right: Vector3) -> Vector3 { let two: N = ::one::() + ::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 } } diff --git a/src/structs/vector_macros.rs b/src/structs/vector_macros.rs index 3b6d832f..e2343261 100644 --- a/src/structs/vector_macros.rs +++ b/src/structs/vector_macros.rs @@ -333,6 +333,19 @@ macro_rules! vector_impl( Some(*self / n) } } + + #[inline] + fn try_normalize_mut(&mut self, min_norm: N) -> Option { + let n = ::norm(self); + + if n <= min_norm { + None + } + else { + *self /= n; + Some(n) + } + } } diff --git a/src/structs/vectorn_macros.rs b/src/structs/vectorn_macros.rs index 52fc0b67..f6f81188 100644 --- a/src/structs/vectorn_macros.rs +++ b/src/structs/vectorn_macros.rs @@ -538,6 +538,19 @@ macro_rules! vecn_dvec_common_impl( Some(self / n) } } + + #[inline] + fn try_normalize_mut(&mut self, min_norm: N) -> Option { + let n = ::norm(self); + + if n <= min_norm { + None + } + else { + *self /= n; + Some(n) + } + } } /* diff --git a/src/traits/geometry.rs b/src/traits/geometry.rs index 7131f0ed..559160c5 100644 --- a/src/traits/geometry.rs +++ b/src/traits/geometry.rs @@ -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; + + /// 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; } /** diff --git a/tests/quat.rs b/tests/quat.rs index 868f0092..9912f057 100644 --- a/tests/quat.rs +++ b/tests/quat.rs @@ -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 = random(); - let vector: Vector3 = random(); - let point: Point3 = random(); + let vector: Vector3 = random(); + let point: Point3 = 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 = random(); + let decomp = q1.polar_decomposition(); + let q2 = Quaternion::from_polar_decomposition(decomp.0, decomp.1, decomp.2); + + assert!(na::approx_eq(&q1, &q2)) + } +}