2018-05-19 21:41:58 +08:00
|
|
|
|
use approx::{AbsDiffEq, RelativeEq, UlpsEq};
|
|
|
|
|
use num::Zero;
|
2016-12-05 05:44:42 +08:00
|
|
|
|
use std::fmt;
|
2017-08-03 01:37:44 +08:00
|
|
|
|
use std::hash;
|
2018-07-20 21:25:55 +08:00
|
|
|
|
#[cfg(feature = "abomonation-serialize")]
|
|
|
|
|
use std::io::{Result as IOResult, Write};
|
2016-12-05 05:44:42 +08:00
|
|
|
|
|
2017-08-03 01:37:44 +08:00
|
|
|
|
#[cfg(feature = "serde-serialize")]
|
2018-05-19 23:15:15 +08:00
|
|
|
|
use base::storage::Owned;
|
2018-05-19 21:41:58 +08:00
|
|
|
|
#[cfg(feature = "serde-serialize")]
|
2018-10-22 13:00:10 +08:00
|
|
|
|
use serde::{Deserialize, Deserializer, Serialize, Serializer};
|
2017-05-04 10:02:30 +08:00
|
|
|
|
|
2017-08-14 20:32:02 +08:00
|
|
|
|
#[cfg(feature = "abomonation-serialize")]
|
|
|
|
|
use abomonation::Abomonation;
|
|
|
|
|
|
2016-12-05 05:44:42 +08:00
|
|
|
|
use alga::general::Real;
|
|
|
|
|
|
2018-05-19 23:15:15 +08:00
|
|
|
|
use base::dimension::{U1, U3, U4};
|
|
|
|
|
use base::storage::{CStride, RStride};
|
|
|
|
|
use base::{Matrix3, MatrixN, MatrixSlice, MatrixSliceMut, Unit, Vector3, Vector4};
|
2016-12-05 05:44:42 +08:00
|
|
|
|
|
2017-08-03 01:37:44 +08:00
|
|
|
|
use geometry::Rotation;
|
2016-12-05 05:44:42 +08:00
|
|
|
|
|
2017-08-03 01:37:44 +08:00
|
|
|
|
/// A quaternion. See the type alias `UnitQuaternion = Unit<Quaternion>` for a quaternion
|
2016-12-05 05:44:42 +08:00
|
|
|
|
/// that may be used as a rotation.
|
|
|
|
|
#[repr(C)]
|
2017-08-03 01:37:44 +08:00
|
|
|
|
#[derive(Debug)]
|
|
|
|
|
pub struct Quaternion<N: Real> {
|
2017-02-13 01:17:09 +08:00
|
|
|
|
/// This quaternion as a 4D vector of coordinates in the `[ x, y, z, w ]` storage order.
|
2018-02-02 19:26:35 +08:00
|
|
|
|
pub coords: Vector4<N>,
|
2017-05-04 10:02:30 +08:00
|
|
|
|
}
|
|
|
|
|
|
2017-08-14 20:32:02 +08:00
|
|
|
|
#[cfg(feature = "abomonation-serialize")]
|
2017-08-16 01:36:38 +08:00
|
|
|
|
impl<N: Real> Abomonation for Quaternion<N>
|
2018-10-22 13:00:10 +08:00
|
|
|
|
where Vector4<N>: Abomonation
|
2017-08-14 20:32:02 +08:00
|
|
|
|
{
|
2018-07-20 21:25:55 +08:00
|
|
|
|
unsafe fn entomb<W: Write>(&self, writer: &mut W) -> IOResult<()> {
|
2017-08-14 20:32:02 +08:00
|
|
|
|
self.coords.entomb(writer)
|
|
|
|
|
}
|
|
|
|
|
|
2018-07-20 21:25:55 +08:00
|
|
|
|
fn extent(&self) -> usize {
|
|
|
|
|
self.coords.extent()
|
2017-08-14 20:32:02 +08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
unsafe fn exhume<'a, 'b>(&'a mut self, bytes: &'b mut [u8]) -> Option<&'b mut [u8]> {
|
|
|
|
|
self.coords.exhume(bytes)
|
|
|
|
|
}
|
|
|
|
|
}
|
2017-05-04 10:02:30 +08:00
|
|
|
|
|
2018-02-02 19:26:35 +08:00
|
|
|
|
impl<N: Real + Eq> Eq for Quaternion<N> {}
|
2016-12-05 05:44:42 +08:00
|
|
|
|
|
2017-08-03 01:37:44 +08:00
|
|
|
|
impl<N: Real> PartialEq for Quaternion<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
|
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)
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2017-08-03 01:37:44 +08:00
|
|
|
|
impl<N: Real + hash::Hash> hash::Hash for Quaternion<N> {
|
|
|
|
|
fn hash<H: hash::Hasher>(&self, state: &mut H) {
|
|
|
|
|
self.coords.hash(state)
|
2016-12-05 05:44:42 +08:00
|
|
|
|
}
|
2017-08-03 01:37:44 +08:00
|
|
|
|
}
|
2016-12-05 05:44:42 +08:00
|
|
|
|
|
2018-02-02 19:26:35 +08:00
|
|
|
|
impl<N: Real> Copy for Quaternion<N> {}
|
2016-12-05 05:44:42 +08:00
|
|
|
|
|
2017-08-03 01:37:44 +08:00
|
|
|
|
impl<N: Real> Clone for Quaternion<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
|
#[inline]
|
2017-08-03 01:37:44 +08:00
|
|
|
|
fn clone(&self) -> Self {
|
2018-10-28 14:33:39 +08:00
|
|
|
|
Quaternion::from(self.coords.clone())
|
2016-12-05 05:44:42 +08:00
|
|
|
|
}
|
2017-08-03 01:37:44 +08:00
|
|
|
|
}
|
2016-12-05 05:44:42 +08:00
|
|
|
|
|
2017-08-03 01:37:44 +08:00
|
|
|
|
#[cfg(feature = "serde-serialize")]
|
2018-09-13 12:55:58 +08:00
|
|
|
|
impl<N: Real> Serialize for Quaternion<N>
|
2018-10-22 13:00:10 +08:00
|
|
|
|
where Owned<N, U4>: Serialize
|
2018-02-02 19:26:35 +08:00
|
|
|
|
{
|
2017-08-03 01:37:44 +08:00
|
|
|
|
fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
|
2018-10-22 13:00:10 +08:00
|
|
|
|
where S: Serializer {
|
2018-02-02 19:26:35 +08:00
|
|
|
|
self.coords.serialize(serializer)
|
|
|
|
|
}
|
2017-08-03 01:37:44 +08:00
|
|
|
|
}
|
2016-12-05 05:44:42 +08:00
|
|
|
|
|
2017-08-03 01:37:44 +08:00
|
|
|
|
#[cfg(feature = "serde-serialize")]
|
2018-09-13 12:55:58 +08:00
|
|
|
|
impl<'a, N: Real> Deserialize<'a> for Quaternion<N>
|
2018-10-22 13:00:10 +08:00
|
|
|
|
where Owned<N, U4>: Deserialize<'a>
|
2018-02-02 19:26:35 +08:00
|
|
|
|
{
|
2017-08-03 01:37:44 +08:00
|
|
|
|
fn deserialize<Des>(deserializer: Des) -> Result<Self, Des::Error>
|
2018-10-22 13:00:10 +08:00
|
|
|
|
where Des: Deserializer<'a> {
|
2018-02-02 19:26:35 +08:00
|
|
|
|
let coords = Vector4::<N>::deserialize(deserializer)?;
|
2017-08-03 01:37:44 +08:00
|
|
|
|
|
2018-10-28 14:33:39 +08:00
|
|
|
|
Ok(Quaternion::from(coords))
|
2018-02-02 19:26:35 +08:00
|
|
|
|
}
|
2017-08-03 01:37:44 +08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
impl<N: Real> Quaternion<N> {
|
|
|
|
|
/// Moves this unit quaternion into one that owns its data.
|
2016-12-05 05:44:42 +08:00
|
|
|
|
#[inline]
|
2017-08-14 01:53:02 +08:00
|
|
|
|
#[deprecated(note = "This method is a no-op and will be removed in a future release.")]
|
2017-08-03 01:37:44 +08:00
|
|
|
|
pub fn into_owned(self) -> Quaternion<N> {
|
|
|
|
|
self
|
2016-12-05 05:44:42 +08:00
|
|
|
|
}
|
|
|
|
|
|
2017-08-03 01:37:44 +08:00
|
|
|
|
/// Clones this unit quaternion into one that owns its data.
|
2016-12-05 05:44:42 +08:00
|
|
|
|
#[inline]
|
2017-08-14 01:53:02 +08:00
|
|
|
|
#[deprecated(note = "This method is a no-op and will be removed in a future release.")]
|
2017-08-03 01:37:44 +08:00
|
|
|
|
pub fn clone_owned(&self) -> Quaternion<N> {
|
2018-10-28 14:33:39 +08:00
|
|
|
|
Quaternion::from(self.coords.clone_owned())
|
2016-12-05 05:44:42 +08:00
|
|
|
|
}
|
|
|
|
|
|
2017-02-13 01:17:09 +08:00
|
|
|
|
/// Normalizes this quaternion.
|
2018-10-27 17:00:11 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::Quaternion;
|
|
|
|
|
/// let q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
|
|
|
|
|
/// let q_normalized = q.normalize();
|
|
|
|
|
/// relative_eq!(q_normalized.norm(), 1.0);
|
|
|
|
|
/// ```
|
2017-02-13 01:17:09 +08:00
|
|
|
|
#[inline]
|
2017-08-03 01:37:44 +08:00
|
|
|
|
pub fn normalize(&self) -> Quaternion<N> {
|
2018-10-28 14:33:39 +08:00
|
|
|
|
Quaternion::from(self.coords.normalize())
|
2017-02-13 01:17:09 +08:00
|
|
|
|
}
|
|
|
|
|
|
2018-10-27 17:00:11 +08:00
|
|
|
|
/// The conjugate of this quaternion.
|
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # use nalgebra::Quaternion;
|
|
|
|
|
/// let q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
|
|
|
|
|
/// let conj = q.conjugate();
|
|
|
|
|
/// assert!(conj.i == -2.0 && conj.j == -3.0 && conj.k == -4.0 && conj.w == 1.0);
|
|
|
|
|
/// ```
|
2016-12-05 05:44:42 +08:00
|
|
|
|
#[inline]
|
2017-08-03 01:37:44 +08:00
|
|
|
|
pub fn conjugate(&self) -> Quaternion<N> {
|
2018-02-02 19:26:35 +08:00
|
|
|
|
let v = Vector4::new(
|
|
|
|
|
-self.coords[0],
|
|
|
|
|
-self.coords[1],
|
|
|
|
|
-self.coords[2],
|
|
|
|
|
self.coords[3],
|
|
|
|
|
);
|
2018-10-28 14:33:39 +08:00
|
|
|
|
Quaternion::from(v)
|
2016-12-05 05:44:42 +08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Inverts this quaternion if it is not zero.
|
2018-10-27 17:00:11 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::Quaternion;
|
|
|
|
|
/// let q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
|
|
|
|
|
/// let inv_q = q.try_inverse();
|
|
|
|
|
///
|
|
|
|
|
/// assert!(inv_q.is_some());
|
2018-12-10 23:52:19 +08:00
|
|
|
|
/// assert_relative_eq!(inv_q.unwrap() * q, Quaternion::identity());
|
2018-10-27 17:00:11 +08:00
|
|
|
|
///
|
|
|
|
|
/// //Non-invertible case
|
|
|
|
|
/// let q = Quaternion::new(0.0, 0.0, 0.0, 0.0);
|
|
|
|
|
/// let inv_q = q.try_inverse();
|
|
|
|
|
///
|
|
|
|
|
/// assert!(inv_q.is_none());
|
|
|
|
|
/// ```
|
2016-12-05 05:44:42 +08:00
|
|
|
|
#[inline]
|
2017-08-03 01:37:44 +08:00
|
|
|
|
pub fn try_inverse(&self) -> Option<Quaternion<N>> {
|
2018-10-28 14:33:39 +08:00
|
|
|
|
let mut res = Quaternion::from(self.coords.clone_owned());
|
2016-12-05 05:44:42 +08:00
|
|
|
|
|
|
|
|
|
if res.try_inverse_mut() {
|
|
|
|
|
Some(res)
|
2018-02-02 19:26:35 +08:00
|
|
|
|
} else {
|
2016-12-05 05:44:42 +08:00
|
|
|
|
None
|
|
|
|
|
}
|
|
|
|
|
}
|
2017-02-13 01:17:09 +08:00
|
|
|
|
|
|
|
|
|
/// Linear interpolation between two quaternion.
|
2018-10-27 17:00:11 +08:00
|
|
|
|
///
|
|
|
|
|
/// Computes `self * (1 - t) + other * t`.
|
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # use nalgebra::Quaternion;
|
|
|
|
|
/// let q1 = Quaternion::new(1.0, 2.0, 3.0, 4.0);
|
|
|
|
|
/// let q2 = Quaternion::new(10.0, 20.0, 30.0, 40.0);
|
|
|
|
|
///
|
|
|
|
|
/// assert_eq!(q1.lerp(&q2, 0.1), Quaternion::new(1.9, 3.8, 5.7, 7.6));
|
|
|
|
|
/// ```
|
2017-02-13 01:17:09 +08:00
|
|
|
|
#[inline]
|
2017-08-03 01:37:44 +08:00
|
|
|
|
pub fn lerp(&self, other: &Quaternion<N>, t: N) -> Quaternion<N> {
|
2017-02-13 01:17:09 +08:00
|
|
|
|
self * (N::one() - t) + other * t
|
|
|
|
|
}
|
2016-12-05 05:44:42 +08:00
|
|
|
|
|
2017-08-03 01:37:44 +08:00
|
|
|
|
/// The vector part `(i, j, k)` of this quaternion.
|
2018-10-27 17:00:11 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # use nalgebra::Quaternion;
|
|
|
|
|
/// let q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
|
|
|
|
|
/// assert_eq!(q.vector()[0], 2.0);
|
|
|
|
|
/// assert_eq!(q.vector()[1], 3.0);
|
|
|
|
|
/// assert_eq!(q.vector()[2], 4.0);
|
|
|
|
|
/// ```
|
2017-08-03 01:37:44 +08:00
|
|
|
|
#[inline]
|
|
|
|
|
pub fn vector(&self) -> MatrixSlice<N, U3, U1, RStride<N, U4, U1>, CStride<N, U4, U1>> {
|
|
|
|
|
self.coords.fixed_rows::<U3>(0)
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// The scalar part `w` of this quaternion.
|
2018-10-27 17:00:11 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # use nalgebra::Quaternion;
|
|
|
|
|
/// let q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
|
|
|
|
|
/// assert_eq!(q.scalar(), 1.0);
|
|
|
|
|
/// ```
|
2017-08-03 01:37:44 +08:00
|
|
|
|
#[inline]
|
|
|
|
|
pub fn scalar(&self) -> N {
|
|
|
|
|
self.coords[3]
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Reinterprets this quaternion as a 4D vector.
|
2018-10-27 17:00:11 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # use nalgebra::{Vector4, Quaternion};
|
|
|
|
|
/// let q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
|
|
|
|
|
/// // Recall that the quaternion is stored internally as (i, j, k, w)
|
|
|
|
|
/// // while the ::new constructor takes the arguments as (w, i, j, k).
|
|
|
|
|
/// assert_eq!(*q.as_vector(), Vector4::new(2.0, 3.0, 4.0, 1.0));
|
|
|
|
|
/// ```
|
2017-08-03 01:37:44 +08:00
|
|
|
|
#[inline]
|
|
|
|
|
pub fn as_vector(&self) -> &Vector4<N> {
|
|
|
|
|
&self.coords
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// The norm of this quaternion.
|
2018-10-27 17:00:11 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::Quaternion;
|
|
|
|
|
/// let q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
|
|
|
|
|
/// assert_relative_eq!(q.norm(), 5.47722557, epsilon = 1.0e-6);
|
|
|
|
|
/// ```
|
2017-08-03 01:37:44 +08:00
|
|
|
|
#[inline]
|
|
|
|
|
pub fn norm(&self) -> N {
|
|
|
|
|
self.coords.norm()
|
|
|
|
|
}
|
|
|
|
|
|
2018-09-21 23:25:43 +08:00
|
|
|
|
/// A synonym for the norm of this quaternion.
|
|
|
|
|
///
|
|
|
|
|
/// Aka the length.
|
2018-10-27 17:00:11 +08:00
|
|
|
|
/// This is the same as `.norm()`
|
2018-09-21 23:25:43 +08:00
|
|
|
|
///
|
2018-10-27 17:00:11 +08:00
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::Quaternion;
|
|
|
|
|
/// let q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
|
|
|
|
|
/// assert_relative_eq!(q.magnitude(), 5.47722557, epsilon = 1.0e-6);
|
|
|
|
|
/// ```
|
2018-09-21 23:25:43 +08:00
|
|
|
|
#[inline]
|
|
|
|
|
pub fn magnitude(&self) -> N {
|
|
|
|
|
self.norm()
|
|
|
|
|
}
|
|
|
|
|
|
2018-10-27 17:00:11 +08:00
|
|
|
|
/// The squared norm of this quaternion.
|
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # use nalgebra::Quaternion;
|
|
|
|
|
/// let q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
|
|
|
|
|
/// assert_eq!(q.magnitude_squared(), 30.0);
|
|
|
|
|
/// ```
|
|
|
|
|
#[inline]
|
|
|
|
|
pub fn norm_squared(&self) -> N {
|
|
|
|
|
self.coords.norm_squared()
|
|
|
|
|
}
|
|
|
|
|
|
2018-09-21 23:25:43 +08:00
|
|
|
|
/// A synonym for the squared norm of this quaternion.
|
|
|
|
|
///
|
|
|
|
|
/// Aka the squared length.
|
2018-10-27 17:00:11 +08:00
|
|
|
|
/// This is the same as `.norm_squared()`
|
2018-09-21 23:25:43 +08:00
|
|
|
|
///
|
2018-10-27 17:00:11 +08:00
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # use nalgebra::Quaternion;
|
|
|
|
|
/// let q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
|
|
|
|
|
/// assert_eq!(q.magnitude_squared(), 30.0);
|
|
|
|
|
/// ```
|
2018-09-21 23:25:43 +08:00
|
|
|
|
#[inline]
|
|
|
|
|
pub fn magnitude_squared(&self) -> N {
|
|
|
|
|
self.norm_squared()
|
|
|
|
|
}
|
|
|
|
|
|
2018-09-22 21:39:21 +08:00
|
|
|
|
/// The dot product of two quaternions.
|
2018-10-27 17:00:11 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # use nalgebra::Quaternion;
|
|
|
|
|
/// let q1 = Quaternion::new(1.0, 2.0, 3.0, 4.0);
|
|
|
|
|
/// let q2 = Quaternion::new(5.0, 6.0, 7.0, 8.0);
|
|
|
|
|
/// assert_eq!(q1.dot(&q2), 70.0);
|
|
|
|
|
/// ```
|
2018-09-22 21:39:21 +08:00
|
|
|
|
#[inline]
|
|
|
|
|
pub fn dot(&self, rhs: &Self) -> N {
|
|
|
|
|
self.coords.dot(&rhs.coords)
|
|
|
|
|
}
|
|
|
|
|
|
2016-12-05 05:44:42 +08:00
|
|
|
|
/// 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`.
|
2018-10-27 17:00:11 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # use std::f32;
|
|
|
|
|
/// # use nalgebra::{Vector3, Quaternion};
|
|
|
|
|
/// let q = Quaternion::new(0.0, 5.0, 0.0, 0.0);
|
|
|
|
|
/// let (norm, half_ang, axis) = q.polar_decomposition();
|
|
|
|
|
/// assert_eq!(norm, 5.0);
|
|
|
|
|
/// assert_eq!(half_ang, f32::consts::FRAC_PI_2);
|
|
|
|
|
/// assert_eq!(axis, Some(Vector3::x_axis()));
|
|
|
|
|
/// ```
|
2017-08-03 01:37:44 +08:00
|
|
|
|
pub fn polar_decomposition(&self) -> (N, N, Option<Unit<Vector3<N>>>) {
|
2017-08-14 01:53:02 +08:00
|
|
|
|
if let Some((q, n)) = Unit::try_new_and_get(*self, N::zero()) {
|
2016-12-05 05:44:42 +08:00
|
|
|
|
if let Some(axis) = Unit::try_new(self.vector().clone_owned(), N::zero()) {
|
|
|
|
|
let angle = q.angle() / ::convert(2.0f64);
|
|
|
|
|
|
|
|
|
|
(n, angle, Some(axis))
|
2018-02-02 19:26:35 +08:00
|
|
|
|
} else {
|
2016-12-05 05:44:42 +08:00
|
|
|
|
(n, N::zero(), None)
|
|
|
|
|
}
|
2018-02-02 19:26:35 +08:00
|
|
|
|
} else {
|
2016-12-05 05:44:42 +08:00
|
|
|
|
(N::zero(), N::zero(), None)
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2018-10-27 17:00:11 +08:00
|
|
|
|
/// Compute the natural logarithm of a quaternion.
|
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::Quaternion;
|
|
|
|
|
/// let q = Quaternion::new(2.0, 5.0, 0.0, 0.0);
|
|
|
|
|
/// assert_relative_eq!(q.ln(), Quaternion::new(1.683647, 1.190289, 0.0, 0.0), epsilon = 1.0e-6)
|
|
|
|
|
/// ```
|
|
|
|
|
#[inline]
|
|
|
|
|
pub fn ln(&self) -> Quaternion<N> {
|
|
|
|
|
let n = self.norm();
|
|
|
|
|
let v = self.vector();
|
|
|
|
|
let s = self.scalar();
|
|
|
|
|
|
|
|
|
|
Quaternion::from_parts(n.ln(), v.normalize() * (s / n).acos())
|
|
|
|
|
}
|
|
|
|
|
|
2016-12-05 05:44:42 +08:00
|
|
|
|
/// Compute the exponential of a quaternion.
|
2018-10-27 17:00:11 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::Quaternion;
|
|
|
|
|
/// let q = Quaternion::new(1.683647, 1.190289, 0.0, 0.0);
|
|
|
|
|
/// assert_relative_eq!(q.exp(), Quaternion::new(2.0, 5.0, 0.0, 0.0), epsilon = 1.0e-5)
|
|
|
|
|
/// ```
|
2016-12-05 05:44:42 +08:00
|
|
|
|
#[inline]
|
2017-08-03 01:37:44 +08:00
|
|
|
|
pub fn exp(&self) -> Quaternion<N> {
|
2018-05-03 18:06:11 +08:00
|
|
|
|
self.exp_eps(N::default_epsilon())
|
|
|
|
|
}
|
|
|
|
|
|
2018-10-27 17:00:11 +08:00
|
|
|
|
/// Compute the exponential of a quaternion. Returns the identity if the vector part of this quaternion
|
|
|
|
|
/// has a norm smaller than `eps`.
|
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::Quaternion;
|
|
|
|
|
/// let q = Quaternion::new(1.683647, 1.190289, 0.0, 0.0);
|
|
|
|
|
/// assert_relative_eq!(q.exp_eps(1.0e-6), Quaternion::new(2.0, 5.0, 0.0, 0.0), epsilon = 1.0e-5);
|
|
|
|
|
///
|
|
|
|
|
/// // Singular case.
|
|
|
|
|
/// let q = Quaternion::new(0.0000001, 0.0, 0.0, 0.0);
|
|
|
|
|
/// assert_eq!(q.exp_eps(1.0e-6), Quaternion::identity());
|
|
|
|
|
/// ```
|
2018-05-03 18:06:11 +08:00
|
|
|
|
#[inline]
|
|
|
|
|
pub fn exp_eps(&self, eps: N) -> Quaternion<N> {
|
2018-02-02 19:26:35 +08:00
|
|
|
|
let v = self.vector();
|
2016-12-05 05:44:42 +08:00
|
|
|
|
let nn = v.norm_squared();
|
|
|
|
|
|
2018-05-03 18:06:11 +08:00
|
|
|
|
if nn <= eps * eps {
|
2017-08-03 01:37:44 +08:00
|
|
|
|
Quaternion::identity()
|
2018-02-02 19:26:35 +08:00
|
|
|
|
} else {
|
2016-12-05 05:44:42 +08:00
|
|
|
|
let w_exp = self.scalar().exp();
|
2018-02-02 19:26:35 +08:00
|
|
|
|
let n = nn.sqrt();
|
2016-12-05 05:44:42 +08:00
|
|
|
|
let nv = v * (w_exp * n.sin() / n);
|
|
|
|
|
|
2018-10-27 17:00:11 +08:00
|
|
|
|
Quaternion::from_parts(w_exp * n.cos(), nv)
|
2016-12-05 05:44:42 +08:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Raise the quaternion to a given floating power.
|
2018-10-27 17:00:11 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::Quaternion;
|
|
|
|
|
/// let q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
|
|
|
|
|
/// assert_relative_eq!(q.powf(1.5), Quaternion::new( -6.2576659, 4.1549037, 6.2323556, 8.3098075), epsilon = 1.0e-6);
|
|
|
|
|
/// ```
|
2016-12-05 05:44:42 +08:00
|
|
|
|
#[inline]
|
2017-08-03 01:37:44 +08:00
|
|
|
|
pub fn powf(&self, n: N) -> Quaternion<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
|
(self.ln() * n).exp()
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Transforms this quaternion into its 4D vector form (Vector part, Scalar part).
|
2018-10-27 17:00:11 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # use nalgebra::{Quaternion, Vector4};
|
|
|
|
|
/// let mut q = Quaternion::identity();
|
|
|
|
|
/// *q.as_vector_mut() = Vector4::new(1.0, 2.0, 3.0, 4.0);
|
|
|
|
|
/// assert!(q.i == 1.0 && q.j == 2.0 && q.k == 3.0 && q.w == 4.0);
|
|
|
|
|
/// ```
|
2016-12-05 05:44:42 +08:00
|
|
|
|
#[inline]
|
2017-08-03 01:37:44 +08:00
|
|
|
|
pub fn as_vector_mut(&mut self) -> &mut Vector4<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
|
&mut self.coords
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// The mutable vector part `(i, j, k)` of this quaternion.
|
2018-10-27 17:00:11 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # use nalgebra::{Quaternion, Vector4};
|
|
|
|
|
/// let mut q = Quaternion::identity();
|
|
|
|
|
/// {
|
|
|
|
|
/// let mut v = q.vector_mut();
|
|
|
|
|
/// v[0] = 2.0;
|
|
|
|
|
/// v[1] = 3.0;
|
|
|
|
|
/// v[2] = 4.0;
|
|
|
|
|
/// }
|
|
|
|
|
/// assert!(q.i == 2.0 && q.j == 3.0 && q.k == 4.0 && q.w == 1.0);
|
|
|
|
|
/// ```
|
2016-12-05 05:44:42 +08:00
|
|
|
|
#[inline]
|
2018-02-02 19:26:35 +08:00
|
|
|
|
pub fn vector_mut(
|
|
|
|
|
&mut self,
|
|
|
|
|
) -> MatrixSliceMut<N, U3, U1, RStride<N, U4, U1>, CStride<N, U4, U1>> {
|
2016-12-05 05:44:42 +08:00
|
|
|
|
self.coords.fixed_rows_mut::<U3>(0)
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Replaces this quaternion by its conjugate.
|
2018-10-27 17:00:11 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # use nalgebra::Quaternion;
|
|
|
|
|
/// let mut q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
|
|
|
|
|
/// q.conjugate_mut();
|
|
|
|
|
/// assert!(q.i == -2.0 && q.j == -3.0 && q.k == -4.0 && q.w == 1.0);
|
|
|
|
|
/// ```
|
2016-12-05 05:44:42 +08:00
|
|
|
|
#[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.
|
2018-10-27 17:00:11 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::Quaternion;
|
|
|
|
|
/// let mut q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
|
|
|
|
|
///
|
|
|
|
|
/// assert!(q.try_inverse_mut());
|
|
|
|
|
/// assert_relative_eq!(q * Quaternion::new(1.0, 2.0, 3.0, 4.0), Quaternion::identity());
|
|
|
|
|
///
|
|
|
|
|
/// //Non-invertible case
|
|
|
|
|
/// let mut q = Quaternion::new(0.0, 0.0, 0.0, 0.0);
|
|
|
|
|
/// assert!(!q.try_inverse_mut());
|
|
|
|
|
/// ```
|
2016-12-05 05:44:42 +08:00
|
|
|
|
#[inline]
|
|
|
|
|
pub fn try_inverse_mut(&mut self) -> bool {
|
|
|
|
|
let norm_squared = self.norm_squared();
|
|
|
|
|
|
|
|
|
|
if relative_eq!(&norm_squared, &N::zero()) {
|
|
|
|
|
false
|
2018-02-02 19:26:35 +08:00
|
|
|
|
} else {
|
2016-12-05 05:44:42 +08:00
|
|
|
|
self.conjugate_mut();
|
|
|
|
|
self.coords /= norm_squared;
|
|
|
|
|
|
|
|
|
|
true
|
|
|
|
|
}
|
|
|
|
|
}
|
2017-02-13 01:17:09 +08:00
|
|
|
|
|
|
|
|
|
/// Normalizes this quaternion.
|
2018-10-27 17:00:11 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::Quaternion;
|
|
|
|
|
/// let mut q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
|
|
|
|
|
/// q.normalize_mut();
|
|
|
|
|
/// assert_relative_eq!(q.norm(), 1.0);
|
|
|
|
|
/// ```
|
2017-02-13 01:17:09 +08:00
|
|
|
|
#[inline]
|
|
|
|
|
pub fn normalize_mut(&mut self) -> N {
|
|
|
|
|
self.coords.normalize_mut()
|
|
|
|
|
}
|
2016-12-05 05:44:42 +08:00
|
|
|
|
}
|
|
|
|
|
|
2018-05-19 21:41:58 +08:00
|
|
|
|
impl<N: Real + AbsDiffEq<Epsilon = N>> AbsDiffEq for Quaternion<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
|
type Epsilon = N;
|
|
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
|
fn default_epsilon() -> Self::Epsilon {
|
|
|
|
|
N::default_epsilon()
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#[inline]
|
2018-05-19 21:41:58 +08:00
|
|
|
|
fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
|
|
|
|
|
self.as_vector().abs_diff_eq(other.as_vector(), epsilon) ||
|
|
|
|
|
// Account for the double-covering of S², i.e. q = -q
|
|
|
|
|
self.as_vector().iter().zip(other.as_vector().iter()).all(|(a, b)| a.abs_diff_eq(&-*b, epsilon))
|
2016-12-05 05:44:42 +08:00
|
|
|
|
}
|
2018-05-19 21:41:58 +08:00
|
|
|
|
}
|
2016-12-05 05:44:42 +08:00
|
|
|
|
|
2018-05-19 21:41:58 +08:00
|
|
|
|
impl<N: Real + RelativeEq<Epsilon = N>> RelativeEq for Quaternion<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
|
#[inline]
|
2018-05-19 21:41:58 +08:00
|
|
|
|
fn default_max_relative() -> Self::Epsilon {
|
|
|
|
|
N::default_max_relative()
|
2016-12-05 05:44:42 +08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#[inline]
|
2018-02-02 19:26:35 +08:00
|
|
|
|
fn relative_eq(
|
|
|
|
|
&self,
|
|
|
|
|
other: &Self,
|
|
|
|
|
epsilon: Self::Epsilon,
|
|
|
|
|
max_relative: Self::Epsilon,
|
2018-10-22 13:00:10 +08:00
|
|
|
|
) -> bool
|
|
|
|
|
{
|
2016-12-05 05:44:42 +08:00
|
|
|
|
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))
|
|
|
|
|
}
|
2018-05-19 21:41:58 +08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
impl<N: Real + UlpsEq<Epsilon = N>> UlpsEq for Quaternion<N> {
|
|
|
|
|
#[inline]
|
|
|
|
|
fn default_max_ulps() -> u32 {
|
|
|
|
|
N::default_max_ulps()
|
|
|
|
|
}
|
2016-12-05 05:44:42 +08:00
|
|
|
|
|
|
|
|
|
#[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))
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2017-08-03 01:37:44 +08:00
|
|
|
|
impl<N: Real + fmt::Display> fmt::Display for Quaternion<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
|
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
|
2018-02-02 19:26:35 +08:00
|
|
|
|
write!(
|
|
|
|
|
f,
|
|
|
|
|
"Quaternion {} − ({}, {}, {})",
|
|
|
|
|
self[3], self[0], self[1], self[2]
|
|
|
|
|
)
|
2016-12-05 05:44:42 +08:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// A unit quaternions. May be used to represent a rotation.
|
2017-08-03 01:37:44 +08:00
|
|
|
|
pub type UnitQuaternion<N> = Unit<Quaternion<N>>;
|
2016-12-05 05:44:42 +08:00
|
|
|
|
|
2017-08-03 01:37:44 +08:00
|
|
|
|
impl<N: Real> UnitQuaternion<N> {
|
2017-02-13 01:17:09 +08:00
|
|
|
|
/// Moves this unit quaternion into one that owns its data.
|
|
|
|
|
#[inline]
|
2018-10-28 14:33:39 +08:00
|
|
|
|
#[deprecated(
|
|
|
|
|
note = "This method is unnecessary and will be removed in a future release. Use `.clone()` instead."
|
|
|
|
|
)]
|
2017-08-03 01:37:44 +08:00
|
|
|
|
pub fn into_owned(self) -> UnitQuaternion<N> {
|
|
|
|
|
self
|
2017-02-13 01:17:09 +08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Clones this unit quaternion into one that owns its data.
|
|
|
|
|
#[inline]
|
2018-10-28 14:33:39 +08:00
|
|
|
|
#[deprecated(
|
|
|
|
|
note = "This method is unnecessary and will be removed in a future release. Use `.clone()` instead."
|
|
|
|
|
)]
|
2017-08-03 01:37:44 +08:00
|
|
|
|
pub fn clone_owned(&self) -> UnitQuaternion<N> {
|
2017-08-14 01:53:02 +08:00
|
|
|
|
*self
|
2017-02-13 01:17:09 +08:00
|
|
|
|
}
|
|
|
|
|
|
2016-12-05 05:44:42 +08:00
|
|
|
|
/// The rotation angle in [0; pi] of this unit quaternion.
|
2018-10-27 17:00:11 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # use nalgebra::{Unit, UnitQuaternion, Vector3};
|
|
|
|
|
/// let axis = Unit::new_normalize(Vector3::new(1.0, 2.0, 3.0));
|
|
|
|
|
/// let rot = UnitQuaternion::from_axis_angle(&axis, 1.78);
|
|
|
|
|
/// assert_eq!(rot.angle(), 1.78);
|
|
|
|
|
/// ```
|
2016-12-05 05:44:42 +08:00
|
|
|
|
#[inline]
|
|
|
|
|
pub fn angle(&self) -> N {
|
|
|
|
|
let w = self.quaternion().scalar().abs();
|
|
|
|
|
|
2018-09-24 12:48:42 +08:00
|
|
|
|
// Handle inaccuracies that make break `.acos`.
|
2016-12-05 05:44:42 +08:00
|
|
|
|
if w >= N::one() {
|
|
|
|
|
N::zero()
|
2018-02-02 19:26:35 +08:00
|
|
|
|
} else {
|
2016-12-05 05:44:42 +08:00
|
|
|
|
w.acos() * ::convert(2.0f64)
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// The underlying quaternion.
|
|
|
|
|
///
|
|
|
|
|
/// Same as `self.as_ref()`.
|
2018-10-27 17:00:11 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # use nalgebra::{UnitQuaternion, Quaternion};
|
|
|
|
|
/// let axis = UnitQuaternion::identity();
|
|
|
|
|
/// assert_eq!(*axis.quaternion(), Quaternion::new(1.0, 0.0, 0.0, 0.0));
|
|
|
|
|
/// ```
|
2016-12-05 05:44:42 +08:00
|
|
|
|
#[inline]
|
2017-08-03 01:37:44 +08:00
|
|
|
|
pub fn quaternion(&self) -> &Quaternion<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
|
self.as_ref()
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Compute the conjugate of this unit quaternion.
|
2018-10-27 17:00:11 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # use nalgebra::{Unit, UnitQuaternion, Vector3};
|
|
|
|
|
/// let axis = Unit::new_normalize(Vector3::new(1.0, 2.0, 3.0));
|
|
|
|
|
/// let rot = UnitQuaternion::from_axis_angle(&axis, 1.78);
|
|
|
|
|
/// let conj = rot.conjugate();
|
|
|
|
|
/// assert_eq!(conj, UnitQuaternion::from_axis_angle(&-axis, 1.78));
|
|
|
|
|
/// ```
|
2016-12-05 05:44:42 +08:00
|
|
|
|
#[inline]
|
2017-08-03 01:37:44 +08:00
|
|
|
|
pub fn conjugate(&self) -> UnitQuaternion<N> {
|
|
|
|
|
UnitQuaternion::new_unchecked(self.as_ref().conjugate())
|
2016-12-05 05:44:42 +08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Inverts this quaternion if it is not zero.
|
2018-10-27 17:00:11 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # use nalgebra::{Unit, UnitQuaternion, Vector3};
|
|
|
|
|
/// let axis = Unit::new_normalize(Vector3::new(1.0, 2.0, 3.0));
|
|
|
|
|
/// let rot = UnitQuaternion::from_axis_angle(&axis, 1.78);
|
|
|
|
|
/// let inv = rot.inverse();
|
|
|
|
|
/// assert_eq!(rot * inv, UnitQuaternion::identity());
|
|
|
|
|
/// assert_eq!(inv * rot, UnitQuaternion::identity());
|
|
|
|
|
/// ```
|
2016-12-05 05:44:42 +08:00
|
|
|
|
#[inline]
|
2017-08-03 01:37:44 +08:00
|
|
|
|
pub fn inverse(&self) -> UnitQuaternion<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
|
self.conjugate()
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// The rotation angle needed to make `self` and `other` coincide.
|
2018-10-27 17:00:11 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::{UnitQuaternion, Vector3};
|
|
|
|
|
/// let rot1 = UnitQuaternion::from_axis_angle(&Vector3::y_axis(), 1.0);
|
|
|
|
|
/// let rot2 = UnitQuaternion::from_axis_angle(&Vector3::x_axis(), 0.1);
|
|
|
|
|
/// assert_relative_eq!(rot1.angle_to(&rot2), 1.0045657, epsilon = 1.0e-6);
|
|
|
|
|
/// ```
|
2016-12-05 05:44:42 +08:00
|
|
|
|
#[inline]
|
2017-08-03 01:37:44 +08:00
|
|
|
|
pub fn angle_to(&self, other: &UnitQuaternion<N>) -> N {
|
2016-12-05 05:44:42 +08:00
|
|
|
|
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`.
|
2018-10-27 17:00:11 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::{UnitQuaternion, Vector3};
|
|
|
|
|
/// let rot1 = UnitQuaternion::from_axis_angle(&Vector3::y_axis(), 1.0);
|
|
|
|
|
/// let rot2 = UnitQuaternion::from_axis_angle(&Vector3::x_axis(), 0.1);
|
|
|
|
|
/// let rot_to = rot1.rotation_to(&rot2);
|
|
|
|
|
/// assert_relative_eq!(rot_to * rot1, rot2, epsilon = 1.0e-6);
|
|
|
|
|
/// ```
|
2016-12-05 05:44:42 +08:00
|
|
|
|
#[inline]
|
2017-08-03 01:37:44 +08:00
|
|
|
|
pub fn rotation_to(&self, other: &UnitQuaternion<N>) -> UnitQuaternion<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
|
other / self
|
|
|
|
|
}
|
2017-02-13 01:17:09 +08:00
|
|
|
|
|
|
|
|
|
/// Linear interpolation between two unit quaternions.
|
|
|
|
|
///
|
|
|
|
|
/// The result is not normalized.
|
2018-10-27 22:28:09 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # use nalgebra::{UnitQuaternion, Quaternion};
|
|
|
|
|
/// let q1 = UnitQuaternion::new_normalize(Quaternion::new(1.0, 0.0, 0.0, 0.0));
|
|
|
|
|
/// let q2 = UnitQuaternion::new_normalize(Quaternion::new(0.0, 1.0, 0.0, 0.0));
|
|
|
|
|
/// assert_eq!(q1.lerp(&q2, 0.1), Quaternion::new(0.9, 0.1, 0.0, 0.0));
|
|
|
|
|
/// ```
|
2017-02-13 01:17:09 +08:00
|
|
|
|
#[inline]
|
2017-08-03 01:37:44 +08:00
|
|
|
|
pub fn lerp(&self, other: &UnitQuaternion<N>, t: N) -> Quaternion<N> {
|
2017-02-13 01:17:09 +08:00
|
|
|
|
self.as_ref().lerp(other.as_ref(), t)
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Normalized linear interpolation between two unit quaternions.
|
2018-10-27 17:00:11 +08:00
|
|
|
|
///
|
|
|
|
|
/// This is the same as `self.lerp` except that the result is normalized.
|
2018-10-27 22:28:09 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # use nalgebra::{UnitQuaternion, Quaternion};
|
|
|
|
|
/// let q1 = UnitQuaternion::new_normalize(Quaternion::new(1.0, 0.0, 0.0, 0.0));
|
|
|
|
|
/// let q2 = UnitQuaternion::new_normalize(Quaternion::new(0.0, 1.0, 0.0, 0.0));
|
|
|
|
|
/// assert_eq!(q1.nlerp(&q2, 0.1), UnitQuaternion::new_normalize(Quaternion::new(0.9, 0.1, 0.0, 0.0)));
|
|
|
|
|
/// ```
|
2017-02-13 01:17:09 +08:00
|
|
|
|
#[inline]
|
2017-08-03 01:37:44 +08:00
|
|
|
|
pub fn nlerp(&self, other: &UnitQuaternion<N>, t: N) -> UnitQuaternion<N> {
|
2017-02-13 01:17:09 +08:00
|
|
|
|
let mut res = self.lerp(other, t);
|
|
|
|
|
let _ = res.normalize_mut();
|
|
|
|
|
|
2017-08-03 01:37:44 +08:00
|
|
|
|
UnitQuaternion::new_unchecked(res)
|
2017-02-13 01:17:09 +08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Spherical linear interpolation between two unit quaternions.
|
|
|
|
|
///
|
|
|
|
|
/// Panics if the angle between both quaternion is 180 degrees (in which case the interpolation
|
2018-10-27 22:28:09 +08:00
|
|
|
|
/// is not well-defined). Use `.try_slerp` instead to avoid the panic.
|
2017-02-13 01:17:09 +08:00
|
|
|
|
#[inline]
|
2017-08-03 01:37:44 +08:00
|
|
|
|
pub fn slerp(&self, other: &UnitQuaternion<N>, t: N) -> UnitQuaternion<N> {
|
2018-10-28 14:33:39 +08:00
|
|
|
|
Unit::new_unchecked(Quaternion::from(
|
2018-10-22 13:00:10 +08:00
|
|
|
|
Unit::new_unchecked(self.coords)
|
|
|
|
|
.slerp(&Unit::new_unchecked(other.coords), t)
|
2018-12-10 04:08:14 +08:00
|
|
|
|
.into_inner(),
|
2018-10-22 13:00:10 +08:00
|
|
|
|
))
|
2017-02-13 01:17:09 +08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// 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.
|
2017-10-27 12:13:35 +08:00
|
|
|
|
/// * `epsilon`: the value below which the sinus of the angle separating both quaternion
|
2017-02-13 01:17:09 +08:00
|
|
|
|
/// must be to return `None`.
|
|
|
|
|
#[inline]
|
2018-02-02 19:26:35 +08:00
|
|
|
|
pub fn try_slerp(
|
|
|
|
|
&self,
|
|
|
|
|
other: &UnitQuaternion<N>,
|
|
|
|
|
t: N,
|
|
|
|
|
epsilon: N,
|
2018-10-22 13:00:10 +08:00
|
|
|
|
) -> Option<UnitQuaternion<N>>
|
|
|
|
|
{
|
|
|
|
|
Unit::new_unchecked(self.coords)
|
|
|
|
|
.try_slerp(&Unit::new_unchecked(other.coords), t, epsilon)
|
2018-12-10 04:08:14 +08:00
|
|
|
|
.map(|q| Unit::new_unchecked(Quaternion::from(q.into_inner())))
|
2017-02-13 01:17:09 +08:00
|
|
|
|
}
|
2016-12-05 05:44:42 +08:00
|
|
|
|
|
|
|
|
|
/// 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.
|
2018-10-27 22:28:09 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::{UnitQuaternion, Vector3, Unit};
|
|
|
|
|
/// let axisangle = Vector3::new(0.1, 0.2, 0.3);
|
|
|
|
|
/// let mut rot = UnitQuaternion::new(axisangle);
|
|
|
|
|
/// rot.inverse_mut();
|
|
|
|
|
/// assert_relative_eq!(rot * UnitQuaternion::new(axisangle), UnitQuaternion::identity());
|
|
|
|
|
/// assert_relative_eq!(UnitQuaternion::new(axisangle) * rot, UnitQuaternion::identity());
|
|
|
|
|
/// ```
|
2016-12-05 05:44:42 +08:00
|
|
|
|
#[inline]
|
|
|
|
|
pub fn inverse_mut(&mut self) {
|
|
|
|
|
self.as_mut_unchecked().conjugate_mut()
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// The rotation axis of this unit quaternion or `None` if the rotation is zero.
|
2018-10-27 22:28:09 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # use nalgebra::{UnitQuaternion, Vector3, Unit};
|
|
|
|
|
/// let axis = Unit::new_normalize(Vector3::new(1.0, 2.0, 3.0));
|
|
|
|
|
/// let angle = 1.2;
|
|
|
|
|
/// let rot = UnitQuaternion::from_axis_angle(&axis, angle);
|
|
|
|
|
/// assert_eq!(rot.axis(), Some(axis));
|
|
|
|
|
///
|
|
|
|
|
/// // Case with a zero angle.
|
|
|
|
|
/// let rot = UnitQuaternion::from_axis_angle(&axis, 0.0);
|
|
|
|
|
/// assert!(rot.axis().is_none());
|
|
|
|
|
/// ```
|
2016-12-05 05:44:42 +08:00
|
|
|
|
#[inline]
|
2017-08-03 01:37:44 +08:00
|
|
|
|
pub fn axis(&self) -> Option<Unit<Vector3<N>>> {
|
2018-02-02 19:26:35 +08:00
|
|
|
|
let v = if self.quaternion().scalar() >= N::zero() {
|
|
|
|
|
self.as_ref().vector().clone_owned()
|
|
|
|
|
} else {
|
|
|
|
|
-self.as_ref().vector()
|
|
|
|
|
};
|
2016-12-05 05:44:42 +08:00
|
|
|
|
|
|
|
|
|
Unit::try_new(v, N::zero())
|
|
|
|
|
}
|
|
|
|
|
|
2018-09-24 12:48:42 +08:00
|
|
|
|
/// The rotation axis of this unit quaternion multiplied by the rotation angle.
|
2018-10-27 22:28:09 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::{UnitQuaternion, Vector3, Unit};
|
|
|
|
|
/// let axisangle = Vector3::new(0.1, 0.2, 0.3);
|
|
|
|
|
/// let rot = UnitQuaternion::new(axisangle);
|
|
|
|
|
/// assert_relative_eq!(rot.scaled_axis(), axisangle, epsilon = 1.0e-6);
|
|
|
|
|
/// ```
|
2016-12-05 05:44:42 +08:00
|
|
|
|
#[inline]
|
2017-08-03 01:37:44 +08:00
|
|
|
|
pub fn scaled_axis(&self) -> Vector3<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
|
if let Some(axis) = self.axis() {
|
2018-12-10 04:08:14 +08:00
|
|
|
|
axis.into_inner() * self.angle()
|
2018-02-02 19:26:35 +08:00
|
|
|
|
} else {
|
2017-08-03 01:37:44 +08:00
|
|
|
|
Vector3::zero()
|
2016-12-05 05:44:42 +08:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2018-03-09 00:30:59 +08:00
|
|
|
|
/// The rotation axis and angle in ]0, pi] of this unit quaternion.
|
|
|
|
|
///
|
|
|
|
|
/// Returns `None` if the angle is zero.
|
2018-10-27 22:28:09 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # use nalgebra::{UnitQuaternion, Vector3, Unit};
|
|
|
|
|
/// let axis = Unit::new_normalize(Vector3::new(1.0, 2.0, 3.0));
|
|
|
|
|
/// let angle = 1.2;
|
|
|
|
|
/// let rot = UnitQuaternion::from_axis_angle(&axis, angle);
|
|
|
|
|
/// assert_eq!(rot.axis_angle(), Some((axis, angle)));
|
|
|
|
|
///
|
|
|
|
|
/// // Case with a zero angle.
|
|
|
|
|
/// let rot = UnitQuaternion::from_axis_angle(&axis, 0.0);
|
|
|
|
|
/// assert!(rot.axis_angle().is_none());
|
|
|
|
|
/// ```
|
2018-03-09 00:30:59 +08:00
|
|
|
|
#[inline]
|
|
|
|
|
pub fn axis_angle(&self) -> Option<(Unit<Vector3<N>>, N)> {
|
|
|
|
|
if let Some(axis) = self.axis() {
|
|
|
|
|
Some((axis, self.angle()))
|
|
|
|
|
} else {
|
|
|
|
|
None
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2016-12-05 05:44:42 +08:00
|
|
|
|
/// Compute the exponential of a quaternion.
|
|
|
|
|
///
|
2017-08-03 01:37:44 +08:00
|
|
|
|
/// Note that this function yields a `Quaternion<N>` because it looses the unit property.
|
2016-12-05 05:44:42 +08:00
|
|
|
|
#[inline]
|
2017-08-03 01:37:44 +08:00
|
|
|
|
pub fn exp(&self) -> Quaternion<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
|
self.as_ref().exp()
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Compute the natural logarithm of a quaternion.
|
|
|
|
|
///
|
2017-08-03 01:37:44 +08:00
|
|
|
|
/// Note that this function yields a `Quaternion<N>` because it looses the unit property.
|
2016-12-05 05:44:42 +08:00
|
|
|
|
/// The vector part of the return value corresponds to the axis-angle representation (divided
|
|
|
|
|
/// by 2.0) of this unit quaternion.
|
2018-10-27 22:28:09 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::{Vector3, UnitQuaternion};
|
|
|
|
|
/// let axisangle = Vector3::new(0.1, 0.2, 0.3);
|
|
|
|
|
/// let q = UnitQuaternion::new(axisangle);
|
|
|
|
|
/// assert_relative_eq!(q.ln().vector().into_owned(), axisangle, epsilon = 1.0e-6);
|
|
|
|
|
/// ```
|
2016-12-05 05:44:42 +08:00
|
|
|
|
#[inline]
|
2017-08-03 01:37:44 +08:00
|
|
|
|
pub fn ln(&self) -> Quaternion<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
|
if let Some(v) = self.axis() {
|
2018-12-10 04:08:14 +08:00
|
|
|
|
Quaternion::from_parts(N::zero(), v.into_inner() * self.angle())
|
2018-02-02 19:26:35 +08:00
|
|
|
|
} else {
|
2017-08-03 01:37:44 +08:00
|
|
|
|
Quaternion::zero()
|
2016-12-05 05:44:42 +08:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// 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`.
|
2018-10-27 22:28:09 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::{UnitQuaternion, Vector3, Unit};
|
|
|
|
|
/// let axis = Unit::new_normalize(Vector3::new(1.0, 2.0, 3.0));
|
|
|
|
|
/// let angle = 1.2;
|
|
|
|
|
/// let rot = UnitQuaternion::from_axis_angle(&axis, angle);
|
|
|
|
|
/// let pow = rot.powf(2.0);
|
2018-12-10 23:52:19 +08:00
|
|
|
|
/// assert_relative_eq!(pow.axis().unwrap(), axis, epsilon = 1.0e-6);
|
2018-10-27 22:28:09 +08:00
|
|
|
|
/// assert_eq!(pow.angle(), 2.4);
|
|
|
|
|
/// ```
|
2016-12-05 05:44:42 +08:00
|
|
|
|
#[inline]
|
2017-08-03 01:37:44 +08:00
|
|
|
|
pub fn powf(&self, n: N) -> UnitQuaternion<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
|
if let Some(v) = self.axis() {
|
2017-08-03 01:37:44 +08:00
|
|
|
|
UnitQuaternion::from_axis_angle(&v, self.angle() * n)
|
2018-02-02 19:26:35 +08:00
|
|
|
|
} else {
|
2017-08-03 01:37:44 +08:00
|
|
|
|
UnitQuaternion::identity()
|
2016-12-05 05:44:42 +08:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Builds a rotation matrix from this unit quaternion.
|
2018-10-27 22:28:09 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
///
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use std::f32;
|
|
|
|
|
/// # use nalgebra::{UnitQuaternion, Vector3, Matrix3};
|
|
|
|
|
/// let q = UnitQuaternion::from_axis_angle(&Vector3::z_axis(), f32::consts::FRAC_PI_6);
|
|
|
|
|
/// let rot = q.to_rotation_matrix();
|
|
|
|
|
/// let expected = Matrix3::new(0.8660254, -0.5, 0.0,
|
|
|
|
|
/// 0.5, 0.8660254, 0.0,
|
|
|
|
|
/// 0.0, 0.0, 1.0);
|
|
|
|
|
///
|
|
|
|
|
/// assert_relative_eq!(*rot.matrix(), expected, epsilon = 1.0e-6);
|
|
|
|
|
/// ```
|
2016-12-05 05:44:42 +08:00
|
|
|
|
#[inline]
|
2017-08-03 01:37:44 +08:00
|
|
|
|
pub fn to_rotation_matrix(&self) -> Rotation<N, U3> {
|
2016-12-05 05:44:42 +08:00
|
|
|
|
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);
|
|
|
|
|
|
2018-02-02 19:26:35 +08:00
|
|
|
|
Rotation::from_matrix_unchecked(Matrix3::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,
|
|
|
|
|
))
|
2016-12-05 05:44:42 +08:00
|
|
|
|
}
|
|
|
|
|
|
2018-01-10 04:15:57 +08:00
|
|
|
|
/// Converts this unit quaternion into its equivalent Euler angles.
|
|
|
|
|
///
|
2018-10-27 22:28:09 +08:00
|
|
|
|
/// The angles are produced in the form (roll, pitch, yaw).
|
2018-01-10 04:15:57 +08:00
|
|
|
|
#[inline]
|
2018-10-27 22:28:09 +08:00
|
|
|
|
#[deprecated(note = "This is renamed to use `.euler_angles()`.")]
|
2018-01-10 04:15:57 +08:00
|
|
|
|
pub fn to_euler_angles(&self) -> (N, N, N) {
|
2018-11-04 15:47:17 +08:00
|
|
|
|
self.euler_angles()
|
2018-01-10 04:15:57 +08:00
|
|
|
|
}
|
|
|
|
|
|
2018-10-27 22:28:09 +08:00
|
|
|
|
/// Retrieves the euler angles corresponding to this unit quaternion.
|
|
|
|
|
///
|
|
|
|
|
/// The angles are produced in the form (roll, pitch, yaw).
|
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::UnitQuaternion;
|
|
|
|
|
/// let rot = UnitQuaternion::from_euler_angles(0.1, 0.2, 0.3);
|
|
|
|
|
/// let euler = rot.euler_angles();
|
|
|
|
|
/// assert_relative_eq!(euler.0, 0.1, epsilon = 1.0e-6);
|
|
|
|
|
/// assert_relative_eq!(euler.1, 0.2, epsilon = 1.0e-6);
|
|
|
|
|
/// assert_relative_eq!(euler.2, 0.3, epsilon = 1.0e-6);
|
|
|
|
|
/// ```
|
|
|
|
|
#[inline]
|
|
|
|
|
pub fn euler_angles(&self) -> (N, N, N) {
|
2018-11-04 15:47:17 +08:00
|
|
|
|
self.to_rotation_matrix().euler_angles()
|
2018-10-27 22:28:09 +08:00
|
|
|
|
}
|
|
|
|
|
|
2016-12-05 05:44:42 +08:00
|
|
|
|
/// Converts this unit quaternion into its equivalent homogeneous transformation matrix.
|
2018-10-27 22:28:09 +08:00
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
///
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use std::f32;
|
|
|
|
|
/// # use nalgebra::{UnitQuaternion, Vector3, Matrix4};
|
|
|
|
|
/// let rot = UnitQuaternion::from_axis_angle(&Vector3::z_axis(), f32::consts::FRAC_PI_6);
|
|
|
|
|
/// let expected = Matrix4::new(0.8660254, -0.5, 0.0, 0.0,
|
|
|
|
|
/// 0.5, 0.8660254, 0.0, 0.0,
|
|
|
|
|
/// 0.0, 0.0, 1.0, 0.0,
|
|
|
|
|
/// 0.0, 0.0, 0.0, 1.0);
|
|
|
|
|
///
|
|
|
|
|
/// assert_relative_eq!(rot.to_homogeneous(), expected, epsilon = 1.0e-6);
|
|
|
|
|
/// ```
|
2016-12-05 05:44:42 +08:00
|
|
|
|
#[inline]
|
2017-08-03 01:37:44 +08:00
|
|
|
|
pub fn to_homogeneous(&self) -> MatrixN<N, U4> {
|
2016-12-05 05:44:42 +08:00
|
|
|
|
self.to_rotation_matrix().to_homogeneous()
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2017-08-03 01:37:44 +08:00
|
|
|
|
impl<N: Real + fmt::Display> fmt::Display for UnitQuaternion<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
|
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
|
|
|
|
|
if let Some(axis) = self.axis() {
|
2018-12-10 04:08:14 +08:00
|
|
|
|
let axis = axis.into_inner();
|
2018-02-02 19:26:35 +08:00
|
|
|
|
write!(
|
|
|
|
|
f,
|
|
|
|
|
"UnitQuaternion angle: {} − axis: ({}, {}, {})",
|
|
|
|
|
self.angle(),
|
|
|
|
|
axis[0],
|
|
|
|
|
axis[1],
|
|
|
|
|
axis[2]
|
|
|
|
|
)
|
|
|
|
|
} else {
|
|
|
|
|
write!(
|
|
|
|
|
f,
|
|
|
|
|
"UnitQuaternion angle: {} − axis: (undefined)",
|
|
|
|
|
self.angle()
|
|
|
|
|
)
|
2016-12-05 05:44:42 +08:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
2017-02-13 01:17:09 +08:00
|
|
|
|
|
2018-05-19 21:41:58 +08:00
|
|
|
|
impl<N: Real + AbsDiffEq<Epsilon = N>> AbsDiffEq for UnitQuaternion<N> {
|
2017-02-13 01:17:09 +08:00
|
|
|
|
type Epsilon = N;
|
|
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
|
fn default_epsilon() -> Self::Epsilon {
|
|
|
|
|
N::default_epsilon()
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#[inline]
|
2018-05-19 21:41:58 +08:00
|
|
|
|
fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
|
|
|
|
|
self.as_ref().abs_diff_eq(other.as_ref(), epsilon)
|
2017-02-13 01:17:09 +08:00
|
|
|
|
}
|
2018-05-19 21:41:58 +08:00
|
|
|
|
}
|
2017-02-13 01:17:09 +08:00
|
|
|
|
|
2018-05-19 21:41:58 +08:00
|
|
|
|
impl<N: Real + RelativeEq<Epsilon = N>> RelativeEq for UnitQuaternion<N> {
|
2017-02-13 01:17:09 +08:00
|
|
|
|
#[inline]
|
2018-05-19 21:41:58 +08:00
|
|
|
|
fn default_max_relative() -> Self::Epsilon {
|
|
|
|
|
N::default_max_relative()
|
2017-02-13 01:17:09 +08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#[inline]
|
2018-02-02 19:26:35 +08:00
|
|
|
|
fn relative_eq(
|
|
|
|
|
&self,
|
|
|
|
|
other: &Self,
|
|
|
|
|
epsilon: Self::Epsilon,
|
|
|
|
|
max_relative: Self::Epsilon,
|
2018-10-22 13:00:10 +08:00
|
|
|
|
) -> bool
|
|
|
|
|
{
|
2018-02-02 19:26:35 +08:00
|
|
|
|
self.as_ref()
|
|
|
|
|
.relative_eq(other.as_ref(), epsilon, max_relative)
|
2017-02-13 01:17:09 +08:00
|
|
|
|
}
|
2018-05-19 21:41:58 +08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
impl<N: Real + UlpsEq<Epsilon = N>> UlpsEq for UnitQuaternion<N> {
|
|
|
|
|
#[inline]
|
|
|
|
|
fn default_max_ulps() -> u32 {
|
|
|
|
|
N::default_max_ulps()
|
|
|
|
|
}
|
2017-02-13 01:17:09 +08:00
|
|
|
|
|
|
|
|
|
#[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)
|
|
|
|
|
}
|
|
|
|
|
}
|