Add Rotation trait, RotationMat type, and implement Mat4 conversions
This commit is contained in:
Brendan Zabarauskas
parent
aa2f9224ce
commit
3daea27696
1 changed files with 374 additions and 1 deletions
|
|
@ -26,16 +26,238 @@
|
|||
//! AxisY::<f32>(0.3).to_quat()
|
||||
//! ~~~
|
||||
|
||||
use std::cast;
|
||||
|
||||
use math::{Dimensioned, SwapComponents};
|
||||
use math::{Mat, NumMat, FloatMat};
|
||||
use math::{Mat3, ToMat3};
|
||||
use math::{Mat4, ToMat4};
|
||||
use math::{Quat, ToQuat};
|
||||
use math::{Vec3, ToVec3, AsVec3};
|
||||
|
||||
use math::{Point3, Ray3};
|
||||
|
||||
/// A generic rotation
|
||||
pub trait Rotation<T>: Eq
|
||||
+ ApproxEq<T>
|
||||
+ ToMat3<T>
|
||||
+ ToQuat<T> {}
|
||||
+ ToMat4<T>
|
||||
+ ToQuat<T> {
|
||||
pub fn rotate_point3(&self, point: Point3<T>) -> Point3<T>;
|
||||
pub fn rotate_vec3(&self, vec: Vec3<T>) -> Point3<T>;
|
||||
pub fn rotate_ray3(&self, vec: Ray3<T>) -> Ray3<T>;
|
||||
}
|
||||
|
||||
impl<T:Float> Rotation<T> for Quat<T> {
|
||||
pub fn rotate_point3(&self, _point: Point3<T>) -> Point3<T> {
|
||||
fail!("Not yet implemented.")
|
||||
}
|
||||
|
||||
pub fn rotate_vec3(&self, _vec: Vec3<T>) -> Point3<T> {
|
||||
fail!("Not yet implemented.")
|
||||
}
|
||||
|
||||
pub fn rotate_ray3(&self, _vec: Ray3<T>) -> Ray3<T> {
|
||||
fail!("Not yet implemented.")
|
||||
}
|
||||
}
|
||||
|
||||
/// A rotation matrix
|
||||
#[deriving(Eq, Clone)]
|
||||
pub struct RotationMat<T> {
|
||||
priv mat: Mat3<T>
|
||||
}
|
||||
|
||||
impl_dimensioned!(RotationMat, Vec3<T>, 3)
|
||||
|
||||
impl<T> RotationMat<T> {
|
||||
#[inline]
|
||||
pub fn as_mat3<'a>(&'a self) -> & 'a Mat3<T> {
|
||||
unsafe { cast::transmute(self) }
|
||||
}
|
||||
}
|
||||
|
||||
impl<T:Float> Rotation<T> for RotationMat<T> {
|
||||
pub fn rotate_point3(&self, _point: Point3<T>) -> Point3<T> {
|
||||
fail!("Not yet implemented.")
|
||||
}
|
||||
|
||||
pub fn rotate_vec3(&self, _vec: Vec3<T>) -> Point3<T> {
|
||||
fail!("Not yet implemented.")
|
||||
}
|
||||
|
||||
pub fn rotate_ray3(&self, _vec: Ray3<T>) -> Ray3<T> {
|
||||
fail!("Not yet implemented.")
|
||||
}
|
||||
}
|
||||
|
||||
impl<T:Clone + Float> ToQuat<T> for RotationMat<T> {
|
||||
#[inline] pub fn to_quat(&self) -> Quat<T> { self.mat.to_quat() }
|
||||
}
|
||||
|
||||
impl<T:Clone + Float> ToMat3<T> for RotationMat<T> {
|
||||
#[inline] pub fn to_mat3(&self) -> Mat3<T> { self.mat.clone() }
|
||||
}
|
||||
|
||||
impl<T:Clone + Float> ToMat4<T> for RotationMat<T> {
|
||||
#[inline] pub fn to_mat4(&self) -> Mat4<T> { self.mat.to_mat4() }
|
||||
}
|
||||
|
||||
impl<T:Clone> Mat<T,Vec3<T>,[Vec3<T>,..3]> for RotationMat<T> {
|
||||
#[inline]
|
||||
pub fn col<'a>(&'a self, i: uint) -> &'a Vec3<T> { self.mat.col(i) }
|
||||
|
||||
#[inline]
|
||||
pub fn col_mut<'a>(&'a mut self, i: uint) -> &'a mut Vec3<T> { self.mat.col_mut(i) }
|
||||
|
||||
#[inline]
|
||||
pub fn elem<'a>(&'a self, col: uint, row: uint) -> &'a T { self.mat.elem(col, row) }
|
||||
|
||||
#[inline]
|
||||
pub fn elem_mut<'a>(&'a mut self, col: uint, row: uint) -> &'a mut T { self.mat.elem_mut(col, row) }
|
||||
|
||||
#[inline]
|
||||
pub fn swap_cols(&mut self, a: uint, b: uint) { self.mat.swap_cols(a, b) }
|
||||
|
||||
#[inline]
|
||||
pub fn row(&self, i: uint) -> Vec3<T> { self.mat.row(i) }
|
||||
|
||||
#[inline]
|
||||
pub fn swap_rows(&mut self, a: uint, b: uint) { self.mat.swap_rows(a, b) }
|
||||
|
||||
#[inline]
|
||||
pub fn swap_elem(&mut self, a: (uint, uint), b: (uint, uint)) { self.mat.swap_elem(a, b) }
|
||||
|
||||
#[inline]
|
||||
pub fn transpose(&self) -> RotationMat<T> { RotationMat { mat: self.mat.transpose() } }
|
||||
|
||||
#[inline]
|
||||
pub fn transpose_self(&mut self) { self.mat.transpose_self() }
|
||||
|
||||
}
|
||||
|
||||
impl<T:Num> RotationMat<T> {
|
||||
#[inline]
|
||||
pub fn identity() -> RotationMat<T> {
|
||||
RotationMat { mat: Mat3::identity() }
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn zero() -> RotationMat<T> {
|
||||
RotationMat { mat: Mat3::zero() }
|
||||
}
|
||||
}
|
||||
|
||||
impl<T:Clone + Num> RotationMat<T> {
|
||||
#[inline]
|
||||
pub fn from_value(value: T) -> RotationMat<T> {
|
||||
RotationMat { mat: Mat3::from_value(value) }
|
||||
}
|
||||
}
|
||||
|
||||
impl<T:Clone + Num> NumMat<T,Vec3<T>,[Vec3<T>,..3]> for RotationMat<T> {
|
||||
#[inline]
|
||||
pub fn mul_t(&self, value: T) -> RotationMat<T> {
|
||||
RotationMat { mat: self.mat.mul_t(value) }
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn mul_v(&self, vec: &Vec3<T>) -> Vec3<T> { self.mat.mul_v(vec) }
|
||||
|
||||
#[inline]
|
||||
pub fn add_m(&self, other: &RotationMat<T>) -> RotationMat<T> {
|
||||
RotationMat { mat: self.mat.add_m(&other.mat) }
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn sub_m(&self, other: &RotationMat<T>) -> RotationMat<T> {
|
||||
RotationMat { mat: self.mat.sub_m(&other.mat) }
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn mul_m(&self, other: &RotationMat<T>) -> RotationMat<T> {
|
||||
RotationMat { mat: self.mat.mul_m(&other.mat) }
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn mul_self_t(&mut self, value: T) { self.mat.mul_self_t(value) }
|
||||
|
||||
#[inline]
|
||||
pub fn add_self_m(&mut self, other: &RotationMat<T>) { self.mat.add_self_m(&other.mat) }
|
||||
|
||||
#[inline]
|
||||
pub fn sub_self_m(&mut self, other: &RotationMat<T>) { self.mat.sub_self_m(&other.mat) }
|
||||
|
||||
#[inline]
|
||||
pub fn dot(&self, other: &RotationMat<T>) -> T { self.mat.dot(&other.mat) }
|
||||
|
||||
#[inline]
|
||||
pub fn determinant(&self) -> T { self.mat.determinant() }
|
||||
|
||||
#[inline]
|
||||
pub fn trace(&self) -> T { self.mat.trace() }
|
||||
|
||||
#[inline]
|
||||
pub fn to_identity(&mut self) { self.mat.to_identity() }
|
||||
|
||||
#[inline]
|
||||
pub fn to_zero(&mut self) { self.mat.to_zero() }
|
||||
}
|
||||
|
||||
impl<T:Clone + Num> Neg<RotationMat<T>> for RotationMat<T> {
|
||||
#[inline]
|
||||
pub fn neg(&self) -> RotationMat<T> {
|
||||
RotationMat { mat: -self.mat }
|
||||
}
|
||||
}
|
||||
|
||||
impl<T:Float> RotationMat<T> {
|
||||
pub fn look_at(dir: &Vec3<T>, up: &Vec3<T>) -> RotationMat<T> {
|
||||
RotationMat { mat: Mat3::look_at(dir, up) }
|
||||
}
|
||||
}
|
||||
|
||||
impl<T:Clone + Float> FloatMat<T,Vec3<T>,[Vec3<T>,..3]> for RotationMat<T> {
|
||||
#[inline]
|
||||
pub fn inverse(&self) -> Option<RotationMat<T>> {
|
||||
unsafe { cast::transmute(self.mat.inverse()) }
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn invert_self(&mut self) { self.mat.invert_self() }
|
||||
|
||||
#[inline]
|
||||
pub fn is_identity(&self) -> bool { self.mat.is_identity() }
|
||||
|
||||
#[inline]
|
||||
pub fn is_diagonal(&self) -> bool { self.mat.is_diagonal() }
|
||||
|
||||
#[inline]
|
||||
pub fn is_rotated(&self) -> bool { self.mat.is_rotated() }
|
||||
|
||||
#[inline]
|
||||
pub fn is_symmetric(&self) -> bool { self.mat.is_symmetric() }
|
||||
|
||||
#[inline]
|
||||
pub fn is_invertible(&self) -> bool { self.mat.is_invertible() }
|
||||
}
|
||||
|
||||
impl<T:Clone + Eq + ApproxEq<T>> ApproxEq<T> for RotationMat<T> {
|
||||
#[inline]
|
||||
pub fn approx_epsilon() -> T {
|
||||
ApproxEq::approx_epsilon::<T,T>()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn approx_eq(&self, other: &RotationMat<T>) -> bool {
|
||||
self.approx_eq_eps(other, &ApproxEq::approx_epsilon::<T,T>())
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn approx_eq_eps(&self, other: &RotationMat<T>, epsilon: &T) -> bool {
|
||||
self.mat.approx_eq_eps(&other.mat, epsilon)
|
||||
}
|
||||
}
|
||||
|
||||
/// Euler angles
|
||||
///
|
||||
|
|
@ -64,6 +286,20 @@ impl<T:Float> Euler<T> {
|
|||
}
|
||||
}
|
||||
|
||||
impl<T:Float> Rotation<T> for Euler<T> {
|
||||
pub fn rotate_point3(&self, _point: Point3<T>) -> Point3<T> {
|
||||
fail!("Not yet implemented.")
|
||||
}
|
||||
|
||||
pub fn rotate_vec3(&self, _vec: Vec3<T>) -> Point3<T> {
|
||||
fail!("Not yet implemented.")
|
||||
}
|
||||
|
||||
pub fn rotate_ray3(&self, _vec: Ray3<T>) -> Ray3<T> {
|
||||
fail!("Not yet implemented.")
|
||||
}
|
||||
}
|
||||
|
||||
impl<T:Float> ToQuat<T> for Euler<T> {
|
||||
pub fn to_quat(&self) -> Quat<T> {
|
||||
// http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Conversion
|
||||
|
|
@ -93,6 +329,23 @@ impl<T:Float> ToMat3<T> for Euler<T> {
|
|||
}
|
||||
}
|
||||
|
||||
impl<T:Float> ToMat4<T> for Euler<T> {
|
||||
pub fn to_mat4(&self) -> Mat4<T> {
|
||||
// http://en.wikipedia.org/wiki/Rotation_matrix#General_rotations
|
||||
let cx = self.pitch.cos();
|
||||
let sx = self.pitch.sin();
|
||||
let cy = self.yaw.cos();
|
||||
let sy = self.yaw.sin();
|
||||
let cz = self.roll.cos();
|
||||
let sz = self.roll.sin();
|
||||
|
||||
Mat4::new(cy * cz, cy * sz, -sy, zero!(T),
|
||||
-cx * sz + sx * sy * cz, cx * cz + sx * sy * sz, sx * cy, zero!(T),
|
||||
sx * sz + cx * sy * cz, -sx * cz + cx * sy * sz, cx * cy, zero!(T),
|
||||
zero!(T), zero!(T), zero!(T), one!(T))
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod euler_tests {
|
||||
// TODO
|
||||
|
|
@ -122,6 +375,20 @@ impl<T:Float> AxisAngle<T> {
|
|||
}
|
||||
}
|
||||
|
||||
impl<T:Float> Rotation<T> for AxisAngle<T> {
|
||||
pub fn rotate_point3(&self, _point: Point3<T>) -> Point3<T> {
|
||||
fail!("Not yet implemented.")
|
||||
}
|
||||
|
||||
pub fn rotate_vec3(&self, _vec: Vec3<T>) -> Point3<T> {
|
||||
fail!("Not yet implemented.")
|
||||
}
|
||||
|
||||
pub fn rotate_ray3(&self, _vec: Ray3<T>) -> Ray3<T> {
|
||||
fail!("Not yet implemented.")
|
||||
}
|
||||
}
|
||||
|
||||
impl<T:Float> ToQuat<T> for AxisAngle<T> {
|
||||
pub fn to_quat(&self) -> Quat<T> {
|
||||
let half = self.angle / two!(T);
|
||||
|
|
@ -149,6 +416,31 @@ impl<T:Float> ToMat3<T> for AxisAngle<T> {
|
|||
}
|
||||
}
|
||||
|
||||
impl<T:Float> ToMat4<T> for AxisAngle<T> {
|
||||
pub fn to_mat4(&self) -> Mat4<T> {
|
||||
let c = self.angle.cos();
|
||||
let s = self.angle.sin();
|
||||
let _1_c = one!(T) - c;
|
||||
|
||||
Mat4::new(_1_c * self.axis.x * self.axis.x + c,
|
||||
_1_c * self.axis.x * self.axis.y + s * self.axis.z,
|
||||
_1_c * self.axis.x * self.axis.z - s * self.axis.y,
|
||||
zero!(T),
|
||||
|
||||
_1_c * self.axis.x * self.axis.y - s * self.axis.z,
|
||||
_1_c * self.axis.y * self.axis.y + c,
|
||||
_1_c * self.axis.y * self.axis.z + s * self.axis.x,
|
||||
zero!(T),
|
||||
|
||||
_1_c * self.axis.x * self.axis.z + s * self.axis.y,
|
||||
_1_c * self.axis.y * self.axis.z - s * self.axis.x,
|
||||
_1_c * self.axis.z * self.axis.z + c,
|
||||
zero!(T),
|
||||
|
||||
zero!(T), zero!(T), zero!(T), one!(T))
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod axis_angle_tests {
|
||||
use math::mat::*;
|
||||
|
|
@ -177,6 +469,20 @@ pub struct AngleX<T>(T);
|
|||
|
||||
impl_approx!(AngleX)
|
||||
|
||||
impl<T:Float> Rotation<T> for AngleX<T> {
|
||||
pub fn rotate_point3(&self, _point: Point3<T>) -> Point3<T> {
|
||||
fail!("Not yet implemented.")
|
||||
}
|
||||
|
||||
pub fn rotate_vec3(&self, _vec: Vec3<T>) -> Point3<T> {
|
||||
fail!("Not yet implemented.")
|
||||
}
|
||||
|
||||
pub fn rotate_ray3(&self, _vec: Ray3<T>) -> Ray3<T> {
|
||||
fail!("Not yet implemented.")
|
||||
}
|
||||
}
|
||||
|
||||
impl<T:Float> ToQuat<T> for AngleX<T> {
|
||||
pub fn to_quat(&self) -> Quat<T> {
|
||||
Quat::new(((**self) / two!(T)).cos(), (**self).sin(), zero!(T), zero!(T))
|
||||
|
|
@ -195,6 +501,19 @@ impl<T:Clone + Float> ToMat3<T> for AngleX<T> {
|
|||
}
|
||||
}
|
||||
|
||||
impl<T:Clone + Float> ToMat4<T> for AngleX<T> {
|
||||
pub fn to_mat4(&self) -> Mat4<T> {
|
||||
// http://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations
|
||||
let cos_theta = (**self).cos();
|
||||
let sin_theta = (**self).sin();
|
||||
|
||||
Mat4::new(one!(T), zero!(T), zero!(T), zero!(T),
|
||||
zero!(T), cos_theta.clone(), sin_theta.clone(), zero!(T),
|
||||
zero!(T), -sin_theta.clone(), cos_theta.clone(), zero!(T),
|
||||
zero!(T), zero!(T), zero!(T), one!(T))
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod angle_x_tests {
|
||||
// TODO
|
||||
|
|
@ -206,6 +525,20 @@ pub struct AngleY<T>(T);
|
|||
|
||||
impl_approx!(AngleY)
|
||||
|
||||
impl<T:Float> Rotation<T> for AngleY<T> {
|
||||
pub fn rotate_point3(&self, _point: Point3<T>) -> Point3<T> {
|
||||
fail!("Not yet implemented.")
|
||||
}
|
||||
|
||||
pub fn rotate_vec3(&self, _vec: Vec3<T>) -> Point3<T> {
|
||||
fail!("Not yet implemented.")
|
||||
}
|
||||
|
||||
pub fn rotate_ray3(&self, _vec: Ray3<T>) -> Ray3<T> {
|
||||
fail!("Not yet implemented.")
|
||||
}
|
||||
}
|
||||
|
||||
impl<T:Float> ToQuat<T> for AngleY<T> {
|
||||
pub fn to_quat(&self) -> Quat<T> {
|
||||
Quat::new(((**self) / two!(T)).cos(), zero!(T), (**self).sin(), zero!(T))
|
||||
|
|
@ -224,6 +557,19 @@ impl<T:Clone + Float> ToMat3<T> for AngleY<T> {
|
|||
}
|
||||
}
|
||||
|
||||
impl<T:Clone + Float> ToMat4<T> for AngleY<T> {
|
||||
pub fn to_mat4(&self) -> Mat4<T> {
|
||||
// http://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations
|
||||
let cos_theta = (**self).cos();
|
||||
let sin_theta = (**self).sin();
|
||||
|
||||
Mat4::new(cos_theta.clone(), zero!(T), -sin_theta.clone(), zero!(T),
|
||||
zero!(T), one!(T), zero!(T), zero!(T),
|
||||
sin_theta.clone(), zero!(T), cos_theta.clone(), zero!(T),
|
||||
zero!(T), zero!(T), zero!(T), one!(T))
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod angle_y_tests {
|
||||
// TODO
|
||||
|
|
@ -235,6 +581,20 @@ pub struct AngleZ<T>(T);
|
|||
|
||||
impl_approx!(AngleZ)
|
||||
|
||||
impl<T:Float> Rotation<T> for AngleZ<T> {
|
||||
pub fn rotate_point3(&self, _point: Point3<T>) -> Point3<T> {
|
||||
fail!("Not yet implemented.")
|
||||
}
|
||||
|
||||
pub fn rotate_vec3(&self, _vec: Vec3<T>) -> Point3<T> {
|
||||
fail!("Not yet implemented.")
|
||||
}
|
||||
|
||||
pub fn rotate_ray3(&self, _vec: Ray3<T>) -> Ray3<T> {
|
||||
fail!("Not yet implemented.")
|
||||
}
|
||||
}
|
||||
|
||||
impl<T:Float> ToQuat<T> for AngleZ<T> {
|
||||
pub fn to_quat(&self) -> Quat<T> {
|
||||
Quat::new(((**self) / two!(T)).cos(), zero!(T), zero!(T), (**self).sin())
|
||||
|
|
@ -253,6 +613,19 @@ impl<T:Clone + Float> ToMat3<T> for AngleZ<T> {
|
|||
}
|
||||
}
|
||||
|
||||
impl<T:Clone + Float> ToMat4<T> for AngleZ<T> {
|
||||
pub fn to_mat4(&self) -> Mat4<T> {
|
||||
// http://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations
|
||||
let cos_theta = (**self).cos();
|
||||
let sin_theta = (**self).sin();
|
||||
|
||||
Mat4::new(cos_theta.clone(), sin_theta.clone(), zero!(T), zero!(T),
|
||||
-sin_theta.clone(), cos_theta.clone(), zero!(T), zero!(T),
|
||||
zero!(T), zero!(T), one!(T), zero!(T),
|
||||
zero!(T), zero!(T), zero!(T), one!(T))
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod angle_z_tests {
|
||||
// TODO
|
||||
|
|
|
|||
Loading…
Reference in a new issue