From 172c60277f64e56e56d121de481f722c15bb72ba Mon Sep 17 00:00:00 2001 From: Brendan Zabarauskas Date: Tue, 17 Sep 2013 12:32:07 +1000 Subject: [PATCH] Flesh out Rotation{2, 3} traits, and impl for some types --- src/cgmath/matrix.rs | 2 + src/cgmath/quaternion.rs | 47 +++-- src/cgmath/rotation.rs | 395 +++++++++++++++++++++++++++++++++++---- 3 files changed, 393 insertions(+), 51 deletions(-) diff --git a/src/cgmath/matrix.rs b/src/cgmath/matrix.rs index 5f8cebd..c4fac4f 100644 --- a/src/cgmath/matrix.rs +++ b/src/cgmath/matrix.rs @@ -242,6 +242,8 @@ pub trait Matrix #[inline] fn add_self_m(&mut self, other: &Self) { self.each_mut(|i, c| *c = c.add_v(other.c(i))) } #[inline] fn sub_self_m(&mut self, other: &Self) { self.each_mut(|i, c| *c = c.sub_v(other.c(i))) } + #[inline] fn mul_self_m(&mut self, other: &Self) { *self = self.mul_m(other); } + fn transpose(&self) -> Self; fn transpose_self(&mut self); fn determinant(&self) -> S; diff --git a/src/cgmath/quaternion.rs b/src/cgmath/quaternion.rs index 7495755..60096c3 100644 --- a/src/cgmath/quaternion.rs +++ b/src/cgmath/quaternion.rs @@ -16,6 +16,7 @@ use std::num::{zero, one, sqrt}; use angle::{Angle, Rad, acos, cos, sin}; +use array::{Array, build}; use matrix::{Mat3, ToMat3}; use vector::{Vec3, Vector, EuclideanVector}; use util::two; @@ -24,6 +25,9 @@ use util::two; #[deriving(Clone, Eq)] pub struct Quat { s: S, v: Vec3 } +array!(impl Quat -> [S, ..4] _4) +approx_eq!(impl Quat) + pub trait ToQuat { fn to_quat(&self) -> Quat; } @@ -81,19 +85,13 @@ impl Quat { /// The sum of this quaternion and `other` #[inline] pub fn add_q(&self, other: &Quat) -> Quat { - Quat::new(self.s + other.s, - self.v.x + other.v.x, - self.v.y + other.v.y, - self.v.z + other.v.z) + build(|i| self.i(i).add(other.i(i))) } - /// The sum of this quaternion and `other` + /// The difference between this quaternion and `other` #[inline] pub fn sub_q(&self, other: &Quat) -> Quat { - Quat::new(self.s - other.s, - self.v.x - other.v.x, - self.v.y - other.v.y, - self.v.z - other.v.z) + build(|i| self.i(i).add(other.i(i))) } /// The the result of multipliplying the quaternion by `other` @@ -104,6 +102,31 @@ impl Quat { self.s * other.v.z + self.v.z * other.s + self.v.x * other.v.y - self.v.y * other.v.x) } + #[inline] + pub fn mul_self_s(&mut self, s: S) { + self.each_mut(|_, x| *x = x.mul(&s)) + } + + #[inline] + pub fn div_self_s(&mut self, s: S) { + self.each_mut(|_, x| *x = x.div(&s)) + } + + #[inline] + pub fn add_self_q(&mut self, other: &Quat) { + self.each_mut(|i, x| *x = x.add(other.i(i))); + } + + #[inline] + pub fn sub_self_q(&mut self, other: &Quat) { + self.each_mut(|i, x| *x = x.sub(other.i(i))); + } + + #[inline] + pub fn mul_self_q(&mut self, other: &Quat) { + *self = self.mul_q(other); + } + /// The dot product of the quaternion and `other` #[inline] pub fn dot(&self, other: &Quat) -> S { @@ -116,12 +139,6 @@ impl Quat { Quat::from_sv(self.s.clone(), -self.v.clone()) } - /// The multiplicative inverse of the quaternion - #[inline] - pub fn inverse(&self) -> Quat { - self.conjugate().div_s(self.magnitude2()) - } - /// The squared magnitude of the quaternion. This is useful for /// magnitude comparisons where the exact magnitude does not need to be /// calculated. diff --git a/src/cgmath/rotation.rs b/src/cgmath/rotation.rs index 22b4993..0d5e53c 100644 --- a/src/cgmath/rotation.rs +++ b/src/cgmath/rotation.rs @@ -13,14 +13,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -use angle::Angle; +use std::num::{cast, one}; + +use angle::{Angle, sin, cos, sin_cos}; use array::Array; +use matrix::Matrix; use matrix::{Mat2, ToMat2}; use matrix::{Mat3, ToMat3}; use point::{Point2, Point3}; -use quaternion::ToQuat; +use quaternion::{Quat, ToQuat}; use ray::{Ray2, Ray3}; -use vector::{Vec2, Vec3}; +use vector::{Vector, Vec2, Vec3}; /// A two-dimensional rotation pub trait Rotation2 @@ -29,15 +32,16 @@ pub trait Rotation2 > : Eq + ApproxEq -+ Neg -+ Add -+ Sub + ToMat2 + ToRot2 { - fn rotate_point2(&self, point: Point2) -> Point2; + fn rotate_point2(&self, point: &Point2) -> Point2; fn rotate_vec2(&self, vec: &Vec2) -> Vec2; fn rotate_ray2(&self, ray: &Ray2) -> Ray2; + fn concat(&self, other: &Self) -> Self; + fn concat_self(&mut self, other: &Self); + fn invert(&self) -> Self; + fn invert_self(&mut self); } /// A three-dimensional rotation @@ -47,9 +51,6 @@ pub trait Rotation3 > : Eq + ApproxEq -+ Neg -+ Add -+ Sub + ToMat3 + ToRot3 + ToQuat @@ -57,6 +58,10 @@ pub trait Rotation3 fn rotate_point3(&self, point: &Point3) -> Point3; fn rotate_vec3(&self, vec: &Vec3) -> Vec3; fn rotate_ray3(&self, ray: &Ray3) -> Ray3; + fn concat(&self, other: &Self) -> Self; + fn concat_self(&mut self, other: &Self); + fn invert(&self) -> Self; + fn invert_self(&mut self); } /// A two-dimensional rotation matrix. @@ -70,57 +75,386 @@ pub struct Rot2 { priv mat: Mat2 } -pub trait ToRot2 { +impl Rot2 { + #[inline] + pub fn as_mat2<'a>(&'a self) -> &'a Mat2 { &'a self.mat } +} + +pub trait ToRot2 { fn to_rot2(&self) -> Rot2; } +impl ToRot2 for Rot2 { + #[inline] + fn to_rot2(&self) -> Rot2 { self.clone() } +} + +impl ToMat2 for Rot2 { + #[inline] + fn to_mat2(&self) -> Mat2 { self.mat.clone() } +} + +impl Rotation2 for Rot2 { + #[inline] + fn rotate_point2(&self, _point: &Point2) -> Point2 { fail!("Not yet implemented") } + + #[inline] + fn rotate_vec2(&self, vec: &Vec2) -> Vec2 { self.mat.mul_v(vec) } + + #[inline] + fn rotate_ray2(&self, _ray: &Ray2) -> Ray2 { fail!("Not yet implemented") } + + #[inline] + fn concat(&self, other: &Rot2) -> Rot2 { Rot2 { mat: self.mat.mul_m(&other.mat) } } + + #[inline] + fn concat_self(&mut self, other: &Rot2) { self.mat.mul_self_m(&other.mat); } + + // TODO: we know the matrix is orthogonal, so this could be re-written + // to be faster + #[inline] + fn invert(&self) -> Rot2 { Rot2 { mat: self.mat.invert().unwrap() } } + + // TODO: we know the matrix is orthogonal, so this could be re-written + // to be faster + #[inline] + fn invert_self(&mut self) { self.mat.invert_self(); } +} + +impl ApproxEq for Rot2 { + #[inline] + fn approx_epsilon() -> S { + // TODO: fix this after static methods are fixed in rustc + fail!(~"Doesn't work!"); + } + + #[inline] + fn approx_eq(&self, other: &Rot2) -> bool { + self.mat.approx_eq(&other.mat) + } + + #[inline] + fn approx_eq_eps(&self, other: &Rot2, approx_epsilon: &S) -> bool { + self.mat.approx_eq_eps(&other.mat, approx_epsilon) + } +} + /// 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 +/// `math::Mat3`. To ensure orthogonality is maintained, the operations have /// been restricted to a subeset of those implemented on `Mat3`. #[deriving(Eq, Clone)] pub struct Rot3 { priv mat: Mat3 } -pub trait ToRot3 { +impl Rot3 { + #[inline] + pub fn look_at(dir: &Vec3, up: &Vec3) -> Rot3 { + Rot3 { mat: Mat3::look_at(dir, up) } + } + + #[inline] + pub fn as_mat3<'a>(&'a self) -> &'a Mat3 { &'a self.mat } +} + +pub trait ToRot3 { fn to_rot3(&self) -> Rot3; } +impl ToRot3 for Rot3 { + #[inline] + fn to_rot3(&self) -> Rot3 { self.clone() } +} + +impl ToMat3 for Rot3 { + #[inline] + fn to_mat3(&self) -> Mat3 { self.mat.clone() } +} + +impl ToQuat for Rot3 { + #[inline] + fn to_quat(&self) -> Quat { self.mat.to_quat() } +} + +impl Rotation3 for Rot3 { + #[inline] + fn rotate_point3(&self, _point: &Point3) -> Point3 { fail!("Not yet implemented") } + + #[inline] + fn rotate_vec3(&self, vec: &Vec3) -> Vec3 { self.mat.mul_v(vec) } + + #[inline] + fn rotate_ray3(&self, _ray: &Ray3) -> Ray3 { fail!("Not yet implemented") } + + #[inline] + fn concat(&self, other: &Rot3) -> Rot3 { Rot3 { mat: self.mat.mul_m(&other.mat) } } + + #[inline] + fn concat_self(&mut self, other: &Rot3) { self.mat.mul_self_m(&other.mat); } + + // TODO: we know the matrix is orthogonal, so this could be re-written + // to be faster + #[inline] + fn invert(&self) -> Rot3 { Rot3 { mat: self.mat.invert().unwrap() } } + + // TODO: we know the matrix is orthogonal, so this could be re-written + // to be faster + #[inline] + fn invert_self(&mut self) { self.mat.invert_self(); } +} + +impl ApproxEq for Rot3 { + #[inline] + fn approx_epsilon() -> S { + // TODO: fix this after static methods are fixed in rustc + fail!(~"Doesn't work!"); + } + + #[inline] + fn approx_eq(&self, other: &Rot3) -> bool { + self.mat.approx_eq(&other.mat) + } + + #[inline] + fn approx_eq_eps(&self, other: &Rot3, approx_epsilon: &S) -> bool { + self.mat.approx_eq_eps(&other.mat, approx_epsilon) + } +} + +// Quaternion Rotation impls + +impl ToRot3 for Quat { + #[inline] + fn to_rot3(&self) -> Rot3 { Rot3 { mat: self.to_mat3() } } +} + +impl ToQuat for Quat { + #[inline] + fn to_quat(&self) -> Quat { self.clone() } +} + +impl Rotation3 for Quat { + #[inline] + fn rotate_point3(&self, _point: &Point3) -> Point3 { fail!("Not yet implemented") } + + #[inline] + fn rotate_vec3(&self, vec: &Vec3) -> Vec3 { self.mul_v(vec) } + + #[inline] + fn rotate_ray3(&self, _ray: &Ray3) -> Ray3 { fail!("Not yet implemented") } + + #[inline] + fn concat(&self, other: &Quat) -> Quat { self.mul_q(other) } + + #[inline] + fn concat_self(&mut self, other: &Quat) { self.mul_self_q(other); } + + #[inline] + fn invert(&self) -> Quat { self.conjugate().div_s(self.magnitude2()) } + + #[inline] + fn invert_self(&mut self) { *self = self.invert() } +} + /// Euler angles /// -/// Whilst Euler angles are easier to visualise, and more intuitive to specify, -/// they are not reccomended for general use because they are prone to gimble -/// lock. -/// /// # Fields /// /// - `x`: the angular rotation around the `x` axis (pitch) /// - `y`: the angular rotation around the `y` axis (yaw) /// - `z`: the angular rotation around the `z` axis (roll) +/// +/// # Notes +/// +/// Whilst Euler angles are more intuitive to specify than quaternions, +/// they are not reccomended for general use because they are prone to gimble +/// lock. #[deriving(Eq, Clone)] pub struct Euler { x: A, y: A, z: A } array!(impl Euler -> [A, ..3] _3) -pub trait ToEuler { - fn to_euler(&self) -> Euler; -} - -impl> Euler { +impl> Euler { #[inline] pub fn new(x: A, y: A, z: A) -> Euler { Euler { x: x, y: y, z: z } } } +pub trait ToEuler { + fn to_euler(&self) -> Euler; +} + +impl> ToMat3 for Euler { + fn to_mat3(&self) -> Mat3 { + // http://en.wikipedia.org/wiki/Rotation_matrix#General_rotations + let (sx, cx) = sin_cos(self.x.clone()); + let (sy, cy) = sin_cos(self.y.clone()); + let (sz, cz) = sin_cos(self.z.clone()); + + 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> ToRot3 for Euler { + #[inline] + fn to_rot3(&self) -> Rot3 { + Rot3 { mat: self.to_mat3() } + } +} + +impl> ToQuat for Euler { + fn to_quat(&self) -> Quat { + // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Conversion + let (sx2, cx2) = sin_cos(self.x.div_s(cast(2))); + let (sy2, cy2) = sin_cos(self.y.div_s(cast(2))); + let (sz2, cz2) = sin_cos(self.z.div_s(cast(2))); + + Quat::new(cz2 * cx2 * cy2 + sz2 * sx2 * sy2, + sz2 * cx2 * cy2 - cz2 * sx2 * sy2, + cz2 * sx2 * cy2 + sz2 * cx2 * sy2, + cz2 * cx2 * sy2 - sz2 * sx2 * cy2) + } +} + +impl> Rotation3 for Euler { + #[inline] + fn rotate_point3(&self, _point: &Point3) -> Point3 { fail!("Not yet implemented") } + + #[inline] + fn rotate_vec3(&self, _vec: &Vec3) -> Vec3 { fail!("Not yet implemented"); } + + #[inline] + fn rotate_ray3(&self, _ray: &Ray3) -> Ray3 { fail!("Not yet implemented") } + + #[inline] + fn concat(&self, _other: &Euler) -> Euler { fail!("Not yet implemented") } + + #[inline] + fn concat_self(&mut self, _other: &Euler) { fail!("Not yet implemented"); } + + #[inline] + fn invert(&self) -> Euler { fail!("Not yet implemented") } + + #[inline] + fn invert_self(&mut self) { fail!("Not yet implemented"); } +} + +impl> ApproxEq for Euler { + #[inline] + fn approx_epsilon() -> S { + // TODO: fix this after static methods are fixed in rustc + fail!(~"Doesn't work!"); + } + + #[inline] + fn approx_eq(&self, other: &Euler) -> bool { + self.x.approx_eq(&other.x) && + self.y.approx_eq(&other.y) && + self.z.approx_eq(&other.z) + } + + #[inline] + fn approx_eq_eps(&self, other: &Euler, approx_epsilon: &S) -> bool { + self.x.approx_eq_eps(&other.x, approx_epsilon) && + self.y.approx_eq_eps(&other.y, approx_epsilon) && + self.z.approx_eq_eps(&other.z, approx_epsilon) + } +} + /// A rotation about an arbitrary axis #[deriving(Eq, Clone)] -pub struct AxisAngle { - axis: Vec3, - angle: A, +pub struct AxisAngle { v: Vec3, a: A } + +impl> AxisAngle { + #[inline] + pub fn new(v: Vec3, a: A) -> AxisAngle { + AxisAngle { v: v, a: a } + } +} + +impl> ToMat3 for AxisAngle { + fn to_mat3(&self) -> Mat3 { + let (s, c) = sin_cos(self.a.clone()); + let _1subc = one::() - c; + + Mat3::new(_1subc * self.v.x * self.v.x + c, + _1subc * self.v.x * self.v.y + s * self.v.z, + _1subc * self.v.x * self.v.z - s * self.v.y, + + _1subc * self.v.x * self.v.y - s * self.v.z, + _1subc * self.v.y * self.v.y + c, + _1subc * self.v.y * self.v.z + s * self.v.x, + + _1subc * self.v.x * self.v.z + s * self.v.y, + _1subc * self.v.y * self.v.z - s * self.v.x, + _1subc * self.v.z * self.v.z + c) + } +} + +impl> ToRot3 for AxisAngle { + #[inline] + fn to_rot3(&self) -> Rot3 { + Rot3 { mat: self.to_mat3() } + } +} + +impl> ToQuat for AxisAngle { + fn to_quat(&self) -> Quat { + let half = self.a.div_s(cast(2)); + Quat::from_sv( + cos(half.clone()), + self.v.mul_s(sin(half.clone())) + ) + } +} + +impl> Rotation3 for AxisAngle { + #[inline] + fn rotate_point3(&self, _point: &Point3) -> Point3 { fail!("Not yet implemented") } + + #[inline] + fn rotate_vec3(&self, _vec: &Vec3) -> Vec3 { fail!("Not yet implemented"); } + + #[inline] + fn rotate_ray3(&self, _ray: &Ray3) -> Ray3 { fail!("Not yet implemented") } + + #[inline] + fn concat(&self, _other: &AxisAngle) -> AxisAngle { fail!("Not yet implemented") } + + #[inline] + fn concat_self(&mut self, _other: &AxisAngle) { fail!("Not yet implemented"); } + + #[inline] + fn invert(&self) -> AxisAngle { fail!("Not yet implemented") } + + #[inline] + fn invert_self(&mut self) { fail!("Not yet implemented"); } +} + +impl> ApproxEq for AxisAngle { + #[inline] + fn approx_epsilon() -> S { + // TODO: fix this after static methods are fixed in rustc + fail!(~"Doesn't work!"); + } + + #[inline] + fn approx_eq(&self, other: &AxisAngle) -> bool { + self.v.approx_eq(&other.v) && + self.a.approx_eq(&other.a) + } + + #[inline] + fn approx_eq_eps(&self, other: &AxisAngle, approx_epsilon: &S) -> bool { + self.v.approx_eq_eps(&other.v, approx_epsilon) && + self.a.approx_eq_eps(&other.a, approx_epsilon) + } } /// An angle around the X axis (pitch). @@ -134,14 +468,3 @@ pub struct AngleY(A); /// An angle around the Z axis (roll). #[deriving(Eq, Ord, Clone)] pub struct AngleZ(A); - -impl> Neg> for AngleX { #[inline] fn neg(&self) -> AngleX { AngleX(-**self) } } -impl> Neg> for AngleY { #[inline] fn neg(&self) -> AngleY { AngleY(-**self) } } -impl> Neg> for AngleZ { #[inline] fn neg(&self) -> AngleZ { AngleZ(-**self) } } - -impl> Add, AngleX> for AngleX { #[inline] fn add(&self, other: &AngleX) -> AngleX { AngleX((**self).add_a(*other.clone())) } } -impl> Add, AngleY> for AngleY { #[inline] fn add(&self, other: &AngleY) -> AngleY { AngleY((**self).add_a(*other.clone())) } } -impl> Add, AngleZ> for AngleZ { #[inline] fn add(&self, other: &AngleZ) -> AngleZ { AngleZ((**self).add_a(*other.clone())) } } -impl> Sub, AngleX> for AngleX { #[inline] fn sub(&self, other: &AngleX) -> AngleX { AngleX((**self).sub_a(*other.clone())) } } -impl> Sub, AngleY> for AngleY { #[inline] fn sub(&self, other: &AngleY) -> AngleY { AngleY((**self).sub_a(*other.clone())) } } -impl> Sub, AngleZ> for AngleZ { #[inline] fn sub(&self, other: &AngleZ) -> AngleZ { AngleZ((**self).sub_a(*other.clone())) } }