hodasemi/cgmath
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Add rotation module

Browse source
This commit is contained in:
Brendan Zabarauskas 2013-07-15 12:03:21 +10:00
parent 2ecc99b6a7
commit e9cc75f06d
5 changed files with 282 additions and 134 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

18
src/macros.rs
View file

@ -51,5 +51,23 @@ macro_rules! impl_approx(
$( self.$field.approx_eq_eps(&other.$field, epsilon) )&&+
}
}
);
($T:ident) => (
impl<T:Clone + Eq + ApproxEq<T>> ApproxEq<T> for $T<T> {
#[inline]
pub fn approx_epsilon() -> T {
ApproxEq::approx_epsilon::<T,T>()
}
#[inline]
pub fn approx_eq(&self, other: &$T<T>) -> bool {
self.approx_eq_eps(other, &ApproxEq::approx_epsilon::<T,T>())
}
#[inline]
pub fn approx_eq_eps(&self, other: &$T<T>, epsilon: &T) -> bool {
(**self).approx_eq_eps(&**other, epsilon)
}
}
)
)

69
src/math/mat.rs
View file

@ -758,75 +758,6 @@ impl<T:Clone + Num> Neg<Mat3<T>> for Mat3<T> {
}
impl<T:Clone + Float> Mat3<T> {
/// Construct a matrix from an angular rotation around the `x` axis
pub fn from_angle_x(radians: T) -> Mat3<T> {
// 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<T> {
// 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<T> {
// 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<T> {
// 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<T>) -> Mat3<T> {
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<T>, y: Vec3<T>, z: Vec3<T>) -> Mat3<T> {
Mat3::from_cols(x, y, z)

5
src/math/math.rs
View file

@ -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<T,Slice> {
pub fn index<'a>(&'a self, i: uint) -> &'a T;

65
src/math/quat.rs
View file

@ -82,42 +82,6 @@ impl<T:Clone + Float> Quat<T> {
Quat::new(zero!(T), zero!(T), zero!(T), zero!(T))
}
#[inline]
pub fn from_angle_x(radians: T) -> Quat<T> {
Quat::new((radians / two!(T)).cos(), radians.sin(), zero!(T), zero!(T))
}
#[inline]
pub fn from_angle_y(radians: T) -> Quat<T> {
Quat::new((radians / two!(T)).cos(), zero!(T), radians.sin(), zero!(T))
}
#[inline]
pub fn from_angle_z(radians: T) -> Quat<T> {
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<T> {
// 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<T>) -> Quat<T> {
let half = radians / two!(T);
Quat::from_sv(half.cos(), axis.mul_t(half.sin()))
}
pub fn get_angle_axis(&self) -> (T, Vec3<T>) {
fail!(~"Not yet implemented.")
}
/// The result of multiplying the quaternion a scalar
#[inline]
pub fn mul_t(&self, value: T) -> Quat<T> {
@ -305,32 +269,3 @@ impl<T:Clone + Float> Quat<T> {
}
}
}
#[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::<float>(0.000001, 0.000001, 0.000001, 0.000001)
.approx_eq(&Quat::new::<float>(0.0, 0.0, 0.0, 0.0)));
assert!(Quat::new::<float>(0.0000001, 0.0000001, 0.0000001, 0.0000001)
.approx_eq(&Quat::new::<float>(0.0, 0.0, 0.0, 0.0)));
}
}

259
src/math/rotation.rs Normal file
View file

@ -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::<f32>(1.0, 2.0, 0.0).to_mat3()
//! ~~~
//!
//! ~~~rust
//! AxisY::<f32>(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<T>: Eq
+ ApproxEq<T>
+ ToMat3<T>
+ ToQuat<T> {}
/// 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<T> { 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<T> {
pub fn to_euler(&self) -> Euler<T>;
}
impl<T:Float> Euler<T> {
#[inline]
pub fn new(pitch: T, yaw: T, roll: T) -> Euler<T> {
Euler { pitch: pitch, yaw: yaw, roll: roll }
}
}
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
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<T:Float> ToMat3<T> for Euler<T> {
pub fn to_mat3(&self) -> Mat3<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();
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<T> {
axis: Vec3<T>,
angle: T,
}
impl_approx!(AxisAngle { axis, angle })
pub trait ToAxisAngle<T> {
pub fn to_axis_angle(&self) -> AxisAngle<T>;
}
impl<T:Float> AxisAngle<T> {
pub fn new(axis: Vec3<T>, angle: T) -> AxisAngle<T> {
AxisAngle { axis: axis, angle: angle }
}
}
impl<T:Float> ToQuat<T> for AxisAngle<T> {
pub fn to_quat(&self) -> Quat<T> {
let half = self.angle / two!(T);
Quat::from_sv(half.cos(), self.axis.mul_t(half.sin()))
}
}
impl<T:Float> ToMat3<T> for AxisAngle<T> {
pub fn to_mat3(&self) -> Mat3<T> {
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>(T);
impl_approx!(AngleX)
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))
}
}
impl<T:Clone + Float> ToMat3<T> for AngleX<T> {
pub fn to_mat3(&self) -> Mat3<T> {
// 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>(T);
impl_approx!(AngleY)
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))
}
}
impl<T:Clone + Float> ToMat3<T> for AngleY<T> {
pub fn to_mat3(&self) -> Mat3<T> {
// 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>(T);
impl_approx!(AngleZ)
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())
}
}
impl<T:Clone + Float> ToMat3<T> for AngleZ<T> {
pub fn to_mat3(&self) -> Mat3<T> {
// 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
}