diff --git a/src/macros.rs b/src/macros.rs index d81afa0..a27c906 100644 --- a/src/macros.rs +++ b/src/macros.rs @@ -51,5 +51,23 @@ macro_rules! impl_approx( $( self.$field.approx_eq_eps(&other.$field, epsilon) )&&+ } } + ); + ($T:ident) => ( + impl> ApproxEq for $T { + #[inline] + pub fn approx_epsilon() -> T { + ApproxEq::approx_epsilon::() + } + + #[inline] + pub fn approx_eq(&self, other: &$T) -> bool { + self.approx_eq_eps(other, &ApproxEq::approx_epsilon::()) + } + + #[inline] + pub fn approx_eq_eps(&self, other: &$T, epsilon: &T) -> bool { + (**self).approx_eq_eps(&**other, epsilon) + } + } ) ) diff --git a/src/math/mat.rs b/src/math/mat.rs index 7b935dd..01c28f8 100644 --- a/src/math/mat.rs +++ b/src/math/mat.rs @@ -758,75 +758,6 @@ impl Neg> for Mat3 { } impl Mat3 { - /// Construct a matrix from an angular rotation around the `x` axis - pub fn from_angle_x(radians: T) -> Mat3 { - // http://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations - let cos_theta = radians.cos(); - let sin_theta = radians.sin(); - - Mat3::new(one!(T), zero!(T), zero!(T), - zero!(T), cos_theta.clone(), sin_theta.clone(), - zero!(T), -sin_theta.clone(), cos_theta.clone()) - } - - /// Construct a matrix from an angular rotation around the `y` axis - pub fn from_angle_y(radians: T) -> Mat3 { - // http://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations - let cos_theta = radians.cos(); - let sin_theta = radians.sin(); - - Mat3::new(cos_theta.clone(), zero!(T), -sin_theta.clone(), - zero!(T), one!(T), zero!(T), - sin_theta.clone(), zero!(T), cos_theta.clone()) - } - - /// Construct a matrix from an angular rotation around the `z` axis - pub fn from_angle_z(radians: T) -> Mat3 { - // http://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations - let cos_theta = radians.cos(); - let sin_theta = radians.sin(); - - Mat3::new(cos_theta.clone(), sin_theta.clone(), zero!(T), - -sin_theta.clone(), cos_theta.clone(), zero!(T), - zero!(T), zero!(T), one!(T)) - } - - /// Construct a matrix from Euler angles - /// - /// # Arguments - /// - /// - `theta_x`: the angular rotation around the `x` axis (pitch) - /// - `theta_y`: the angular rotation around the `y` axis (yaw) - /// - `theta_z`: the angular rotation around the `z` axis (roll) - pub fn from_angle_xyz(radians_x: T, radians_y: T, radians_z: T) -> Mat3 { - // http://en.wikipedia.org/wiki/Rotation_matrix#General_rotations - let cx = radians_x.cos(); - let sx = radians_x.sin(); - let cy = radians_y.cos(); - let sy = radians_y.sin(); - let cz = radians_z.cos(); - let sz = radians_z.sin(); - - Mat3::new(cy*cz, cy*sz, -sy, - -cx*sz + sx*sy*cz, cx*cz + sx*sy*sz, sx*cy, - sx*sz + cx*sy*cz, -sx*cz + cx*sy*sz, cx*cy) - } - - /// Construct a matrix from an axis and an angular rotation - pub fn from_angle_axis(radians: T, axis: &Vec3) -> Mat3 { - let c = radians.cos(); - let s = radians.sin(); - let _1_c = one!(T) - c; - - let x = axis.x.clone(); - let y = axis.y.clone(); - let z = axis.z.clone(); - - Mat3::new(_1_c*x*x + c, _1_c*x*y + s*z, _1_c*x*z - s*y, - _1_c*x*y - s*z, _1_c*y*y + c, _1_c*y*z + s*x, - _1_c*x*z + s*y, _1_c*y*z - s*x, _1_c*z*z + c) - } - #[inline] pub fn from_axes(x: Vec3, y: Vec3, z: Vec3) -> Mat3 { Mat3::from_cols(x, y, z) diff --git a/src/math/math.rs b/src/math/math.rs index 97e8a22..fb36498 100644 --- a/src/math/math.rs +++ b/src/math/math.rs @@ -31,6 +31,10 @@ pub use self::point::Point; pub use self::point::{Point2, AsPoint2}; pub use self::point::{Point3, AsPoint3}; pub use self::ray::{Ray2, Ray3}; +pub use self::rotation::Rotation; +pub use self::rotation::{Euler, ToEuler}; +pub use self::rotation::{AxisAngle, ToAxisAngle}; +pub use self::rotation::{AngleX, AngleY, AngleZ}; pub mod mat; pub mod quat; @@ -39,6 +43,7 @@ pub mod vec; pub mod plane; pub mod point; pub mod ray; +pub mod rotation; pub trait Dimensioned { pub fn index<'a>(&'a self, i: uint) -> &'a T; diff --git a/src/math/quat.rs b/src/math/quat.rs index 76221ac..78a684d 100644 --- a/src/math/quat.rs +++ b/src/math/quat.rs @@ -82,42 +82,6 @@ impl Quat { Quat::new(zero!(T), zero!(T), zero!(T), zero!(T)) } - #[inline] - pub fn from_angle_x(radians: T) -> Quat { - Quat::new((radians / two!(T)).cos(), radians.sin(), zero!(T), zero!(T)) - } - - #[inline] - pub fn from_angle_y(radians: T) -> Quat { - Quat::new((radians / two!(T)).cos(), zero!(T), radians.sin(), zero!(T)) - } - - #[inline] - pub fn from_angle_z(radians: T) -> Quat { - Quat::new((radians / two!(T)).cos(), zero!(T), zero!(T), radians.sin()) - } - - pub fn from_angle_xyz(radians_x: T, radians_y: T, radians_z: T) -> Quat { - // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Conversion - let xdiv2 = radians_x / two!(T); - let ydiv2 = radians_y / two!(T); - let zdiv2 = radians_z / two!(T); - Quat::new(zdiv2.cos() * xdiv2.cos() * ydiv2.cos() + zdiv2.sin() * xdiv2.sin() * ydiv2.sin(), - zdiv2.sin() * xdiv2.cos() * ydiv2.cos() - zdiv2.cos() * xdiv2.sin() * ydiv2.sin(), - zdiv2.cos() * xdiv2.sin() * ydiv2.cos() + zdiv2.sin() * xdiv2.cos() * ydiv2.sin(), - zdiv2.cos() * xdiv2.cos() * ydiv2.sin() - zdiv2.sin() * xdiv2.sin() * ydiv2.cos()) - } - - #[inline] - pub fn from_angle_axis(radians: T, axis: &Vec3) -> Quat { - let half = radians / two!(T); - Quat::from_sv(half.cos(), axis.mul_t(half.sin())) - } - - pub fn get_angle_axis(&self) -> (T, Vec3) { - fail!(~"Not yet implemented.") - } - /// The result of multiplying the quaternion a scalar #[inline] pub fn mul_t(&self, value: T) -> Quat { @@ -305,32 +269,3 @@ impl Quat { } } } - -#[cfg(test)] -mod tests { - use math::mat::*; - use math::quat::*; - use math::vec::*; - - #[test] - fn test_from_angle_axis() { - let v = Vec3::new(1f, 0f, 0f); - - let q = Quat::from_angle_axis((-45f).to_radians(), &Vec3::new(0f, 0f, -1f)); - - // http://www.wolframalpha.com/input/?i={1,0}+rotate+-45+degrees - assert_approx_eq!(q.mul_v(&v), Vec3::new(1f/2f.sqrt(), 1f/2f.sqrt(), 0f)); - assert_eq!(q.mul_v(&v).magnitude(), v.magnitude()); - assert_approx_eq!(q.to_mat3(), Mat3::new( 1f/2f.sqrt(), 1f/2f.sqrt(), 0f, - -1f/2f.sqrt(), 1f/2f.sqrt(), 0f, - 0f, 0f, 1f)); - } - - #[test] - fn test_approx_eq() { - assert!(!Quat::new::(0.000001, 0.000001, 0.000001, 0.000001) - .approx_eq(&Quat::new::(0.0, 0.0, 0.0, 0.0))); - assert!(Quat::new::(0.0000001, 0.0000001, 0.0000001, 0.0000001) - .approx_eq(&Quat::new::(0.0, 0.0, 0.0, 0.0))); - } -} diff --git a/src/math/rotation.rs b/src/math/rotation.rs new file mode 100644 index 0000000..6c6d4cd --- /dev/null +++ b/src/math/rotation.rs @@ -0,0 +1,259 @@ +// Copyright 2013 The Lmath Developers. For a full listing of the authors, +// refer to the AUTHORS file at the top-level directory of this distribution. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +//! Various three-dimensional rotation types that are useful for constructing +//! matricies and quaternions. +//! +//! # Examples +//! +//! ~~~rust +//! Euler::new::(1.0, 2.0, 0.0).to_mat3() +//! ~~~ +//! +//! ~~~rust +//! AxisY::(0.3).to_quat() +//! ~~~ + +use math::{Dimensioned, SwapComponents}; +use math::{Mat3, ToMat3}; +use math::{Quat, ToQuat}; +use math::{Vec3, ToVec3, AsVec3}; + +/// A generic rotation +pub trait Rotation: Eq + + ApproxEq + + ToMat3 + + ToQuat {} + +/// Euler angles +/// +/// # Fields +/// +/// - `pitch`: the angular rotation around the `x` axis in radians +/// - `yaw`: the angular rotation around the `y` axis in radians +/// - `roll`: the angular rotation around the `z` axis in radians +#[deriving(Eq, Clone)] +pub struct Euler { pitch: T, yaw: T, roll: T } + +impl_dimensioned!(Euler, T, 3) +impl_to_vec!(Euler, 3) +impl_as_vec!(Euler, 3) +impl_swap_components!(Euler) +impl_approx!(Euler { pitch, yaw, roll }) + +pub trait ToEuler { + pub fn to_euler(&self) -> Euler; +} + +impl Euler { + #[inline] + pub fn new(pitch: T, yaw: T, roll: T) -> Euler { + Euler { pitch: pitch, yaw: yaw, roll: roll } + } +} + +impl ToQuat for Euler { + pub fn to_quat(&self) -> Quat { + // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Conversion + let xdiv2 = self.pitch / two!(T); + let ydiv2 = self.yaw / two!(T); + let zdiv2 = self.roll / two!(T); + Quat::new(zdiv2.cos() * xdiv2.cos() * ydiv2.cos() + zdiv2.sin() * xdiv2.sin() * ydiv2.sin(), + zdiv2.sin() * xdiv2.cos() * ydiv2.cos() - zdiv2.cos() * xdiv2.sin() * ydiv2.sin(), + zdiv2.cos() * xdiv2.sin() * ydiv2.cos() + zdiv2.sin() * xdiv2.cos() * ydiv2.sin(), + zdiv2.cos() * xdiv2.cos() * ydiv2.sin() - zdiv2.sin() * xdiv2.sin() * ydiv2.cos()) + } +} + +impl ToMat3 for Euler { + pub fn to_mat3(&self) -> Mat3 { + // 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(); + + Mat3::new(cy * cz, cy * sz, -sy, + -cx * sz + sx * sy * cz, cx * cz + sx * sy * sz, sx * cy, + sx * sz + cx * sy * cz, -sx * cz + cx * sy * sz, cx * cy) + } +} + +#[cfg(test)] +mod euler_tests { + // TODO +} + +/// A rotation about an arbitrary axis +/// +/// # Fields +/// +/// - `axis`: The axis vector about which to rotate. +/// - `angle`: The angle of rotation in radians. +#[deriving(Eq, Clone)] +pub struct AxisAngle { + axis: Vec3, + angle: T, +} + +impl_approx!(AxisAngle { axis, angle }) + +pub trait ToAxisAngle { + pub fn to_axis_angle(&self) -> AxisAngle; +} + +impl AxisAngle { + pub fn new(axis: Vec3, angle: T) -> AxisAngle { + AxisAngle { axis: axis, angle: angle } + } +} + +impl ToQuat for AxisAngle { + pub fn to_quat(&self) -> Quat { + let half = self.angle / two!(T); + Quat::from_sv(half.cos(), self.axis.mul_t(half.sin())) + } +} + +impl ToMat3 for AxisAngle { + pub fn to_mat3(&self) -> Mat3 { + let c = self.angle.cos(); + let s = self.angle.sin(); + let _1_c = one!(T) - c; + + Mat3::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, + + _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, + + _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) + } +} + +#[cfg(test)] +mod axis_angle_tests { + use math::mat::*; + use math::quat::*; + use math::rotation::*; + use math::vec::*; + + #[test] + fn test_to_quat() { + let v = Vec3::new(1f, 0f, 0f); + + let q = AxisAngle::new(Vec3::new(0f, 0f, -1f), (-45f).to_radians()).to_quat(); + + // http://www.wolframalpha.com/input/?i={1,0}+rotate+-45+degrees + assert_approx_eq!(q.mul_v(&v), Vec3::new(1f/2f.sqrt(), 1f/2f.sqrt(), 0f)); + assert_eq!(q.mul_v(&v).magnitude(), v.magnitude()); + assert_approx_eq!(q.to_mat3(), Mat3::new( 1f/2f.sqrt(), 1f/2f.sqrt(), 0f, + -1f/2f.sqrt(), 1f/2f.sqrt(), 0f, + 0f, 0f, 1f)); + } +} + +/// An angle around the X axis (pitch), in radians. +#[deriving(Eq, Clone)] +pub struct AngleX(T); + +impl_approx!(AngleX) + +impl ToQuat for AngleX { + pub fn to_quat(&self) -> Quat { + Quat::new(((**self) / two!(T)).cos(), (**self).sin(), zero!(T), zero!(T)) + } +} + +impl ToMat3 for AngleX { + pub fn to_mat3(&self) -> Mat3 { + // http://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations + let cos_theta = (**self).cos(); + let sin_theta = (**self).sin(); + + Mat3::new(one!(T), zero!(T), zero!(T), + zero!(T), cos_theta.clone(), sin_theta.clone(), + zero!(T), -sin_theta.clone(), cos_theta.clone()) + } +} + +#[cfg(test)] +mod angle_x_tests { + // TODO +} + +/// An angle around the X axis (yaw), in radians. +#[deriving(Eq, Clone)] +pub struct AngleY(T); + +impl_approx!(AngleY) + +impl ToQuat for AngleY { + pub fn to_quat(&self) -> Quat { + Quat::new(((**self) / two!(T)).cos(), zero!(T), (**self).sin(), zero!(T)) + } +} + +impl ToMat3 for AngleY { + pub fn to_mat3(&self) -> Mat3 { + // http://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations + let cos_theta = (**self).cos(); + let sin_theta = (**self).sin(); + + Mat3::new(cos_theta.clone(), zero!(T), -sin_theta.clone(), + zero!(T), one!(T), zero!(T), + sin_theta.clone(), zero!(T), cos_theta.clone()) + } +} + +#[cfg(test)] +mod angle_y_tests { + // TODO +} + +/// An angle around the Z axis (roll), in radians. +#[deriving(Eq, Clone)] +pub struct AngleZ(T); + +impl_approx!(AngleZ) + +impl ToQuat for AngleZ { + pub fn to_quat(&self) -> Quat { + Quat::new(((**self) / two!(T)).cos(), zero!(T), zero!(T), (**self).sin()) + } +} + +impl ToMat3 for AngleZ { + pub fn to_mat3(&self) -> Mat3 { + // http://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations + let cos_theta = (**self).cos(); + let sin_theta = (**self).sin(); + + Mat3::new(cos_theta.clone(), sin_theta.clone(), zero!(T), + -sin_theta.clone(), cos_theta.clone(), zero!(T), + zero!(T), zero!(T), one!(T)) + } +} + +#[cfg(test)] +mod angle_z_tests { + // TODO +}