nalgebra/src/geometry/quaternion_construction.rs

430 lines
14 KiB
Rust
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

#[cfg(feature = "arbitrary")]
use quickcheck::{Arbitrary, Gen};
#[cfg(feature = "arbitrary")]
use base::storage::Owned;
#[cfg(feature = "arbitrary")]
use base::dimension::U4;
use rand::{Rand, Rng};
use num::{One, Zero};
use alga::general::Real;
use base::{Unit, Vector, Vector3, Vector4};
use base::storage::Storage;
use base::dimension::U3;
use geometry::{Quaternion, Rotation, UnitQuaternion};
impl<N: Real> Quaternion<N> {
/// Creates a quaternion from a 4D vector. The quaternion scalar part corresponds to the `w`
/// vector component.
#[inline]
pub fn from_vector(vector: Vector4<N>) -> Self {
Quaternion { coords: vector }
}
/// Creates a new quaternion from its individual components. Note that the arguments order does
/// **not** follow the storage order.
///
/// The storage order is `[ x, y, z, w ]`.
#[inline]
pub fn new(w: N, x: N, y: N, z: N) -> Self {
let v = Vector4::<N>::new(x, y, z, w);
Self::from_vector(v)
}
/// Creates a new quaternion from its scalar and vector parts. Note that the arguments order does
/// **not** follow the storage order.
///
/// The storage order is [ vector, scalar ].
#[inline]
// FIXME: take a reference to `vector`?
pub fn from_parts<SB>(scalar: N, vector: Vector<N, U3, SB>) -> Self
where
SB: Storage<N, U3>,
{
Self::new(scalar, vector[0], vector[1], vector[2])
}
/// Creates a new quaternion from its polar decomposition.
///
/// Note that `axis` is assumed to be a unit vector.
// FIXME: take a reference to `axis`?
pub fn from_polar_decomposition<SB>(scale: N, theta: N, axis: Unit<Vector<N, U3, SB>>) -> Self
where
SB: Storage<N, U3>,
{
let rot = UnitQuaternion::<N>::from_axis_angle(&axis, theta * ::convert(2.0f64));
rot.unwrap() * scale
}
/// The quaternion multiplicative identity.
#[inline]
pub fn identity() -> Self {
Self::new(N::one(), N::zero(), N::zero(), N::zero())
}
}
impl<N: Real> One for Quaternion<N> {
#[inline]
fn one() -> Self {
Self::identity()
}
}
impl<N: Real> Zero for Quaternion<N> {
#[inline]
fn zero() -> Self {
Self::new(N::zero(), N::zero(), N::zero(), N::zero())
}
#[inline]
fn is_zero(&self) -> bool {
self.coords.is_zero()
}
}
impl<N: Real + Rand> Rand for Quaternion<N> {
#[inline]
fn rand<R: Rng>(rng: &mut R) -> Self {
Quaternion::new(rng.gen(), rng.gen(), rng.gen(), rng.gen())
}
}
#[cfg(feature = "arbitrary")]
impl<N: Real + Arbitrary> Arbitrary for Quaternion<N>
where
Owned<N, U4>: Send,
{
#[inline]
fn arbitrary<G: Gen>(g: &mut G) -> Self {
Quaternion::new(
N::arbitrary(g),
N::arbitrary(g),
N::arbitrary(g),
N::arbitrary(g),
)
}
}
impl<N: Real> UnitQuaternion<N> {
/// The quaternion multiplicative identity.
#[inline]
pub fn identity() -> Self {
Self::new_unchecked(Quaternion::identity())
}
/// Creates a new quaternion from a unit vector (the rotation axis) and an angle
/// (the rotation angle).
#[inline]
pub fn from_axis_angle<SB>(axis: &Unit<Vector<N, U3, SB>>, angle: N) -> Self
where
SB: Storage<N, U3>,
{
let (sang, cang) = (angle / ::convert(2.0f64)).sin_cos();
let q = Quaternion::from_parts(cang, axis.as_ref() * sang);
Self::new_unchecked(q)
}
/// Creates a new unit quaternion from a quaternion.
///
/// The input quaternion will be normalized.
#[inline]
pub fn from_quaternion(q: Quaternion<N>) -> Self {
Self::new_normalize(q)
}
/// Creates a new unit quaternion from Euler angles.
///
/// The primitive rotations are applied in order: 1 roll 2 pitch 3 yaw.
#[inline]
pub fn from_euler_angles(roll: N, pitch: N, yaw: N) -> Self {
let (sr, cr) = (roll * ::convert(0.5f64)).sin_cos();
let (sp, cp) = (pitch * ::convert(0.5f64)).sin_cos();
let (sy, cy) = (yaw * ::convert(0.5f64)).sin_cos();
let q = Quaternion::new(
cr * cp * cy + sr * sp * sy,
sr * cp * cy - cr * sp * sy,
cr * sp * cy + sr * cp * sy,
cr * cp * sy - sr * sp * cy,
);
Self::new_unchecked(q)
}
/// Builds an unit quaternion from a rotation matrix.
#[inline]
pub fn from_rotation_matrix(rotmat: &Rotation<N, U3>) -> Self {
// Robust matrix to quaternion transformation.
// See http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion
let tr = rotmat[(0, 0)] + rotmat[(1, 1)] + rotmat[(2, 2)];
let res;
let _0_25: N = ::convert(0.25);
if tr > N::zero() {
let denom = (tr + N::one()).sqrt() * ::convert(2.0);
res = Quaternion::new(
_0_25 * denom,
(rotmat[(2, 1)] - rotmat[(1, 2)]) / denom,
(rotmat[(0, 2)] - rotmat[(2, 0)]) / denom,
(rotmat[(1, 0)] - rotmat[(0, 1)]) / denom,
);
} else if rotmat[(0, 0)] > rotmat[(1, 1)] && rotmat[(0, 0)] > rotmat[(2, 2)] {
let denom = (N::one() + rotmat[(0, 0)] - rotmat[(1, 1)] - rotmat[(2, 2)]).sqrt()
* ::convert(2.0);
res = Quaternion::new(
(rotmat[(2, 1)] - rotmat[(1, 2)]) / denom,
_0_25 * denom,
(rotmat[(0, 1)] + rotmat[(1, 0)]) / denom,
(rotmat[(0, 2)] + rotmat[(2, 0)]) / denom,
);
} else if rotmat[(1, 1)] > rotmat[(2, 2)] {
let denom = (N::one() + rotmat[(1, 1)] - rotmat[(0, 0)] - rotmat[(2, 2)]).sqrt()
* ::convert(2.0);
res = Quaternion::new(
(rotmat[(0, 2)] - rotmat[(2, 0)]) / denom,
(rotmat[(0, 1)] + rotmat[(1, 0)]) / denom,
_0_25 * denom,
(rotmat[(1, 2)] + rotmat[(2, 1)]) / denom,
);
} else {
let denom = (N::one() + rotmat[(2, 2)] - rotmat[(0, 0)] - rotmat[(1, 1)]).sqrt()
* ::convert(2.0);
res = Quaternion::new(
(rotmat[(1, 0)] - rotmat[(0, 1)]) / denom,
(rotmat[(0, 2)] + rotmat[(2, 0)]) / denom,
(rotmat[(1, 2)] + rotmat[(2, 1)]) / denom,
_0_25 * denom,
);
}
Self::new_unchecked(res)
}
/// The unit quaternion needed to make `a` and `b` be collinear and point toward the same
/// direction.
#[inline]
pub fn rotation_between<SB, SC>(a: &Vector<N, U3, SB>, b: &Vector<N, U3, SC>) -> Option<Self>
where
SB: Storage<N, U3>,
SC: Storage<N, U3>,
{
Self::scaled_rotation_between(a, b, N::one())
}
/// The smallest rotation needed to make `a` and `b` collinear and point toward the same
/// direction, raised to the power `s`.
#[inline]
pub fn scaled_rotation_between<SB, SC>(
a: &Vector<N, U3, SB>,
b: &Vector<N, U3, SC>,
s: N,
) -> Option<Self>
where
SB: Storage<N, U3>,
SC: Storage<N, U3>,
{
// FIXME: code duplication with Rotation.
if let (Some(na), Some(nb)) = (
Unit::try_new(a.clone_owned(), N::zero()),
Unit::try_new(b.clone_owned(), N::zero()),
) {
Self::scaled_rotation_between_axis(&na, &nb, s)
} else {
Some(Self::identity())
}
}
/// The unit quaternion needed to make `a` and `b` be collinear and point toward the same
/// direction.
#[inline]
pub fn rotation_between_axis<SB, SC>(
a: &Unit<Vector<N, U3, SB>>,
b: &Unit<Vector<N, U3, SC>>,
) -> Option<Self>
where
SB: Storage<N, U3>,
SC: Storage<N, U3>,
{
Self::scaled_rotation_between_axis(a, b, N::one())
}
/// The smallest rotation needed to make `a` and `b` collinear and point toward the same
/// direction, raised to the power `s`.
#[inline]
pub fn scaled_rotation_between_axis<SB, SC>(
na: &Unit<Vector<N, U3, SB>>,
nb: &Unit<Vector<N, U3, SC>>,
s: N,
) -> Option<Self>
where
SB: Storage<N, U3>,
SC: Storage<N, U3>,
{
// FIXME: code duplication with Rotation.
let c = na.cross(&nb);
if let Some(axis) = Unit::try_new(c, N::default_epsilon()) {
let cos = na.dot(&nb);
// The cosinus may be out of [-1, 1] because of innacuracies.
if cos <= -N::one() {
return None;
} else if cos >= N::one() {
return Some(Self::identity());
} else {
return Some(Self::from_axis_angle(&axis, cos.acos() * s));
}
} else if na.dot(&nb) < N::zero() {
// PI
//
// The rotation axis is undefined but the angle not zero. This is not a
// simple rotation.
return None;
} else {
// Zero
Some(Self::identity())
}
}
/// Creates an unit quaternion that corresponds to the local frame of an observer standing at the
/// origin and looking toward `dir`.
///
/// It maps the view direction `dir` to the positive `z` axis.
///
/// # Arguments
/// * dir - The look direction, that is, direction the matrix `z` axis will be aligned with.
/// * up - The vertical direction. The only requirement of this parameter is to not be
/// collinear
/// to `dir`. Non-collinearity is not checked.
#[inline]
pub fn new_observer_frame<SB, SC>(dir: &Vector<N, U3, SB>, up: &Vector<N, U3, SC>) -> Self
where
SB: Storage<N, U3>,
SC: Storage<N, U3>,
{
Self::from_rotation_matrix(&Rotation::<N, U3>::new_observer_frame(dir, up))
}
/// Builds a right-handed look-at view matrix without translation.
///
/// This conforms to the common notion of right handed look-at matrix from the computer
/// graphics community.
///
/// # Arguments
/// * eye - The eye position.
/// * target - The target position.
/// * up - A vector approximately aligned with required the vertical axis. The only
/// requirement of this parameter is to not be collinear to `target - eye`.
#[inline]
pub fn look_at_rh<SB, SC>(dir: &Vector<N, U3, SB>, up: &Vector<N, U3, SC>) -> Self
where
SB: Storage<N, U3>,
SC: Storage<N, U3>,
{
Self::new_observer_frame(&-dir, up).inverse()
}
/// Builds a left-handed look-at view matrix without translation.
///
/// This conforms to the common notion of left handed look-at matrix from the computer
/// graphics community.
///
/// # Arguments
/// * eye - The eye position.
/// * target - The target position.
/// * up - A vector approximately aligned with required the vertical axis. The only
/// requirement of this parameter is to not be collinear to `target - eye`.
#[inline]
pub fn look_at_lh<SB, SC>(dir: &Vector<N, U3, SB>, up: &Vector<N, U3, SC>) -> Self
where
SB: Storage<N, U3>,
SC: Storage<N, U3>,
{
Self::new_observer_frame(dir, up).inverse()
}
/// Creates a new unit quaternion rotation from a rotation axis scaled by the rotation angle.
///
/// If `axisangle` has a magnitude smaller than `N::default_epsilon()`, this returns the identity rotation.
#[inline]
pub fn new<SB>(axisangle: Vector<N, U3, SB>) -> Self
where
SB: Storage<N, U3>,
{
let two: N = ::convert(2.0f64);
let q = Quaternion::<N>::from_parts(N::zero(), axisangle / two).exp();
Self::new_unchecked(q)
}
/// Creates a new unit quaternion rotation from a rotation axis scaled by the rotation angle.
///
/// If `axisangle` has a magnitude smaller than `eps`, this returns the identity rotation.
#[inline]
pub fn new_eps<SB>(axisangle: Vector<N, U3, SB>, eps: N) -> Self
where
SB: Storage<N, U3>,
{
let two: N = ::convert(2.0f64);
let q = Quaternion::<N>::from_parts(N::zero(), axisangle / two).exp_eps(eps);
Self::new_unchecked(q)
}
/// Creates a new unit quaternion rotation from a rotation axis scaled by the rotation angle.
///
/// If `axisangle` has a magnitude smaller than `N::default_epsilon()`, this returns the identity rotation.
/// Same as `Self::new(axisangle)`.
#[inline]
pub fn from_scaled_axis<SB>(axisangle: Vector<N, U3, SB>) -> Self
where
SB: Storage<N, U3>,
{
Self::new(axisangle)
}
/// Creates a new unit quaternion rotation from a rotation axis scaled by the rotation angle.
///
/// If `axisangle` has a magnitude smaller than `eps`, this returns the identity rotation.
/// Same as `Self::new(axisangle)`.
#[inline]
pub fn from_scaled_axis_eps<SB>(axisangle: Vector<N, U3, SB>, eps: N) -> Self
where
SB: Storage<N, U3>,
{
Self::new_eps(axisangle, eps)
}
}
impl<N: Real> One for UnitQuaternion<N> {
#[inline]
fn one() -> Self {
Self::identity()
}
}
impl<N: Real + Rand> Rand for UnitQuaternion<N> {
#[inline]
fn rand<R: Rng>(rng: &mut R) -> Self {
let axisangle = Vector3::rand(rng);
UnitQuaternion::from_scaled_axis(axisangle)
}
}
#[cfg(feature = "arbitrary")]
impl<N: Real + Arbitrary> Arbitrary for UnitQuaternion<N>
where
Owned<N, U4>: Send,
Owned<N, U3>: Send,
{
#[inline]
fn arbitrary<G: Gen>(g: &mut G) -> Self {
let axisangle = Vector3::arbitrary(g);
UnitQuaternion::from_scaled_axis(axisangle)
}
}