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

Rotation2 and Rotation3 improved with from_* methods. Betwee_vecs implemented for all rotations.

Browse source
This commit is contained in:
Dzmitry Malyshau 2014-01-29 19:24:34 -05:00
parent dd1d35b2db
commit 21f10ee0ec
2 changed files with 143 additions and 125 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

120
src/cgmath/quaternion.rs
View file

@ -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<S> {
Quat { s: s, v: v }
}
/// Create a quaternion from a rotation around the `x` axis (pitch).
#[inline]
pub fn from_angle_x(theta: Rad<S>) -> Quat<S> {
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<S>) -> Quat<S> {
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<S>) -> Quat<S> {
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<S>, y: Rad<S>, z: Rad<S>) -> Quat<S> {
// 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<S>, angle: Rad<S>) -> Quat<S> {
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<S> {
@ -331,3 +285,71 @@ impl<S: fmt::Default> ToStr for Quat<S> {
format!("{} + {}i + {}j + {}k", self.s, self.v.x, self.v.y, self.v.z)
}
}
// Quaternion Rotation impls
impl<S: Float + ApproxEq<S>>
ToBasis3<S> for Quat<S> {
#[inline]
fn to_rot3(&self) -> Basis3<S> { Basis3::from_quat(self) }
}
impl<S: Float> ToQuat<S> for Quat<S> {
#[inline]
fn to_quat(&self) -> Quat<S> { self.clone() }
}
impl<S: Float + ApproxEq<S>>
Rotation<S, [S, ..3], Vec3<S>, Point3<S>> for Quat<S> {
#[inline]
fn identity() -> Quat<S> { Quat::identity() }
#[inline]
fn look_at(dir: &Vec3<S>, up: &Vec3<S>) -> Quat<S> {
Mat3::look_at(dir, up).to_quat()
}
#[inline]
fn between_vecs(a: &Vec3<S>, b: &Vec3<S>) -> Quat<S> {
//http://stackoverflow.com/questions/1171849/
//finding-quaternion-representing-the-rotation-from-one-vector-to-another
Quat::from_sv(one::<S>() + a.dot(b), a.cross(b)).normalize()
}
#[inline]
fn rotate_vec(&self, vec: &Vec3<S>) -> Vec3<S> { self.mul_v(vec) }
#[inline]
fn concat(&self, other: &Quat<S>) -> Quat<S> { self.mul_q(other) }
#[inline]
fn concat_self(&mut self, other: &Quat<S>) { self.mul_self_q(other); }
#[inline]
fn invert(&self) -> Quat<S> { self.conjugate().div_s(self.magnitude2()) }
#[inline]
fn invert_self(&mut self) { *self = self.invert() }
}
impl<S: Float + ApproxEq<S>>
Rotation3<S> for Quat<S>
{
#[inline]
fn from_axis_angle(axis: &Vec3<S>, angle: Rad<S>) -> Quat<S> {
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<S>, y: Rad<S>, z: Rad<S>) -> Quat<S> {
// 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)
}
}

142
src/cgmath/rotation.rs
View file

@ -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,8 +36,14 @@ pub trait Rotation
+ ApproxEq<S>
{
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;
#[inline]
@ -74,18 +80,50 @@ pub trait Rotation2
: Rotation<S, [S, ..2], Vec2<S>, Point2<S>>
+ ToMat2<S>
+ ToBasis2<S>
{}
{
// 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<S>) -> Self;
}
/// A three-dimensional rotation
pub trait Rotation3
<
S
S: Primitive
>
: Rotation<S, [S, ..3], Vec3<S>, Point3<S>>
+ ToMat3<S>
+ ToBasis3<S>
+ ToQuat<S>
{}
{
/// Create a rotation around a given axis.
fn from_axis_angle(axis: &Vec3<S>, angle: Rad<S>) -> 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<S>, y: Rad<S>, z: Rad<S>) -> Self;
/// Create a rotation matrix from a rotation around the `x` axis (pitch).
#[inline]
fn from_angle_x(theta: Rad<S>) -> Self {
Rotation3::from_axis_angle( &Vec3::unit_x(), theta )
}
#[inline]
fn from_angle_y(theta: Rad<S>) -> Self {
Rotation3::from_axis_angle( &Vec3::unit_y(), theta )
}
#[inline]
fn from_angle_z(theta: Rad<S>) -> 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<S> {
priv mat: Mat2<S>
@ -129,8 +167,8 @@ Rotation<S, [S, ..2], Vec2<S>, Point2<S>> for Basis2<S> {
}
#[inline]
fn look_at(dir: &Vec2<S>, up: &Vec2<S>) -> Basis2<S> {
Basis2 { mat: Mat2::look_at(dir, up) }
fn between_vecs(a: &Vec2<S>, b: &Vec2<S>) -> Basis2<S> {
Rotation2::from_angle( acos(a.dot(b)) )
}
#[inline]
@ -162,7 +200,9 @@ ApproxEq<S> for Basis2<S> {
}
impl<S: Float + ApproxEq<S>>
Rotation2<S> for Basis2<S> {}
Rotation2<S> for Basis2<S> {
fn from_angle(theta: Rad<S>) -> Basis2<S> { Basis2 { mat: Mat2::from_angle(theta) } }
}
/// A three-dimensional rotation matrix.
///
@ -177,36 +217,8 @@ pub struct Basis3<S> {
impl<S: Float + ApproxEq<S>>
Basis3<S> {
/// Create a rotation matrix from a rotation around the `x` axis (pitch).
pub fn from_angle_x(theta: Rad<S>) -> Basis3<S> {
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<S>) -> Basis3<S> {
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<S>) -> Basis3<S> {
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<S>, y: Rad<S>, z: Rad<S>) -> Basis3<S> {
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<S>, angle: Rad<S>) -> Basis3<S> {
Basis3 { mat: Mat3::from_axis_angle(axis, angle) }
}
#[inline]
pub fn from_quat(quat: &Quat<S>) -> Basis3<S> { Basis3 { mat: quat.to_mat3() } }
#[inline]
pub fn as_mat3<'a>(&'a self) -> &'a Mat3<S> { &'a self.mat }
@ -242,6 +254,12 @@ Rotation<S, [S, ..3], Vec3<S>, Point3<S>> for Basis3<S> {
Basis3 { mat: Mat3::look_at(dir, up) }
}
#[inline]
fn between_vecs(a: &Vec3<S>, b: &Vec3<S>) -> Basis3<S> {
let q: Quat<S> = Rotation::between_vecs(a, b);
q.to_rot3()
}
#[inline]
fn rotate_vec(&self, vec: &Vec3<S>) -> Vec3<S> { self.mat.mul_v(vec) }
@ -271,46 +289,24 @@ ApproxEq<S> for Basis3<S> {
}
impl<S: Float + ApproxEq<S>>
Rotation3<S> for Basis3<S> {}
// Quaternion Rotation impls
impl<S: Float + ApproxEq<S>>
ToBasis3<S> for Quat<S> {
#[inline]
fn to_rot3(&self) -> Basis3<S> { Basis3 { mat: self.to_mat3() } }
Rotation3<S> for Basis3<S> {
fn from_axis_angle(axis: &Vec3<S>, angle: Rad<S>) -> Basis3<S> {
Basis3 { mat: Mat3::from_axis_angle(axis, angle) }
}
impl<S: Float> ToQuat<S> for Quat<S> {
#[inline]
fn to_quat(&self) -> Quat<S> { self.clone() }
fn from_euler(x: Rad<S>, y: Rad<S>, z: Rad<S>) -> Basis3<S> {
Basis3 { mat: Mat3::from_euler(x, y ,z) }
}
impl<S: Float + ApproxEq<S>>
Rotation<S, [S, ..3], Vec3<S>, Point3<S>> for Quat<S> {
#[inline]
fn identity() -> Quat<S> { Quat::identity() }
#[inline]
fn look_at(dir: &Vec3<S>, up: &Vec3<S>) -> Quat<S> {
Mat3::look_at(dir, up).to_quat()
fn from_angle_x(theta: Rad<S>) -> Basis3<S> {
Basis3 { mat: Mat3::from_angle_x(theta) }
}
#[inline]
fn rotate_vec(&self, vec: &Vec3<S>) -> Vec3<S> { self.mul_v(vec) }
#[inline]
fn concat(&self, other: &Quat<S>) -> Quat<S> { self.mul_q(other) }
#[inline]
fn concat_self(&mut self, other: &Quat<S>) { self.mul_self_q(other); }
#[inline]
fn invert(&self) -> Quat<S> { self.conjugate().div_s(self.magnitude2()) }
#[inline]
fn invert_self(&mut self) { *self = self.invert() }
fn from_angle_y(theta: Rad<S>) -> Basis3<S> {
Basis3 { mat: Mat3::from_angle_y(theta) }
}
impl<S: Float + ApproxEq<S>>
Rotation3<S> for Quat<S> {}
fn from_angle_z(theta: Rad<S>) -> Basis3<S> {
Basis3 { mat: Mat3::from_angle_z(theta) }
}
}