parent
162346ab47
commit
5ba9f27530
|
@ -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::{
|
||||
|
|
|
@ -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};
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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> {
|
||||
|
|
|
@ -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};
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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))
|
||||
}
|
||||
}
|
Loading…
Reference in New Issue