forked from M-Labs/nalgebra
Add a Unit wrapper type, remove UnitQuaternion.
The `Unit` wrapper type ensures that elements of the underlying type has a unit norm. For example, `Unit<Vector3>` designates an element of S². In particular `UnitQuaternion<N>` is now a type alias for `Unit<Quaternion<N>>`.
This commit is contained in:
Sébastien Crozet
parent
88a74ca4e5
commit
d45c242a15
28
CHANGELOG.md
28
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<T>` type that wraps normalized values. In particular,
|
||||
`UnitQuaternion<N>` is now replaced by `Unit<Quaternion<N>>`.
|
||||
- `.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:
|
||||
|
||||
@ -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
|
||||
|
||||
21
src/lib.rs
21
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: Origin>() -> P {
|
||||
/// Returns the center of two points.
|
||||
#[inline]
|
||||
pub fn center<N: BaseFloat, P: FloatPoint<N>>(a: &P, b: &P) -> P
|
||||
where <P as PointAsVector>::Vector: Norm<N> {
|
||||
where <P as PointAsVector>::Vector: Norm<NormType = N> {
|
||||
(*a + b.to_vector()) / ::cast(2.0)
|
||||
}
|
||||
|
||||
@ -321,14 +322,14 @@ pub fn center<N: BaseFloat, P: FloatPoint<N>>(a: &P, b: &P) -> P
|
||||
*/
|
||||
/// Returns the distance between two points.
|
||||
#[inline]
|
||||
pub fn distance<N: BaseFloat, P: FloatPoint<N>>(a: &P, b: &P) -> N where <P as PointAsVector>::Vector: Norm<N> {
|
||||
pub fn distance<N: BaseFloat, P: FloatPoint<N>>(a: &P, b: &P) -> N where <P as PointAsVector>::Vector: Norm<NormType = N> {
|
||||
a.distance(b)
|
||||
}
|
||||
|
||||
/// Returns the squared distance between two points.
|
||||
#[inline]
|
||||
pub fn distance_squared<N: BaseFloat, P: FloatPoint<N>>(a: &P, b: &P) -> N
|
||||
where <P as PointAsVector>::Vector: Norm<N> {
|
||||
where <P as PointAsVector>::Vector: Norm<NormType = N> {
|
||||
a.distance_squared(b)
|
||||
}
|
||||
|
||||
@ -664,22 +665,28 @@ pub fn dot<V: Dot<N>, N>(a: &V, b: &V) -> N {
|
||||
|
||||
/// Computes the L2 norm of a vector.
|
||||
#[inline]
|
||||
pub fn norm<V: Norm<N>, N: BaseFloat>(v: &V) -> N {
|
||||
pub fn norm<V: Norm>(v: &V) -> V::NormType {
|
||||
Norm::norm(v)
|
||||
}
|
||||
|
||||
/// Computes the squared L2 norm of a vector.
|
||||
#[inline]
|
||||
pub fn norm_squared<V: Norm<N>, N: BaseFloat>(v: &V) -> N {
|
||||
pub fn norm_squared<V: Norm>(v: &V) -> V::NormType {
|
||||
Norm::norm_squared(v)
|
||||
}
|
||||
|
||||
/// Gets the normalized version of a vector.
|
||||
#[inline]
|
||||
pub fn normalize<V: Norm<N>, N: BaseFloat>(v: &V) -> V {
|
||||
pub fn normalize<V: Norm>(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: Norm>(v: &V, min_norm: V::NormType) -> Option<V> {
|
||||
Norm::try_normalize(v, min_norm)
|
||||
}
|
||||
|
||||
/*
|
||||
* Determinant<N>
|
||||
*/
|
||||
|
||||
@ -40,7 +40,7 @@ pub fn householder_matrix<N, V, M>(dimension: usize, start: usize, vector: V) ->
|
||||
/// * `m` - matrix to decompose
|
||||
pub fn qr<N, V, M>(m: &M) -> (M, M)
|
||||
where N: BaseFloat,
|
||||
V: Indexable<usize, N> + Norm<N>,
|
||||
V: Indexable<usize, N> + Norm<NormType = N>,
|
||||
M: Copy + Eye + ColumnSlice<V> + Transpose + Indexable<(usize, usize), N> +
|
||||
Mul<M, Output = M> {
|
||||
let (rows, cols) = m.shape();
|
||||
@ -75,7 +75,7 @@ pub fn qr<N, V, M>(m: &M) -> (M, M)
|
||||
pub fn eigen_qr<N, V, VS, M>(m: &M, eps: &N, niter: usize) -> (M, V)
|
||||
where N: BaseFloat,
|
||||
V: Mul<M, Output = V>,
|
||||
VS: Indexable<usize, N> + Norm<N>,
|
||||
VS: Indexable<usize, N> + Norm<NormType = N>,
|
||||
M: Indexable<(usize, usize), N> + SquareMatrix<N, V> + Add<M, Output = M> +
|
||||
Sub<M, Output = M> + ColumnSlice<VS> +
|
||||
ApproxEq<N> + Copy {
|
||||
@ -264,7 +264,7 @@ pub fn eigen_qr<N, V, VS, M>(m: &M, eps: &N, niter: usize) -> (M, V)
|
||||
pub fn cholesky<N, V, VS, M>(m: &M) -> Result<M, &'static str>
|
||||
where N: BaseFloat,
|
||||
V: Mul<M, Output = V>,
|
||||
VS: Indexable<usize, N> + Norm<N>,
|
||||
VS: Indexable<usize, N> + Norm<NormType = N>,
|
||||
M: Indexable<(usize, usize), N> + SquareMatrix<N, V> + Add<M, Output = M> +
|
||||
Sub<M, Output = M> + ColumnSlice<VS> +
|
||||
ApproxEq<N> + Copy {
|
||||
@ -316,7 +316,7 @@ pub fn cholesky<N, V, VS, M>(m: &M) -> Result<M, &'static str>
|
||||
/// * Second return value `h` - Matrix m in Hessenberg form
|
||||
pub fn hessenberg<N, V, M>(m: &M) -> (M, M)
|
||||
where N: BaseFloat,
|
||||
V: Indexable<usize, N> + Norm<N>,
|
||||
V: Indexable<usize, N> + Norm<NormType = N>,
|
||||
M: Copy + Eye + ColumnSlice<V> + Transpose + Indexable<(usize, usize), N> +
|
||||
Mul<M, Output = M> {
|
||||
|
||||
|
||||
@ -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)]
|
||||
|
||||
@ -195,10 +195,8 @@ impl<N: BaseFloat> OrthographicMatrix3<N> {
|
||||
}
|
||||
|
||||
/// 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<N>) -> OrthographicMatrix3<N> {
|
||||
pub fn new_with_matrix_unchecked(matrix: Matrix4<N>) -> OrthographicMatrix3<N> {
|
||||
OrthographicMatrix3 {
|
||||
matrix: matrix
|
||||
}
|
||||
|
||||
@ -154,10 +154,8 @@ impl<N: BaseFloat> PerspectiveMatrix3<N> {
|
||||
}
|
||||
|
||||
/// 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<N>) -> PerspectiveMatrix3<N> {
|
||||
pub fn new_with_matrix_unchecked(matrix: Matrix4<N>) -> PerspectiveMatrix3<N> {
|
||||
PerspectiveMatrix3 {
|
||||
matrix: matrix
|
||||
}
|
||||
|
||||
@ -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<N> {
|
||||
pub k: N
|
||||
}
|
||||
|
||||
impl<N> Quaternion<N> {
|
||||
impl<N: Copy> Quaternion<N> {
|
||||
/// The vector part `(i, j, k)` of this quaternion.
|
||||
#[inline]
|
||||
pub fn vector(&self) -> &Vector3<N> {
|
||||
// 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<N: Neg<Output = N> + Copy> Quaternion<N> {
|
||||
impl<N: BaseNum + Neg<Output = N>> Quaternion<N> {
|
||||
/// Compute the conjugate of this quaternion.
|
||||
#[inline]
|
||||
pub fn conjugate(&self) -> Quaternion<N> {
|
||||
@ -64,7 +61,54 @@ impl<N: Neg<Output = N> + Copy> Quaternion<N> {
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: BaseFloat + ApproxEq<N>> Inverse for Quaternion<N> {
|
||||
impl<N: BaseFloat> Quaternion<N> {
|
||||
/// Creates a new quaternion from its scalar and vector parts.
|
||||
pub fn from_parts(scalar: N, vector: Vector3<N>) -> Quaternion<N> {
|
||||
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<Vector3<N>>) -> Quaternion<N> {
|
||||
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<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);
|
||||
|
||||
(n, angle, axis)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: BaseFloat> Inverse for Quaternion<N> {
|
||||
#[inline]
|
||||
fn inverse(&self) -> Option<Quaternion<N>> {
|
||||
let mut res = *self;
|
||||
@ -86,17 +130,16 @@ impl<N: BaseFloat + ApproxEq<N>> Inverse for Quaternion<N> {
|
||||
}
|
||||
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<N: BaseFloat> Norm<N> for Quaternion<N> {
|
||||
impl<N: BaseFloat> Norm for Quaternion<N> {
|
||||
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<N: BaseFloat> Norm<N> for Quaternion<N> {
|
||||
#[inline]
|
||||
fn normalize(&self) -> Quaternion<N> {
|
||||
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<Quaternion<N>> {
|
||||
let n = ::norm(self);
|
||||
|
||||
if n <= min_norm {
|
||||
None
|
||||
}
|
||||
else {
|
||||
Some(*self / n)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl<N> Mul<Quaternion<N>> for Quaternion<N>
|
||||
@ -143,7 +194,7 @@ impl<N> MulAssign<Quaternion<N>> for Quaternion<N>
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: ApproxEq<N> + BaseFloat> Div<Quaternion<N>> for Quaternion<N> {
|
||||
impl<N: BaseFloat> Div<Quaternion<N>> for Quaternion<N> {
|
||||
type Output = Quaternion<N>;
|
||||
|
||||
#[inline]
|
||||
@ -152,135 +203,159 @@ impl<N: ApproxEq<N> + BaseFloat> Div<Quaternion<N>> for Quaternion<N> {
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: ApproxEq<N> + BaseFloat> DivAssign<Quaternion<N>> for Quaternion<N> {
|
||||
impl<N: BaseFloat> DivAssign<Quaternion<N>> for Quaternion<N> {
|
||||
#[inline]
|
||||
fn div_assign(&mut self, right: Quaternion<N>) {
|
||||
*self *= right.inverse().expect("Unable to invert the denominator.")
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: BaseFloat> Quaternion<N> {
|
||||
/// 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<T> One for Quaternion<T> where T: Copy + One + Zero + Sub<T, Output = T> + Add<T, Output = T> {
|
||||
#[inline]
|
||||
fn one() -> Self {
|
||||
Quaternion::new(T::one(), T::zero(), T::zero(), T::zero())
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: fmt::Display> fmt::Display for Quaternion<N> {
|
||||
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<N> {
|
||||
q: 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: BaseFloat> UnitQuaternion<N> {
|
||||
/// 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<N>) -> UnitQuaternion<N> {
|
||||
let sqang = Norm::norm_squared(&axisangle);
|
||||
pub fn from_axisangle(axis: Unit<Vector3<N>>, angle: N) -> UnitQuaternion<N> {
|
||||
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<N>) -> UnitQuaternion<N> {
|
||||
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<N>) -> UnitQuaternion<N> {
|
||||
UnitQuaternion { q: q.normalize() }
|
||||
pub fn from_quaternion(q: &Quaternion<N>) -> UnitQuaternion<N> {
|
||||
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<N> {
|
||||
pub fn from_euler_angles(roll: N, pitch: N, yaw: N) -> UnitQuaternion<N> {
|
||||
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<Vector3<N>>> {
|
||||
Unit::try_new(self.as_ref().vector(), ::zero())
|
||||
}
|
||||
|
||||
/// Builds a rotation matrix from this quaternion.
|
||||
pub fn to_rotation_matrix(&self) -> Rotation3<N> {
|
||||
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<N> UnitQuaternion<N> {
|
||||
/// 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<N>) -> UnitQuaternion<N> {
|
||||
UnitQuaternion {
|
||||
q: q
|
||||
}
|
||||
}
|
||||
|
||||
/// The `Quaternion` representation of this unit quaternion.
|
||||
#[inline]
|
||||
pub fn quaternion(&self) -> &Quaternion<N> {
|
||||
&self.q
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: BaseNum> One for UnitQuaternion<N> {
|
||||
#[inline]
|
||||
fn one() -> UnitQuaternion<N> {
|
||||
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<N: Copy + Neg<Output = N>> Inverse for UnitQuaternion<N> {
|
||||
impl<N: BaseNum + Neg<Output = N>> Inverse for UnitQuaternion<N> {
|
||||
#[inline]
|
||||
fn inverse(&self) -> Option<UnitQuaternion<N>> {
|
||||
let mut cpy = *self;
|
||||
@ -291,7 +366,7 @@ impl<N: Copy + Neg<Output = N>> Inverse for UnitQuaternion<N> {
|
||||
|
||||
#[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<N: Copy + Neg<Output = N>> Inverse for UnitQuaternion<N> {
|
||||
impl<N: Rand + BaseFloat> Rand for UnitQuaternion<N> {
|
||||
#[inline]
|
||||
fn rand<R: Rng>(rng: &mut R) -> UnitQuaternion<N> {
|
||||
UnitQuaternion::new(rng.gen())
|
||||
UnitQuaternion::new(&rng.gen())
|
||||
}
|
||||
}
|
||||
|
||||
@ -317,28 +392,28 @@ impl<N: ApproxEq<N>> ApproxEq<N> for UnitQuaternion<N> {
|
||||
|
||||
#[inline]
|
||||
fn approx_eq_eps(&self, other: &UnitQuaternion<N>, 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<N>, ulps: u32) -> bool {
|
||||
ApproxEq::approx_eq_ulps(&self.q, &other.q, ulps)
|
||||
ApproxEq::approx_eq_ulps(self.as_ref(), other.as_ref(), ulps)
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: BaseFloat + ApproxEq<N>> Div<UnitQuaternion<N>> for UnitQuaternion<N> {
|
||||
impl<N: BaseFloat> Div<UnitQuaternion<N>> for UnitQuaternion<N> {
|
||||
type Output = UnitQuaternion<N>;
|
||||
|
||||
#[inline]
|
||||
fn div(self, other: UnitQuaternion<N>) -> UnitQuaternion<N> {
|
||||
UnitQuaternion { q: self.q / other.q }
|
||||
Unit::from_unit_value_unchecked(self.unwrap() / other.unwrap())
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: BaseFloat + ApproxEq<N>> DivAssign<UnitQuaternion<N>> for UnitQuaternion<N> {
|
||||
impl<N: BaseFloat> DivAssign<UnitQuaternion<N>> for UnitQuaternion<N> {
|
||||
#[inline]
|
||||
fn div_assign(&mut self, other: UnitQuaternion<N>) {
|
||||
self.q /= other.q
|
||||
*self = Unit::from_unit_value_unchecked(*self.as_ref() / *other.as_ref())
|
||||
}
|
||||
}
|
||||
|
||||
@ -347,14 +422,14 @@ impl<N: BaseNum> Mul<UnitQuaternion<N>> for UnitQuaternion<N> {
|
||||
|
||||
#[inline]
|
||||
fn mul(self, right: UnitQuaternion<N>) -> UnitQuaternion<N> {
|
||||
UnitQuaternion { q: self.q * right.q }
|
||||
Unit::from_unit_value_unchecked(self.unwrap() * right.unwrap())
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: BaseNum> MulAssign<UnitQuaternion<N>> for UnitQuaternion<N> {
|
||||
#[inline]
|
||||
fn mul_assign(&mut self, right: UnitQuaternion<N>) {
|
||||
self.q *= right.q
|
||||
*self = Unit::from_unit_value_unchecked(*self.as_ref() * *right.as_ref())
|
||||
}
|
||||
}
|
||||
|
||||
@ -364,12 +439,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 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<N: BaseNum + Neg<Output = N>> MulAssign<UnitQuaternion<N>> for Point3<N> {
|
||||
impl<N: BaseFloat> Rotation<Vector3<N>> for UnitQuaternion<N> {
|
||||
#[inline]
|
||||
fn rotation(&self) -> Vector3<N> {
|
||||
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<N: BaseFloat> Rotation<Vector3<N>> for UnitQuaternion<N> {
|
||||
|
||||
#[inline]
|
||||
fn append_rotation(&self, amount: &Vector3<N>) -> UnitQuaternion<N> {
|
||||
*self * UnitQuaternion::new(*amount)
|
||||
*self * UnitQuaternion::from_scaled_axis(*amount)
|
||||
}
|
||||
|
||||
#[inline]
|
||||
@ -454,12 +523,12 @@ impl<N: BaseFloat> Rotation<Vector3<N>> for UnitQuaternion<N> {
|
||||
|
||||
#[inline]
|
||||
fn prepend_rotation(&self, amount: &Vector3<N>) -> UnitQuaternion<N> {
|
||||
UnitQuaternion::new(*amount) * *self
|
||||
UnitQuaternion::from_scaled_axis(*amount) * *self
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn set_rotation(&mut self, v: Vector3<N>) {
|
||||
*self = UnitQuaternion::new(v)
|
||||
*self = UnitQuaternion::from_scaled_axis(v);
|
||||
}
|
||||
}
|
||||
|
||||
@ -496,7 +565,7 @@ impl<N: BaseNum + Neg<Output = N>> Rotate<Point3<N>> for UnitQuaternion<N> {
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: BaseFloat + ApproxEq<N>> RotationTo for UnitQuaternion<N> {
|
||||
impl<N: BaseFloat> RotationTo for UnitQuaternion<N> {
|
||||
type AngleType = N;
|
||||
type DeltaRotationType = UnitQuaternion<N>;
|
||||
|
||||
@ -504,7 +573,7 @@ impl<N: BaseFloat + ApproxEq<N>> RotationTo for UnitQuaternion<N> {
|
||||
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<N: BaseNum + Neg<Output = N>> Transform<Point3<N>> for UnitQuaternion<N> {
|
||||
|
||||
impl<N: fmt::Display> fmt::Display for UnitQuaternion<N> {
|
||||
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<N> Dimension for UnitQuaternion<N> {
|
||||
#[cfg(feature="arbitrary")]
|
||||
impl<N: Arbitrary + BaseFloat> Arbitrary for UnitQuaternion<N> {
|
||||
fn arbitrary<G: Gen>(g: &mut G) -> UnitQuaternion<N> {
|
||||
UnitQuaternion::new(Arbitrary::arbitrary(g))
|
||||
UnitQuaternion::new(&Arbitrary::arbitrary(g))
|
||||
}
|
||||
}
|
||||
|
||||
impl Quaternion<f32> {
|
||||
/// 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<N: BaseFloat> UnitQuaternion<N> {
|
||||
/// Compute the exponential of a quaternion.
|
||||
///
|
||||
/// Note that this function yields a `Quaternion<N>` because it looses the unit property.
|
||||
pub fn exp(&self) -> Quaternion<N> {
|
||||
self.as_ref().exp()
|
||||
}
|
||||
}
|
||||
|
||||
/// Compute the natural logarithm (a.k.a ln()) of a quaternion.
|
||||
///
|
||||
/// Becareful, this function yields a `Quaternion<f32>`, 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<N>` 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<N> {
|
||||
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<T> One for Quaternion<T> where T: Copy + One + Zero + Sub<T, Output = T> + Add<T, Output = T> {
|
||||
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);
|
||||
|
||||
@ -157,9 +157,7 @@ impl<N: BaseFloat> Rotation3<N> {
|
||||
}
|
||||
|
||||
/// 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<N>) -> Rotation3<N> {
|
||||
pub fn new_with_matrix_unchecked(matrix: Matrix3<N>) -> Rotation3<N> {
|
||||
Rotation3 {
|
||||
submatrix: matrix
|
||||
}
|
||||
@ -173,15 +171,13 @@ impl<N: BaseFloat> Rotation3<N> {
|
||||
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<N: BaseFloat> Rotation3<N> {
|
||||
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))
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -172,10 +172,10 @@ impl<N: BaseFloat> Basis for Vector3<N> {
|
||||
fn orthonormal_subspace_basis<F: FnMut(Vector3<N>) -> bool>(n: &Vector3<N>, 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 };
|
||||
|
||||
@ -299,7 +299,9 @@ macro_rules! vector_impl(
|
||||
* Norm
|
||||
*
|
||||
*/
|
||||
impl<N: BaseFloat> Norm<N> for $t<N> {
|
||||
impl<N: BaseFloat> Norm for $t<N> {
|
||||
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<N>> {
|
||||
let n = ::norm(self);
|
||||
|
||||
if n <= min_norm {
|
||||
None
|
||||
}
|
||||
else {
|
||||
Some(*self / n)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -461,7 +474,7 @@ macro_rules! vector_impl(
|
||||
}
|
||||
|
||||
impl<N> FloatVector<N> for $t<N>
|
||||
where N: BaseFloat + ApproxEq<N> {
|
||||
where N: BaseFloat {
|
||||
}
|
||||
|
||||
|
||||
@ -508,7 +521,7 @@ macro_rules! vector_impl(
|
||||
|
||||
macro_rules! basis_impl(
|
||||
($t: ident, $dimension: expr) => (
|
||||
impl<N: BaseFloat + ApproxEq<N>> Basis for $t<N> {
|
||||
impl<N: BaseFloat> Basis for $t<N> {
|
||||
#[inline]
|
||||
fn canonical_basis<F: FnMut($t<N>) -> 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 };
|
||||
|
||||
|
||||
@ -253,6 +253,15 @@ macro_rules! vecn_dvec_common_impl(
|
||||
}
|
||||
}
|
||||
|
||||
impl<'a, N: Copy + Div<N, Output = N> + Zero $(, $param : ArrayLength<N>)*> Div<N> for &'a $vecn<N $(, $param)*> {
|
||||
type Output = $vecn<N $(, $param)*>;
|
||||
|
||||
#[inline]
|
||||
fn div(self, right: N) -> $vecn<N $(, $param)*> {
|
||||
self.clone() / right
|
||||
}
|
||||
}
|
||||
|
||||
impl<N $(, $param: ArrayLength<N>)*> DivAssign<$vecn<N $(, $param)*>> for $vecn<N $(, $param)*>
|
||||
where N: Copy + DivAssign<N> + Zero $(, $param : ArrayLength<N>)* {
|
||||
#[inline]
|
||||
@ -495,10 +504,12 @@ macro_rules! vecn_dvec_common_impl(
|
||||
* Norm.
|
||||
*
|
||||
*/
|
||||
impl<N: BaseFloat $(, $param : ArrayLength<N>)*> Norm<N> for $vecn<N $(, $param)*> {
|
||||
impl<N: BaseFloat $(, $param : ArrayLength<N>)*> Norm for $vecn<N $(, $param)*> {
|
||||
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<N $(, $param)*>> {
|
||||
let n = ::norm(self);
|
||||
|
||||
if n <= min_norm {
|
||||
None
|
||||
}
|
||||
else {
|
||||
Some(self / n)
|
||||
}
|
||||
|
||||
l
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -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<N> {
|
||||
}
|
||||
|
||||
/// Traits of objects having an euclidian norm.
|
||||
pub trait Norm<N: BaseFloat> {
|
||||
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<Self>;
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@ -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<Eps>: Sized {
|
||||
impl ApproxEq<f32> for f32 {
|
||||
#[inline]
|
||||
fn approx_epsilon(_: Option<f32>) -> f32 {
|
||||
1.0e-6
|
||||
f32::EPSILON * 10.0
|
||||
}
|
||||
|
||||
#[inline]
|
||||
@ -204,7 +204,7 @@ impl ApproxEq<f32> for f32 {
|
||||
impl ApproxEq<f64> for f64 {
|
||||
#[inline]
|
||||
fn approx_epsilon(_: Option<f64>) -> f64 {
|
||||
1.0e-6
|
||||
f64::EPSILON * 10.0
|
||||
}
|
||||
|
||||
#[inline]
|
||||
|
||||
@ -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<f64> + BaseNum + Neg {
|
||||
pub trait BaseFloat: Float + Cast<f64> + BaseNum + ApproxEq<Self> + Neg {
|
||||
/// Archimedes' constant.
|
||||
fn pi() -> Self;
|
||||
/// 2.0 * pi.
|
||||
@ -243,7 +243,7 @@ pub trait NumVector<N>: Add<Self, Output = Self> + Sub<Self, Output = Self> +
|
||||
}
|
||||
|
||||
/// Trait of vector with components implementing the `BaseFloat` trait.
|
||||
pub trait FloatVector<N: BaseFloat>: NumVector<N> + Norm<N> + Neg<Output = Self> + Basis {
|
||||
pub trait FloatVector<N: BaseFloat>: NumVector<N> + Norm<NormType = N> + Neg<Output = Self> + Basis + ApproxEq<N> {
|
||||
}
|
||||
|
||||
/*
|
||||
@ -289,7 +289,7 @@ pub trait NumPoint<N>:
|
||||
|
||||
/// Trait of points with components implementing the `BaseFloat` trait.
|
||||
pub trait FloatPoint<N: BaseFloat>: NumPoint<N> + Sized
|
||||
where <Self as PointAsVector>::Vector: Norm<N> {
|
||||
where <Self as PointAsVector>::Vector: Norm<NormType = N> {
|
||||
/// Computes the square distance between two points.
|
||||
#[inline]
|
||||
fn distance_squared(&self, other: &Self) -> N {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user