diff --git a/src/math/rotation.rs b/src/math/rotation.rs index 6c6d4cd..1ade5e7 100644 --- a/src/math/rotation.rs +++ b/src/math/rotation.rs @@ -26,16 +26,238 @@ //! AxisY::(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: Eq + ApproxEq + ToMat3 - + ToQuat {} + + ToMat4 + + ToQuat { + pub fn rotate_point3(&self, point: Point3) -> Point3; + pub fn rotate_vec3(&self, vec: Vec3) -> Point3; + pub fn rotate_ray3(&self, vec: Ray3) -> Ray3; +} + +impl Rotation for Quat { + pub fn rotate_point3(&self, _point: Point3) -> Point3 { + fail!("Not yet implemented.") + } + + pub fn rotate_vec3(&self, _vec: Vec3) -> Point3 { + fail!("Not yet implemented.") + } + + pub fn rotate_ray3(&self, _vec: Ray3) -> Ray3 { + fail!("Not yet implemented.") + } +} + +/// A rotation matrix +#[deriving(Eq, Clone)] +pub struct RotationMat { + priv mat: Mat3 +} + +impl_dimensioned!(RotationMat, Vec3, 3) + +impl RotationMat { + #[inline] + pub fn as_mat3<'a>(&'a self) -> & 'a Mat3 { + unsafe { cast::transmute(self) } + } +} + +impl Rotation for RotationMat { + pub fn rotate_point3(&self, _point: Point3) -> Point3 { + fail!("Not yet implemented.") + } + + pub fn rotate_vec3(&self, _vec: Vec3) -> Point3 { + fail!("Not yet implemented.") + } + + pub fn rotate_ray3(&self, _vec: Ray3) -> Ray3 { + fail!("Not yet implemented.") + } +} + +impl ToQuat for RotationMat { + #[inline] pub fn to_quat(&self) -> Quat { self.mat.to_quat() } +} + +impl ToMat3 for RotationMat { + #[inline] pub fn to_mat3(&self) -> Mat3 { self.mat.clone() } +} + +impl ToMat4 for RotationMat { + #[inline] pub fn to_mat4(&self) -> Mat4 { self.mat.to_mat4() } +} + +impl Mat,[Vec3,..3]> for RotationMat { + #[inline] + pub fn col<'a>(&'a self, i: uint) -> &'a Vec3 { self.mat.col(i) } + + #[inline] + pub fn col_mut<'a>(&'a mut self, i: uint) -> &'a mut Vec3 { 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 { 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 { RotationMat { mat: self.mat.transpose() } } + + #[inline] + pub fn transpose_self(&mut self) { self.mat.transpose_self() } + +} + +impl RotationMat { + #[inline] + pub fn identity() -> RotationMat { + RotationMat { mat: Mat3::identity() } + } + + #[inline] + pub fn zero() -> RotationMat { + RotationMat { mat: Mat3::zero() } + } +} + +impl RotationMat { + #[inline] + pub fn from_value(value: T) -> RotationMat { + RotationMat { mat: Mat3::from_value(value) } + } +} + +impl NumMat,[Vec3,..3]> for RotationMat { + #[inline] + pub fn mul_t(&self, value: T) -> RotationMat { + RotationMat { mat: self.mat.mul_t(value) } + } + + #[inline] + pub fn mul_v(&self, vec: &Vec3) -> Vec3 { self.mat.mul_v(vec) } + + #[inline] + pub fn add_m(&self, other: &RotationMat) -> RotationMat { + RotationMat { mat: self.mat.add_m(&other.mat) } + } + + #[inline] + pub fn sub_m(&self, other: &RotationMat) -> RotationMat { + RotationMat { mat: self.mat.sub_m(&other.mat) } + } + + #[inline] + pub fn mul_m(&self, other: &RotationMat) -> RotationMat { + 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) { self.mat.add_self_m(&other.mat) } + + #[inline] + pub fn sub_self_m(&mut self, other: &RotationMat) { self.mat.sub_self_m(&other.mat) } + + #[inline] + pub fn dot(&self, other: &RotationMat) -> 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 Neg> for RotationMat { + #[inline] + pub fn neg(&self) -> RotationMat { + RotationMat { mat: -self.mat } + } +} + +impl RotationMat { + pub fn look_at(dir: &Vec3, up: &Vec3) -> RotationMat { + RotationMat { mat: Mat3::look_at(dir, up) } + } +} + +impl FloatMat,[Vec3,..3]> for RotationMat { + #[inline] + pub fn inverse(&self) -> Option> { + 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> ApproxEq for RotationMat { + #[inline] + pub fn approx_epsilon() -> T { + ApproxEq::approx_epsilon::() + } + + #[inline] + pub fn approx_eq(&self, other: &RotationMat) -> bool { + self.approx_eq_eps(other, &ApproxEq::approx_epsilon::()) + } + + #[inline] + pub fn approx_eq_eps(&self, other: &RotationMat, epsilon: &T) -> bool { + self.mat.approx_eq_eps(&other.mat, epsilon) + } +} /// Euler angles /// @@ -64,6 +286,20 @@ impl Euler { } } +impl Rotation for Euler { + pub fn rotate_point3(&self, _point: Point3) -> Point3 { + fail!("Not yet implemented.") + } + + pub fn rotate_vec3(&self, _vec: Vec3) -> Point3 { + fail!("Not yet implemented.") + } + + pub fn rotate_ray3(&self, _vec: Ray3) -> Ray3 { + fail!("Not yet implemented.") + } +} + impl ToQuat for Euler { pub fn to_quat(&self) -> Quat { // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Conversion @@ -93,6 +329,23 @@ impl ToMat3 for Euler { } } +impl ToMat4 for Euler { + pub fn to_mat4(&self) -> Mat4 { + // 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 AxisAngle { } } +impl Rotation for AxisAngle { + pub fn rotate_point3(&self, _point: Point3) -> Point3 { + fail!("Not yet implemented.") + } + + pub fn rotate_vec3(&self, _vec: Vec3) -> Point3 { + fail!("Not yet implemented.") + } + + pub fn rotate_ray3(&self, _vec: Ray3) -> Ray3 { + fail!("Not yet implemented.") + } +} + impl ToQuat for AxisAngle { pub fn to_quat(&self) -> Quat { let half = self.angle / two!(T); @@ -149,6 +416,31 @@ impl ToMat3 for AxisAngle { } } +impl ToMat4 for AxisAngle { + pub fn to_mat4(&self) -> Mat4 { + 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); impl_approx!(AngleX) +impl Rotation for AngleX { + pub fn rotate_point3(&self, _point: Point3) -> Point3 { + fail!("Not yet implemented.") + } + + pub fn rotate_vec3(&self, _vec: Vec3) -> Point3 { + fail!("Not yet implemented.") + } + + pub fn rotate_ray3(&self, _vec: Ray3) -> Ray3 { + fail!("Not yet implemented.") + } +} + impl ToQuat for AngleX { pub fn to_quat(&self) -> Quat { Quat::new(((**self) / two!(T)).cos(), (**self).sin(), zero!(T), zero!(T)) @@ -195,6 +501,19 @@ impl ToMat3 for AngleX { } } +impl ToMat4 for AngleX { + pub fn to_mat4(&self) -> Mat4 { + // 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); impl_approx!(AngleY) +impl Rotation for AngleY { + pub fn rotate_point3(&self, _point: Point3) -> Point3 { + fail!("Not yet implemented.") + } + + pub fn rotate_vec3(&self, _vec: Vec3) -> Point3 { + fail!("Not yet implemented.") + } + + pub fn rotate_ray3(&self, _vec: Ray3) -> Ray3 { + fail!("Not yet implemented.") + } +} + impl ToQuat for AngleY { pub fn to_quat(&self) -> Quat { Quat::new(((**self) / two!(T)).cos(), zero!(T), (**self).sin(), zero!(T)) @@ -224,6 +557,19 @@ impl ToMat3 for AngleY { } } +impl ToMat4 for AngleY { + pub fn to_mat4(&self) -> Mat4 { + // 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); impl_approx!(AngleZ) +impl Rotation for AngleZ { + pub fn rotate_point3(&self, _point: Point3) -> Point3 { + fail!("Not yet implemented.") + } + + pub fn rotate_vec3(&self, _vec: Vec3) -> Point3 { + fail!("Not yet implemented.") + } + + pub fn rotate_ray3(&self, _vec: Ray3) -> Ray3 { + fail!("Not yet implemented.") + } +} + impl ToQuat for AngleZ { pub fn to_quat(&self) -> Quat { Quat::new(((**self) / two!(T)).cos(), zero!(T), zero!(T), (**self).sin()) @@ -253,6 +613,19 @@ impl ToMat3 for AngleZ { } } +impl ToMat4 for AngleZ { + pub fn to_mat4(&self) -> Mat4 { + // 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