diff --git a/src/cgmath/quaternion.rs b/src/cgmath/quaternion.rs index 44d7260..5670166 100644 --- a/src/cgmath/quaternion.rs +++ b/src/cgmath/quaternion.rs @@ -16,10 +16,12 @@ use std::fmt; use std::num::{zero, one, cast, sqrt}; -use angle::{Angle, Rad, acos, cos, sin, sin_cos}; +use angle::{Angle, Rad, acos, sin, sin_cos}; use approx::ApproxEq; use array::{Array, build}; use matrix::{Mat3, ToMat3, ToMat4, Mat4}; +use point::{Point3}; +use rotation::{Rotation, Rotation3, Basis3, ToBasis3}; use vector::{Vec3, Vector, EuclideanVector}; /// A quaternion in scalar/vector form @@ -47,54 +49,6 @@ Quat { Quat { s: s, v: v } } - /// Create a quaternion from a rotation around the `x` axis (pitch). - #[inline] - pub fn from_angle_x(theta: Rad) -> Quat { - let (s, c) = sin_cos(theta.mul_s(cast(0.5).unwrap())); - Quat::new(c, s, zero(), zero()) - } - - /// Create a quaternion from a rotation around the `y` axis (yaw). - #[inline] - pub fn from_angle_y(theta: Rad) -> Quat { - let (s, c) = sin_cos(theta.mul_s(cast(0.5).unwrap())); - Quat::new(c, zero(), s, zero()) - } - - /// Create a quaternion from a rotation around the `z` axis (roll). - #[inline] - pub fn from_angle_z(theta: Rad) -> Quat { - let (s, c) = sin_cos(theta.mul_s(cast(0.5).unwrap())); - Quat::new(c, zero(), zero(), s) - } - - /// Create a quaternion from a set of euler angles. - /// - /// # Parameters - /// - /// - `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). - pub fn from_euler(x: Rad, y: Rad, z: Rad) -> Quat { - // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Conversion - let (sx2, cx2) = sin_cos(x.mul_s(cast(0.5).unwrap())); - let (sy2, cy2) = sin_cos(y.mul_s(cast(0.5).unwrap())); - let (sz2, cz2) = sin_cos(z.mul_s(cast(0.5).unwrap())); - - 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) - } - - /// Create a quaternion from a rotation around an arbitrary axis - #[inline] - pub fn from_axis_angle(axis: &Vec3, angle: Rad) -> Quat { - let half = angle.mul_s(cast(0.5).unwrap()); - Quat::from_sv(cos(half.clone()), - axis.mul_s(sin(half))) - } - /// The additive identity, ie: `q = 0 + 0i + 0j + 0i` #[inline] pub fn zero() -> Quat { @@ -331,3 +285,71 @@ impl ToStr for Quat { format!("{} + {}i + {}j + {}k", self.s, self.v.x, self.v.y, self.v.z) } } + +// Quaternion Rotation impls + +impl> +ToBasis3 for Quat { + #[inline] + fn to_rot3(&self) -> Basis3 { Basis3::from_quat(self) } +} + +impl ToQuat for Quat { + #[inline] + fn to_quat(&self) -> Quat { self.clone() } +} + +impl> +Rotation, Point3> for Quat { + #[inline] + fn identity() -> Quat { Quat::identity() } + + #[inline] + fn look_at(dir: &Vec3, up: &Vec3) -> Quat { + Mat3::look_at(dir, up).to_quat() + } + + #[inline] + fn between_vecs(a: &Vec3, b: &Vec3) -> Quat { + //http://stackoverflow.com/questions/1171849/ + //finding-quaternion-representing-the-rotation-from-one-vector-to-another + Quat::from_sv(one::() + a.dot(b), a.cross(b)).normalize() + } + + #[inline] + fn rotate_vec(&self, vec: &Vec3) -> Vec3 { self.mul_v(vec) } + + #[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() } +} + +impl> +Rotation3 for Quat +{ + #[inline] + fn from_axis_angle(axis: &Vec3, angle: Rad) -> Quat { + let (s, c) = sin_cos(angle.mul_s(cast(0.5).unwrap())); + Quat::from_sv(c, axis.mul_s(s)) + } + + fn from_euler(x: Rad, y: Rad, z: Rad) -> Quat { + // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Conversion + let (sx2, cx2) = sin_cos(x.mul_s(cast(0.5).unwrap())); + let (sy2, cy2) = sin_cos(y.mul_s(cast(0.5).unwrap())); + let (sz2, cz2) = sin_cos(z.mul_s(cast(0.5).unwrap())); + + 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) + } +} diff --git a/src/cgmath/rotation.rs b/src/cgmath/rotation.rs index 5e9a29f..2cf680c 100644 --- a/src/cgmath/rotation.rs +++ b/src/cgmath/rotation.rs @@ -13,7 +13,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -use angle::Rad; +use angle::{Rad, acos}; use approx::ApproxEq; use array::Array; use matrix::Matrix; @@ -36,7 +36,13 @@ pub trait Rotation + ApproxEq { fn identity() -> Self; + + /// Create a rotation to a given direction with an 'up' vector fn look_at(dir: &V, up: &V) -> Self; + + /// Create a shortest rotation to transform vector 'a' into 'b'. + /// Both given vectors are assumed to have unit length. + fn between_vecs(a: &V, b: &V) -> Self; fn rotate_vec(&self, vec: &V) -> V; @@ -74,18 +80,50 @@ pub trait Rotation2 : Rotation, Point2> + ToMat2 + ToBasis2 -{} +{ + // Create a rotation by a given angle. Thus is a redundant case of both + // from_axis_angle() and from_euler() for 2D space. + fn from_angle(theta: Rad) -> Self; +} /// A three-dimensional rotation pub trait Rotation3 < - S + S: Primitive > : Rotation, Point3> + ToMat3 + ToBasis3 + ToQuat -{} +{ + /// Create a rotation around a given axis. + fn from_axis_angle(axis: &Vec3, angle: Rad) -> Self; + + /// Create a rotation from a set of euler angles. + /// + /// # Parameters + /// + /// - `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). + fn from_euler(x: Rad, y: Rad, z: Rad) -> Self; + + /// Create a rotation matrix from a rotation around the `x` axis (pitch). + #[inline] + fn from_angle_x(theta: Rad) -> Self { + Rotation3::from_axis_angle( &Vec3::unit_x(), theta ) + } + + #[inline] + fn from_angle_y(theta: Rad) -> Self { + Rotation3::from_axis_angle( &Vec3::unit_y(), theta ) + } + + #[inline] + fn from_angle_z(theta: Rad) -> Self { + Rotation3::from_axis_angle( &Vec3::unit_z(), theta ) + } +} /// A two-dimensional rotation matrix. @@ -93,7 +131,7 @@ pub trait Rotation3 /// 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 `Mat2`. +/// to a subset of those implemented on `Mat2`. #[deriving(Eq, Clone)] pub struct Basis2 { priv mat: Mat2 @@ -129,8 +167,8 @@ Rotation, Point2> for Basis2 { } #[inline] - fn look_at(dir: &Vec2, up: &Vec2) -> Basis2 { - Basis2 { mat: Mat2::look_at(dir, up) } + fn between_vecs(a: &Vec2, b: &Vec2) -> Basis2 { + Rotation2::from_angle( acos(a.dot(b)) ) } #[inline] @@ -162,7 +200,9 @@ ApproxEq for Basis2 { } impl> -Rotation2 for Basis2 {} +Rotation2 for Basis2 { + fn from_angle(theta: Rad) -> Basis2 { Basis2 { mat: Mat2::from_angle(theta) } } +} /// A three-dimensional rotation matrix. /// @@ -177,37 +217,9 @@ pub struct Basis3 { impl> Basis3 { - /// Create a rotation matrix from a rotation around the `x` axis (pitch). - pub fn from_angle_x(theta: Rad) -> Basis3 { - Basis3 { mat: Mat3::from_angle_x(theta) } - } - - /// Create a rotation matrix from a rotation around the `y` axis (yaw). - pub fn from_angle_y(theta: Rad) -> Basis3 { - Basis3 { mat: Mat3::from_angle_y(theta) } - } - - /// Create a rotation matrix from a rotation around the `z` axis (roll). - pub fn from_angle_z(theta: Rad) -> Basis3 { - Basis3 { mat: Mat3::from_angle_z(theta) } - } - - /// Create a rotation matrix from a set of euler angles. - /// - /// # Parameters - /// - /// - `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). - pub fn from_euler(x: Rad, y: Rad, z: Rad) -> Basis3 { - Basis3 { mat: Mat3::from_euler(x, y ,z) } - } - - /// Create a rotation matrix from a rotation around an arbitrary axis. - pub fn from_axis_angle(axis: &Vec3, angle: Rad) -> Basis3 { - Basis3 { mat: Mat3::from_axis_angle(axis, angle) } - } - + #[inline] + pub fn from_quat(quat: &Quat) -> Basis3 { Basis3 { mat: quat.to_mat3() } } + #[inline] pub fn as_mat3<'a>(&'a self) -> &'a Mat3 { &'a self.mat } } @@ -241,6 +253,12 @@ Rotation, Point3> for Basis3 { fn look_at(dir: &Vec3, up: &Vec3) -> Basis3 { Basis3 { mat: Mat3::look_at(dir, up) } } + + #[inline] + fn between_vecs(a: &Vec3, b: &Vec3) -> Basis3 { + let q: Quat = Rotation::between_vecs(a, b); + q.to_rot3() + } #[inline] fn rotate_vec(&self, vec: &Vec3) -> Vec3 { self.mat.mul_v(vec) } @@ -271,46 +289,24 @@ ApproxEq for Basis3 { } impl> -Rotation3 for Basis3 {} - -// Quaternion Rotation impls - -impl> -ToBasis3 for Quat { - #[inline] - fn to_rot3(&self) -> Basis3 { Basis3 { mat: self.to_mat3() } } -} - -impl ToQuat for Quat { - #[inline] - fn to_quat(&self) -> Quat { self.clone() } -} - -impl> -Rotation, Point3> for Quat { - #[inline] - fn identity() -> Quat { Quat::identity() } - - #[inline] - fn look_at(dir: &Vec3, up: &Vec3) -> Quat { - Mat3::look_at(dir, up).to_quat() +Rotation3 for Basis3 { + fn from_axis_angle(axis: &Vec3, angle: Rad) -> Basis3 { + Basis3 { mat: Mat3::from_axis_angle(axis, angle) } } - - #[inline] - fn rotate_vec(&self, vec: &Vec3) -> Vec3 { self.mul_v(vec) } - #[inline] - fn concat(&self, other: &Quat) -> Quat { self.mul_q(other) } + fn from_euler(x: Rad, y: Rad, z: Rad) -> Basis3 { + Basis3 { mat: Mat3::from_euler(x, y ,z) } + } - #[inline] - fn concat_self(&mut self, other: &Quat) { self.mul_self_q(other); } + fn from_angle_x(theta: Rad) -> Basis3 { + Basis3 { mat: Mat3::from_angle_x(theta) } + } - #[inline] - fn invert(&self) -> Quat { self.conjugate().div_s(self.magnitude2()) } + fn from_angle_y(theta: Rad) -> Basis3 { + Basis3 { mat: Mat3::from_angle_y(theta) } + } - #[inline] - fn invert_self(&mut self) { *self = self.invert() } + fn from_angle_z(theta: Rad) -> Basis3 { + Basis3 { mat: Mat3::from_angle_z(theta) } + } } - -impl> -Rotation3 for Quat {}