2013-10-06 22:54:09 +08:00
|
|
|
|
//! Rotations matrices.
|
|
|
|
|
|
|
|
|
|
#[allow(missing_doc)];
|
|
|
|
|
|
2013-10-10 04:59:44 +08:00
|
|
|
|
use std::num::{Zero, One};
|
2013-10-06 22:54:09 +08:00
|
|
|
|
use std::rand::{Rand, Rng};
|
|
|
|
|
use traits::geometry::{Rotate, Rotation, AbsoluteRotate, RotationMatrix, Transform, ToHomogeneous,
|
|
|
|
|
Norm, Cross};
|
2013-10-10 04:59:44 +08:00
|
|
|
|
use traits::structure::{Cast, Dim, Indexable, Row, Col};
|
2013-10-06 22:54:09 +08:00
|
|
|
|
use traits::operations::{Absolute, Inv, Transpose};
|
|
|
|
|
use structs::vec::{Vec1, Vec2, Vec3, Vec4, Vec2MulRhs, Vec3MulRhs, Vec4MulRhs};
|
|
|
|
|
use structs::mat::{Mat2, Mat3, Mat4, Mat5};
|
|
|
|
|
|
|
|
|
|
mod metal;
|
|
|
|
|
mod rot_macros;
|
|
|
|
|
|
|
|
|
|
/// Two dimensional rotation matrix.
|
|
|
|
|
#[deriving(Eq, Encodable, Decodable, Clone, DeepClone, ToStr)]
|
|
|
|
|
pub struct Rot2<N> {
|
|
|
|
|
priv submat: Mat2<N>
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
impl<N: Clone + Trigonometric + Neg<N>> Rot2<N> {
|
|
|
|
|
/// Builds a 2 dimensional rotation matrix from an angle in radian.
|
|
|
|
|
pub fn from_angle(angle: N) -> Rot2<N> {
|
|
|
|
|
let (sia, coa) = angle.sin_cos();
|
|
|
|
|
|
|
|
|
|
Rot2 {
|
|
|
|
|
submat: Mat2::new(coa.clone(), -sia, sia.clone(), coa)
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
impl<N: Trigonometric + Num + Clone>
|
|
|
|
|
Rotation<Vec1<N>> for Rot2<N> {
|
|
|
|
|
#[inline]
|
|
|
|
|
fn rotation(&self) -> Vec1<N> {
|
|
|
|
|
Vec1::new((-self.submat.at((0, 1))).atan2(&self.submat.at((0, 0))))
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
|
fn inv_rotation(&self) -> Vec1<N> {
|
|
|
|
|
-self.rotation()
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
|
fn rotate_by(&mut self, rot: &Vec1<N>) {
|
|
|
|
|
*self = self.rotated(rot)
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
|
fn rotated(&self, rot: &Vec1<N>) -> Rot2<N> {
|
|
|
|
|
Rot2::from_angle(rot.x.clone()) * *self
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
|
fn set_rotation(&mut self, rot: Vec1<N>) {
|
|
|
|
|
*self = Rot2::from_angle(rot.x)
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
impl<N: Clone + Rand + Trigonometric + Neg<N>> Rand for Rot2<N> {
|
|
|
|
|
#[inline]
|
|
|
|
|
fn rand<R: Rng>(rng: &mut R) -> Rot2<N> {
|
|
|
|
|
Rot2::from_angle(rng.gen())
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
impl<N: Signed> AbsoluteRotate<Vec2<N>> for Rot2<N> {
|
|
|
|
|
#[inline]
|
|
|
|
|
fn absolute_rotate(&self, v: &Vec2<N>) -> Vec2<N> {
|
|
|
|
|
// the matrix is skew-symetric, so we dont need to compute the absolute value of every
|
|
|
|
|
// component.
|
|
|
|
|
let m11 = self.submat.m11.abs();
|
|
|
|
|
let m12 = self.submat.m12.abs();
|
|
|
|
|
let m22 = self.submat.m22.abs();
|
|
|
|
|
|
|
|
|
|
Vec2::new(m11 * v.x + m12 * v.y, m12 * v.x + m22 * v.y)
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
* 3d rotation
|
|
|
|
|
*/
|
|
|
|
|
/// Three dimensional rotation matrix.
|
|
|
|
|
#[deriving(Eq, Encodable, Decodable, Clone, DeepClone, ToStr)]
|
|
|
|
|
pub struct Rot3<N> {
|
|
|
|
|
priv submat: Mat3<N>
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
impl<N: Clone + Trigonometric + Num + Algebraic> Rot3<N> {
|
|
|
|
|
/// Builds a 3 dimensional rotation matrix from an axis and an angle.
|
|
|
|
|
///
|
|
|
|
|
/// # Arguments
|
|
|
|
|
/// * `axisangle` - A vector representing the rotation. Its magnitude is the amount of rotation
|
|
|
|
|
/// in radian. Its direction is the axis of rotation.
|
|
|
|
|
pub fn from_axis_angle(axisangle: Vec3<N>) -> Rot3<N> {
|
|
|
|
|
if axisangle.sqnorm().is_zero() {
|
|
|
|
|
One::one()
|
|
|
|
|
}
|
|
|
|
|
else {
|
|
|
|
|
let mut axis = axisangle;
|
|
|
|
|
let angle = axis.normalize();
|
|
|
|
|
let _1: N = One::one();
|
|
|
|
|
let ux = axis.x.clone();
|
|
|
|
|
let uy = axis.y.clone();
|
|
|
|
|
let uz = axis.z.clone();
|
|
|
|
|
let sqx = ux * ux;
|
|
|
|
|
let sqy = uy * uy;
|
|
|
|
|
let sqz = uz * uz;
|
|
|
|
|
let (sin, cos) = angle.sin_cos();
|
|
|
|
|
let one_m_cos = _1 - cos;
|
|
|
|
|
|
|
|
|
|
Rot3 {
|
|
|
|
|
submat: Mat3::new(
|
|
|
|
|
(sqx + (_1 - sqx) * cos),
|
|
|
|
|
(ux * uy * one_m_cos - uz * sin),
|
|
|
|
|
(ux * uz * one_m_cos + uy * sin),
|
|
|
|
|
|
|
|
|
|
(ux * uy * one_m_cos + uz * sin),
|
|
|
|
|
(sqy + (_1 - sqy) * cos),
|
|
|
|
|
(uy * uz * one_m_cos - ux * sin),
|
|
|
|
|
|
|
|
|
|
(ux * uz * one_m_cos - uy * sin),
|
|
|
|
|
(uy * uz * one_m_cos + ux * sin),
|
|
|
|
|
(sqz + (_1 - sqz) * cos))
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
impl<N: Clone + Num + Algebraic> Rot3<N> {
|
|
|
|
|
/// Reorient this matrix such that its local `x` axis points to a given point. Note that the
|
|
|
|
|
/// usually known `look_at` function does the same thing but with the `z` axis. See `look_at_z`
|
|
|
|
|
/// for that.
|
|
|
|
|
///
|
|
|
|
|
/// # Arguments
|
|
|
|
|
/// * at - The point to look at. It is also the direction the matrix `x` axis will be aligned
|
|
|
|
|
/// with
|
|
|
|
|
/// * up - Vector pointing `up`. The only requirement of this parameter is to not be colinear
|
|
|
|
|
/// with `at`. Non-colinearity is not checked.
|
|
|
|
|
pub fn look_at(&mut self, at: &Vec3<N>, up: &Vec3<N>) {
|
|
|
|
|
let xaxis = at.normalized();
|
|
|
|
|
let zaxis = up.cross(&xaxis).normalized();
|
|
|
|
|
let yaxis = zaxis.cross(&xaxis);
|
|
|
|
|
|
|
|
|
|
self.submat = Mat3::new(xaxis.x.clone(), yaxis.x.clone(), zaxis.x.clone(),
|
|
|
|
|
xaxis.y.clone(), yaxis.y.clone(), zaxis.y.clone(),
|
|
|
|
|
xaxis.z , yaxis.z , zaxis.z)
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Reorient this matrix such that its local `z` axis points to a given point.
|
|
|
|
|
///
|
|
|
|
|
/// # Arguments
|
|
|
|
|
/// * at - The point to look at. It is also the direction the matrix `y` axis will be aligned
|
|
|
|
|
/// with
|
|
|
|
|
/// * up - Vector pointing `up`. The only requirement of this parameter is to not be colinear
|
|
|
|
|
/// with `at`. Non-colinearity is not checked.
|
|
|
|
|
pub fn look_at_z(&mut self, at: &Vec3<N>, up: &Vec3<N>) {
|
|
|
|
|
let zaxis = at.normalized();
|
|
|
|
|
let xaxis = up.cross(&zaxis).normalized();
|
|
|
|
|
let yaxis = zaxis.cross(&xaxis);
|
|
|
|
|
|
|
|
|
|
self.submat = Mat3::new(xaxis.x.clone(), yaxis.x.clone(), zaxis.x.clone(),
|
|
|
|
|
xaxis.y.clone(), yaxis.y.clone(), zaxis.y.clone(),
|
|
|
|
|
xaxis.z , yaxis.z , zaxis.z)
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2013-10-10 04:59:44 +08:00
|
|
|
|
impl<N: Clone + Trigonometric + Num + Algebraic + Cast<f32>>
|
2013-10-06 22:54:09 +08:00
|
|
|
|
Rotation<Vec3<N>> for Rot3<N> {
|
|
|
|
|
#[inline]
|
|
|
|
|
fn rotation(&self) -> Vec3<N> {
|
2013-10-10 04:59:44 +08:00
|
|
|
|
let angle = ((self.submat.m11 + self.submat.m22 + self.submat.m33 - One::one()) / Cast::from(2.0)).acos();
|
2013-10-06 22:54:09 +08:00
|
|
|
|
|
|
|
|
|
if angle != angle {
|
|
|
|
|
// FIXME: handle that correctly
|
|
|
|
|
Zero::zero()
|
|
|
|
|
}
|
|
|
|
|
else if angle.is_zero() {
|
|
|
|
|
Zero::zero()
|
|
|
|
|
}
|
|
|
|
|
else {
|
|
|
|
|
let m32_m23 = self.submat.m32 - self.submat.m23;
|
|
|
|
|
let m13_m31 = self.submat.m13 - self.submat.m31;
|
|
|
|
|
let m21_m12 = self.submat.m21 - self.submat.m12;
|
|
|
|
|
|
|
|
|
|
let denom = (m32_m23 * m32_m23 + m13_m31 * m13_m31 + m21_m12 * m21_m12).sqrt();
|
|
|
|
|
|
|
|
|
|
if denom.is_zero() {
|
|
|
|
|
// XXX: handle that properly
|
|
|
|
|
// fail!("Internal error: singularity.")
|
|
|
|
|
Zero::zero()
|
|
|
|
|
}
|
|
|
|
|
else {
|
|
|
|
|
let a_d = angle / denom;
|
|
|
|
|
|
|
|
|
|
Vec3::new(m32_m23 * a_d, m13_m31 * a_d, m21_m12 * a_d)
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
|
fn inv_rotation(&self) -> Vec3<N> {
|
|
|
|
|
-self.rotation()
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
|
fn rotate_by(&mut self, rot: &Vec3<N>) {
|
|
|
|
|
*self = self.rotated(rot)
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
|
fn rotated(&self, axisangle: &Vec3<N>) -> Rot3<N> {
|
|
|
|
|
Rot3::from_axis_angle(axisangle.clone()) * *self
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
|
fn set_rotation(&mut self, axisangle: Vec3<N>) {
|
|
|
|
|
*self = Rot3::from_axis_angle(axisangle)
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
impl<N: Clone + Rand + Trigonometric + Num + Algebraic>
|
|
|
|
|
Rand for Rot3<N> {
|
|
|
|
|
#[inline]
|
|
|
|
|
fn rand<R: Rng>(rng: &mut R) -> Rot3<N> {
|
|
|
|
|
Rot3::from_axis_angle(rng.gen())
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
impl<N: Signed> AbsoluteRotate<Vec3<N>> for Rot3<N> {
|
|
|
|
|
#[inline]
|
|
|
|
|
fn absolute_rotate(&self, v: &Vec3<N>) -> Vec3<N> {
|
|
|
|
|
Vec3::new(
|
|
|
|
|
self.submat.m11.abs() * v.x + self.submat.m12.abs() * v.y + self.submat.m13.abs() * v.z,
|
|
|
|
|
self.submat.m21.abs() * v.x + self.submat.m22.abs() * v.y + self.submat.m23.abs() * v.z,
|
|
|
|
|
self.submat.m31.abs() * v.x + self.submat.m32.abs() * v.y + self.submat.m33.abs() * v.z)
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Four dimensional rotation matrix.
|
|
|
|
|
#[deriving(Eq, Encodable, Decodable, Clone, DeepClone, ToStr)]
|
|
|
|
|
pub struct Rot4<N> {
|
|
|
|
|
priv submat: Mat4<N>
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
impl<N: Signed> AbsoluteRotate<Vec4<N>> for Rot4<N> {
|
|
|
|
|
#[inline]
|
|
|
|
|
fn absolute_rotate(&self, v: &Vec4<N>) -> Vec4<N> {
|
|
|
|
|
Vec4::new(
|
|
|
|
|
self.submat.m11.abs() * v.x + self.submat.m12.abs() * v.y +
|
|
|
|
|
self.submat.m13.abs() * v.z + self.submat.m14.abs() * v.w,
|
|
|
|
|
|
|
|
|
|
self.submat.m21.abs() * v.x + self.submat.m22.abs() * v.y +
|
|
|
|
|
self.submat.m23.abs() * v.z + self.submat.m24.abs() * v.w,
|
|
|
|
|
|
|
|
|
|
self.submat.m31.abs() * v.x + self.submat.m32.abs() * v.y +
|
|
|
|
|
self.submat.m33.abs() * v.z + self.submat.m34.abs() * v.w,
|
|
|
|
|
|
|
|
|
|
self.submat.m41.abs() * v.x + self.submat.m42.abs() * v.y +
|
|
|
|
|
self.submat.m43.abs() * v.z + self.submat.m44.abs() * v.w)
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
* Common implementations.
|
|
|
|
|
*/
|
|
|
|
|
|
|
|
|
|
double_dispatch_binop_decl_trait!(Rot2, Rot2MulRhs)
|
|
|
|
|
mul_redispatch_impl!(Rot2, Rot2MulRhs)
|
|
|
|
|
submat_impl!(Rot2, Mat2)
|
|
|
|
|
rotate_impl!(Rot2, Vec2)
|
|
|
|
|
transform_impl!(Rot2, Vec2)
|
|
|
|
|
dim_impl!(Rot2, 2)
|
|
|
|
|
rot_mul_rot_impl!(Rot2, Rot2MulRhs)
|
|
|
|
|
rot_mul_vec_impl!(Rot2, Vec2, Rot2MulRhs)
|
|
|
|
|
vec_mul_rot_impl!(Rot2, Vec2, Vec2MulRhs)
|
|
|
|
|
one_impl!(Rot2)
|
|
|
|
|
rotation_matrix_impl!(Rot2, Vec2, Vec1)
|
|
|
|
|
col_impl!(Rot2, Vec2)
|
|
|
|
|
row_impl!(Rot2, Vec2)
|
|
|
|
|
absolute_impl!(Rot2, Mat2)
|
|
|
|
|
to_homogeneous_impl!(Rot2, Mat3)
|
|
|
|
|
inv_impl!(Rot2)
|
|
|
|
|
transpose_impl!(Rot2)
|
|
|
|
|
approx_eq_impl!(Rot2)
|
|
|
|
|
|
|
|
|
|
double_dispatch_binop_decl_trait!(Rot3, Rot3MulRhs)
|
|
|
|
|
mul_redispatch_impl!(Rot3, Rot3MulRhs)
|
|
|
|
|
submat_impl!(Rot3, Mat3)
|
|
|
|
|
rotate_impl!(Rot3, Vec3)
|
|
|
|
|
transform_impl!(Rot3, Vec3)
|
|
|
|
|
dim_impl!(Rot3, 3)
|
|
|
|
|
rot_mul_rot_impl!(Rot3, Rot3MulRhs)
|
|
|
|
|
rot_mul_vec_impl!(Rot3, Vec3, Rot3MulRhs)
|
|
|
|
|
vec_mul_rot_impl!(Rot3, Vec3, Vec3MulRhs)
|
|
|
|
|
one_impl!(Rot3)
|
|
|
|
|
rotation_matrix_impl!(Rot3, Vec3, Vec3)
|
|
|
|
|
col_impl!(Rot3, Vec3)
|
|
|
|
|
row_impl!(Rot3, Vec3)
|
|
|
|
|
absolute_impl!(Rot3, Mat3)
|
|
|
|
|
to_homogeneous_impl!(Rot3, Mat4)
|
|
|
|
|
inv_impl!(Rot3)
|
|
|
|
|
transpose_impl!(Rot3)
|
|
|
|
|
approx_eq_impl!(Rot3)
|
|
|
|
|
|
|
|
|
|
double_dispatch_binop_decl_trait!(Rot4, Rot4MulRhs)
|
|
|
|
|
mul_redispatch_impl!(Rot4, Rot4MulRhs)
|
|
|
|
|
submat_impl!(Rot4, Mat4)
|
|
|
|
|
rotate_impl!(Rot4, Vec4)
|
|
|
|
|
transform_impl!(Rot4, Vec4)
|
|
|
|
|
dim_impl!(Rot4, 4)
|
|
|
|
|
rot_mul_rot_impl!(Rot4, Rot4MulRhs)
|
|
|
|
|
rot_mul_vec_impl!(Rot4, Vec4, Rot4MulRhs)
|
|
|
|
|
vec_mul_rot_impl!(Rot4, Vec4, Vec4MulRhs)
|
|
|
|
|
one_impl!(Rot4)
|
|
|
|
|
// rotation_matrix_impl!(Rot4, Vec4, Vec4)
|
|
|
|
|
col_impl!(Rot4, Vec4)
|
|
|
|
|
row_impl!(Rot4, Vec4)
|
|
|
|
|
absolute_impl!(Rot4, Mat4)
|
|
|
|
|
to_homogeneous_impl!(Rot4, Mat5)
|
|
|
|
|
inv_impl!(Rot4)
|
|
|
|
|
transpose_impl!(Rot4)
|
|
|
|
|
approx_eq_impl!(Rot4)
|