nalgebra/src/geometry/quaternion.rs
2017-08-14 14:32:02 +02:00

754 lines
25 KiB
Rust
Raw Blame History

This file contains invisible Unicode characters

This file contains invisible Unicode characters that are indistinguishable to humans but may be processed differently by a computer. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

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.

use std::fmt;
use num::Zero;
use approx::ApproxEq;
#[cfg(feature = "serde-serialize")]
use serde::{Serialize, Serializer, Deserialize, Deserializer};
#[cfg(feature = "abomonation-serialize")]
use abomonation::Abomonation;
use alga::general::Real;
use core::{Unit, ColumnVector, OwnedColumnVector, MatrixSlice, MatrixSliceMut, SquareMatrix,
OwnedSquareMatrix};
use core::storage::{Storage, StorageMut};
use core::allocator::Allocator;
use core::dimension::{U1, U3, U4};
use geometry::{RotationBase, OwnedRotation};
/// A quaternion with an owned storage allocated by `A`.
pub type OwnedQuaternionBase<N, A> = QuaternionBase<N, <A as Allocator<N, U4, U1>>::Buffer>;
/// A unit quaternion with an owned storage allocated by `A`.
pub type OwnedUnitQuaternionBase<N, A> = UnitQuaternionBase<N, <A as Allocator<N, U4, U1>>::Buffer>;
/// A quaternion. See the type alias `UnitQuaternionBase = Unit<QuaternionBase>` for a quaternion
/// that may be used as a rotation.
#[repr(C)]
#[derive(Hash, Debug, Copy, Clone)]
pub struct QuaternionBase<N: Real, S: Storage<N, U4, U1>> {
/// This quaternion as a 4D vector of coordinates in the `[ x, y, z, w ]` storage order.
pub coords: ColumnVector<N, U4, S>
}
#[cfg(feature = "serde-serialize")]
impl<N, S> Serialize for QuaternionBase<N, S>
where N: Real,
S: Storage<N, U4, U1>,
ColumnVector<N, U4, S>: Serialize,
{
fn serialize<T>(&self, serializer: T) -> Result<T::Ok, T::Error>
where T: Serializer
{
self.coords.serialize(serializer)
}
}
#[cfg(feature = "serde-serialize")]
impl<'de, N, S> Deserialize<'de> for QuaternionBase<N, S>
where N: Real,
S: Storage<N, U4, U1>,
ColumnVector<N, U4, S>: Deserialize<'de>,
{
fn deserialize<D>(deserializer: D) -> Result<Self, D::Error>
where D: Deserializer<'de>
{
ColumnVector::deserialize(deserializer).map(|x| QuaternionBase { coords: x })
}
}
#[cfg(feature = "abomonation-serialize")]
impl<N, S> Abomonation for QuaternionBase<N, S>
where N: Real,
S: Storage<N, U4, U1>,
ColumnVector<N, U4, S>: Abomonation
{
unsafe fn entomb(&self, writer: &mut Vec<u8>) {
self.coords.entomb(writer)
}
unsafe fn embalm(&mut self) {
self.coords.embalm()
}
unsafe fn exhume<'a, 'b>(&'a mut self, bytes: &'b mut [u8]) -> Option<&'b mut [u8]> {
self.coords.exhume(bytes)
}
}
impl<N, S> Eq for QuaternionBase<N, S>
where N: Real + Eq,
S: Storage<N, U4, U1> {
}
impl<N, S> PartialEq for QuaternionBase<N, S>
where N: Real,
S: Storage<N, U4, U1> {
fn eq(&self, rhs: &Self) -> bool {
self.coords == rhs.coords ||
// Account for the double-covering of S², i.e. q = -q
self.as_vector().iter().zip(rhs.as_vector().iter()).all(|(a, b)| *a == -*b)
}
}
impl<N, S> QuaternionBase<N, S>
where N: Real,
S: Storage<N, U4, U1> {
/// Moves this quaternion into one that owns its data.
#[inline]
pub fn into_owned(self) -> OwnedQuaternionBase<N, S::Alloc> {
QuaternionBase::from_vector(self.coords.into_owned())
}
/// Clones this quaternion into one that owns its data.
#[inline]
pub fn clone_owned(&self) -> OwnedQuaternionBase<N, S::Alloc> {
QuaternionBase::from_vector(self.coords.clone_owned())
}
/// The vector part `(i, j, k)` of this quaternion.
#[inline]
pub fn vector(&self) -> MatrixSlice<N, U3, U1, S::RStride, S::CStride, S::Alloc> {
self.coords.fixed_rows::<U3>(0)
}
/// The scalar part `w` of this quaternion.
#[inline]
pub fn scalar(&self) -> N {
self.coords[3]
}
/// Reinterprets this quaternion as a 4D vector.
#[inline]
pub fn as_vector(&self) -> &ColumnVector<N, U4, S> {
&self.coords
}
/// The norm of this quaternion.
#[inline]
pub fn norm(&self) -> N {
self.coords.norm()
}
/// The squared norm of this quaternion.
#[inline]
pub fn norm_squared(&self) -> N {
self.coords.norm_squared()
}
/// Normalizes this quaternion.
#[inline]
pub fn normalize(&self) -> OwnedQuaternionBase<N, S::Alloc> {
QuaternionBase::from_vector(self.coords.normalize())
}
/// Compute the conjugate of this quaternion.
#[inline]
pub fn conjugate(&self) -> OwnedQuaternionBase<N, S::Alloc> {
let v = OwnedColumnVector::<N, U4, S::Alloc>::new(-self.coords[0],
-self.coords[1],
-self.coords[2],
self.coords[3]);
QuaternionBase::from_vector(v)
}
/// Inverts this quaternion if it is not zero.
#[inline]
pub fn try_inverse(&self) -> Option<OwnedQuaternionBase<N, S::Alloc>> {
let mut res = QuaternionBase::from_vector(self.coords.clone_owned());
if res.try_inverse_mut() {
Some(res)
}
else {
None
}
}
/// Linear interpolation between two quaternion.
#[inline]
pub fn lerp<S2>(&self, other: &QuaternionBase<N, S2>, t: N) -> OwnedQuaternionBase<N, S::Alloc>
where S2: Storage<N, U4, U1> {
self * (N::one() - t) + other * t
}
}
impl<N, S> QuaternionBase<N, S>
where N: Real,
S: Storage<N, U4, U1>,
S::Alloc: Allocator<N, U3, U1> {
/// The polar decomposition of this quaternion.
///
/// Returns, from left to right: the quaternion norm, the half rotation angle, the rotation
/// axis. If the rotation angle is zero, the rotation axis is set to `None`.
pub fn polar_decomposition(&self) -> (N, N, Option<Unit<OwnedColumnVector<N, U3, S::Alloc>>>) {
if let Some((q, n)) = Unit::try_new_and_get(self.clone_owned(), N::zero()) {
if let Some(axis) = Unit::try_new(self.vector().clone_owned(), N::zero()) {
let angle = q.angle() / ::convert(2.0f64);
(n, angle, Some(axis))
}
else {
(n, N::zero(), None)
}
}
else {
(N::zero(), N::zero(), None)
}
}
/// Compute the exponential of a quaternion.
#[inline]
pub fn exp(&self) -> OwnedQuaternionBase<N, S::Alloc> {
let v = self.vector();
let nn = v.norm_squared();
if relative_eq!(nn, N::zero()) {
QuaternionBase::identity()
}
else {
let w_exp = self.scalar().exp();
let n = nn.sqrt();
let nv = v * (w_exp * n.sin() / n);
QuaternionBase::from_parts(n.cos(), nv)
}
}
/// Compute the natural logarithm of a quaternion.
#[inline]
pub fn ln(&self) -> OwnedQuaternionBase<N, S::Alloc> {
let n = self.norm();
let v = self.vector();
let s = self.scalar();
QuaternionBase::from_parts(n.ln(), v.normalize() * (s / n).acos())
}
/// Raise the quaternion to a given floating power.
#[inline]
pub fn powf(&self, n: N) -> OwnedQuaternionBase<N, S::Alloc> {
(self.ln() * n).exp()
}
}
impl<N, S> QuaternionBase<N, S>
where N: Real,
S: StorageMut<N, U4, U1> {
/// Transforms this quaternion into its 4D vector form (Vector part, Scalar part).
#[inline]
pub fn as_vector_mut(&mut self) -> &mut ColumnVector<N, U4, S> {
&mut self.coords
}
/// The mutable vector part `(i, j, k)` of this quaternion.
#[inline]
pub fn vector_mut(&mut self) -> MatrixSliceMut<N, U3, U1, S::RStride, S::CStride, S::Alloc> {
self.coords.fixed_rows_mut::<U3>(0)
}
/// Replaces this quaternion by its conjugate.
#[inline]
pub fn conjugate_mut(&mut self) {
self.coords[0] = -self.coords[0];
self.coords[1] = -self.coords[1];
self.coords[2] = -self.coords[2];
}
/// Inverts this quaternion in-place if it is not zero.
#[inline]
pub fn try_inverse_mut(&mut self) -> bool {
let norm_squared = self.norm_squared();
if relative_eq!(&norm_squared, &N::zero()) {
false
}
else {
self.conjugate_mut();
self.coords /= norm_squared;
true
}
}
/// Normalizes this quaternion.
#[inline]
pub fn normalize_mut(&mut self) -> N {
self.coords.normalize_mut()
}
}
impl<N, S> ApproxEq for QuaternionBase<N, S>
where N: Real + ApproxEq<Epsilon = N>,
S: Storage<N, U4, U1> {
type Epsilon = N;
#[inline]
fn default_epsilon() -> Self::Epsilon {
N::default_epsilon()
}
#[inline]
fn default_max_relative() -> Self::Epsilon {
N::default_max_relative()
}
#[inline]
fn default_max_ulps() -> u32 {
N::default_max_ulps()
}
#[inline]
fn relative_eq(&self, other: &Self, epsilon: Self::Epsilon, max_relative: Self::Epsilon) -> bool {
self.as_vector().relative_eq(other.as_vector(), epsilon, max_relative) ||
// Account for the double-covering of S², i.e. q = -q
self.as_vector().iter().zip(other.as_vector().iter()).all(|(a, b)| a.relative_eq(&-*b, epsilon, max_relative))
}
#[inline]
fn ulps_eq(&self, other: &Self, epsilon: Self::Epsilon, max_ulps: u32) -> bool {
self.as_vector().ulps_eq(other.as_vector(), epsilon, max_ulps) ||
// Account for the double-covering of S², i.e. q = -q.
self.as_vector().iter().zip(other.as_vector().iter()).all(|(a, b)| a.ulps_eq(&-*b, epsilon, max_ulps))
}
}
impl<N, S> fmt::Display for QuaternionBase<N, S>
where N: Real + fmt::Display,
S: Storage<N, U4, U1> {
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
write!(f, "Quaternion {} ({}, {}, {})", self[3], self[0], self[1], self[2])
}
}
/// A unit quaternions. May be used to represent a rotation.
///
///
/// <center>
/// <big><b>
/// Due to a [bug](https://github.com/rust-lang/rust/issues/32077) in rustdoc, the documentation
/// below has been written manually lists only method signatures.<br>
/// Trait implementations are not listed either.
/// </b></big>
/// </center>
///
/// Please refer directly to the documentation written above each function definition on the source
/// code for more details.
///
/// <h2 id="methods">Methods</h2>
///
///
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">angle</a>(&self) -> N</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">angle_to</a>(&self, other: &UnitQuaternionBase) -> N</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">axis</a>(&self) -> Option&lt;Unit&lt;OwnedColumnVector&gt;&gt;</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">clone_owned</a>(&self) -> OwnedUnitQuaternionBase</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">conjugate_mut</a>(&mut self)</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">conjugate</a>(&self) -> OwnedUnitQuaternionBase</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">exp</a>(&self) -> OwnedQuaternionBase</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">from_axis_angle</a>(axis: &Unit&lt;ColumnVector&gt;, angle: N) -> Self</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">from_euler_angles</a>(roll: N, pitch: N, yaw: N) -> Self</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">from_quaternion</a>(q: QuaternionBase) -> Self</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">from_rotation_matrix</a>(rotmat: &RotationBase) -> Self</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">from_scaled_axis</a>(axisangle: ColumnVector) -> Self</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">identity</a>() -> Self</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">into_owned</a>(self) -> OwnedUnitQuaternionBase</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">inverse_mut</a>(&mut self)</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">inverse</a>(&self) -> OwnedUnitQuaternionBase</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">lerp</a>(&self, other: &UnitQuaternionBase, t: N) -> OwnedQuaternionBase</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">ln</a>(&self) -> OwnedQuaternionBase</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">look_at_lh</a>(dir: &ColumnVector, up: &ColumnVector) -> Self</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">look_at_rh</a>(dir: &ColumnVector, up: &ColumnVector) -> Self</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">new</a>(axisangle: ColumnVector) -> Self</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">new_observer_frame</a>(dir: &ColumnVector, up: &ColumnVector) -> Self</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">nlerp</a>(&self, other: &UnitQuaternionBase, t: N) -> OwnedUnitQuaternionBase</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">powf</a>(&self, n: N) -> OwnedUnitQuaternionBase</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">quaternion</a>(&self) -> &QuaternionBase</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">rotation_between</a>(a: &ColumnVector, b: &ColumnVector) -> Option&lt;Self&gt;</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">rotation_to</a>(&self, other: &UnitQuaternionBase) -> OwnedUnitQuaternionBase</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">scaled_axis</a>(&self) -> OwnedColumnVector</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">scaled_rotation_between</a>(a: &ColumnVector, b: &ColumnVector, s: N) -> Option&lt;Self&gt;</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">slerp</a>(&self, other: &UnitQuaternionBase, t: N) -> OwnedUnitQuaternionBase</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">to_homogeneous</a>(&self) -> OwnedSquareMatrix</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">to_rotation_matrix</a>(&self) -> OwnedRotation</code>
/// </h4>
/// <h4 class="method"><span class="invisible">
/// <code>fn <a class="fnname">try_slerp</a>(&self, other: &UnitQuaternionBase, t: N, epsilon: N) -> Option&lt;OwnedUnitQuaternionBase&gt;</code>
/// </h4>
pub type UnitQuaternionBase<N, S> = Unit<QuaternionBase<N, S>>;
impl<N, S> UnitQuaternionBase<N, S>
where N: Real,
S: Storage<N, U4, U1> {
/// Moves this unit quaternion into one that owns its data.
#[inline]
pub fn into_owned(self) -> OwnedUnitQuaternionBase<N, S::Alloc> {
UnitQuaternionBase::new_unchecked(self.unwrap().into_owned())
}
/// Clones this unit quaternion into one that owns its data.
#[inline]
pub fn clone_owned(&self) -> OwnedUnitQuaternionBase<N, S::Alloc> {
UnitQuaternionBase::new_unchecked(self.as_ref().clone_owned())
}
/// The rotation angle in [0; pi] of this unit quaternion.
#[inline]
pub fn angle(&self) -> N {
let w = self.quaternion().scalar().abs();
// Handle innacuracies that make break `.acos`.
if w >= N::one() {
N::zero()
}
else {
w.acos() * ::convert(2.0f64)
}
}
/// The underlying quaternion.
///
/// Same as `self.as_ref()`.
#[inline]
pub fn quaternion(&self) -> &QuaternionBase<N, S> {
self.as_ref()
}
/// Compute the conjugate of this unit quaternion.
#[inline]
pub fn conjugate(&self) -> OwnedUnitQuaternionBase<N, S::Alloc> {
UnitQuaternionBase::new_unchecked(self.as_ref().conjugate())
}
/// Inverts this quaternion if it is not zero.
#[inline]
pub fn inverse(&self) -> OwnedUnitQuaternionBase<N, S::Alloc> {
self.conjugate()
}
/// The rotation angle needed to make `self` and `other` coincide.
#[inline]
pub fn angle_to<S2>(&self, other: &UnitQuaternionBase<N, S2>) -> N
where S2: Storage<N, U4, U1> {
let delta = self.rotation_to(other);
delta.angle()
}
/// The unit quaternion needed to make `self` and `other` coincide.
///
/// The result is such that: `self.rotation_to(other) * self == other`.
#[inline]
pub fn rotation_to<S2>(&self, other: &UnitQuaternionBase<N, S2>) -> OwnedUnitQuaternionBase<N, S2::Alloc>
where S2: Storage<N, U4, U1> {
other / self
}
/// Linear interpolation between two unit quaternions.
///
/// The result is not normalized.
#[inline]
pub fn lerp<S2>(&self, other: &UnitQuaternionBase<N, S2>, t: N) -> OwnedQuaternionBase<N, S::Alloc>
where S2: Storage<N, U4, U1> {
self.as_ref().lerp(other.as_ref(), t)
}
/// Normalized linear interpolation between two unit quaternions.
#[inline]
pub fn nlerp<S2>(&self, other: &UnitQuaternionBase<N, S2>, t: N) -> OwnedUnitQuaternionBase<N, S::Alloc>
where S2: Storage<N, U4, U1> {
let mut res = self.lerp(other, t);
let _ = res.normalize_mut();
UnitQuaternionBase::new_unchecked(res)
}
/// Spherical linear interpolation between two unit quaternions.
///
/// Panics if the angle between both quaternion is 180 degrees (in which case the interpolation
/// is not well-defined).
#[inline]
pub fn slerp<S2>(&self, other: &UnitQuaternionBase<N, S2>, t: N) -> OwnedUnitQuaternionBase<N, S::Alloc>
where S2: Storage<N, U4, U1, Alloc = S::Alloc> {
self.try_slerp(other, t, N::zero()).expect(
"Unable to perform a spherical quaternion interpolation when they \
are 180 degree apart (the result is not unique).")
}
/// Computes the spherical linear interpolation between two unit quaternions or returns `None`
/// if both quaternions are approximately 180 degrees apart (in which case the interpolation is
/// not well-defined).
///
/// # Arguments
/// * `self`: the first quaternion to interpolate from.
/// * `other`: the second quaternion to interpolate toward.
/// * `t`: the interpolation parameter. Should be between 0 and 1.
/// * `epsilon`: the value bellow which the sinus of the angle separating both quaternion
/// must be to return `None`.
#[inline]
pub fn try_slerp<S2>(&self, other: &UnitQuaternionBase<N, S2>, t: N, epsilon: N)
-> Option<OwnedUnitQuaternionBase<N, S::Alloc>>
where S2: Storage<N, U4, U1, Alloc = S::Alloc> {
let c_hang = self.coords.dot(&other.coords);
// self == other
if c_hang.abs() >= N::one() {
return Some(self.clone_owned())
}
let hang = c_hang.acos();
let s_hang = (N::one() - c_hang * c_hang).sqrt();
// FIXME: what if s_hang is 0.0 ? The result is not well-defined.
if relative_eq!(s_hang, N::zero(), epsilon = epsilon) {
None
}
else {
let ta = ((N::one() - t) * hang).sin() / s_hang;
let tb = (t * hang).sin() / s_hang;
let res = self.as_ref() * ta + other.as_ref() * tb;
Some(UnitQuaternionBase::new_unchecked(res))
}
}
}
impl<N, S> UnitQuaternionBase<N, S>
where N: Real,
S: StorageMut<N, U4, U1> {
/// Compute the conjugate of this unit quaternion in-place.
#[inline]
pub fn conjugate_mut(&mut self) {
self.as_mut_unchecked().conjugate_mut()
}
/// Inverts this quaternion if it is not zero.
#[inline]
pub fn inverse_mut(&mut self) {
self.as_mut_unchecked().conjugate_mut()
}
}
impl<N, S> UnitQuaternionBase<N, S>
where N: Real,
S: Storage<N, U4, U1>,
S::Alloc: Allocator<N, U3, U1> {
/// The rotation axis of this unit quaternion or `None` if the rotation is zero.
#[inline]
pub fn axis(&self) -> Option<Unit<OwnedColumnVector<N, U3, S::Alloc>>> {
let v =
if self.quaternion().scalar() >= N::zero() {
self.as_ref().vector().clone_owned()
}
else {
-self.as_ref().vector()
};
Unit::try_new(v, N::zero())
}
/// The rotation axis of this unit quaternion multiplied by the rotation agle.
#[inline]
pub fn scaled_axis(&self) -> OwnedColumnVector<N, U3, S::Alloc> {
if let Some(axis) = self.axis() {
axis.unwrap() * self.angle()
}
else {
ColumnVector::zero()
}
}
/// Compute the exponential of a quaternion.
///
/// Note that this function yields a `QuaternionBase<N>` because it looses the unit property.
#[inline]
pub fn exp(&self) -> OwnedQuaternionBase<N, S::Alloc> {
self.as_ref().exp()
}
/// Compute the natural logarithm of a quaternion.
///
/// Note that this function yields a `QuaternionBase<N>` because it looses the unit property.
/// The vector part of the return value corresponds to the axis-angle representation (divided
/// by 2.0) of this unit quaternion.
#[inline]
pub fn ln(&self) -> OwnedQuaternionBase<N, S::Alloc> {
if let Some(v) = self.axis() {
QuaternionBase::from_parts(N::zero(), v.unwrap() * self.angle())
}
else {
QuaternionBase::zero()
}
}
/// Raise the quaternion to a given floating power.
///
/// This returns the unit quaternion that identifies a rotation with axis `self.axis()` and
/// angle `self.angle() × n`.
#[inline]
pub fn powf(&self, n: N) -> OwnedUnitQuaternionBase<N, S::Alloc> {
if let Some(v) = self.axis() {
UnitQuaternionBase::from_axis_angle(&v, self.angle() * n)
}
else {
UnitQuaternionBase::identity()
}
}
}
impl<N, S> UnitQuaternionBase<N, S>
where N: Real,
S: Storage<N, U4, U1>,
S::Alloc: Allocator<N, U3, U3> {
/// Builds a rotation matrix from this unit quaternion.
#[inline]
pub fn to_rotation_matrix(&self) -> OwnedRotation<N, U3, S::Alloc> {
let i = self.as_ref()[0];
let j = self.as_ref()[1];
let k = self.as_ref()[2];
let w = self.as_ref()[3];
let ww = w * w;
let ii = i * i;
let jj = j * j;
let kk = k * k;
let ij = i * j * ::convert(2.0f64);
let wk = w * k * ::convert(2.0f64);
let wj = w * j * ::convert(2.0f64);
let ik = i * k * ::convert(2.0f64);
let jk = j * k * ::convert(2.0f64);
let wi = w * i * ::convert(2.0f64);
RotationBase::from_matrix_unchecked(
SquareMatrix::<_, U3, _>::new(
ww + ii - jj - kk, ij - wk, wj + ik,
wk + ij, ww - ii + jj - kk, jk - wi,
ik - wj, wi + jk, ww - ii - jj + kk
)
)
}
/// Converts this unit quaternion into its equivalent homogeneous transformation matrix.
#[inline]
pub fn to_homogeneous(&self) -> OwnedSquareMatrix<N, U4, S::Alloc>
where S::Alloc: Allocator<N, U4, U4> {
self.to_rotation_matrix().to_homogeneous()
}
}
impl<N, S> fmt::Display for UnitQuaternionBase<N, S>
where N: Real + fmt::Display,
S: Storage<N, U4, U1>,
S::Alloc: Allocator<N, U3, U1> {
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
if let Some(axis) = self.axis() {
let axis = axis.unwrap();
write!(f, "UnitQuaternion angle: {} axis: ({}, {}, {})", self.angle(), axis[0], axis[1], axis[2])
}
else {
write!(f, "UnitQuaternion angle: {} axis: (undefined)", self.angle())
}
}
}
impl<N, S> ApproxEq for UnitQuaternionBase<N, S>
where N: Real + ApproxEq<Epsilon = N>,
S: Storage<N, U4, U1> {
type Epsilon = N;
#[inline]
fn default_epsilon() -> Self::Epsilon {
N::default_epsilon()
}
#[inline]
fn default_max_relative() -> Self::Epsilon {
N::default_max_relative()
}
#[inline]
fn default_max_ulps() -> u32 {
N::default_max_ulps()
}
#[inline]
fn relative_eq(&self, other: &Self, epsilon: Self::Epsilon, max_relative: Self::Epsilon) -> bool {
self.as_ref().relative_eq(other.as_ref(), epsilon, max_relative)
}
#[inline]
fn ulps_eq(&self, other: &Self, epsilon: Self::Epsilon, max_ulps: u32) -> bool {
self.as_ref().ulps_eq(other.as_ref(), epsilon, max_ulps)
}
}