From d45c242a15ee07f802c9b2d7b41cf9e82b064587 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Fri, 12 Aug 2016 21:46:40 +0200 Subject: [PATCH] Add a `Unit` wrapper type, remove UnitQuaternion. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The `Unit` wrapper type ensures that elements of the underlying type has a unit norm. For example, `Unit` designates an element of S². In particular `UnitQuaternion` is now a type alias for `Unit>`. --- CHANGELOG.md | 28 +- Cargo.toml | 1 + src/lib.rs | 21 +- src/linalg/decompositions.rs | 8 +- src/structs/mod.rs | 2 + src/structs/orthographic.rs | 4 +- src/structs/perspective.rs | 4 +- src/structs/quaternion.rs | 388 +++++++++++++++----------- src/structs/rotation.rs | 20 +- src/structs/specializations/vector.rs | 4 +- src/structs/vector_macros.rs | 29 +- src/structs/vectorn_macros.rs | 34 ++- src/traits/geometry.rs | 21 +- src/traits/operations.rs | 6 +- src/traits/structure.rs | 8 +- 15 files changed, 358 insertions(+), 220 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 100ed835..e29422a6 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,7 +5,7 @@ documented here. This project adheres to [Semantic Versioning](http://semver.org/). ## [0.9.0] - WIP -## Modified +### Modified * Renamed: - `::from_col_vector` -> `::from_column_vector` - `::from_col_iter` -> `::from_column_iter` @@ -14,11 +14,31 @@ This project adheres to [Semantic Versioning](http://semver.org/). - `::canonical_basis_with_dim` -> `::canonical_basis_with_dimension` - `::from_elem` -> `::from_element` - `DiagMut` -> `DiagonalMut` - * Added: - - Added `.exp()` and `.pow()` for quaternions. + +The `Norm` trait now uses an associated type instead of a type parameter. +Other similar trait changes are to be expected in the future, e.g., for the +`Diagonal` trait. + +Methods marked `unsafe` for reasons unrelated to memory safety are no +longer unsafe. Instead, their name end with `_unchecked`. In particular: + * `Rotation3::new_with_matrix` -> `Rotation3::new_with_matrix_unchecked` + * `PerspectiveMatrix3::new_with_matrix` -> `PerspectiveMatrix3::new_with_matrix_unchecked` + * `OrthographicMatrix3::new_with_matrix` -> `OrthographicMatrix3::new_with_matrix_unchecked` + +### Added + - A `Unit` type that wraps normalized values. In particular, + `UnitQuaternion` is now replaced by `Unit>`. + - `.ln()`, `.exp()` and `.powf(..)` for quaternions. + - `::from_parts(...)` to build a quaternion from its scalar and vector + parts. + - The `Norm` trait now has a `try_normalize()` that returns `None` if the + norm is too small. + - The `BaseFloat` and `FloatVector` traits now inherit from `ApproxEq` as + well. It is clear that performing computations with floats requires + approximate equality. ## [0.8.0] -## Modified +### Modified * Almost everything (types, methods, and traits) now use full names instead of abbreviations (e.g. `Vec3` becomes `Vector3`). Most changes are abvious. Note however that: diff --git a/Cargo.toml b/Cargo.toml index ecfca36f..fb3c090a 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -24,6 +24,7 @@ generic_sizes = [ "generic-array", "typenum" ] rustc-serialize = "0.3.*" rand = "0.3.*" num = "0.1.*" +algebra = "*" [dependencies.generic-array] optional = true diff --git a/src/lib.rs b/src/lib.rs index a489d249..df41c059 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -154,7 +154,8 @@ pub use structs::{ Point1, Point2, Point3, Point4, Point5, Point6, Perspective3, PerspectiveMatrix3, Orthographic3, OrthographicMatrix3, - Quaternion, UnitQuaternion + Quaternion, UnitQuaternion, + Unit }; pub use linalg::{ @@ -312,7 +313,7 @@ pub fn origin() -> P { /// Returns the center of two points. #[inline] pub fn center>(a: &P, b: &P) -> P - where

::Vector: Norm { + where

::Vector: Norm { (*a + b.to_vector()) / ::cast(2.0) } @@ -321,14 +322,14 @@ pub fn center>(a: &P, b: &P) -> P */ /// Returns the distance between two points. #[inline] -pub fn distance>(a: &P, b: &P) -> N where

::Vector: Norm { +pub fn distance>(a: &P, b: &P) -> N where

::Vector: Norm { a.distance(b) } /// Returns the squared distance between two points. #[inline] pub fn distance_squared>(a: &P, b: &P) -> N - where

::Vector: Norm { + where

::Vector: Norm { a.distance_squared(b) } @@ -664,22 +665,28 @@ pub fn dot, N>(a: &V, b: &V) -> N { /// Computes the L2 norm of a vector. #[inline] -pub fn norm, N: BaseFloat>(v: &V) -> N { +pub fn norm(v: &V) -> V::NormType { Norm::norm(v) } /// Computes the squared L2 norm of a vector. #[inline] -pub fn norm_squared, N: BaseFloat>(v: &V) -> N { +pub fn norm_squared(v: &V) -> V::NormType { Norm::norm_squared(v) } /// Gets the normalized version of a vector. #[inline] -pub fn normalize, N: BaseFloat>(v: &V) -> V { +pub fn normalize(v: &V) -> V { Norm::normalize(v) } +/// Gets the normalized version of a vector or `None` if its norm is smaller than `min_norm`. +#[inline] +pub fn try_normalize(v: &V, min_norm: V::NormType) -> Option { + Norm::try_normalize(v, min_norm) +} + /* * Determinant */ diff --git a/src/linalg/decompositions.rs b/src/linalg/decompositions.rs index ba839070..9a6751fd 100644 --- a/src/linalg/decompositions.rs +++ b/src/linalg/decompositions.rs @@ -40,7 +40,7 @@ pub fn householder_matrix(dimension: usize, start: usize, vector: V) -> /// * `m` - matrix to decompose pub fn qr(m: &M) -> (M, M) where N: BaseFloat, - V: Indexable + Norm, + V: Indexable + Norm, M: Copy + Eye + ColumnSlice + Transpose + Indexable<(usize, usize), N> + Mul { let (rows, cols) = m.shape(); @@ -75,7 +75,7 @@ pub fn qr(m: &M) -> (M, M) pub fn eigen_qr(m: &M, eps: &N, niter: usize) -> (M, V) where N: BaseFloat, V: Mul, - VS: Indexable + Norm, + VS: Indexable + Norm, M: Indexable<(usize, usize), N> + SquareMatrix + Add + Sub + ColumnSlice + ApproxEq + Copy { @@ -264,7 +264,7 @@ pub fn eigen_qr(m: &M, eps: &N, niter: usize) -> (M, V) pub fn cholesky(m: &M) -> Result where N: BaseFloat, V: Mul, - VS: Indexable + Norm, + VS: Indexable + Norm, M: Indexable<(usize, usize), N> + SquareMatrix + Add + Sub + ColumnSlice + ApproxEq + Copy { @@ -316,7 +316,7 @@ pub fn cholesky(m: &M) -> Result /// * Second return value `h` - Matrix m in Hessenberg form pub fn hessenberg(m: &M) -> (M, M) where N: BaseFloat, - V: Indexable + Norm, + V: Indexable + Norm, M: Copy + Eye + ColumnSlice + Transpose + Indexable<(usize, usize), N> + Mul { diff --git a/src/structs/mod.rs b/src/structs/mod.rs index c443782a..983b8ac5 100644 --- a/src/structs/mod.rs +++ b/src/structs/mod.rs @@ -11,6 +11,7 @@ pub use self::similarity::{Similarity2, Similarity3}; pub use self::perspective::{Perspective3, PerspectiveMatrix3}; pub use self::orthographic::{Orthographic3, OrthographicMatrix3}; pub use self::quaternion::{Quaternion, UnitQuaternion}; +pub use self::unit::Unit; #[cfg(feature="generic_sizes")] pub use self::vectorn::VectorN; @@ -38,6 +39,7 @@ mod similarity_macros; mod similarity; mod perspective; mod orthographic; +mod unit; // Specialization for some 1d, 2d and 3d operations. #[doc(hidden)] diff --git a/src/structs/orthographic.rs b/src/structs/orthographic.rs index 3ec39bbd..9564fcfb 100644 --- a/src/structs/orthographic.rs +++ b/src/structs/orthographic.rs @@ -195,10 +195,8 @@ impl OrthographicMatrix3 { } /// Creates a new orthographic matrix from a 4D matrix. - /// - /// This is unsafe because the input matrix is not checked to be a orthographic projection. #[inline] - pub unsafe fn new_with_matrix(matrix: Matrix4) -> OrthographicMatrix3 { + pub fn new_with_matrix_unchecked(matrix: Matrix4) -> OrthographicMatrix3 { OrthographicMatrix3 { matrix: matrix } diff --git a/src/structs/perspective.rs b/src/structs/perspective.rs index 10c6216e..0a03c604 100644 --- a/src/structs/perspective.rs +++ b/src/structs/perspective.rs @@ -154,10 +154,8 @@ impl PerspectiveMatrix3 { } /// Creates a new perspective projection matrix from a 4D matrix. - /// - /// This is unsafe because the input matrix is not checked to be a perspective projection. #[inline] - pub unsafe fn new_with_matrix(matrix: Matrix4) -> PerspectiveMatrix3 { + pub fn new_with_matrix_unchecked(matrix: Matrix4) -> PerspectiveMatrix3 { PerspectiveMatrix3 { matrix: matrix } diff --git a/src/structs/quaternion.rs b/src/structs/quaternion.rs index 78b87d1e..0752a0ac 100644 --- a/src/structs/quaternion.rs +++ b/src/structs/quaternion.rs @@ -7,7 +7,7 @@ use std::ops::{Add, Sub, Mul, Div, Neg, AddAssign, SubAssign, MulAssign, DivAssi use std::iter::{FromIterator, IntoIterator}; use rand::{Rand, Rng}; use num::{Zero, One}; -use structs::{Vector3, Point3, Rotation3, Matrix3}; +use structs::{Vector3, Point3, Rotation3, Matrix3, Unit}; use traits::operations::{ApproxEq, Inverse, PartialOrder, PartialOrdering, Axpy}; use traits::structure::{Cast, Indexable, Iterable, IterableMut, Dimension, Shape, BaseFloat, BaseNum, Bounded, Repeat}; @@ -31,24 +31,21 @@ pub struct Quaternion { pub k: N } -impl Quaternion { +impl Quaternion { /// The vector part `(i, j, k)` of this quaternion. #[inline] pub fn vector(&self) -> &Vector3 { - // FIXME: do this require a `repr(C)` ? - unsafe { - mem::transmute(&self.i) - } + unsafe { mem::transmute(&self.i) } } /// The scalar part `w` of this quaternion. #[inline] - pub fn scalar(&self) -> &N { - &self.w + pub fn scalar(&self) -> N { + self.w } } -impl + Copy> Quaternion { +impl> Quaternion { /// Compute the conjugate of this quaternion. #[inline] pub fn conjugate(&self) -> Quaternion { @@ -64,7 +61,54 @@ impl + Copy> Quaternion { } } -impl> Inverse for Quaternion { +impl Quaternion { + /// Creates a new quaternion from its scalar and vector parts. + pub fn from_parts(scalar: N, vector: Vector3) -> Quaternion { + Quaternion::new(scalar, vector.x, vector.y, vector.z) + } + + /// 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 { + let rot = UnitQuaternion::from_axisangle(axis, theta * ::cast(2.0)); + + rot.unwrap() * scalar + } + + /// The polar decomposition of this 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); + + (n, angle, axis) + } + } + } +} + +impl Inverse for Quaternion { #[inline] fn inverse(&self) -> Option> { let mut res = *self; @@ -86,17 +130,16 @@ impl> Inverse for Quaternion { } else { self.conjugate_mut(); - self.w = self.w / norm_squared; - self.i = self.i / norm_squared; - self.j = self.j / norm_squared; - self.k = self.k / norm_squared; + *self /= norm_squared; true } } } -impl Norm for Quaternion { +impl Norm for Quaternion { + type NormType = N; + #[inline] fn norm_squared(&self) -> N { self.w * self.w + self.i * self.i + self.j * self.j + self.k * self.k @@ -105,20 +148,28 @@ impl Norm for Quaternion { #[inline] fn normalize(&self) -> Quaternion { let n = self.norm(); - Quaternion::new(self.w / n, self.i / n, self.j / n, self.k / n) + *self / n } #[inline] fn normalize_mut(&mut self) -> N { - let n = Norm::norm(self); - - self.w = self.w / n; - self.i = self.i / n; - self.j = self.j / n; - self.k = self.k / n; + let n = ::norm(self); + *self /= n; n } + + #[inline] + fn try_normalize(&self, min_norm: N) -> Option> { + let n = ::norm(self); + + if n <= min_norm { + None + } + else { + Some(*self / n) + } + } } impl Mul> for Quaternion @@ -143,7 +194,7 @@ impl MulAssign> for Quaternion } } -impl + BaseFloat> Div> for Quaternion { +impl Div> for Quaternion { type Output = Quaternion; #[inline] @@ -152,135 +203,159 @@ impl + BaseFloat> Div> for Quaternion { } } -impl + BaseFloat> DivAssign> for Quaternion { +impl DivAssign> for Quaternion { #[inline] fn div_assign(&mut self, right: Quaternion) { *self *= right.inverse().expect("Unable to invert the denominator.") } } +impl Quaternion { + /// Compute the exponential of a quaternion. + #[inline] + pub fn exp(&self) -> Self { + let v = *self.vector(); + let nn = v.norm_squared(); + + if nn.is_zero() { + ::one() + } + else { + let n = nn.sqrt(); + let nv = v / n * n.sin(); + Quaternion::from_parts(n.cos(), nv) * self.scalar().exp() + } + } + + /// Compute the natural logarithm of a quaternion. + #[inline] + pub fn ln(&self) -> Self { + let n = self.norm(); + let v = self.vector(); + let s = self.scalar(); + + Quaternion::from_parts(n.ln(), v.normalize() * (s / n).acos()) + } + + /// Raise the quaternion to a given floating power. + #[inline] + pub fn powf(&self, n: N) -> Self { + (self.ln() * n).exp() + } +} + +impl One for Quaternion where T: Copy + One + Zero + Sub + Add { + #[inline] + fn one() -> Self { + Quaternion::new(T::one(), T::zero(), T::zero(), T::zero()) + } +} + impl fmt::Display for Quaternion { fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { write!(f, "Quaternion {} − ({}, {}, {})", self.w, self.i, self.j, self.k) } } -/// 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 -} +/// 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 { - /// Creates a new unit quaternion from the axis-angle representation of a rotation. + /// Creates a new quaternion from a unit vector (the rotation axis) and an angle + /// (the rotation angle). #[inline] - pub fn new(axisangle: Vector3) -> UnitQuaternion { - let sqang = Norm::norm_squared(&axisangle); + pub fn from_axisangle(axis: Unit>, angle: N) -> UnitQuaternion { + let (sang, cang) = (angle / ::cast(2.0)).sin_cos(); - if ::is_zero(&sqang) { - ::one() - } - else { - let ang = sqang.sqrt(); - let (s, c) = (ang / Cast::from(2.0)).sin_cos(); + let q = Quaternion::from_parts(cang, axis.unwrap() * sang); + Unit::from_unit_value_unchecked(q) + } - let s_ang = s / ang; - - unsafe { - UnitQuaternion::new_with_unit_quaternion( - Quaternion::new( - c, - axisangle.x * s_ang, - axisangle.y * s_ang, - axisangle.z * s_ang) - ) - } - } + /// Same as `::from_axisangle` with the axis multiplied with the angle. + #[inline] + pub fn from_scaled_axis(axis: Vector3) -> UnitQuaternion { + let two: N = ::cast(2.0); + let q = Quaternion::from_parts(::zero(), axis * two).exp(); + UnitQuaternion::from_unit_value_unchecked(q) } /// Creates a new unit quaternion from a quaternion. /// /// The input quaternion will be normalized. #[inline] - pub fn new_with_quaternion(q: Quaternion) -> UnitQuaternion { - UnitQuaternion { q: q.normalize() } + pub fn from_quaternion(q: &Quaternion) -> UnitQuaternion { + Unit::new(&q) } /// Creates a new unit quaternion from Euler angles. /// /// The primitive rotations are applied in order: 1 roll − 2 pitch − 3 yaw. #[inline] - pub fn new_with_euler_angles(roll: N, pitch: N, yaw: N) -> UnitQuaternion { + pub fn from_euler_angles(roll: N, pitch: N, yaw: N) -> UnitQuaternion { let (sr, cr) = (roll * ::cast(0.5)).sin_cos(); let (sp, cp) = (pitch * ::cast(0.5)).sin_cos(); let (sy, cy) = (yaw * ::cast(0.5)).sin_cos(); - unsafe { - UnitQuaternion::new_with_unit_quaternion( - Quaternion::new( + let q = Quaternion::new( cr * cp * cy + sr * sp * sy, sr * cp * cy - cr * sp * sy, cr * sp * cy + sr * cp * sy, - cr * cp * sy - sr * sp * cy) - ) - } + cr * cp * sy - sr * sp * cy); + + Unit::from_unit_value_unchecked(q) + } + + /// The rotation angle of this unit quaternion. + #[inline] + pub fn angle(&self) -> N { + self.as_ref().scalar().acos() + } + + /// The rotation axis of this unit quaternion or `None` if the rotation is zero. + #[inline] + pub fn axis(&self) -> Option>> { + Unit::try_new(self.as_ref().vector(), ::zero()) } /// Builds a rotation matrix from this quaternion. pub fn to_rotation_matrix(&self) -> Rotation3 { - let ww = self.q.w * self.q.w; - let ii = self.q.i * self.q.i; - let jj = self.q.j * self.q.j; - let kk = self.q.k * self.q.k; - let ij = self.q.i * self.q.j * ::cast(2.0); - let wk = self.q.w * self.q.k * ::cast(2.0); - let wj = self.q.w * self.q.j * ::cast(2.0); - let ik = self.q.i * self.q.k * ::cast(2.0); - let jk = self.q.j * self.q.k * ::cast(2.0); - let wi = self.q.w * self.q.i * ::cast(2.0); + let ww = self.as_ref().w * self.as_ref().w; + let ii = self.as_ref().i * self.as_ref().i; + let jj = self.as_ref().j * self.as_ref().j; + let kk = self.as_ref().k * self.as_ref().k; + let ij = self.as_ref().i * self.as_ref().j * ::cast(2.0); + let wk = self.as_ref().w * self.as_ref().k * ::cast(2.0); + let wj = self.as_ref().w * self.as_ref().j * ::cast(2.0); + let ik = self.as_ref().i * self.as_ref().k * ::cast(2.0); + let jk = self.as_ref().j * self.as_ref().k * ::cast(2.0); + let wi = self.as_ref().w * self.as_ref().i * ::cast(2.0); - unsafe { - Rotation3::new_with_matrix( - Matrix3::new( - ww + ii - jj - kk, ij - wk, wj + ik, - wk + ij, ww - ii + jj - kk, jk - wi, - ik - wj, wi + jk, ww - ii - jj + kk - ) + Rotation3::new_with_matrix_unchecked( + Matrix3::new( + ww + ii - jj - kk, ij - wk, wj + ik, + wk + ij, ww - ii + jj - kk, jk - wi, + ik - wj, wi + jk, ww - ii - jj + kk ) - } - } -} - - -impl UnitQuaternion { - /// Creates a new unit quaternion from a quaternion. - /// - /// This is unsafe because the input quaternion will not be normalized. - #[inline] - pub unsafe fn new_with_unit_quaternion(q: Quaternion) -> UnitQuaternion { - UnitQuaternion { - q: q - } - } - - /// The `Quaternion` representation of this unit quaternion. - #[inline] - pub fn quaternion(&self) -> &Quaternion { - &self.q + ) } } impl One for UnitQuaternion { #[inline] fn one() -> UnitQuaternion { - unsafe { - UnitQuaternion::new_with_unit_quaternion(Quaternion::new(::one(), ::zero(), ::zero(), ::zero())) - } + let one = Quaternion::new(::one(), ::zero(), ::zero(), ::zero()); + UnitQuaternion::from_unit_value_unchecked(one) } } -impl> Inverse for UnitQuaternion { +impl> Inverse for UnitQuaternion { #[inline] fn inverse(&self) -> Option> { let mut cpy = *self; @@ -291,7 +366,7 @@ impl> Inverse for UnitQuaternion { #[inline] fn inverse_mut(&mut self) -> bool { - self.q.conjugate_mut(); + *self = Unit::from_unit_value_unchecked(self.as_ref().conjugate()); true } @@ -300,7 +375,7 @@ impl> Inverse for UnitQuaternion { impl Rand for UnitQuaternion { #[inline] fn rand(rng: &mut R) -> UnitQuaternion { - UnitQuaternion::new(rng.gen()) + UnitQuaternion::new(&rng.gen()) } } @@ -317,28 +392,28 @@ impl> ApproxEq for UnitQuaternion { #[inline] fn approx_eq_eps(&self, other: &UnitQuaternion, eps: &N) -> bool { - ApproxEq::approx_eq_eps(&self.q, &other.q, eps) + ApproxEq::approx_eq_eps(self.as_ref(), other.as_ref(), eps) } #[inline] fn approx_eq_ulps(&self, other: &UnitQuaternion, ulps: u32) -> bool { - ApproxEq::approx_eq_ulps(&self.q, &other.q, ulps) + ApproxEq::approx_eq_ulps(self.as_ref(), other.as_ref(), ulps) } } -impl> Div> for UnitQuaternion { +impl Div> for UnitQuaternion { type Output = UnitQuaternion; #[inline] fn div(self, other: UnitQuaternion) -> UnitQuaternion { - UnitQuaternion { q: self.q / other.q } + Unit::from_unit_value_unchecked(self.unwrap() / other.unwrap()) } } -impl> DivAssign> for UnitQuaternion { +impl DivAssign> for UnitQuaternion { #[inline] fn div_assign(&mut self, other: UnitQuaternion) { - self.q /= other.q + *self = Unit::from_unit_value_unchecked(*self.as_ref() / *other.as_ref()) } } @@ -347,14 +422,14 @@ impl Mul> for UnitQuaternion { #[inline] fn mul(self, right: UnitQuaternion) -> UnitQuaternion { - UnitQuaternion { q: self.q * right.q } + Unit::from_unit_value_unchecked(self.unwrap() * right.unwrap()) } } impl MulAssign> for UnitQuaternion { #[inline] fn mul_assign(&mut self, right: UnitQuaternion) { - self.q *= right.q + *self = Unit::from_unit_value_unchecked(*self.as_ref() * *right.as_ref()) } } @@ -364,12 +439,9 @@ impl Mul> for UnitQuaternion { #[inline] fn mul(self, right: Vector3) -> Vector3 { let two: N = ::one::() + ::one(); - let mut t = ::cross(self.q.vector(), &right); - t.x = t.x * two; - t.y = t.y * two; - t.z = t.z * two; + let t = ::cross(self.as_ref().vector(), &right); - Vector3::new(t.x * self.q.w, t.y * self.q.w, t.z * self.q.w) + ::cross(self.q.vector(), &t) + right + t * (two * self.as_ref().w) + ::cross(self.as_ref().vector(), &t) + right } } @@ -421,14 +493,11 @@ impl> MulAssign> for Point3 { impl Rotation> for UnitQuaternion { #[inline] fn rotation(&self) -> Vector3 { - let mut v = *self.q.vector(); - let ang = v.normalize_mut().atan2(self.q.w) * ::cast(2.0); - - if ::is_zero(&ang) { - ::zero() + if let Some(v) = self.axis() { + v.unwrap() * self.angle() } else { - Vector3::new(v.x * ang, v.y * ang, v.z * ang) + ::zero() } } @@ -444,7 +513,7 @@ impl Rotation> for UnitQuaternion { #[inline] fn append_rotation(&self, amount: &Vector3) -> UnitQuaternion { - *self * UnitQuaternion::new(*amount) + *self * UnitQuaternion::from_scaled_axis(*amount) } #[inline] @@ -454,12 +523,12 @@ impl Rotation> for UnitQuaternion { #[inline] fn prepend_rotation(&self, amount: &Vector3) -> UnitQuaternion { - UnitQuaternion::new(*amount) * *self + UnitQuaternion::from_scaled_axis(*amount) * *self } #[inline] fn set_rotation(&mut self, v: Vector3) { - *self = UnitQuaternion::new(v) + *self = UnitQuaternion::from_scaled_axis(v); } } @@ -496,7 +565,7 @@ impl> Rotate> for UnitQuaternion { } } -impl> RotationTo for UnitQuaternion { +impl RotationTo for UnitQuaternion { type AngleType = N; type DeltaRotationType = UnitQuaternion; @@ -504,7 +573,7 @@ impl> RotationTo for UnitQuaternion { fn angle_to(&self, other: &Self) -> N { let delta = self.rotation_to(other); - delta.q.vector().norm().atan2(delta.q.w) * ::cast(2.0) + delta.as_ref().w.acos() * ::cast(2.0) } #[inline] @@ -539,7 +608,8 @@ impl> Transform> for UnitQuaternion { impl fmt::Display for UnitQuaternion { fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { - write!(f, "Unit quaternion {} − ({}, {}, {})", self.q.w, self.q.i, self.q.j, self.q.k) + write!(f, "Unit quaternion {} − ({}, {}, {})", + self.as_ref().w, self.as_ref().i, self.as_ref().j, self.as_ref().k) } } @@ -558,42 +628,44 @@ impl Dimension for UnitQuaternion { #[cfg(feature="arbitrary")] impl Arbitrary for UnitQuaternion { fn arbitrary(g: &mut G) -> UnitQuaternion { - UnitQuaternion::new(Arbitrary::arbitrary(g)) + UnitQuaternion::new(&Arbitrary::arbitrary(g)) } } -impl Quaternion { - /// Compute the exponential of a quaternion. - pub fn exp(&self) -> Self { - let v = *self.vector(); - let nn = v.norm_squared(); - - if nn.is_zero() { - ::one() - } else { - let n = nn.sqrt(); - let nv = v / n * n.sin(); - Quaternion::new(n.cos(), nv.x, nv.y, nv.z) +impl UnitQuaternion { + /// Compute the exponential of a quaternion. + /// + /// Note that this function yields a `Quaternion` because it looses the unit property. + pub fn exp(&self) -> Quaternion { + self.as_ref().exp() } - } - /// Compute the natural logarithm (a.k.a ln()) of a quaternion. - /// - /// Becareful, this function yields a `Quaternion`, losing the unit property. - pub fn log(&self) -> Self { - (Quaternion { w: 0., .. *self }).normalize() * self.w.acos() - } + /// Compute the natural logarithm of a quaternion. + /// + /// Note that this function yields a `Quaternion` because it looses the unit property. The + /// vector part of the return value corresponds to the axis-angle representation (divided by + /// 2.0) of this unit quaternion. + pub fn ln(&self) -> Quaternion { + if let Some(v) = self.axis() { + Quaternion::from_parts(::zero(), v.unwrap() * self.angle()) + } + else { + ::zero() + } + } - /// Raise the quaternion to a given floating power. - pub fn powf(&self, n: f32) -> Self { - (self.log() * n).exp() - } -} - -impl One for Quaternion where T: Copy + One + Zero + Sub + Add { - fn one() -> Self { - Quaternion::new(T::one(), T::zero(), T::zero(), T::zero()) - } + /// Raise this unit quaternion to a given floating power. + /// + /// If this unit quaternion represents a rotation by `theta`, then the resulting quaternion + /// rotates by `n * theta`. + pub fn powf(&self, n: N) -> Self { + if let Some(v) = self.axis() { + UnitQuaternion::from_axisangle(v, self.angle() * n) + } + else { + ::one() + } + } } componentwise_zero!(Quaternion, w, i, j, k); diff --git a/src/structs/rotation.rs b/src/structs/rotation.rs index 0bcf940f..4c8f63a8 100644 --- a/src/structs/rotation.rs +++ b/src/structs/rotation.rs @@ -157,9 +157,7 @@ impl Rotation3 { } /// Builds a rotation matrix from an orthogonal matrix. - /// - /// This is unsafe because the orthogonality of `matrix` is not checked. - pub unsafe fn new_with_matrix(matrix: Matrix3) -> Rotation3 { + pub fn new_with_matrix_unchecked(matrix: Matrix3) -> Rotation3 { Rotation3 { submatrix: matrix } @@ -173,15 +171,13 @@ impl Rotation3 { let (sp, cp) = pitch.sin_cos(); let (sy, cy) = yaw.sin_cos(); - unsafe { - Rotation3::new_with_matrix( - Matrix3::new( - cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr, - sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr, - -sp, cp * sr, cp * cr + Rotation3::new_with_matrix_unchecked( + Matrix3::new( + cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr, + sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr, + -sp, cp * sr, cp * cr ) ) - } } } @@ -202,12 +198,10 @@ impl Rotation3 { let xaxis = Norm::normalize(&Cross::cross(up, &zaxis)); let yaxis = Norm::normalize(&Cross::cross(&zaxis, &xaxis)); - unsafe { - Rotation3::new_with_matrix(Matrix3::new( + Rotation3::new_with_matrix_unchecked(Matrix3::new( xaxis.x, yaxis.x, zaxis.x, xaxis.y, yaxis.y, zaxis.y, xaxis.z, yaxis.z, zaxis.z)) - } } diff --git a/src/structs/specializations/vector.rs b/src/structs/specializations/vector.rs index 4910d420..681c82fe 100644 --- a/src/structs/specializations/vector.rs +++ b/src/structs/specializations/vector.rs @@ -172,10 +172,10 @@ impl Basis for Vector3 { fn orthonormal_subspace_basis) -> bool>(n: &Vector3, mut f: F) { let a = if n.x.abs() > n.y.abs() { - Norm::normalize(&Vector3::new(n.z, ::zero(), -n.x)) + ::normalize(&Vector3::new(n.z, ::zero(), -n.x)) } else { - Norm::normalize(&Vector3::new(::zero(), -n.z, n.y)) + ::normalize(&Vector3::new(::zero(), -n.z, n.y)) }; if !f(Cross::cross(&a, n)) { return }; diff --git a/src/structs/vector_macros.rs b/src/structs/vector_macros.rs index 5cdd2cda..3b6d832f 100644 --- a/src/structs/vector_macros.rs +++ b/src/structs/vector_macros.rs @@ -299,7 +299,9 @@ macro_rules! vector_impl( * Norm * */ - impl Norm for $t { + impl Norm for $t { + type NormType = N; + #[inline] fn norm_squared(&self) -> N { Dot::dot(self, self) @@ -314,11 +316,22 @@ macro_rules! vector_impl( #[inline] fn normalize_mut(&mut self) -> N { - let l = Norm::norm(self); + let n = ::norm(self); + *self /= n; - $(self.$compN = self.$compN / l;)* + n + } - l + #[inline] + fn try_normalize(&self, min_norm: N) -> Option<$t> { + let n = ::norm(self); + + if n <= min_norm { + None + } + else { + Some(*self / n) + } } } @@ -461,7 +474,7 @@ macro_rules! vector_impl( } impl FloatVector for $t - where N: BaseFloat + ApproxEq { + where N: BaseFloat { } @@ -508,7 +521,7 @@ macro_rules! vector_impl( macro_rules! basis_impl( ($t: ident, $dimension: expr) => ( - impl> Basis for $t { + impl Basis for $t { #[inline] fn canonical_basis) -> bool>(mut f: F) { for i in 0 .. $dimension { @@ -541,8 +554,8 @@ macro_rules! basis_impl( elt = elt - *v * Dot::dot(&elt, v) }; - if !ApproxEq::approx_eq(&Norm::norm_squared(&elt), &::zero()) { - let new_element = Norm::normalize(&elt); + if !ApproxEq::approx_eq(&::norm_squared(&elt), &::zero()) { + let new_element = ::normalize(&elt); if !f(new_element) { return }; diff --git a/src/structs/vectorn_macros.rs b/src/structs/vectorn_macros.rs index 7e7f2d54..52fc0b67 100644 --- a/src/structs/vectorn_macros.rs +++ b/src/structs/vectorn_macros.rs @@ -253,6 +253,15 @@ macro_rules! vecn_dvec_common_impl( } } + impl<'a, N: Copy + Div + Zero $(, $param : ArrayLength)*> Div for &'a $vecn { + type Output = $vecn; + + #[inline] + fn div(self, right: N) -> $vecn { + self.clone() / right + } + } + impl)*> DivAssign<$vecn> for $vecn where N: Copy + DivAssign + Zero $(, $param : ArrayLength)* { #[inline] @@ -495,10 +504,12 @@ macro_rules! vecn_dvec_common_impl( * Norm. * */ - impl)*> Norm for $vecn { + impl)*> Norm for $vecn { + type NormType = N; + #[inline] fn norm_squared(&self) -> N { - Dot::dot(self, self) + ::dot(self, self) } #[inline] @@ -510,13 +521,22 @@ macro_rules! vecn_dvec_common_impl( #[inline] fn normalize_mut(&mut self) -> N { - let l = Norm::norm(self); + let n = ::norm(self); + *self /= n; - for n in self.as_mut().iter_mut() { - *n = *n / l; + n + } + + #[inline] + fn try_normalize(&self, min_norm: N) -> Option<$vecn> { + let n = ::norm(self); + + if n <= min_norm { + None + } + else { + Some(self / n) } - - l } } diff --git a/src/traits/geometry.rs b/src/traits/geometry.rs index 8c41ae33..7131f0ed 100644 --- a/src/traits/geometry.rs +++ b/src/traits/geometry.rs @@ -1,6 +1,7 @@ //! Traits of operations having a well-known or explicit geometric meaning. use std::ops::{Neg, Mul}; +use num::Float; use traits::structure::{BaseFloat, SquareMatrix}; /// Trait of object which represent a translation, and to wich new translation @@ -224,23 +225,35 @@ pub trait Dot { } /// Traits of objects having an euclidian norm. -pub trait Norm { +pub trait Norm: Sized { + /// The scalar type for the norm (i.e. the undelying field). + type NormType : BaseFloat; + /// Computes the norm of `self`. #[inline] - fn norm(&self) -> N { + fn norm(&self) -> Self::NormType { self.norm_squared().sqrt() } /// Computes the squared norm of `self`. /// /// This is usually faster than computing the norm itself. - fn norm_squared(&self) -> N; + fn norm_squared(&self) -> Self::NormType; /// Gets the normalized version of a copy of `v`. + /// + /// Might return an invalid result if the vector is zero or close to zero. fn normalize(&self) -> Self; /// Normalizes `self`. - fn normalize_mut(&mut self) -> N; + /// + /// The result might be invalid if the vector is zero or close to zero. + fn normalize_mut(&mut self) -> Self::NormType; + + /// Gets the normalized version of a copy of `v` or `None` if the vector has a norm smaller + /// 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; } /** diff --git a/src/traits/operations.rs b/src/traits/operations.rs index 48f80be9..5934293f 100644 --- a/src/traits/operations.rs +++ b/src/traits/operations.rs @@ -1,6 +1,6 @@ //! Low level operations on vectors and matrices. -use std::mem; +use std::{mem, f32, f64}; use num::Signed; use std::ops::Mul; use std::cmp::Ordering; @@ -173,7 +173,7 @@ pub trait ApproxEq: Sized { impl ApproxEq for f32 { #[inline] fn approx_epsilon(_: Option) -> f32 { - 1.0e-6 + f32::EPSILON * 10.0 } #[inline] @@ -204,7 +204,7 @@ impl ApproxEq for f32 { impl ApproxEq for f64 { #[inline] fn approx_epsilon(_: Option) -> f64 { - 1.0e-6 + f64::EPSILON * 10.0 } #[inline] diff --git a/src/traits/structure.rs b/src/traits/structure.rs index 96c38b3b..02e6d165 100644 --- a/src/traits/structure.rs +++ b/src/traits/structure.rs @@ -6,7 +6,7 @@ use std::ops::{Add, Sub, Mul, Div, Rem, AddAssign, SubAssign, MulAssign, DivAssign, RemAssign, Index, IndexMut, Neg}; use num::{Float, Zero, One}; -use traits::operations::{Axpy, Transpose, Inverse, Absolute}; +use traits::operations::{Axpy, Transpose, Inverse, Absolute, ApproxEq}; use traits::geometry::{Dot, Norm, Origin}; /// Basic integral numeric trait. @@ -21,7 +21,7 @@ pub trait BaseNum: Copy + Zero + One + } /// Basic floating-point number numeric trait. -pub trait BaseFloat: Float + Cast + BaseNum + Neg { +pub trait BaseFloat: Float + Cast + BaseNum + ApproxEq + Neg { /// Archimedes' constant. fn pi() -> Self; /// 2.0 * pi. @@ -243,7 +243,7 @@ pub trait NumVector: Add + Sub + } /// Trait of vector with components implementing the `BaseFloat` trait. -pub trait FloatVector: NumVector + Norm + Neg + Basis { +pub trait FloatVector: NumVector + Norm + Neg + Basis + ApproxEq { } /* @@ -289,7 +289,7 @@ pub trait NumPoint: /// Trait of points with components implementing the `BaseFloat` trait. pub trait FloatPoint: NumPoint + Sized - where ::Vector: Norm { + where ::Vector: Norm { /// Computes the square distance between two points. #[inline] fn distance_squared(&self, other: &Self) -> N {