Add quaternions.

Fix #24.
This commit is contained in:
Sébastien Crozet 2014-10-14 21:37:44 +02:00
parent 162346ab47
commit 5ba9f27530
8 changed files with 603 additions and 45 deletions

View File

@ -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::{

View File

@ -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};

View File

@ -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;

View File

@ -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<N> {
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<N> = (Quat<N>, Quat<N>)
impl<N> Quat<N> {
pub fn new(w: N, x: N, y: N, z: N) -> Quat<N> {
/// Creates a new quaternion from its components.
#[inline]
pub fn new(w: N, i: N, j: N, k: N) -> Quat<N> {
Quat {
w: w,
i: i,
@ -23,53 +37,497 @@ impl<N> Quat<N> {
k: k
}
}
}
impl<N: Add<N, N>> Add<Quat<N>, Quat<N>> for Quat<N> {
fn add(&self, other: &Quat<N>) -> Quat<N> {
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<N> {
// FIXME: do this require a `repr(C)` ?
unsafe {
mem::transmute(&self.i)
}
}
impl<N> Mul<Quat<N>, Quat<N>> for Quat<N> {
fn mul(&self, other: &Quat<N>) -> Quat<N> {
/// The scalar part `w` of this quaternion.
#[inline]
pub fn scalar<'a>(&'a self) -> &'a N {
&self.w
}
}
impl<N: Neg<N>> Quat<N> {
/// 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<N: Float + ApproxEq<N> + Clone> Inv for Quat<N> {
#[inline]
fn inv_cpy(m: &Quat<N>) -> Option<Quat<N>> {
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<N: Float> Norm<N> for Quat<N> {
#[inline]
fn sqnorm(q: &Quat<N>) -> N {
q.w * q.w + q.i * q.i + q.j * q.j + q.k * q.k
}
#[inline]
fn normalize_cpy(v: &Quat<N>) -> Quat<N> {
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<N: Mul<N, N> + Sub<N, N> + Add<N, N>> QuatMulRhs<N, Quat<N>> for Quat<N> {
#[inline]
fn binop(left: &Quat<N>, right: &Quat<N>) -> Quat<N> {
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<N: ApproxEq<N> + Float + Clone> QuatDivRhs<N, Quat<N>> for Quat<N> {
#[inline]
fn binop(left: &Quat<N>, right: &Quat<N>) -> Quat<N> {
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<N> {
q: Quat<N>
}
impl<N: FloatMath> UnitQuat<N> {
/// Creates a new unit quaternion from the axis-angle representation of a rotation.
#[inline]
pub fn new(axisangle: Vec3<N>) -> UnitQuat<N> {
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<N>) -> UnitQuat<N> {
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<N> {
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)
)
}
}
impl<N: Zero> Rotate<Vec3<N>> for Quat<N> {
/// Builds a rotation matrix from this quaternion.
pub fn to_rot(&self) -> Rot3<N> {
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<N> UnitQuat<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_quat(q: Quat<N>) -> UnitQuat<N> {
UnitQuat {
q: q
}
}
/// The `Quat` representation of this unit quaternion.
#[inline]
pub fn quat<'a>(&'a self) -> &'a Quat<N> {
&self.q
}
}
impl<N: Num + Clone> One for UnitQuat<N> {
#[inline]
fn one() -> UnitQuat<N> {
unsafe {
UnitQuat::new_with_unit_quat(Quat::new(One::one(), Zero::zero(), Zero::zero(), Zero::zero()))
}
}
}
impl<N: Clone + Neg<N>> Inv for UnitQuat<N> {
#[inline]
fn inv_cpy(m: &UnitQuat<N>) -> Option<UnitQuat<N>> {
let mut cpy = m.clone();
cpy.inv();
Some(cpy)
}
#[inline]
fn inv(&mut self) -> bool {
self.q.conjugate();
true
}
}
impl<N: Clone + Rand + FloatMath> Rand for UnitQuat<N> {
#[inline]
fn rand<R: Rng>(rng: &mut R) -> UnitQuat<N> {
UnitQuat::new(rng.gen())
}
}
impl<N: ApproxEq<N>> ApproxEq<N> for UnitQuat<N> {
#[inline]
fn approx_epsilon(_: Option<UnitQuat<N>>) -> N {
ApproxEq::approx_epsilon(None::<N>)
}
#[inline]
fn approx_eq(a: &UnitQuat<N>, b: &UnitQuat<N>) -> bool {
ApproxEq::approx_eq(&a.q, &b.q)
}
#[inline]
fn approx_eq_eps(a: &UnitQuat<N>, b: &UnitQuat<N>, eps: &N) -> bool {
ApproxEq::approx_eq_eps(&a.q, &b.q, eps)
}
}
impl<N: Float + ApproxEq<N> + Clone> Div<UnitQuat<N>, UnitQuat<N>> for UnitQuat<N> {
#[inline]
fn div(&self, other: &UnitQuat<N>) -> UnitQuat<N> {
UnitQuat { q: self.q / other.q }
}
}
impl<N: Num + Clone> UnitQuatMulRhs<N, UnitQuat<N>> for UnitQuat<N> {
#[inline]
fn binop(left: &UnitQuat<N>, right: &UnitQuat<N>) -> UnitQuat<N> {
UnitQuat { q: left.q * right.q }
}
}
impl<N: Num + Clone> UnitQuatMulRhs<N, Vec3<N>> for Vec3<N> {
#[inline]
fn binop(left: &UnitQuat<N>, right: &Vec3<N>) -> Vec3<N> {
let _2: N = num::one::<N>() + 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<N: Num + Clone> UnitQuatMulRhs<N, Pnt3<N>> for Pnt3<N> {
#[inline]
fn binop(left: &UnitQuat<N>, right: &Pnt3<N>) -> Pnt3<N> {
(left * *right.as_vec()).to_pnt()
}
}
impl<N: Num + Clone> Vec3MulRhs<N, Vec3<N>> for UnitQuat<N> {
#[inline]
fn binop(left: &Vec3<N>, right: &UnitQuat<N>) -> Vec3<N> {
let mut inv_quat = right.clone();
inv_quat.inv();
inv_quat * *left
}
}
impl<N: Num + Clone> Pnt3MulRhs<N, Pnt3<N>> for UnitQuat<N> {
#[inline]
fn binop(left: &Pnt3<N>, right: &UnitQuat<N>) -> Pnt3<N> {
(left.as_vec() * *right).to_pnt()
}
}
impl<N: FloatMath + Clone> Rotation<Vec3<N>> for UnitQuat<N> {
#[inline]
fn rotation(&self) -> Vec3<N> {
let _2 = num::one::<N>() + 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<N> {
-self.rotation()
}
#[inline]
fn append_rotation(&mut self, amount: &Vec3<N>) {
*self = Rotation::append_rotation_cpy(self, amount)
}
#[inline]
fn append_rotation_cpy(t: &UnitQuat<N>, amount: &Vec3<N>) -> UnitQuat<N> {
*t * UnitQuat::new(amount.clone())
}
#[inline]
fn prepend_rotation(&mut self, amount: &Vec3<N>) {
*self = Rotation::prepend_rotation_cpy(self, amount)
}
#[inline]
fn prepend_rotation_cpy(t: &UnitQuat<N>, amount: &Vec3<N>) -> UnitQuat<N> {
UnitQuat::new(amount.clone()) * *t
}
#[inline]
fn set_rotation(&mut self, v: Vec3<N>) {
*self = UnitQuat::new(v)
}
}
impl<N: Num + Clone> Rotate<Vec3<N>> for UnitQuat<N> {
#[inline]
fn rotate(&self, v: &Vec3<N>) -> Vec3<N> {
*self * *v_quat * self.inv()
*self * *v
}
#[inline]
fn inv_rotate(&self, v: &Vec3<N>) -> Vec3<N> {
-self * *v
*v * *self
}
}
impl Rotate<Vec4<N>> for (QuatPair<N>, QuatPair<N>) {
impl<N: Num + Clone> Rotate<Pnt3<N>> for UnitQuat<N> {
#[inline]
fn rotate(&self, v: &Vec4<N>) -> Vec4<N> {
let (ref l, ref r) = *self;
*l * *v * *r
fn rotate(&self, p: &Pnt3<N>) -> Pnt3<N> {
*self * *p
}
#[inline]
fn inv_rotate(&self, v: &Vec4<N>) -> Vec4<N> {
let (ref l, ref r) = *self;
fn inv_rotate(&self, p: &Pnt3<N>) -> Pnt3<N> {
*p * *self
}
}
(-r) * **v * (-l)
impl<N: Num + Clone> Transform<Vec3<N>> for UnitQuat<N> {
#[inline]
fn transform(&self, v: &Vec3<N>) -> Vec3<N> {
*self * *v
}
#[inline]
fn inv_transform(&self, v: &Vec3<N>) -> Vec3<N> {
*v * *self
}
}
impl<N: Num + Clone> Transform<Pnt3<N>> for UnitQuat<N> {
#[inline]
fn transform(&self, p: &Pnt3<N>) -> Pnt3<N> {
*self * *p
}
#[inline]
fn inv_transform(&self, p: &Pnt3<N>) -> Pnt3<N> {
*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)

View File

@ -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<N: Clone + FloatMath> Rot3<N> {
}
}
}
/// 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<N>) -> Rot3<N> {
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<N> {
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<N: Clone + Float> Rot3<N> {

View File

@ -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};

View File

@ -215,15 +215,12 @@ pub trait Norm<N: Float> {
/// 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;
}

73
tests/quat.rs Normal file
View File

@ -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<f64> = 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<f64> = random();
let vec: Vec3<f64> = random();
let pnt: Pnt3<f64> = 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<f64> = random();
let axis_angle2: Vec3<f64> = 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<f64> = 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<f64> = 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))
}
}