Rotation2 and Rotation3 improved with from_* methods. Betwee_vecs implemented for all rotations.
This commit is contained in:
parent
dd1d35b2db
commit
21f10ee0ec
2 changed files with 143 additions and 125 deletions
|
@ -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)
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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<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;
|
||||
|
||||
|
@ -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,37 +217,9 @@ 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 }
|
||||
}
|
||||
|
@ -241,6 +253,12 @@ Rotation<S, [S, ..3], Vec3<S>, Point3<S>> for Basis3<S> {
|
|||
fn look_at(dir: &Vec3<S>, up: &Vec3<S>) -> 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() } }
|
||||
}
|
||||
|
||||
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()
|
||||
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) }
|
||||
}
|
||||
|
||||
#[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) }
|
||||
fn from_euler(x: Rad<S>, y: Rad<S>, z: Rad<S>) -> Basis3<S> {
|
||||
Basis3 { mat: Mat3::from_euler(x, y ,z) }
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn concat_self(&mut self, other: &Quat<S>) { self.mul_self_q(other); }
|
||||
fn from_angle_x(theta: Rad<S>) -> Basis3<S> {
|
||||
Basis3 { mat: Mat3::from_angle_x(theta) }
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn invert(&self) -> Quat<S> { self.conjugate().div_s(self.magnitude2()) }
|
||||
fn from_angle_y(theta: Rad<S>) -> Basis3<S> {
|
||||
Basis3 { mat: Mat3::from_angle_y(theta) }
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn invert_self(&mut self) { *self = self.invert() }
|
||||
fn from_angle_z(theta: Rad<S>) -> Basis3<S> {
|
||||
Basis3 { mat: Mat3::from_angle_z(theta) }
|
||||
}
|
||||
}
|
||||
|
||||
impl<S: Float + ApproxEq<S>>
|
||||
Rotation3<S> for Quat<S> {}
|
||||
|
|
Loading…
Reference in a new issue