diff --git a/src/lib.rs b/src/lib.rs index 7ff728f8..5e0a8c45 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -102,7 +102,6 @@ Feel free to add your project to this list if you happen to use **nalgebra**! #![feature(globs)] #![doc(html_root_url = "http://nalgebra.org/doc")] -extern crate rand; extern crate serialize; #[cfg(test)] @@ -176,7 +175,8 @@ pub use structs::{ Vec0, Vec1, Vec2, Vec3, Vec4, Vec5, Vec6, Pnt0, Pnt1, Pnt2, Pnt3, Pnt4, Pnt5, Pnt6, Persp3, PerspMat3, - Ortho3, OrthoMat3 + Ortho3, OrthoMat3, + Quat, UnitQuat }; pub use linalg::{ diff --git a/src/structs/iso.rs b/src/structs/iso.rs index 096dcc1c..88b6d5af 100644 --- a/src/structs/iso.rs +++ b/src/structs/iso.rs @@ -3,7 +3,7 @@ #![allow(missing_doc)] use std::num::{Zero, One}; -use rand::{Rand, Rng}; +use std::rand::{Rand, Rng}; use structs::mat::{Mat3, Mat4, Mat5}; use traits::structure::{Cast, Dim, Col}; use traits::operations::{Inv, ApproxEq}; diff --git a/src/structs/mod.rs b/src/structs/mod.rs index 091da160..88fe3fbd 100644 --- a/src/structs/mod.rs +++ b/src/structs/mod.rs @@ -9,6 +9,7 @@ pub use self::rot::{Rot2, Rot3, Rot4}; pub use self::iso::{Iso2, Iso3, Iso4}; pub use self::persp::{Persp3, PerspMat3}; pub use self::ortho::{Ortho3, OrthoMat3}; +pub use self::quat::{Quat, UnitQuat}; pub use self::vec::{Vec1MulRhs, Vec2MulRhs, Vec3MulRhs, Vec4MulRhs, Vec5MulRhs, Vec6MulRhs, Vec1DivRhs, Vec2DivRhs, Vec3DivRhs, Vec4DivRhs, Vec5DivRhs, Vec6DivRhs, @@ -31,6 +32,7 @@ mod vec_macros; mod vec; mod pnt_macros; mod pnt; +mod quat; mod mat_macros; mod mat; mod rot_macros; diff --git a/src/structs/quat.rs b/src/structs/quat.rs index b08ff0eb..67d802b5 100644 --- a/src/structs/quat.rs +++ b/src/structs/quat.rs @@ -1,21 +1,35 @@ //! Quaternion definition. +#![allow(missing_doc)] // we allow missing to avoid having to document the dispatch trait. + +use std::mem; +use std::num::{Zero, One, Bounded}; +use std::num; +use std::rand::{Rand, Rng}; +use std::slice::{Items, MutItems}; +use structs::{Vec3, Pnt3, Rot3, Mat3, Vec3MulRhs, Pnt3MulRhs}; +use traits::operations::{ApproxEq, Inv, PartialOrd, PartialOrdering, NotComparable, PartialLess, + PartialGreater, PartialEqual, Axpy}; +use traits::structure::{Cast, Indexable, Iterable, IterableMut, Dim}; +use traits::geometry::{Norm, Cross, Rotation, Rotate, Transform}; + /// A quaternion. -/// -/// A single unit quaternion can represent a 3d rotation while a pair of unit quaternions can -/// represent a 4d rotation. +#[deriving(Eq, PartialEq, Encodable, Decodable, Clone, Hash, Rand, Zero, Show)] pub struct Quat { - w: N - i: N, - j: N, - k: N, + /// The scalar component of the quaternion. + pub w: N, + /// The first vector component of the quaternion. + pub i: N, + /// The second vector component of the quaternion. + pub j: N, + /// The third vector component of the quaternion. + pub k: N } -// FIXME: find a better name -type QuatPair = (Quat, Quat) - impl Quat { - pub fn new(w: N, x: N, y: N, z: N) -> Quat { + /// Creates a new quaternion from its components. + #[inline] + pub fn new(w: N, i: N, j: N, k: N) -> Quat { Quat { w: w, i: i, @@ -23,53 +37,497 @@ impl Quat { k: k } } -} -impl> Add, Quat> for Quat { - fn add(&self, other: &Quat) -> Quat { - Quat::new( - self.w + other.w, - self.i + other.i, - self.j + other.j, - self.k + other.k) + /// The vector part `(i, j, k)` of this quaternion. + #[inline] + pub fn vector<'a>(&'a self) -> &'a Vec3 { + // FIXME: do this require a `repr(C)` ? + unsafe { + mem::transmute(&self.i) + } + } + + /// The scalar part `w` of this quaternion. + #[inline] + pub fn scalar<'a>(&'a self) -> &'a N { + &self.w } } -impl Mul, Quat> for Quat { - fn mul(&self, other: &Quat) -> Quat { +impl> Quat { + /// Replaces this quaternion by its conjugate. + #[inline] + pub fn conjugate(&mut self) { + self.i = -self.i; + self.j = -self.j; + self.k = -self.k; + } +} + +impl + Clone> Inv for Quat { + #[inline] + fn inv_cpy(m: &Quat) -> Option> { + let mut res = m.clone(); + + if res.inv() { + Some(res) + } + else { + None + } + } + + #[inline] + fn inv(&mut self) -> bool { + let sqnorm = Norm::sqnorm(self); + + if ApproxEq::approx_eq(&sqnorm, &Zero::zero()) { + false + } + else { + self.conjugate(); + self.w = self.w / sqnorm; + self.i = self.i / sqnorm; + self.j = self.j / sqnorm; + self.k = self.k / sqnorm; + + true + } + } +} + +impl Norm for Quat { + #[inline] + fn sqnorm(q: &Quat) -> N { + q.w * q.w + q.i * q.i + q.j * q.j + q.k * q.k + } + + #[inline] + fn normalize_cpy(v: &Quat) -> Quat { + let n = Norm::norm(v); + Quat::new(v.w / n, v.i / n, v.j / n, v.k / n) + } + + #[inline] + fn normalize(&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; + + n + } +} + +impl + Sub + Add> QuatMulRhs> for Quat { + #[inline] + fn binop(left: &Quat, right: &Quat) -> Quat { Quat::new( - self.w * other.w - self.x * other.x - self.y * other.y - self.z * other.z, - self.a * other.b - self.b * other.a - self.c * other.d - self.d * other.c, - self.a * other.c - self.b * other.d - self.c * other.a - self.d * other.b, - self.w * other.w - self.x * other.x - self.y * other.y - self.z * other.z, + left.w * right.w - left.i * right.i - left.j * right.j - left.k * right.k, + left.w * right.i + left.i * right.w + left.j * right.k - left.k * right.j, + left.w * right.j - left.i * right.k + left.j * right.w + left.k * right.i, + left.w * right.k + left.i * right.j - left.j * right.i + left.k * right.w) + } +} + +impl + Float + Clone> QuatDivRhs> for Quat { + #[inline] + fn binop(left: &Quat, right: &Quat) -> Quat { + left * Inv::inv_cpy(right).expect("Unable to invert the denominator.") + } +} + +/// A unit quaternion that can represent a 3D rotation. +#[deriving(Eq, PartialEq, Encodable, Decodable, Clone, Hash, Show)] +pub struct UnitQuat { + q: Quat +} + +impl UnitQuat { + /// Creates a new unit quaternion from the axis-angle representation of a rotation. + #[inline] + pub fn new(axisangle: Vec3) -> UnitQuat { + let sqang = Norm::sqnorm(&axisangle); + + if sqang.is_zero() { + One::one() + } + else { + let ang = sqang.sqrt(); + let (s, c) = (ang / num::cast(2.0f64).unwrap()).sin_cos(); + + let s_ang = s / ang; + + unsafe { + UnitQuat::new_with_unit_quat( + Quat::new( + c, + axisangle.x * s_ang, + axisangle.y * s_ang, + axisangle.z * s_ang) + ) + } + } + } + + /// Creates a new unit quaternion from a quaternion. + /// + /// The input quaternion will be normalized. + #[inline] + pub fn new_with_quat(q: Quat) -> UnitQuat { + let mut q = q; + let _ = q.normalize(); + + UnitQuat { + q: 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) -> UnitQuat { + let _0_5: N = num::cast(0.5f64).unwrap(); + let (sr, cr) = (roll * _0_5).sin_cos(); + let (sp, cp) = (pitch * _0_5).sin_cos(); + let (sy, cy) = (yaw * _0_5).sin_cos(); + + unsafe { + UnitQuat::new_with_unit_quat( + Quat::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) ) + } + } + + /// Builds a rotation matrix from this quaternion. + pub fn to_rot(&self) -> Rot3 { + let _2: N = num::cast(2.0f64).unwrap(); + 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 = _2 * self.q.i * self.q.j; + let wk = _2 * self.q.w * self.q.k; + let wj = _2 * self.q.w * self.q.j; + let ik = _2 * self.q.i * self.q.k; + let jk = _2 * self.q.j * self.q.k; + let wi = _2 * self.q.w * self.q.i; + + unsafe { + Rot3::new_with_mat( + Mat3::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 Rotate> for Quat { +impl UnitQuat { + /// 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_quat(q: Quat) -> UnitQuat { + UnitQuat { + q: q + } + } + + /// The `Quat` representation of this unit quaternion. + #[inline] + pub fn quat<'a>(&'a self) -> &'a Quat { + &self.q + } +} + +impl One for UnitQuat { + #[inline] + fn one() -> UnitQuat { + unsafe { + UnitQuat::new_with_unit_quat(Quat::new(One::one(), Zero::zero(), Zero::zero(), Zero::zero())) + } + } +} + +impl> Inv for UnitQuat { + #[inline] + fn inv_cpy(m: &UnitQuat) -> Option> { + let mut cpy = m.clone(); + + cpy.inv(); + Some(cpy) + } + + #[inline] + fn inv(&mut self) -> bool { + self.q.conjugate(); + + true + } +} + +impl Rand for UnitQuat { + #[inline] + fn rand(rng: &mut R) -> UnitQuat { + UnitQuat::new(rng.gen()) + } +} + +impl> ApproxEq for UnitQuat { + #[inline] + fn approx_epsilon(_: Option>) -> N { + ApproxEq::approx_epsilon(None::) + } + + #[inline] + fn approx_eq(a: &UnitQuat, b: &UnitQuat) -> bool { + ApproxEq::approx_eq(&a.q, &b.q) + } + + #[inline] + fn approx_eq_eps(a: &UnitQuat, b: &UnitQuat, eps: &N) -> bool { + ApproxEq::approx_eq_eps(&a.q, &b.q, eps) + } +} + +impl + Clone> Div, UnitQuat> for UnitQuat { + #[inline] + fn div(&self, other: &UnitQuat) -> UnitQuat { + UnitQuat { q: self.q / other.q } + } +} + +impl UnitQuatMulRhs> for UnitQuat { + #[inline] + fn binop(left: &UnitQuat, right: &UnitQuat) -> UnitQuat { + UnitQuat { q: left.q * right.q } + } +} + +impl UnitQuatMulRhs> for Vec3 { + #[inline] + fn binop(left: &UnitQuat, right: &Vec3) -> Vec3 { + let _2: N = num::one::() + num::one(); + let mut t = Cross::cross(left.q.vector(), right); + t.x = t.x * _2; + t.y = t.y * _2; + t.z = t.z * _2; + + Vec3::new(t.x * left.q.w, t.y * left.q.w, t.z * left.q.w) + + Cross::cross(left.q.vector(), &t) + + *right + } +} + +impl UnitQuatMulRhs> for Pnt3 { + #[inline] + fn binop(left: &UnitQuat, right: &Pnt3) -> Pnt3 { + (left * *right.as_vec()).to_pnt() + } +} + +impl Vec3MulRhs> for UnitQuat { + #[inline] + fn binop(left: &Vec3, right: &UnitQuat) -> Vec3 { + let mut inv_quat = right.clone(); + inv_quat.inv(); + + inv_quat * *left + } +} + +impl Pnt3MulRhs> for UnitQuat { + #[inline] + fn binop(left: &Pnt3, right: &UnitQuat) -> Pnt3 { + (left.as_vec() * *right).to_pnt() + } +} + +impl Rotation> for UnitQuat { + #[inline] + fn rotation(&self) -> Vec3 { + let _2 = num::one::() + num::one(); + let mut v = self.q.vector().clone(); + let ang = _2 * v.normalize().atan2(self.q.w); + + if ang.is_zero() { + num::zero() + } + else { + Vec3::new(v.x * ang, v.y * ang, v.z * ang) + } + } + + #[inline] + fn inv_rotation(&self) -> Vec3 { + -self.rotation() + } + + #[inline] + fn append_rotation(&mut self, amount: &Vec3) { + *self = Rotation::append_rotation_cpy(self, amount) + } + + #[inline] + fn append_rotation_cpy(t: &UnitQuat, amount: &Vec3) -> UnitQuat { + *t * UnitQuat::new(amount.clone()) + } + + #[inline] + fn prepend_rotation(&mut self, amount: &Vec3) { + *self = Rotation::prepend_rotation_cpy(self, amount) + } + + #[inline] + fn prepend_rotation_cpy(t: &UnitQuat, amount: &Vec3) -> UnitQuat { + UnitQuat::new(amount.clone()) * *t + } + + #[inline] + fn set_rotation(&mut self, v: Vec3) { + *self = UnitQuat::new(v) + } +} + +impl Rotate> for UnitQuat { #[inline] fn rotate(&self, v: &Vec3) -> Vec3 { - *self * *v_quat * self.inv() + *self * *v } #[inline] fn inv_rotate(&self, v: &Vec3) -> Vec3 { - -self * *v + *v * *self } } -impl Rotate> for (QuatPair, QuatPair) { +impl Rotate> for UnitQuat { #[inline] - fn rotate(&self, v: &Vec4) -> Vec4 { - let (ref l, ref r) = *self; - - *l * *v * *r + fn rotate(&self, p: &Pnt3) -> Pnt3 { + *self * *p } #[inline] - fn inv_rotate(&self, v: &Vec4) -> Vec4 { - let (ref l, ref r) = *self; - - (-r) * **v * (-l) + fn inv_rotate(&self, p: &Pnt3) -> Pnt3 { + *p * *self } } + +impl Transform> for UnitQuat { + #[inline] + fn transform(&self, v: &Vec3) -> Vec3 { + *self * *v + } + + #[inline] + fn inv_transform(&self, v: &Vec3) -> Vec3 { + *v * *self + } +} + +impl Transform> for UnitQuat { + #[inline] + fn transform(&self, p: &Pnt3) -> Pnt3 { + *self * *p + } + + #[inline] + fn inv_transform(&self, p: &Pnt3) -> Pnt3 { + *p * *self + } +} + +double_dispatch_binop_decl_trait!(Quat, QuatMulRhs) +double_dispatch_binop_decl_trait!(Quat, QuatDivRhs) +double_dispatch_binop_decl_trait!(Quat, QuatAddRhs) +double_dispatch_binop_decl_trait!(Quat, QuatSubRhs) +double_dispatch_cast_decl_trait!(Quat, QuatCast) +mul_redispatch_impl!(Quat, QuatMulRhs) +div_redispatch_impl!(Quat, QuatDivRhs) +add_redispatch_impl!(Quat, QuatAddRhs) +sub_redispatch_impl!(Quat, QuatSubRhs) +cast_redispatch_impl!(Quat, QuatCast) +ord_impl!(Quat, w, i, j, k) +vec_axis_impl!(Quat, w, i, j, k) +vec_cast_impl!(Quat, QuatCast, w, i, j, k) +as_slice_impl!(Quat, 4) +index_impl!(Quat) +indexable_impl!(Quat, 4) +at_fast_impl!(Quat, 4) +new_repeat_impl!(Quat, val, w, i, j, k) +dim_impl!(Quat, 3) +container_impl!(Quat) +add_impl!(Quat, QuatAddRhs, w, i, j, k) +sub_impl!(Quat, QuatSubRhs, w, i, j, k) +neg_impl!(Quat, w, i, j, k) +vec_mul_scalar_impl!(Quat, f64, QuatMulRhs, w, i, j, k) +vec_mul_scalar_impl!(Quat, f32, QuatMulRhs, w, i, j, k) +vec_mul_scalar_impl!(Quat, u64, QuatMulRhs, w, i, j, k) +vec_mul_scalar_impl!(Quat, u32, QuatMulRhs, w, i, j, k) +vec_mul_scalar_impl!(Quat, u16, QuatMulRhs, w, i, j, k) +vec_mul_scalar_impl!(Quat, u8, QuatMulRhs, w, i, j, k) +vec_mul_scalar_impl!(Quat, i64, QuatMulRhs, w, i, j, k) +vec_mul_scalar_impl!(Quat, i32, QuatMulRhs, w, i, j, k) +vec_mul_scalar_impl!(Quat, i16, QuatMulRhs, w, i, j, k) +vec_mul_scalar_impl!(Quat, i8, QuatMulRhs, w, i, j, k) +vec_mul_scalar_impl!(Quat, uint, QuatMulRhs, w, i, j, k) +vec_mul_scalar_impl!(Quat, int, QuatMulRhs, w, i, j, k) +vec_div_scalar_impl!(Quat, f64, QuatDivRhs, w, i, j, k) +vec_div_scalar_impl!(Quat, f32, QuatDivRhs, w, i, j, k) +vec_div_scalar_impl!(Quat, u64, QuatDivRhs, w, i, j, k) +vec_div_scalar_impl!(Quat, u32, QuatDivRhs, w, i, j, k) +vec_div_scalar_impl!(Quat, u16, QuatDivRhs, w, i, j, k) +vec_div_scalar_impl!(Quat, u8, QuatDivRhs, w, i, j, k) +vec_div_scalar_impl!(Quat, i64, QuatDivRhs, w, i, j, k) +vec_div_scalar_impl!(Quat, i32, QuatDivRhs, w, i, j, k) +vec_div_scalar_impl!(Quat, i16, QuatDivRhs, w, i, j, k) +vec_div_scalar_impl!(Quat, i8, QuatDivRhs, w, i, j, k) +vec_div_scalar_impl!(Quat, uint, QuatDivRhs, w, i, j, k) +vec_div_scalar_impl!(Quat, int, QuatDivRhs, w, i, j, k) +vec_add_scalar_impl!(Quat, f64, QuatAddRhs, w, i, j, k) +vec_add_scalar_impl!(Quat, f32, QuatAddRhs, w, i, j, k) +vec_add_scalar_impl!(Quat, u64, QuatAddRhs, w, i, j, k) +vec_add_scalar_impl!(Quat, u32, QuatAddRhs, w, i, j, k) +vec_add_scalar_impl!(Quat, u16, QuatAddRhs, w, i, j, k) +vec_add_scalar_impl!(Quat, u8, QuatAddRhs, w, i, j, k) +vec_add_scalar_impl!(Quat, i64, QuatAddRhs, w, i, j, k) +vec_add_scalar_impl!(Quat, i32, QuatAddRhs, w, i, j, k) +vec_add_scalar_impl!(Quat, i16, QuatAddRhs, w, i, j, k) +vec_add_scalar_impl!(Quat, i8, QuatAddRhs, w, i, j, k) +vec_add_scalar_impl!(Quat, uint, QuatAddRhs, w, i, j, k) +vec_add_scalar_impl!(Quat, int, QuatAddRhs, w, i, j, k) +vec_sub_scalar_impl!(Quat, f64, QuatSubRhs, w, i, j, k) +vec_sub_scalar_impl!(Quat, f32, QuatSubRhs, w, i, j, k) +vec_sub_scalar_impl!(Quat, u64, QuatSubRhs, w, i, j, k) +vec_sub_scalar_impl!(Quat, u32, QuatSubRhs, w, i, j, k) +vec_sub_scalar_impl!(Quat, u16, QuatSubRhs, w, i, j, k) +vec_sub_scalar_impl!(Quat, u8, QuatSubRhs, w, i, j, k) +vec_sub_scalar_impl!(Quat, i64, QuatSubRhs, w, i, j, k) +vec_sub_scalar_impl!(Quat, i32, QuatSubRhs, w, i, j, k) +vec_sub_scalar_impl!(Quat, i16, QuatSubRhs, w, i, j, k) +vec_sub_scalar_impl!(Quat, i8, QuatSubRhs, w, i, j, k) +vec_sub_scalar_impl!(Quat, uint, QuatSubRhs, w, i, j, k) +vec_sub_scalar_impl!(Quat, int, QuatSubRhs, w, i, j, k) +approx_eq_impl!(Quat, w, i, j, k) +from_iterator_impl!(Quat, iterator, iterator, iterator, iterator) +bounded_impl!(Quat, w, i, j, k) +axpy_impl!(Quat, w, i, j, k) +iterable_impl!(Quat, 4) +iterable_mut_impl!(Quat, 4) + +double_dispatch_binop_decl_trait!(UnitQuat, UnitQuatMulRhs) +mul_redispatch_impl!(UnitQuat, UnitQuatMulRhs) +dim_impl!(UnitQuat, 3) +as_slice_impl!(UnitQuat, 4) +index_impl!(UnitQuat) +indexable_impl!(UnitQuat, 5) diff --git a/src/structs/rot.rs b/src/structs/rot.rs index df5bd7ae..333d484b 100644 --- a/src/structs/rot.rs +++ b/src/structs/rot.rs @@ -3,7 +3,7 @@ #![allow(missing_doc)] use std::num::{Zero, One}; -use rand::{Rand, Rng}; +use std::rand::{Rand, Rng}; use traits::geometry::{Rotate, Rotation, AbsoluteRotate, RotationMatrix, Transform, ToHomogeneous, Norm, Cross}; use traits::structure::{Cast, Dim, Row, Col}; @@ -136,6 +136,34 @@ impl Rot3 { } } } + + /// Builds a rotation matrix from an orthogonal matrix. + /// + /// This is unsafe because the orthogonality of `mat` is not checked. + pub unsafe fn new_with_mat(mat: Mat3) -> Rot3 { + Rot3 { + submat: mat + } + } + + /// Creates a new rotation from Euler angles. + /// + /// The primitive rotations are applied in order: 1 roll − 2 pitch − 3 yaw. + pub fn new_with_euler_angles(roll: N, pitch: N, yaw: N) -> Rot3 { + let (sr, cr) = roll.sin_cos(); + let (sp, cp) = pitch.sin_cos(); + let (sy, cy) = yaw.sin_cos(); + + unsafe { + Rot3::new_with_mat( + Mat3::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 + ) + ) + } + } } impl Rot3 { diff --git a/src/structs/vec.rs b/src/structs/vec.rs index 3fe592ef..45c7dc29 100644 --- a/src/structs/vec.rs +++ b/src/structs/vec.rs @@ -1,6 +1,6 @@ //! Vectors with dimensions known at compile-time. -#![allow(missing_doc)] // we allow missing to avoid having to document the vector components. +#![allow(missing_doc)] // we allow missing to avoid having to document the dispatch traits. use std::mem; use std::num::{Zero, One, Float, Bounded}; diff --git a/src/traits/geometry.rs b/src/traits/geometry.rs index afbc6421..4a05a7f0 100644 --- a/src/traits/geometry.rs +++ b/src/traits/geometry.rs @@ -215,15 +215,12 @@ pub trait Norm { /// Computes the squared norm of `self`. /// /// This is usually faster than computing the norm itself. - #[inline] fn sqnorm(&Self) -> N; /// Gets the normalized version of a copy of `v`. - #[inline] fn normalize_cpy(v: &Self) -> Self; /// Normalizes `self`. - #[inline] fn normalize(&mut self) -> N; } diff --git a/tests/quat.rs b/tests/quat.rs new file mode 100644 index 00000000..bec1b231 --- /dev/null +++ b/tests/quat.rs @@ -0,0 +1,73 @@ +#![feature(macro_rules)] + +extern crate debug; +extern crate "nalgebra" as na; + +use na::{Pnt3, Vec3, Rot3, UnitQuat, Rotation}; +use std::rand::random; + +#[test] +fn test_quat_as_mat() { + for _ in range(0u, 10000) { + let axis_angle: Vec3 = random(); + + assert!(na::approx_eq(&UnitQuat::new(axis_angle).to_rot(), &Rot3::new(axis_angle))) + } +} + +#[test] +fn test_quat_mul_vec_or_pnt_as_mat() { + for _ in range(0u, 10000) { + let axis_angle: Vec3 = random(); + let vec: Vec3 = random(); + let pnt: Pnt3 = random(); + + let mat = Rot3::new(axis_angle); + let quat = UnitQuat::new(axis_angle); + + assert!(na::approx_eq(&(mat * vec), &(quat * vec))); + assert!(na::approx_eq(&(mat * pnt), &(quat * pnt))); + assert!(na::approx_eq(&(vec * mat), &(vec * quat))); + assert!(na::approx_eq(&(pnt * mat), &(pnt * quat))); + } +} + +#[test] +fn test_quat_div_quat() { + for _ in range(0u, 10000) { + let axis_angle1: Vec3 = random(); + let axis_angle2: Vec3 = random(); + + let r1 = Rot3::new(axis_angle1); + let r2 = na::inv(&Rot3::new(axis_angle2)).unwrap(); + + let q1 = UnitQuat::new(axis_angle1); + let q2 = UnitQuat::new(axis_angle2); + + assert!(na::approx_eq(&(q1 / q2).to_rot(), &(r1 * r2))) + } +} + +#[test] +fn test_quat_to_axis_angle() { + for _ in range(0u, 10000) { + let axis_angle: Vec3 = random(); + + let q = UnitQuat::new(axis_angle); + + println!("{} {}", q.rotation(), axis_angle); + assert!(na::approx_eq(&q.rotation(), &axis_angle)) + } +} + +#[test] +fn test_quat_euler_angles() { + for _ in range(0u, 10000) { + let angles: Vec3 = random(); + + let q = UnitQuat::new_with_euler_angles(angles.x, angles.y, angles.z); + let m = Rot3::new_with_euler_angles(angles.x, angles.y, angles.z); + + assert!(na::approx_eq(&q.to_rot(), &m)) + } +}