forked from M-Labs/nalgebra
329 lines
10 KiB
Rust
329 lines
10 KiB
Rust
//! Rotations matrices.
|
||
|
||
#[allow(missing_doc)];
|
||
|
||
use std::num::{Zero, One, from_f32};
|
||
use std::rand::{Rand, Rng};
|
||
use traits::geometry::{Rotate, Rotation, AbsoluteRotate, RotationMatrix, Transform, ToHomogeneous,
|
||
Norm, Cross};
|
||
use traits::structure::{Dim, Indexable, Row, Col};
|
||
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)
|
||
}
|
||
}
|
||
|
||
impl<N: Clone + Trigonometric + Num + Algebraic + FromPrimitive>
|
||
Rotation<Vec3<N>> for Rot3<N> {
|
||
#[inline]
|
||
fn rotation(&self) -> Vec3<N> {
|
||
let angle = ((self.submat.m11 + self.submat.m22 + self.submat.m33 - One::one()) / from_f32(2.0).unwrap()).acos();
|
||
|
||
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)
|