cgmath/src-old/transform/rotation.rs

646 lines
19 KiB
Rust
Raw Normal View History

2013-07-15 02:03:21 +00:00
// 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 and impls.
//!
//! Some of these are more useful for constructing matricies and quaternions.
//! than for general use. For example due to issues with gimble lock, it is
//! not reccomended that Euler rotations be used for translations, but
//! they _are_ useful for intuitively specifying rotations.
2013-07-15 02:03:21 +00:00
//!
//! # Examples
//!
//! ~~~rust
//! Euler::new::<f32>(1.0, 2.0, 0.0).to_mat3()
//! ~~~
//!
//! ~~~rust
//! AxisY::<f32>(0.3).to_quat()
//! ~~~
use std::cast;
use math::*;
2013-07-15 02:03:21 +00:00
use math::{Point3, Ray3};
/// A two-dimensional rotation
pub trait Rotation2<T>: Eq
+ ApproxEq<T>
+ ToMat2<T> {
pub fn rotate_point2(&self, point: Point2<T>) -> Point2<T>;
pub fn rotate_vec2(&self, vec: &Vec2<T>) -> Vec2<T>;
pub fn rotate_ray2(&self, ray: &Ray2<T>) -> Ray2<T>;
pub fn to_rotation_mat2(&self) -> RotationMat2<T>;
}
/// A three-dimensional rotation
pub trait Rotation3<T>: Eq
2013-07-15 02:03:21 +00:00
+ ApproxEq<T>
+ ToMat3<T>
+ ToMat4<T>
+ ToQuat<T> {
pub fn rotate_point3(&self, point: &Point3<T>) -> Point3<T>;
pub fn rotate_vec3(&self, vec: &Vec3<T>) -> Vec3<T>;
pub fn rotate_ray3(&self, ray: &Ray3<T>) -> Ray3<T>;
pub fn to_rotation_mat3(&self) -> RotationMat3<T>;
}
/// A two-dimensional rotation matrix.
///
/// The matrix is guaranteed to be orthogonal, so some operations can be
/// implemented more efficiently than the implementations for `math::Mat2`. To
/// enforce orthogonality at the type level the operations have been restricted
/// to a subeset of those implemented on `Mat3`.
#[deriving(Eq, Clone)]
pub struct RotationMat2<T> {
priv mat: Mat2<T>
}
impl_approx!(RotationMat2 { mat })
impl<T> RotationMat2<T> {
#[inline]
pub fn as_mat2<'a>(&'a self) -> & 'a Mat2<T> {
unsafe { cast::transmute(self) }
}
}
impl<T:Clone + Float> Rotation2<T> for RotationMat2<T> {
pub fn rotate_point2(&self, point: Point2<T>) -> Point2<T> {
point.with_vec2(|vec| self.rotate_vec2(vec))
}
pub fn rotate_vec2(&self, vec: &Vec2<T>) -> Vec2<T> {
self.mat.mul_v(vec)
}
pub fn rotate_ray2(&self, _ray: &Ray2<T>) -> Ray2<T> {
fail!("Not yet implemented.")
}
#[inline]
pub fn to_rotation_mat2(&self) -> RotationMat2<T> {
RotationMat2 { mat: self.to_mat2() }
}
}
impl<T:Clone + Float> ToMat2<T> for RotationMat2<T> {
#[inline] pub fn to_mat2(&self) -> Mat2<T> { self.mat.clone() }
}
impl<T:Num> RotationMat2<T> {
#[inline]
pub fn identity() -> RotationMat2<T> {
RotationMat2 { mat: Mat2::identity() }
}
#[inline]
pub fn zero() -> RotationMat2<T> {
RotationMat2 { mat: Mat2::zero() }
}
}
impl<T:Clone + Num> Neg<RotationMat2<T>> for RotationMat2<T> {
#[inline]
pub fn neg(&self) -> RotationMat2<T> {
RotationMat2 { mat: -self.mat }
}
}
impl<T:Clone + Float> Rotation3<T> for Quat<T> {
pub fn rotate_point3(&self, point: &Point3<T>) -> Point3<T> {
point.with_vec3(|vec| self.rotate_vec3(vec))
}
pub fn rotate_vec3(&self, vec: &Vec3<T>) -> Vec3<T> {
self.mul_v(vec)
}
pub fn rotate_ray3(&self, _ray: &Ray3<T>) -> Ray3<T> {
fail!("Not yet implemented.")
}
#[inline]
pub fn to_rotation_mat3(&self) -> RotationMat3<T> {
RotationMat3 { mat: self.to_mat3() }
}
}
/// A three-dimensional rotation matrix.
///
/// The matrix is guaranteed to be orthogonal, so some operations, specifically
/// inversion, can be implemented more efficiently than the implementations for
/// `math::Mat3`. To enforce orthogonality at the type level the operations have
/// been restricted to a subeset of those implemented on `Mat3`.
#[deriving(Eq, Clone)]
pub struct RotationMat3<T> {
priv mat: Mat3<T>
}
impl_approx!(RotationMat3 { mat })
impl<T> RotationMat3<T> {
#[inline]
pub fn as_mat3<'a>(&'a self) -> & 'a Mat3<T> {
unsafe { cast::transmute(self) }
}
}
impl<T:Clone + Float> Rotation3<T> for RotationMat3<T> {
pub fn rotate_point3(&self, point: &Point3<T>) -> Point3<T> {
point.with_vec3(|vec| self.rotate_vec3(vec))
}
pub fn rotate_vec3(&self, vec: &Vec3<T>) -> Vec3<T> {
self.mat.mul_v(vec)
}
pub fn rotate_ray3(&self, _ray: &Ray3<T>) -> Ray3<T> {
fail!("Not yet implemented.")
}
#[inline]
pub fn to_rotation_mat3(&self) -> RotationMat3<T> {
RotationMat3 { mat: self.to_mat3() }
}
}
impl<T:Clone + Float> ToQuat<T> for RotationMat3<T> {
#[inline] pub fn to_quat(&self) -> Quat<T> { self.mat.to_quat() }
}
impl<T:Clone + Float> ToMat3<T> for RotationMat3<T> {
#[inline] pub fn to_mat3(&self) -> Mat3<T> { self.mat.clone() }
}
impl<T:Clone + Float> ToMat4<T> for RotationMat3<T> {
#[inline] pub fn to_mat4(&self) -> Mat4<T> { self.mat.to_mat4() }
}
impl<T:Num> RotationMat3<T> {
#[inline]
pub fn identity() -> RotationMat3<T> {
RotationMat3 { mat: Mat3::identity() }
}
#[inline]
pub fn zero() -> RotationMat3<T> {
RotationMat3 { mat: Mat3::zero() }
}
}
impl<T:Clone + Num> Neg<RotationMat3<T>> for RotationMat3<T> {
#[inline]
pub fn neg(&self) -> RotationMat3<T> {
RotationMat3 { mat: -self.mat }
}
}
impl<T:Float> RotationMat3<T> {
pub fn look_at(dir: &Vec3<T>, up: &Vec3<T>) -> RotationMat3<T> {
RotationMat3 { mat: Mat3::look_at(dir, up) }
}
}
2013-07-15 02:03:21 +00:00
/// 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:Clone + Float> Rotation3<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>) -> Vec3<T> {
fail!("Not yet implemented.")
}
pub fn rotate_ray3(&self, _ray: &Ray3<T>) -> Ray3<T> {
fail!("Not yet implemented.")
}
#[inline]
pub fn to_rotation_mat3(&self) -> RotationMat3<T> {
RotationMat3 { mat: self.to_mat3() }
}
}
2013-07-15 02:03:21 +00:00
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)
}
}
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))
}
}
2013-07-15 02:03:21 +00:00
#[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> Rotation3<T> for AxisAngle<T> {
pub fn rotate_point3(&self, point: &Point3<T>) -> Point3<T> {
point.with_vec3(|vec| self.rotate_vec3(vec))
}
pub fn rotate_vec3(&self, vec: &Vec3<T>) -> Vec3<T> {
// Rodrigues' rotation formula
// http://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula
//
// ~~~
// v cos θ + (k × v) sin θ + k(k ⋅ v)(1 - cos θ)
// ~~~
//
// Where:
// - `v` = vec
// - `k` = self.axis
// - `θ` = self.angle
vec.mul_s(self.angle.cos())
.add_v(&self.axis.cross(vec)).mul_s(self.angle.sin())
.add_v(&self.axis.mul_s(self.axis.dot(vec))
.mul_s(one!(T) - self.angle.cos()))
}
pub fn rotate_ray3(&self, _ray: &Ray3<T>) -> Ray3<T> {
fail!("Not yet implemented.")
}
#[inline]
pub fn to_rotation_mat3(&self) -> RotationMat3<T> {
RotationMat3 { mat: self.to_mat3() }
}
}
2013-07-15 02:03:21 +00:00
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_s(half.sin()))
2013-07-15 02:03:21 +00:00
}
}
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)
}
}
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))
}
}
2013-07-15 02:03:21 +00:00
#[cfg(test)]
mod axis_angle_tests {
use math::*;
use transform::*;
2013-07-15 02:03:21 +00:00
#[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:Clone + Float> Rotation3<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>) -> Vec3<T> {
fail!("Not yet implemented.")
}
pub fn rotate_ray3(&self, _ray: &Ray3<T>) -> Ray3<T> {
fail!("Not yet implemented.")
}
#[inline]
pub fn to_rotation_mat3(&self) -> RotationMat3<T> {
RotationMat3 { mat: self.to_mat3() }
}
}
2013-07-15 02:03:21 +00:00
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())
}
}
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))
}
}
2013-07-15 02:03:21 +00:00
#[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:Clone + Float> Rotation3<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>) -> Vec3<T> {
fail!("Not yet implemented.")
}
pub fn rotate_ray3(&self, _ray: &Ray3<T>) -> Ray3<T> {
fail!("Not yet implemented.")
}
#[inline]
pub fn to_rotation_mat3(&self) -> RotationMat3<T> {
RotationMat3 { mat: self.to_mat3() }
}
}
2013-07-15 02:03:21 +00:00
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())
}
}
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))
}
}
2013-07-15 02:03:21 +00:00
#[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> Rotation2<T> for AngleZ<T> {
pub fn rotate_point2(&self, _point: Point2<T>) -> Point2<T> {
fail!("Not yet implemented.")
}
pub fn rotate_vec2(&self, _vec: &Vec2<T>) -> Vec2<T> {
fail!("Not yet implemented.")
}
pub fn rotate_ray2(&self, _ray: &Ray2<T>) -> Ray2<T> {
fail!("Not yet implemented.")
}
#[inline]
pub fn to_rotation_mat2(&self) -> RotationMat2<T> {
fail!("Not yet implemented.")
}
}
impl<T:Clone + Float> Rotation3<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>) -> Vec3<T> {
fail!("Not yet implemented.")
}
pub fn rotate_ray3(&self, _ray: &Ray3<T>) -> Ray3<T> {
fail!("Not yet implemented.")
}
#[inline]
pub fn to_rotation_mat3(&self) -> RotationMat3<T> {
RotationMat3 { mat: self.to_mat3() }
}
}
2013-07-15 02:03:21 +00:00
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))
}
}
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))
}
}
2013-07-15 02:03:21 +00:00
#[cfg(test)]
mod angle_z_tests {
// TODO
}