Skip to main content

cgmath/
quaternion.rs

1// Copyright 2013-2014 The CGMath Developers. For a full listing of the authors,
2// refer to the Cargo.toml file at the top-level directory of this distribution.
3//
4// Licensed under the Apache License, Version 2.0 (the "License");
5// you may not use this file except in compliance with the License.
6// You may obtain a copy of the License at
7//
8//     http://www.apache.org/licenses/LICENSE-2.0
9//
10// Unless required by applicable law or agreed to in writing, software
11// distributed under the License is distributed on an "AS IS" BASIS,
12// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13// See the License for the specific language governing permissions and
14// limitations under the License.
15
16use std::iter;
17use std::mem;
18use std::ops::*;
19
20use num_traits::{cast, NumCast};
21#[cfg(feature = "rand")]
22use rand::{
23    distributions::{Distribution, Standard},
24    Rng,
25};
26
27use structure::*;
28
29use angle::Rad;
30use approx;
31use euler::Euler;
32use matrix::{Matrix3, Matrix4};
33use num::BaseFloat;
34use point::Point3;
35use rotation::{Basis3, Rotation, Rotation3};
36use vector::Vector3;
37
38#[cfg(feature = "mint")]
39use mint;
40
41/// A [quaternion](https://en.wikipedia.org/wiki/Quaternion) in scalar/vector
42/// form.
43///
44/// This type is marked as `#[repr(C)]`.
45#[repr(C)]
46#[derive(Copy, Clone, Debug, PartialEq)]
47#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
48pub struct Quaternion<S> {
49    /// The vector part of the quaternion.
50    pub v: Vector3<S>,
51    /// The scalar part of the quaternion.
52    pub s: S,
53}
54
55impl<S> Quaternion<S> {
56    /// Construct a new quaternion from one scalar component and three
57    /// imaginary components.
58    #[inline]
59    pub const fn new(w: S, xi: S, yj: S, zk: S) -> Quaternion<S> {
60        Quaternion::from_sv(w, Vector3::new(xi, yj, zk))
61    }
62
63    /// Construct a new quaternion from a scalar and a vector.
64    #[inline]
65    pub const fn from_sv(s: S, v: Vector3<S>) -> Quaternion<S> {
66        Quaternion { s: s, v: v }
67    }
68}
69
70impl<S: BaseFloat> Quaternion<S> {
71    /// Construct a new quaternion as a closest arc between two vectors
72    ///
73    /// Return the closest rotation that turns `src` vector into `dst`.
74    ///
75    /// - [Related StackOverflow question]
76    ///   (http://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another)
77    /// - [Ogre implementation for normalized vectors]
78    ///   (https://bitbucket.org/sinbad/ogre/src/9db75e3ba05c/OgreMain/include/OgreVector3.h?fileviewer=file-view-default#cl-651)
79    pub fn from_arc(
80        src: Vector3<S>,
81        dst: Vector3<S>,
82        fallback: Option<Vector3<S>>,
83    ) -> Quaternion<S> {
84        let mag_avg = (src.magnitude2() * dst.magnitude2()).sqrt();
85        let dot = src.dot(dst);
86        if ulps_eq!(dot, &mag_avg) {
87            Quaternion::<S>::one()
88        } else if ulps_eq!(dot, &-mag_avg) {
89            let axis = fallback.unwrap_or_else(|| {
90                let mut v = Vector3::unit_x().cross(src);
91                if ulps_eq!(v, &Zero::zero()) {
92                    v = Vector3::unit_y().cross(src);
93                }
94                v.normalize()
95            });
96            Quaternion::from_axis_angle(axis, Rad::turn_div_2())
97        } else {
98            Quaternion::from_sv(mag_avg + dot, src.cross(dst)).normalize()
99        }
100    }
101
102    /// The conjugate of the quaternion.
103    #[inline]
104    pub fn conjugate(self) -> Quaternion<S> {
105        Quaternion::from_sv(self.s, -self.v)
106    }
107
108    /// Do a normalized linear interpolation with `other`, by `amount`.
109    /// 
110    /// This takes the shortest path, so if the quaternions have a negative
111    /// dot product, the interpolation will be between `self` and `-other`.
112    pub fn nlerp(self, mut other: Quaternion<S>, amount: S) -> Quaternion<S> {
113        if self.dot(other) < S::zero() {
114            other = -other;
115        }
116
117        (self * (S::one() - amount) + other * amount).normalize()
118    }
119
120    /// Spherical Linear Interpolation
121    ///
122    /// Return the spherical linear interpolation between the quaternion and
123    /// `other`. Both quaternions should be normalized first.
124    /// 
125    /// This takes the shortest path, so if the quaternions have a negative
126    /// dot product, the interpolation will be between `self` and `-other`.
127    ///
128    /// # Performance notes
129    ///
130    /// The `acos` operation used in `slerp` is an expensive operation, so
131    /// unless your quaternions are far away from each other it's generally
132    /// more advisable to use `nlerp` when you know your rotations are going
133    /// to be small.
134    ///
135    /// - [Understanding Slerp, Then Not Using It]
136    ///   (http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/)
137    /// - [Arcsynthesis OpenGL tutorial]
138    ///   (http://www.arcsynthesis.org/gltut/Positioning/Tut08%20Interpolation.html)
139    pub fn slerp(self, mut other: Quaternion<S>, amount: S) -> Quaternion<S> {
140        let mut dot = self.dot(other);
141        let dot_threshold: S = cast(0.9995f64).unwrap();
142
143        if dot < S::zero() {
144            other = -other;
145            dot = -dot;
146        }
147
148        // if quaternions are close together use `nlerp`
149        if dot > dot_threshold {
150            self.nlerp(other, amount)
151        } else {
152            // stay within the domain of acos()
153            let robust_dot = dot.min(S::one()).max(-S::one());
154
155            let theta = Rad::acos(robust_dot);
156
157            let scale1 = Rad::sin(theta * (S::one() - amount));
158            let scale2 = Rad::sin(theta * amount);
159
160            (self * scale1 + other * scale2).normalize()
161        }
162    }
163
164    pub fn is_finite(&self) -> bool {
165        self.s.is_finite() && self.v.is_finite()
166    }
167}
168
169impl<S: BaseFloat> Zero for Quaternion<S> {
170    #[inline]
171    fn zero() -> Quaternion<S> {
172        Quaternion::from_sv(S::zero(), Vector3::zero())
173    }
174
175    #[inline]
176    fn is_zero(&self) -> bool {
177        ulps_eq!(self, &Quaternion::<S>::zero())
178    }
179}
180
181impl<S: BaseFloat> One for Quaternion<S> {
182    #[inline]
183    fn one() -> Quaternion<S> {
184        Quaternion::from_sv(S::one(), Vector3::zero())
185    }
186}
187
188impl<S: BaseFloat> iter::Sum<Quaternion<S>> for Quaternion<S> {
189    #[inline]
190    fn sum<I: Iterator<Item = Quaternion<S>>>(iter: I) -> Quaternion<S> {
191        iter.fold(Quaternion::<S>::zero(), Add::add)
192    }
193}
194
195impl<'a, S: 'a + BaseFloat> iter::Sum<&'a Quaternion<S>> for Quaternion<S> {
196    #[inline]
197    fn sum<I: Iterator<Item = &'a Quaternion<S>>>(iter: I) -> Quaternion<S> {
198        iter.fold(Quaternion::<S>::zero(), Add::add)
199    }
200}
201
202impl<S: BaseFloat> iter::Product<Quaternion<S>> for Quaternion<S> {
203    #[inline]
204    fn product<I: Iterator<Item = Quaternion<S>>>(iter: I) -> Quaternion<S> {
205        iter.fold(Quaternion::<S>::one(), Mul::mul)
206    }
207}
208
209impl<'a, S: 'a + BaseFloat> iter::Product<&'a Quaternion<S>> for Quaternion<S> {
210    #[inline]
211    fn product<I: Iterator<Item = &'a Quaternion<S>>>(iter: I) -> Quaternion<S> {
212        iter.fold(Quaternion::<S>::one(), Mul::mul)
213    }
214}
215
216impl<S: BaseFloat> VectorSpace for Quaternion<S> {
217    type Scalar = S;
218}
219
220impl<S: BaseFloat> MetricSpace for Quaternion<S> {
221    type Metric = S;
222
223    #[inline]
224    fn distance2(self, other: Self) -> S {
225        (other - self).magnitude2()
226    }
227}
228
229impl<S: NumCast + Copy> Quaternion<S> {
230    /// Component-wise casting to another type.
231    pub fn cast<T: BaseFloat>(&self) -> Option<Quaternion<T>> {
232        let s = match NumCast::from(self.s) {
233            Some(s) => s,
234            None => return None,
235        };
236        let v = match self.v.cast() {
237            Some(v) => v,
238            None => return None,
239        };
240        Some(Quaternion::from_sv(s, v))
241    }
242}
243
244impl<S: BaseFloat> InnerSpace for Quaternion<S> {
245    #[inline]
246    default_fn!( dot(self, other: Quaternion<S>) -> S {
247        self.s * other.s + self.v.dot(other.v)
248    } );
249}
250
251impl<A> From<Euler<A>> for Quaternion<A::Unitless>
252where
253    A: Angle + Into<Rad<<A as Angle>::Unitless>>,
254{
255    fn from(src: Euler<A>) -> Quaternion<A::Unitless> {
256        // Euclidean Space has an Euler to quat equation, but it is for a different order (YXZ):
257        // http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm
258        // Page A-2 here has the formula for XYZ:
259        // http://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770024290.pdf
260
261        let half = cast(0.5f64).unwrap();
262        let (s_x, c_x) = Rad::sin_cos(src.x.into() * half);
263        let (s_y, c_y) = Rad::sin_cos(src.y.into() * half);
264        let (s_z, c_z) = Rad::sin_cos(src.z.into() * half);
265
266        Quaternion::new(
267            -s_x * s_y * s_z + c_x * c_y * c_z,
268            s_x * c_y * c_z + s_y * s_z * c_x,
269            -s_x * s_z * c_y + s_y * c_x * c_z,
270            s_x * s_y * c_z + s_z * c_x * c_y,
271        )
272    }
273}
274
275impl_operator!(<S: BaseFloat> Neg for Quaternion<S> {
276    fn neg(quat) -> Quaternion<S> {
277        Quaternion::from_sv(-quat.s, -quat.v)
278    }
279});
280
281impl_operator!(<S: BaseFloat> Mul<S> for Quaternion<S> {
282    fn mul(lhs, rhs) -> Quaternion<S> {
283        Quaternion::from_sv(lhs.s * rhs, lhs.v * rhs)
284    }
285});
286
287impl_assignment_operator!(<S: BaseFloat> MulAssign<S> for Quaternion<S> {
288    fn mul_assign(&mut self, scalar) { self.s *= scalar; self.v *= scalar; }
289});
290
291impl_operator!(<S: BaseFloat> Div<S> for Quaternion<S> {
292    fn div(lhs, rhs) -> Quaternion<S> {
293        Quaternion::from_sv(lhs.s / rhs, lhs.v / rhs)
294    }
295});
296
297impl_assignment_operator!(<S: BaseFloat> DivAssign<S> for Quaternion<S> {
298    fn div_assign(&mut self, scalar) { self.s /= scalar; self.v /= scalar; }
299});
300
301impl_operator!(<S: BaseFloat> Rem<S> for Quaternion<S> {
302    fn rem(lhs, rhs) -> Quaternion<S> {
303        Quaternion::from_sv(lhs.s % rhs, lhs.v % rhs)
304    }
305});
306
307impl_assignment_operator!(<S: BaseFloat> RemAssign<S> for Quaternion<S> {
308    fn rem_assign(&mut self, scalar) { self.s %= scalar; self.v %= scalar; }
309});
310
311impl_operator!(<S: BaseFloat> Mul<Vector3<S> > for Quaternion<S> {
312    fn mul(lhs, rhs) -> Vector3<S> {{
313        let rhs = rhs.clone();
314        let two: S = cast(2i8).unwrap();
315        let tmp = lhs.v.cross(rhs) + (rhs * lhs.s);
316        (lhs.v.cross(tmp) * two) + rhs
317    }}
318});
319
320impl_operator!(<S: BaseFloat> Add<Quaternion<S> > for Quaternion<S> {
321    fn add(lhs, rhs) -> Quaternion<S> {
322        Quaternion::from_sv(lhs.s + rhs.s, lhs.v + rhs.v)
323    }
324});
325
326impl_assignment_operator!(<S: BaseFloat> AddAssign<Quaternion<S> > for Quaternion<S> {
327    fn add_assign(&mut self, other) { self.s += other.s; self.v += other.v; }
328});
329
330impl_operator!(<S: BaseFloat> Sub<Quaternion<S> > for Quaternion<S> {
331    fn sub(lhs, rhs) -> Quaternion<S> {
332        Quaternion::from_sv(lhs.s - rhs.s, lhs.v - rhs.v)
333    }
334});
335
336impl_assignment_operator!(<S: BaseFloat> SubAssign<Quaternion<S> > for Quaternion<S> {
337    fn sub_assign(&mut self, other) { self.s -= other.s; self.v -= other.v; }
338});
339
340impl_operator!(<S: BaseFloat> Mul<Quaternion<S> > for Quaternion<S> {
341    fn mul(lhs, rhs) -> Quaternion<S> {
342        Quaternion::new(
343            lhs.s * rhs.s - lhs.v.x * rhs.v.x - lhs.v.y * rhs.v.y - lhs.v.z * rhs.v.z,
344            lhs.s * rhs.v.x + lhs.v.x * rhs.s + lhs.v.y * rhs.v.z - lhs.v.z * rhs.v.y,
345            lhs.s * rhs.v.y + lhs.v.y * rhs.s + lhs.v.z * rhs.v.x - lhs.v.x * rhs.v.z,
346            lhs.s * rhs.v.z + lhs.v.z * rhs.s + lhs.v.x * rhs.v.y - lhs.v.y * rhs.v.x,
347        )
348    }
349});
350
351macro_rules! impl_scalar_mul {
352    ($S:ident) => {
353        impl_operator!(Mul<Quaternion<$S>> for $S {
354            fn mul(scalar, quat) -> Quaternion<$S> {
355                Quaternion::from_sv(scalar * quat.s, scalar * quat.v)
356            }
357        });
358    };
359}
360
361macro_rules! impl_scalar_div {
362    ($S:ident) => {
363        impl_operator!(Div<Quaternion<$S>> for $S {
364            fn div(scalar, quat) -> Quaternion<$S> {
365                Quaternion::from_sv(scalar / quat.s, scalar / quat.v)
366            }
367        });
368    };
369}
370
371impl_scalar_mul!(f32);
372impl_scalar_mul!(f64);
373impl_scalar_div!(f32);
374impl_scalar_div!(f64);
375
376impl<S: BaseFloat> approx::AbsDiffEq for Quaternion<S> {
377    type Epsilon = S::Epsilon;
378
379    #[inline]
380    fn default_epsilon() -> S::Epsilon {
381        S::default_epsilon()
382    }
383
384    #[inline]
385    fn abs_diff_eq(&self, other: &Self, epsilon: S::Epsilon) -> bool {
386        S::abs_diff_eq(&self.s, &other.s, epsilon)
387            && Vector3::abs_diff_eq(&self.v, &other.v, epsilon)
388    }
389}
390
391impl<S: BaseFloat> approx::RelativeEq for Quaternion<S> {
392    #[inline]
393    fn default_max_relative() -> S::Epsilon {
394        S::default_max_relative()
395    }
396
397    #[inline]
398    fn relative_eq(&self, other: &Self, epsilon: S::Epsilon, max_relative: S::Epsilon) -> bool {
399        S::relative_eq(&self.s, &other.s, epsilon, max_relative)
400            && Vector3::relative_eq(&self.v, &other.v, epsilon, max_relative)
401    }
402}
403
404impl<S: BaseFloat> approx::UlpsEq for Quaternion<S> {
405    #[inline]
406    fn default_max_ulps() -> u32 {
407        S::default_max_ulps()
408    }
409
410    #[inline]
411    fn ulps_eq(&self, other: &Self, epsilon: S::Epsilon, max_ulps: u32) -> bool {
412        S::ulps_eq(&self.s, &other.s, epsilon, max_ulps)
413            && Vector3::ulps_eq(&self.v, &other.v, epsilon, max_ulps)
414    }
415}
416
417impl<S: BaseFloat> From<Quaternion<S>> for Matrix3<S> {
418    /// Convert the quaternion to a 3 x 3 rotation matrix.
419    fn from(quat: Quaternion<S>) -> Matrix3<S> {
420        let x2 = quat.v.x + quat.v.x;
421        let y2 = quat.v.y + quat.v.y;
422        let z2 = quat.v.z + quat.v.z;
423
424        let xx2 = x2 * quat.v.x;
425        let xy2 = x2 * quat.v.y;
426        let xz2 = x2 * quat.v.z;
427
428        let yy2 = y2 * quat.v.y;
429        let yz2 = y2 * quat.v.z;
430        let zz2 = z2 * quat.v.z;
431
432        let sy2 = y2 * quat.s;
433        let sz2 = z2 * quat.s;
434        let sx2 = x2 * quat.s;
435
436        #[cfg_attr(rustfmt, rustfmt_skip)]
437        Matrix3::new(
438            S::one() - yy2 - zz2, xy2 + sz2, xz2 - sy2,
439            xy2 - sz2, S::one() - xx2 - zz2, yz2 + sx2,
440            xz2 + sy2, yz2 - sx2, S::one() - xx2 - yy2,
441        )
442    }
443}
444
445impl<S: BaseFloat> From<Quaternion<S>> for Matrix4<S> {
446    /// Convert the quaternion to a 4 x 4 rotation matrix.
447    fn from(quat: Quaternion<S>) -> Matrix4<S> {
448        let x2 = quat.v.x + quat.v.x;
449        let y2 = quat.v.y + quat.v.y;
450        let z2 = quat.v.z + quat.v.z;
451
452        let xx2 = x2 * quat.v.x;
453        let xy2 = x2 * quat.v.y;
454        let xz2 = x2 * quat.v.z;
455
456        let yy2 = y2 * quat.v.y;
457        let yz2 = y2 * quat.v.z;
458        let zz2 = z2 * quat.v.z;
459
460        let sy2 = y2 * quat.s;
461        let sz2 = z2 * quat.s;
462        let sx2 = x2 * quat.s;
463
464        #[cfg_attr(rustfmt, rustfmt_skip)]
465        Matrix4::new(
466            S::one() - yy2 - zz2, xy2 + sz2, xz2 - sy2, S::zero(),
467            xy2 - sz2, S::one() - xx2 - zz2, yz2 + sx2, S::zero(),
468            xz2 + sy2, yz2 - sx2, S::one() - xx2 - yy2, S::zero(),
469            S::zero(), S::zero(), S::zero(), S::one(),
470        )
471    }
472}
473
474// Quaternion Rotation impls
475
476impl<S: BaseFloat> From<Quaternion<S>> for Basis3<S> {
477    #[inline]
478    fn from(quat: Quaternion<S>) -> Basis3<S> {
479        Basis3::from_quaternion(&quat)
480    }
481}
482
483impl<S: BaseFloat> Rotation for Quaternion<S> {
484    type Space = Point3<S>;
485
486    #[inline]
487    fn look_at(dir: Vector3<S>, up: Vector3<S>) -> Quaternion<S> {
488        Matrix3::look_to_lh(dir, up).into()
489    }
490
491    #[inline]
492    fn between_vectors(a: Vector3<S>, b: Vector3<S>) -> Quaternion<S> {
493        // http://stackoverflow.com/a/11741520/2074937 see 'Half-Way Quaternion Solution'
494
495        let k_cos_theta = a.dot(b);
496
497        // same direction
498        if ulps_eq!(k_cos_theta, S::one()) {
499            return Quaternion::<S>::one();
500        }
501
502        let k = (a.magnitude2() * b.magnitude2()).sqrt();
503
504        // opposite direction
505        if ulps_eq!(k_cos_theta / k, -S::one()) {
506            let mut orthogonal = a.cross(Vector3::unit_x());
507            if ulps_eq!(orthogonal.magnitude2(), S::zero()) {
508                orthogonal = a.cross(Vector3::unit_y());
509            }
510            return Quaternion::from_sv(S::zero(), orthogonal.normalize());
511        }
512
513        // any other direction
514        Quaternion::from_sv(k + k_cos_theta, a.cross(b)).normalize()
515    }
516
517    /// Evaluate the conjugation of `vec` by `self`.
518    ///
519    /// Note that `self` should be a unit quaternion (i.e. normalized) to represent a 3D rotation.
520    #[inline]
521    fn rotate_vector(&self, vec: Vector3<S>) -> Vector3<S> {
522        self * vec
523    }
524
525    #[inline]
526    fn invert(&self) -> Quaternion<S> {
527        self.conjugate() / self.magnitude2()
528    }
529}
530
531impl<S: BaseFloat> Rotation3 for Quaternion<S> {
532    type Scalar = S;
533
534    #[inline]
535    fn from_axis_angle<A: Into<Rad<S>>>(axis: Vector3<S>, angle: A) -> Quaternion<S> {
536        let (s, c) = Rad::sin_cos(angle.into() * cast(0.5f64).unwrap());
537        Quaternion::from_sv(c, axis * s)
538    }
539}
540
541impl<S: BaseFloat> Into<[S; 4]> for Quaternion<S> {
542    #[inline]
543    fn into(self) -> [S; 4] {
544        match self.into() {
545            (xi, yj, zk, w) => [xi, yj, zk, w],
546        }
547    }
548}
549
550impl<S: BaseFloat> AsRef<[S; 4]> for Quaternion<S> {
551    #[inline]
552    fn as_ref(&self) -> &[S; 4] {
553        // SAFETY: `Quaternion<S>` is `#[repr(C)]` with fields `v: Vector3<S>`
554        // (itself `#[repr(C)]`, 3 `S` fields) then `s: S`, and no padding,
555        // so its layout is byte-identical to `[S; 4]` as `[v.x, v.y, v.z, s]`.
556        unsafe { mem::transmute(self) }
557    }
558}
559
560impl<S: BaseFloat> AsMut<[S; 4]> for Quaternion<S> {
561    #[inline]
562    fn as_mut(&mut self) -> &mut [S; 4] {
563        // SAFETY: see `AsRef` above; `self` is uniquely borrowed here.
564        unsafe { mem::transmute(self) }
565    }
566}
567
568impl<S: BaseFloat> From<[S; 4]> for Quaternion<S> {
569    #[inline]
570    fn from(v: [S; 4]) -> Quaternion<S> {
571        Quaternion::new(v[3], v[0], v[1], v[2])
572    }
573}
574
575impl<'a, S: BaseFloat> From<&'a [S; 4]> for &'a Quaternion<S> {
576    #[inline]
577    fn from(v: &'a [S; 4]) -> &'a Quaternion<S> {
578        // SAFETY: see the `AsRef<[S; 4]>` impl above.
579        unsafe { mem::transmute(v) }
580    }
581}
582
583impl<'a, S: BaseFloat> From<&'a mut [S; 4]> for &'a mut Quaternion<S> {
584    #[inline]
585    fn from(v: &'a mut [S; 4]) -> &'a mut Quaternion<S> {
586        // SAFETY: see the `AsMut<[S; 4]>` impl above.
587        unsafe { mem::transmute(v) }
588    }
589}
590
591impl<S: BaseFloat> Into<(S, S, S, S)> for Quaternion<S> {
592    #[inline]
593    fn into(self) -> (S, S, S, S) {
594        match self {
595            Quaternion {
596                s,
597                v: Vector3 { x, y, z },
598            } => (x, y, z, s),
599        }
600    }
601}
602
603// Runtime layout guard for Quaternion<S> <-> (S, S, S, S), same tripwire
604// approach and same caveats as `tuple_layout_matches!` in macros.rs
605// (docs/unsafe-audit.md UNSAFE-002) -- written by hand rather than via
606// that macro because Quaternion's fields (`v.x`/`v.y`/`v.z`/`s`) aren't a
607// flat field list, they're nested through the `v: Vector3<S>` field.
608//
609// SAFETY: `addr_of!` never reads the pointee and never creates a
610// reference (including through the nested `.v.x`/`.v.y`/`.v.z`
611// projections), so this is sound on uninitialized `MaybeUninit` storage.
612//
613// Written as a chain of individually-compared scalar offsets, not as
614// two `[usize; 4]` arrays compared with `==` -- see the comment on
615// `tuple_layout_matches!` in macros.rs for why (the array form was
616// empirically confirmed not to constant-fold away in release builds;
617// this form was).
618#[inline]
619fn quaternion_tuple_layout_matches<S>() -> bool {
620    use std::mem::{align_of, size_of, MaybeUninit};
621    use std::ptr::addr_of;
622
623    size_of::<Quaternion<S>>() == size_of::<(S, S, S, S)>()
624        && align_of::<Quaternion<S>>() == align_of::<(S, S, S, S)>()
625        && {
626            let q = MaybeUninit::<Quaternion<S>>::uninit();
627            let q_base = q.as_ptr() as usize;
628            let t = MaybeUninit::<(S, S, S, S)>::uninit();
629            let t_base = t.as_ptr() as usize;
630            unsafe {
631                (addr_of!((*q.as_ptr()).v.x) as usize - q_base)
632                    == (addr_of!((*t.as_ptr()).0) as usize - t_base)
633                    && (addr_of!((*q.as_ptr()).v.y) as usize - q_base)
634                        == (addr_of!((*t.as_ptr()).1) as usize - t_base)
635                    && (addr_of!((*q.as_ptr()).v.z) as usize - q_base)
636                        == (addr_of!((*t.as_ptr()).2) as usize - t_base)
637                    && (addr_of!((*q.as_ptr()).s) as usize - q_base)
638                        == (addr_of!((*t.as_ptr()).3) as usize - t_base)
639            }
640        }
641}
642
643macro_rules! assert_quaternion_tuple_layout {
644    ($S:ty) => {
645        assert!(
646            quaternion_tuple_layout_matches::<$S>(),
647            "cgmath-next: internal invariant violated -- Quaternion<{}> and \
648             (S, S, S, S) have diverged in memory layout on this platform; \
649             refusing to transmute (see docs/unsafe-audit.md UNSAFE-002)",
650            stringify!($S)
651        );
652    };
653}
654
655impl<S: BaseFloat> AsRef<(S, S, S, S)> for Quaternion<S> {
656    #[inline]
657    fn as_ref(&self) -> &(S, S, S, S) {
658        // SAFETY: guarded -- see `quaternion_tuple_layout_matches` above
659        // and docs/unsafe-audit.md UNSAFE-002.
660        assert_quaternion_tuple_layout!(S);
661        unsafe { mem::transmute(self) }
662    }
663}
664
665impl<S: BaseFloat> AsMut<(S, S, S, S)> for Quaternion<S> {
666    #[inline]
667    fn as_mut(&mut self) -> &mut (S, S, S, S) {
668        // SAFETY: see `AsRef` above (docs/unsafe-audit.md UNSAFE-002).
669        assert_quaternion_tuple_layout!(S);
670        unsafe { mem::transmute(self) }
671    }
672}
673
674impl<S: BaseFloat> From<(S, S, S, S)> for Quaternion<S> {
675    #[inline]
676    fn from(v: (S, S, S, S)) -> Quaternion<S> {
677        match v {
678            (xi, yj, zk, w) => Quaternion::new(w, xi, yj, zk),
679        }
680    }
681}
682
683impl<'a, S: BaseFloat> From<&'a (S, S, S, S)> for &'a Quaternion<S> {
684    #[inline]
685    fn from(v: &'a (S, S, S, S)) -> &'a Quaternion<S> {
686        // SAFETY: see the `AsRef<(S, S, S, S)>` impl above
687        // (docs/unsafe-audit.md UNSAFE-002).
688        assert_quaternion_tuple_layout!(S);
689        unsafe { mem::transmute(v) }
690    }
691}
692
693impl<'a, S: BaseFloat> From<&'a mut (S, S, S, S)> for &'a mut Quaternion<S> {
694    #[inline]
695    fn from(v: &'a mut (S, S, S, S)) -> &'a mut Quaternion<S> {
696        // SAFETY: see the `AsMut<(S, S, S, S)>` impl above
697        // (docs/unsafe-audit.md UNSAFE-002).
698        assert_quaternion_tuple_layout!(S);
699        unsafe { mem::transmute(v) }
700    }
701}
702
703macro_rules! index_operators {
704    ($S:ident, $Output:ty, $I:ty) => {
705        impl<$S: BaseFloat> Index<$I> for Quaternion<$S> {
706            type Output = $Output;
707
708            #[inline]
709            fn index<'a>(&'a self, i: $I) -> &'a $Output {
710                let v: &[$S; 4] = self.as_ref();
711                &v[i]
712            }
713        }
714
715        impl<$S: BaseFloat> IndexMut<$I> for Quaternion<$S> {
716            #[inline]
717            fn index_mut<'a>(&'a mut self, i: $I) -> &'a mut $Output {
718                let v: &mut [$S; 4] = self.as_mut();
719                &mut v[i]
720            }
721        }
722    };
723}
724
725index_operators!(S, S, usize);
726index_operators!(S, [S], Range<usize>);
727index_operators!(S, [S], RangeTo<usize>);
728index_operators!(S, [S], RangeFrom<usize>);
729index_operators!(S, [S], RangeFull);
730
731#[cfg(feature = "rand")]
732impl<S> Distribution<Quaternion<S>> for Standard
733where
734    Standard: Distribution<S>,
735    Standard: Distribution<Vector3<S>>,
736    S: BaseFloat,
737{
738    #[inline]
739    fn sample<R: Rng + ?Sized>(&self, rng: &mut R) -> Quaternion<S> {
740        Quaternion::from_sv(rng.gen(), rng.gen())
741    }
742}
743
744#[cfg(feature = "mint")]
745impl<S> From<mint::Quaternion<S>> for Quaternion<S> {
746    fn from(q: mint::Quaternion<S>) -> Self {
747        Quaternion {
748            s: q.s,
749            v: q.v.into(),
750        }
751    }
752}
753
754#[cfg(feature = "mint")]
755impl<S: Clone> Into<mint::Quaternion<S>> for Quaternion<S> {
756    fn into(self) -> mint::Quaternion<S> {
757        mint::Quaternion {
758            s: self.s,
759            v: self.v.into(),
760        }
761    }
762}
763
764#[cfg(test)]
765mod tests {
766    use quaternion::*;
767    use vector::*;
768
769    const QUATERNION: Quaternion<f32> = Quaternion {
770        v: Vector3 {
771            x: 1.0,
772            y: 2.0,
773            z: 3.0,
774        },
775        s: 4.0,
776    };
777
778    #[test]
779    fn test_into() {
780        let v = QUATERNION;
781        {
782            let v: [f32; 4] = v.into();
783            assert_eq!(v, [1.0, 2.0, 3.0, 4.0]);
784        }
785        {
786            let v: (f32, f32, f32, f32) = v.into();
787            assert_eq!(v, (1.0, 2.0, 3.0, 4.0));
788        }
789    }
790
791    #[test]
792    fn test_as_ref() {
793        let v = QUATERNION;
794        {
795            let v: &[f32; 4] = v.as_ref();
796            assert_eq!(v, &[1.0, 2.0, 3.0, 4.0]);
797        }
798        {
799            let v: &(f32, f32, f32, f32) = v.as_ref();
800            assert_eq!(v, &(1.0, 2.0, 3.0, 4.0));
801        }
802    }
803
804    #[test]
805    fn test_as_mut() {
806        let mut v = QUATERNION;
807        {
808            let v: &mut [f32; 4] = v.as_mut();
809            assert_eq!(v, &mut [1.0, 2.0, 3.0, 4.0]);
810        }
811        {
812            let v: &mut (f32, f32, f32, f32) = v.as_mut();
813            assert_eq!(v, &mut (1.0, 2.0, 3.0, 4.0));
814        }
815    }
816
817    #[test]
818    fn test_from() {
819        assert_eq!(Quaternion::from([1.0, 2.0, 3.0, 4.0]), QUATERNION);
820        {
821            let v = &[1.0, 2.0, 3.0, 4.0];
822            let v: &Quaternion<_> = From::from(v);
823            assert_eq!(v, &QUATERNION);
824        }
825        {
826            let v = &mut [1.0, 2.0, 3.0, 4.0];
827            let v: &mut Quaternion<_> = From::from(v);
828            assert_eq!(v, &QUATERNION);
829        }
830        assert_eq!(Quaternion::from((1.0, 2.0, 3.0, 4.0)), QUATERNION);
831        {
832            let v = &(1.0, 2.0, 3.0, 4.0);
833            let v: &Quaternion<_> = From::from(v);
834            assert_eq!(v, &QUATERNION);
835        }
836        {
837            let v = &mut (1.0, 2.0, 3.0, 4.0);
838            let v: &mut Quaternion<_> = From::from(v);
839            assert_eq!(v, &QUATERNION);
840        }
841    }
842
843    #[test]
844    fn test_nlerp_same() {
845        let q = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
846        assert_ulps_eq!(q, q.nlerp(q, 0.1234));
847    }
848
849    #[test]
850    fn test_nlerp_start() {
851        let q = Quaternion::from([0.5f64.sqrt(), 0.0, 0.5f64.sqrt(), 0.0]);
852        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
853        assert_ulps_eq!(q, q.nlerp(r, 0.0));
854    }
855
856    #[test]
857    fn test_nlerp_end() {
858        let q = Quaternion::from([0.5f64.sqrt(), 0.0, 0.5f64.sqrt(), 0.0]);
859        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
860        assert_ulps_eq!(r, q.nlerp(r, 1.0));
861    }
862
863    #[test]
864    fn test_nlerp_half() {
865        let q = Quaternion::from([-0.5, 0.5, 0.5, 0.5]);
866        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
867
868        let expected =
869            Quaternion::from([0.0, 1.0 / 3f64.sqrt(), 1.0 / 3f64.sqrt(), 1.0 / 3f64.sqrt()]);
870        assert_ulps_eq!(expected, q.nlerp(r, 0.5));
871    }
872
873    #[test]
874    fn test_nlerp_quarter() {
875        let q = Quaternion::from([-0.5, 0.5, 0.5, 0.5]);
876        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
877
878        let expected = Quaternion::from([
879            -1.0 / 13f64.sqrt(),
880            2.0 / 13f64.sqrt(),
881            2.0 / 13f64.sqrt(),
882            2.0 / 13f64.sqrt(),
883        ]);
884        assert_ulps_eq!(expected, q.nlerp(r, 0.25));
885    }
886
887    #[test]
888    fn test_nlerp_zero_dot() {
889        let q = Quaternion::from([-0.5, -0.5, 0.5, 0.5]);
890        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
891
892        let expected = Quaternion::from([
893            -1.0 / 10f64.sqrt(),
894            -1.0 / 10f64.sqrt(),
895            2.0 / 10f64.sqrt(),
896            2.0 / 10f64.sqrt(),
897        ]);
898        assert_ulps_eq!(expected, q.nlerp(r, 0.25));
899    }
900
901    #[test]
902    fn test_nlerp_negative_dot() {
903        let q = Quaternion::from([-0.5, -0.5, -0.5, 0.5]);
904        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
905
906        let expected = Quaternion::from([
907            -2.0 / 13f64.sqrt(),
908            -2.0 / 13f64.sqrt(),
909            -2.0 / 13f64.sqrt(),
910            1.0 / 13f64.sqrt(),
911        ]);
912        assert_ulps_eq!(expected, q.nlerp(r, 0.25));
913    }
914
915    #[test]
916    fn test_nlerp_opposite() {
917        let q = Quaternion::from([-0.5, -0.5, -0.5, -0.5]);
918        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
919
920        assert_ulps_eq!(q, q.nlerp(r, 0.25));
921        assert_ulps_eq!(q, q.nlerp(r, 0.75));
922    }
923
924    #[test]
925    fn test_nlerp_extrapolate() {
926        let q = Quaternion::from([-0.5, -0.5, -0.5, 0.5]);
927        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
928
929        let expected = Quaternion::from([
930            -1.0 / 12f64.sqrt(),
931            -1.0 / 12f64.sqrt(),
932            -1.0 / 12f64.sqrt(),
933            3.0 / 12f64.sqrt(),
934        ]);
935        assert_ulps_eq!(expected, q.nlerp(r, -1.0));
936    }
937
938    #[test]
939    fn test_slerp_same() {
940        let q = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
941        assert_ulps_eq!(q, q.slerp(q, 0.1234));
942    }
943
944    #[test]
945    fn test_slerp_start() {
946        let q = Quaternion::from([0.5f64.sqrt(), 0.0, 0.5f64.sqrt(), 0.0]);
947        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
948        assert_ulps_eq!(q, q.slerp(r, 0.0));
949    }
950
951    #[test]
952    fn test_slerp_end() {
953        let q = Quaternion::from([0.5f64.sqrt(), 0.0, 0.5f64.sqrt(), 0.0]);
954        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
955        assert_ulps_eq!(r, q.slerp(r, 1.0));
956    }
957
958    #[test]
959    fn test_slerp_half() {
960        let q = Quaternion::from([-0.5, 0.5, 0.5, 0.5]);
961        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
962
963        let expected =
964            Quaternion::from([0.0, 1.0 / 3f64.sqrt(), 1.0 / 3f64.sqrt(), 1.0 / 3f64.sqrt()]);
965        assert_ulps_eq!(expected, q.slerp(r, 0.5));
966    }
967
968    #[test]
969    fn test_slerp_quarter() {
970        let q = Quaternion::from([-0.5, 0.5, 0.5, 0.5]);
971        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
972
973        let expected = Quaternion::from([
974            -0.2588190451025208,
975            0.5576775358252053,
976            0.5576775358252053,
977            0.5576775358252053,
978        ]);
979        assert_ulps_eq!(expected, q.slerp(r, 0.25));
980    }
981
982    #[test]
983    fn test_slerp_zero_dot() {
984        let q = Quaternion::from([-0.5, -0.5, 0.5, 0.5]);
985        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
986
987        let expected = Quaternion::from([
988            -0.27059805007309845,
989            -0.27059805007309845,
990            0.6532814824381883,
991            0.6532814824381883,
992        ]);
993        assert_ulps_eq!(expected, q.slerp(r, 0.25));
994    }
995
996    #[test]
997    fn test_slerp_negative_dot() {
998        let q = Quaternion::from([-0.5, -0.5, -0.5, 0.5]);
999        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
1000
1001        let expected = Quaternion::from([
1002            -0.5576775358252053,
1003            -0.5576775358252053,
1004            -0.5576775358252053,
1005            0.2588190451025208
1006        ]);
1007        assert_ulps_eq!(expected, q.slerp(r, 0.25));
1008    }
1009
1010    #[test]
1011    fn test_slerp_opposite() {
1012        let q = Quaternion::from([-0.5, -0.5, -0.5, -0.5]);
1013        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
1014
1015        assert_ulps_eq!(q, q.slerp(r, 0.25));
1016        assert_ulps_eq!(q, q.slerp(r, 0.75));
1017    }
1018
1019    #[test]
1020    fn test_slerp_extrapolate() {
1021        let q = Quaternion::from([-0.5, -0.5, -0.5, 0.5]);
1022        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
1023
1024        let expected = Quaternion::from([0.0, 0.0, 0.0, 1.0]);
1025        assert_ulps_eq!(expected, q.slerp(r, -1.0));
1026    }
1027
1028    #[test]
1029    fn test_slerp_regression() {
1030        let a = Quaternion::<f32>::new(0.00052311074, 0.9999999, 0.00014682197, -0.000016342687);
1031        let b = Quaternion::<f32>::new(0.019973433, -0.99980056, -0.00015678025, 0.000013882192);
1032
1033        assert_ulps_eq!(a.slerp(b, 0.5).magnitude(), 1.0);
1034    }
1035}