vek/quaternion.rs
1//! Quaternions are a convenient representation for rotations in 3D spaces.
2
3use approx::{AbsDiffEq, RelativeEq, UlpsEq};
4use num_traits::{real::Real, One, Zero};
5use crate::ops::*;
6use std::ops::Add;
7use std::ops::*;
8
9#[cfg(feature = "bytemuck")]
10use crate::bytemuck;
11
12macro_rules! impl_mul_by_vec {
13 ($Vec3:ident $Vec4:ident) => {
14 /// 3D vectors can be rotated by being premultiplied by a quaternion, **assuming the
15 /// quaternion is normalized**.
16 /// On `Vec4`s, the `w` element is preserved, so you can safely rotate
17 /// points and directions.
18 ///
19 /// ```
20 /// # extern crate vek;
21 /// # #[macro_use] extern crate approx;
22 /// # use vek::{Quaternion, Vec4};
23 /// use std::f32::consts::PI;
24 ///
25 /// # fn main() {
26 /// let v = Vec4::unit_x();
27 ///
28 /// let q = Quaternion::<f32>::identity();
29 /// assert_relative_eq!(q * v, v);
30 ///
31 /// let q = Quaternion::rotation_z(PI);
32 /// assert_relative_eq!(q * v, -v);
33 ///
34 /// let q = Quaternion::rotation_z(PI * 0.5);
35 /// assert_relative_eq!(q * v, Vec4::unit_y());
36 ///
37 /// let q = Quaternion::rotation_z(PI * 1.5);
38 /// assert_relative_eq!(q * v, -Vec4::unit_y());
39 ///
40 /// let angles = 32;
41 /// for i in 0..angles {
42 /// let theta = PI * 2. * (i as f32) / (angles as f32);
43 ///
44 /// // See what rotating unit vectors do for most angles between 0 and 2*PI.
45 /// // It's helpful to picture this as a right-handed coordinate system.
46 ///
47 /// let v = Vec4::unit_y();
48 /// let q = Quaternion::rotation_x(theta);
49 /// assert_relative_eq!(q * v, Vec4::new(0., theta.cos(), theta.sin(), 0.));
50 ///
51 /// let v = Vec4::unit_z();
52 /// let q = Quaternion::rotation_y(theta);
53 /// assert_relative_eq!(q * v, Vec4::new(theta.sin(), 0., theta.cos(), 0.));
54 ///
55 /// let v = Vec4::unit_x();
56 /// let q = Quaternion::rotation_z(theta);
57 /// assert_relative_eq!(q * v, Vec4::new(theta.cos(), theta.sin(), 0., 0.));
58 /// }
59 /// # }
60 /// ```
61 impl<T: Real + Add<T, Output=T>> Mul<$Vec4<T>> for Quaternion<T> {
62 type Output = $Vec4<T>;
63 fn mul(self, rhs: $Vec4<T>) -> Self::Output {
64 let $Vec4 { x, y, z, w } = rhs;
65 let $Vec3 { x, y, z } = self * $Vec3 { x, y, z };
66 $Vec4 { x, y, z, w }
67 }
68 }
69 /// 3D vectors can be rotated by being premultiplied by a quaternion, **assuming the
70 /// quaternion is normalized**.
71 impl<T: Real + Add<T, Output=T>> Mul<$Vec3<T>> for Quaternion<T> {
72 type Output = $Vec3<T>;
73 fn mul(self, rhs: $Vec3<T>) -> Self::Output {
74 let $Vec3 { x, y, z } = rhs;
75 let v = Self { x, y, z, w: T::zero() };
76 let qi = self.conjugate(); // We want the inverse, and assume self is normalized.
77 (self * v * qi).into()
78 }
79 }
80
81 };
82}
83
84macro_rules! quaternion_vec3_vec4 {
85 ($Vec3:ident $Vec4:ident) => {
86
87 impl_mul_by_vec!{$Vec3 $Vec4}
88
89 /// A quaternion can be created directly from a `Vec4`'s `x`, `y`, `z` and `w` elements.
90 /// **You are responsible for ensuring that the resulting quaternion is normalized.**
91 impl<T> From<$Vec4<T>> for Quaternion<T> {
92 fn from(v: $Vec4<T>) -> Self {
93 let $Vec4 { x, y, z, w } = v;
94 Self { x, y, z, w }
95 }
96 }
97 /// A `Vec4` can be created directly from a quaternion's `x`, `y`, `z` and `w` elements.
98 impl<T> From<Quaternion<T>> for $Vec4<T> {
99 fn from(v: Quaternion<T>) -> Self {
100 let Quaternion { x, y, z, w } = v;
101 Self { x, y, z, w }
102 }
103 }
104 /// A `Vec3` can be created directly from a quaternion's `x`, `y` and `z` elements.
105 impl<T> From<Quaternion<T>> for $Vec3<T> {
106 fn from(v: Quaternion<T>) -> Self {
107 let Quaternion { x, y, z, .. } = v;
108 Self { x, y, z }
109 }
110 }
111 };
112}
113
114macro_rules! quaternion_complete_mod {
115 ($mod:ident #[$attrs:meta]) => {
116
117 use crate::vec::$mod::*;
118
119 /// Quaternions are a convenient representation for rotations in 3D spaces.
120 ///
121 /// **IMPORTANT**: Quaternions are only valid as rotations as long as they are
122 /// **normalized** (i.e their magnitude is 1). Most operations assume
123 /// this, instead of normalizing inputs behind your back, so be careful.
124 ///
125 /// They essentially consist of a vector part (`x`, `y`, `z`), and scalar part (`w`).
126 /// For unit quaternions, the vector part is the unit axis of rotation scaled by the sine of
127 /// the half-angle of the rotation, and the scalar part is the cosine of the half-angle.
128 #[derive(Debug, Clone, Copy, Hash, Eq, PartialEq, /*Ord, PartialOrd*/)]
129 #[cfg_attr(feature="serde", derive(Serialize, Deserialize))]
130 #[$attrs]
131 #[allow(missing_docs)]
132 pub struct Quaternion<T> { pub x: T, pub y: T, pub z: T, pub w: T }
133
134 /// The default value for a quaternion is the identity.
135 ///
136 /// ```
137 /// # use vek::Quaternion;
138 /// assert_eq!(Quaternion::<f32>::identity(), Quaternion::default());
139 /// ```
140 impl<T: Zero + One> Default for Quaternion<T> {
141 fn default() -> Self {
142 Self::identity()
143 }
144 }
145
146 impl<T> Quaternion<T> {
147 /// Creates a new quaternion with `x`, `y`, `z` and `w` elements in order.
148 ///
149 /// **You are responsible for ensuring that the resulting quaternion is normalized.**
150 pub fn from_xyzw(x: T, y: T, z: T, w: T) -> Self {
151 Self { x, y, z, w }
152 }
153 /// Creates a new quaternion from a scalar-and-vector pair.
154 ///
155 /// **You are responsible for ensuring that the resulting quaternion is normalized.**
156 pub fn from_scalar_and_vec3<V: Into<Vec3<T>>>(pair: (T, V)) -> Self {
157 let Vec3 { x, y, z } = pair.1.into();
158 Self { x, y, z, w: pair.0 }
159 }
160 /// Converts this quaternion into a scalar-and-vector pair by destructuring.
161 ///
162 /// **Not to be confused with `into_angle_axis()`**.
163 pub fn into_scalar_and_vec3(self) -> (T, Vec3<T>) {
164 let Self { x, y, z, w } = self;
165 (w, Vec3 { x, y, z })
166 }
167 /// Creates a new quaternion with all elements set to zero.
168 ///
169 /// **Be careful: since it has a magnitude equal to zero, it is not
170 /// valid to use for most operations.**
171 pub fn zero() -> Self where T: Zero {
172 Self {
173 x: T::zero(),
174 y: T::zero(),
175 z: T::zero(),
176 w: T::zero(),
177 }
178 }
179 /// Creates the identity quaternion.
180 ///
181 /// ```
182 /// # extern crate vek;
183 /// # #[macro_use] extern crate approx;
184 /// # use vek::Quaternion;
185 /// use std::f32::consts::PI;
186 ///
187 /// # fn main() {
188 /// let id = Quaternion::<f32>::identity();
189 /// assert_eq!(id, Default::default());
190 /// assert_relative_eq!(id, id.conjugate());
191 /// assert_relative_eq!(id, id.inverse());
192 ///
193 /// let q = Quaternion::rotation_y(PI);
194 /// assert_relative_eq!(id * q, q);
195 /// assert_relative_eq!(q * id, q);
196 /// # }
197 /// ```
198 pub fn identity() -> Self where T: Zero + One {
199 Self {
200 x: T::zero(),
201 y: T::zero(),
202 z: T::zero(),
203 w: T::one(),
204 }
205 }
206 /// Gets this quaternion's conjugate (copy with negated vector part).
207 ///
208 /// On normalized quaternions, the conjugate also happens to be the inverse.
209 ///
210 /// ```
211 /// # extern crate vek;
212 /// # #[macro_use] extern crate approx;
213 /// # use vek::Quaternion;
214 /// use std::f32::consts::PI;
215 ///
216 /// # fn main() {
217 /// let p = Quaternion::rotation_x(PI);
218 /// let q = Quaternion::rotation_z(PI);
219 /// assert_relative_eq!((p*q).conjugate(), q.conjugate() * p.conjugate());
220 ///
221 /// // Rotation quaternions are normalized, so their conjugate is also their inverse.
222 /// assert_relative_eq!(q.conjugate(), q.inverse());
223 /// # }
224 /// ```
225 pub fn conjugate(self) -> Self where T: Neg<Output=T> {
226 Self {
227 x: -self.x,
228 y: -self.y,
229 z: -self.z,
230 w: self.w,
231 }
232 }
233 /// Gets this quaternion's inverse, i.e the one that reverses its effect.
234 ///
235 /// On normalized quaternions, the inverse happens to be the conjugate.
236 ///
237 /// ```
238 /// # extern crate vek;
239 /// # #[macro_use] extern crate approx;
240 /// # use vek::Quaternion;
241 /// use std::f32::consts::PI;
242 ///
243 /// # fn main() {
244 /// let rot = Quaternion::rotation_y(PI);
245 /// let inv = rot.inverse();
246 /// assert_relative_eq!(rot*inv, Quaternion::identity());
247 /// assert_relative_eq!(inv*rot, Quaternion::identity());
248 ///
249 /// let p = Quaternion::rotation_x(PI);
250 /// let q = Quaternion::rotation_z(PI);
251 /// assert_relative_eq!((p*q).inverse(), q.inverse() * p.inverse());
252 /// # }
253 /// ```
254 pub fn inverse(self) -> Self where T: Neg<Output=T> + Copy + Add<T, Output=T> + Mul<Output=T> + Div<Output=T> {
255 self.conjugate() / self.into_vec4().magnitude_squared()
256 }
257 /// Gets the dot product between two quaternions.
258 pub fn dot(self, q: Self) -> T
259 where T: Copy + Add<T, Output=T> + Mul<Output=T>
260 {
261 self.into_vec4().dot(q.into_vec4())
262 }
263 /// Gets a normalized copy of this quaternion.
264 pub fn normalized(self) -> Self where T: Real + Add<T, Output=T> {
265 self.into_vec4().normalized().into()
266 }
267 /// Gets this quaternion's magnitude, squared.
268 pub fn magnitude_squared(self) -> T where T: Real + Add<T, Output=T> {
269 self.into_vec4().magnitude_squared().into()
270 }
271 /// Gets this quaternion's magnitude.
272 pub fn magnitude(self) -> T where T: Real + Add<T, Output=T> {
273 self.into_vec4().magnitude().into()
274 }
275
276 /// Creates a quaternion that would rotate a `from` direction to `to`.
277 ///
278 /// ```
279 /// # extern crate vek;
280 /// # #[macro_use] extern crate approx;
281 /// # use vek::{Vec4, Quaternion};
282 ///
283 /// # fn main() {
284 /// let (from, to) = (Vec4::<f32>::unit_x(), Vec4::<f32>::unit_y());
285 /// let q = Quaternion::<f32>::rotation_from_to_3d(from, to);
286 /// assert_relative_eq!(q * from, to);
287 /// assert_relative_eq!(q * Vec4::unit_y(), -Vec4::unit_x());
288 ///
289 /// let (from, to) = (Vec4::<f32>::unit_x(), -Vec4::<f32>::unit_x());
290 /// let q = Quaternion::<f32>::rotation_from_to_3d(from, to);
291 /// assert_relative_eq!(q * from, to);
292 /// # }
293 /// ```
294 pub fn rotation_from_to_3d<V: Into<Vec3<T>>>(from: V, to: V) -> Self
295 where T: Real + Add<T, Output=T>
296 {
297 // From GLM
298 let (from, to) = (from.into(), to.into());
299 let norm_u_norm_v = (from.dot(from) * to.dot(to)).sqrt();
300 let w = norm_u_norm_v + from.dot(to);
301 let (Vec3 { x, y, z }, w) = if w < norm_u_norm_v * T::epsilon() {
302 // If we are here, it is a 180° rotation, which we have to handle.
303 if from.x.abs() > from.z.abs() {
304 (Vec3::new(-from.y, from.x, T::zero()), T::zero())
305 } else {
306 (Vec3::new(T::zero(), -from.z, from.y), T::zero())
307 }
308 } else {
309 (from.cross(to), w)
310 };
311 Self { x, y, z, w }.normalized()
312 }
313
314 /// Creates a quaternion from an angle and axis.
315 /// The axis is not required to be normalized.
316 pub fn rotation_3d<V: Into<Vec3<T>>>(angle_radians: T, axis: V) -> Self
317 where T: Real + Add<T, Output=T>
318 {
319 let axis = axis.into().normalized();
320 let two = T::one() + T::one();
321 let Vec3 { x, y, z } = axis * (angle_radians/two).sin();
322 let w = (angle_radians/two).cos();
323 Self { x, y, z, w }
324 }
325 /// Creates a quaternion from an angle for a rotation around the X axis.
326 pub fn rotation_x(angle_radians: T) -> Self where T: Real + Add<T, Output=T> {
327 Self::rotation_3d(angle_radians, Vec3::unit_x())
328 }
329 /// Creates a quaternion from an angle for a rotation around the Y axis.
330 pub fn rotation_y(angle_radians: T) -> Self where T: Real + Add<T, Output=T> {
331 Self::rotation_3d(angle_radians, Vec3::unit_y())
332 }
333 /// Creates a quaternion from an angle for a rotation around the Y axis.
334 pub fn rotation_z(angle_radians: T) -> Self where T: Real + Add<T, Output=T> {
335 Self::rotation_3d(angle_radians, Vec3::unit_z())
336 }
337 /// Returns this quaternion rotated around the given axis with given angle.
338 /// The axis is not required to be normalized.
339 pub fn rotated_3d<V: Into<Vec3<T>>>(self, angle_radians: T, axis: V) -> Self where T: Real + Add<T, Output=T> {
340 Self::rotation_3d(angle_radians, axis) * self
341 }
342 /// Returns this quaternion rotated around the X axis with given angle.
343 pub fn rotated_x(self, angle_radians: T) -> Self where T: Real + Add<T, Output=T> {
344 Self::rotation_x(angle_radians) * self
345 }
346 /// Returns this quaternion rotated around the Y axis with given angle.
347 pub fn rotated_y(self, angle_radians: T) -> Self where T: Real + Add<T, Output=T> {
348 Self::rotation_y(angle_radians) * self
349 }
350 /// Returns this quaternion rotated around the Z axis with given angle.
351 pub fn rotated_z(self, angle_radians: T) -> Self where T: Real + Add<T, Output=T> {
352 Self::rotation_z(angle_radians) * self
353 }
354 /// Rotates this quaternion around the given axis with given angle.
355 /// The axis is not required to be normalized.
356 pub fn rotate_3d<V: Into<Vec3<T>>>(&mut self, angle_radians: T, axis: V) where T: Real + Add<T, Output=T> {
357 *self = self.rotated_3d(angle_radians, axis);
358 }
359 /// Rotates this quaternion around the X axis with given angle.
360 pub fn rotate_x(&mut self, angle_radians: T) where T: Real + Add<T, Output=T> {
361 *self = self.rotated_x(angle_radians);
362 }
363 /// Rotates this quaternion around the Y axis with given angle.
364 pub fn rotate_y(&mut self, angle_radians: T) where T: Real + Add<T, Output=T> {
365 *self = self.rotated_y(angle_radians);
366 }
367 /// Rotates this quaternion around the Z axis with given angle.
368 pub fn rotate_z(&mut self, angle_radians: T) where T: Real + Add<T, Output=T> {
369 *self = self.rotated_z(angle_radians);
370 }
371
372 /// Convert this quaternion to angle-axis representation,
373 /// **assuming the quaternion is normalized.**
374 ///
375 /// ```
376 /// # extern crate vek;
377 /// # #[macro_use] extern crate approx;
378 /// # use vek::{Quaternion, Vec3};
379 /// use std::f32::consts::PI;
380 ///
381 /// # fn main() {
382 /// let q = Quaternion::rotation_x(PI/2.);
383 /// let (angle, axis) = q.into_angle_axis();
384 /// assert_relative_eq!(angle, PI/2.);
385 /// assert_relative_eq!(axis, Vec3::unit_x());
386 ///
387 /// let angle = PI*4./5.;
388 /// let axis = Vec3::new(1_f32, 2., 3.);
389 /// let q = Quaternion::rotation_3d(angle, axis);
390 /// let (a, v) = q.into_angle_axis();
391 /// assert_relative_eq!(a, angle);
392 /// assert_relative_eq!(v, axis.normalized());
393 /// # }
394 /// ```
395 pub fn into_angle_axis(self) -> (T, Vec3<T>) where T: Real {
396 // http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/
397 // Also, Q57 of matrix-quaternion FAQ
398 let Self { x, y, z, w } = self;
399 let angle = w.acos();
400 let angle = angle + angle;
401 let s = (T::one() - w*w).sqrt();
402 let axis = if s < T::epsilon() {
403 Vec3::unit_x() // Any axis would do
404 } else {
405 Vec3 { x, y, z } / s
406 };
407 (angle, axis)
408 }
409
410 /// Converts this quaternion to a `Vec4` by destructuring.
411 pub fn into_vec4(self) -> Vec4<T> {
412 self.into()
413 }
414 /// Creates a quaternion from a `Vec4` by destructuring.
415 /// **You are responsible for ensuring that the resulting quaternion is normalized.**
416 pub fn from_vec4(v: Vec4<T>) -> Self {
417 v.into()
418 }
419 /// Converts this quaternion to a `Vec3` by destructuring, dropping the `w` element.
420 pub fn into_vec3(self) -> Vec3<T> {
421 self.into()
422 }
423 }
424
425 #[cfg(feature = "mint")]
426 impl<T> From<mint::Quaternion<T>> for Quaternion<T> {
427 fn from(q: mint::Quaternion<T>) -> Self {
428 let mint::Quaternion { s, v } = q;
429 Self::from_scalar_and_vec3((s, v))
430 }
431 }
432
433 #[cfg(feature = "mint")]
434 impl<T> Into<mint::Quaternion<T>> for Quaternion<T> {
435 fn into(self) -> mint::Quaternion<T> {
436 let (s, v) = self.into_scalar_and_vec3();
437 mint::Quaternion { s, v: v.into() }
438 }
439 }
440
441 /// The `Lerp` implementation for quaternion is the "Normalized LERP".
442 impl<T, Factor> Lerp<Factor> for Quaternion<T>
443 where T: Lerp<Factor,Output=T> + Add<T, Output=T> + Real,
444 Factor: Copy
445 {
446 type Output = Self;
447 fn lerp_unclamped_precise(from: Self, to: Self, factor: Factor) -> Self {
448 let (from, to) = (from.into_vec4(), to.into_vec4());
449 Lerp::lerp_unclamped_precise(from, to, factor).normalized().into()
450 }
451 fn lerp_unclamped(from: Self, to: Self, factor: Factor) -> Self {
452 let (from, to) = (from.into_vec4(), to.into_vec4());
453 Lerp::lerp_unclamped(from, to, factor).normalized().into()
454 }
455 }
456 /// The `Lerp` implementation for quaternion is the "Normalized LERP".
457 impl<'a, T, Factor> Lerp<Factor> for &'a Quaternion<T>
458 // Real implies Copy, so no &'a T here.
459 where T: Lerp<Factor,Output=T> + Add<T, Output=T> + Real,
460 Factor: Copy
461 {
462 type Output = Quaternion<T>;
463 fn lerp_unclamped_precise(from: Self, to: Self, factor: Factor) -> Quaternion<T> {
464 Lerp::lerp_unclamped_precise(*from, *to, factor)
465 }
466 fn lerp_unclamped(from: Self, to: Self, factor: Factor) -> Quaternion<T> {
467 Lerp::lerp_unclamped(*from, *to, factor)
468 }
469 }
470 impl<T> Quaternion<T>
471 where T: Copy + One + Mul<Output=T> + Sub<Output=T> + MulAdd<T,T,Output=T>
472 {
473 /// Performs linear interpolation **without normalizing the result**,
474 /// using an implementation that supposedly yields a more precise result.
475 ///
476 /// This is probably not what you're looking for.
477 /// For an implementation that normalizes the result (which is more commonly wanted), see the `Lerp` implementation.
478 pub fn lerp_precise_unnormalized(from: Self, to: Self, factor: T) -> Self where T: Clamp + Zero {
479 Self::lerp_unclamped_precise_unnormalized(from, to, factor.clamped01())
480 }
481 /// Performs linear interpolation **without normalizing the result** and without
482 /// implicitly constraining `factor` to be between 0 and 1,
483 /// using an implementation that supposedly yields a more precise result.
484 ///
485 /// This is probably not what you're looking for.
486 /// For an implementation that normalizes the result (which is more commonly wanted), see the `Lerp` implementation.
487 pub fn lerp_unclamped_precise_unnormalized(from: Self, to: Self, factor: T) -> Self {
488 Vec4::lerp_unclamped_precise(from.into(), to.into(), factor).into()
489 }
490 }
491 impl<T> Quaternion<T>
492 where T: Copy + Sub<Output=T> + MulAdd<T,T,Output=T>
493 {
494 /// Performs linear interpolation **without normalizing the result**.
495 ///
496 /// This is probably not what you're looking for.
497 /// For an implementation that normalizes the result (which is more commonly wanted), see the `Lerp` implementation.
498 pub fn lerp_unnormalized(from: Self, to: Self, factor: T) -> Self where T: Clamp + Zero + One {
499 Self::lerp_unclamped_unnormalized(from, to, factor.clamped01())
500 }
501 /// Performs linear interpolation **without normalizing the result** and without
502 /// implicitly constraining `factor` to be between 0 and 1.
503 ///
504 /// This is probably not what you're looking for.
505 /// For an implementation that normalizes the result (which is more commonly wanted), see the `Lerp` implementation.
506 pub fn lerp_unclamped_unnormalized(from: Self, to: Self, factor: T) -> Self {
507 Vec4::lerp_unclamped(from.into(), to.into(), factor).into()
508 }
509 }
510 impl<T> Quaternion<T>
511 where T: Lerp<T,Output=T> + Add<T, Output=T> + Real
512 {
513 /// Performs spherical linear interpolation without implictly constraining `factor` to
514 /// be between 0 and 1.
515 ///
516 /// ```
517 /// # extern crate vek;
518 /// # #[macro_use] extern crate approx;
519 /// # use vek::Quaternion;
520 /// use std::f32::consts::PI;
521 ///
522 /// # fn main() {
523 /// let from = Quaternion::rotation_z(0_f32);
524 /// let to = Quaternion::rotation_z(PI*9./10.);
525 ///
526 /// let angles = 32;
527 /// for i in 0..angles {
528 /// let factor = (i as f32) / (angles as f32);
529 /// let expected = Quaternion::rotation_z(factor * PI*9./10.);
530 /// let slerp = Quaternion::slerp(from, to, factor);
531 /// assert_relative_eq!(slerp, expected);
532 /// }
533 /// # }
534 /// ```
535 // From GLM's source code.
536 pub fn slerp_unclamped(from: Self, mut to: Self, factor: T) -> Self {
537 let mut cos_theta = from.dot(to);
538 // If cosTheta < 0, the interpolation will take the long way around the sphere.
539 // To fix this, one quat must be negated.
540 if cos_theta < T::zero() {
541 to = -to;
542 cos_theta = -cos_theta;
543 }
544
545 // Perform a linear interpolation when cosTheta is close to 1 to avoid side effect of sin(angle) becoming a zero denominator
546 if cos_theta > T::one() - T::epsilon() {
547 return Self::lerp_unclamped(from, to, factor);
548 }
549 let angle = cos_theta.acos();
550 (from * ((T::one() - factor) * angle).sin() + to * (factor * angle).sin()) / angle.sin()
551 }
552 /// Perform spherical linear interpolation, constraining `factor` to
553 /// be between 0 and 1.
554 pub fn slerp(from: Self, to: Self, factor: T) -> Self where T: Clamp {
555 Slerp::slerp(from, to, factor)
556 }
557 }
558
559 impl<T, Factor> Slerp<Factor> for Quaternion<T>
560 where T: Lerp<T,Output=T> + Add<T, Output=T> + Real,
561 Factor: Into<T>
562 {
563 type Output = Self;
564 fn slerp_unclamped(from: Self, to: Self, factor: Factor) -> Self {
565 Self::slerp_unclamped(from, to, factor.into())
566 }
567 }
568 impl<'a, T, Factor> Slerp<Factor> for &'a Quaternion<T>
569 where T: Lerp<T,Output=T> + Add<T, Output=T> + Real,
570 Factor: Into<T>
571 {
572 type Output = Quaternion<T>;
573 fn slerp_unclamped(from: Self, to: Self, factor: Factor) -> Quaternion<T> {
574 Quaternion::slerp_unclamped(*from, *to, factor.into())
575 }
576 }
577
578
579 impl<T> Neg for Quaternion<T> where T: Neg<Output=T> {
580 type Output = Self;
581 fn neg(self) -> Self::Output {
582 Self {
583 x: -self.x,
584 y: -self.y,
585 z: -self.z,
586 w: -self.w,
587 }
588 }
589 }
590 impl<T> Div<T> for Quaternion<T> where T: Copy + Div<Output=T> {
591 type Output = Self;
592 fn div(self, rhs: T) -> Self::Output {
593 Self {
594 x: self.x / rhs,
595 y: self.y / rhs,
596 z: self.z / rhs,
597 w: self.w / rhs,
598 }
599 }
600 }
601
602
603 impl<T> Add for Quaternion<T> where T: Add<Output=T> {
604 type Output = Self;
605 fn add(self, rhs: Self) -> Self::Output {
606 Self {
607 x: self.x + rhs.x,
608 y: self.y + rhs.y,
609 z: self.z + rhs.z,
610 w: self.w + rhs.w,
611 }
612 }
613 }
614 impl<T> Sub for Quaternion<T> where T: Sub<Output=T> {
615 type Output = Self;
616 fn sub(self, rhs: Self) -> Self::Output {
617 Self {
618 x: self.x - rhs.x,
619 y: self.y - rhs.y,
620 z: self.z - rhs.z,
621 w: self.w - rhs.w,
622 }
623 }
624 }
625
626 /// The `Mul` implementation for quaternions is concatenation, a.k.a Grassman product.
627 ///
628 /// ```
629 /// # extern crate vek;
630 /// # #[macro_use] extern crate approx;
631 /// # use vek::{Quaternion, Vec4};
632 /// use std::f32::consts::PI;
633 ///
634 /// # fn main() {
635 /// let v = Vec4::unit_x();
636 /// let p = Quaternion::rotation_y(PI/2.);
637 /// let q = Quaternion::rotation_z(PI/2.);
638 /// assert_relative_eq!((p*q)*v, p*(q*v));
639 /// assert_relative_eq!(p*q*v, Vec4::unit_y());
640 /// assert_relative_eq!(q*p*v, -Vec4::unit_z());
641 /// # }
642 /// ```
643 impl<T> Mul for Quaternion<T>
644 where T: Copy + Mul<Output=T> + Sub<Output=T> + Zero + Add<T, Output=T>
645 {
646 type Output = Self;
647 fn mul(self, rhs: Self) -> Self::Output {
648 let ((ps, pv), (qs, qv)) = (
649 self.into_scalar_and_vec3(),
650 rhs.into_scalar_and_vec3()
651 );
652 let Vec3 { x, y, z } = qv*ps + pv*qs + pv.cross(qv);
653 let w = ps*qs - pv.dot(qv);
654 Self { x, y, z, w }
655 }
656 }
657
658 impl<T> Mul<T> for Quaternion<T>
659 where T: Mul<Output=T> + Copy
660 {
661 type Output = Self;
662 fn mul(self, rhs: T) -> Self::Output {
663 Self {
664 x: self.x * rhs,
665 y: self.y * rhs,
666 z: self.z * rhs,
667 w: self.w * rhs,
668 }
669 }
670 }
671
672 /*
673 // WISH: OrthoMat4 * Quaternion; Only for orthogonal matrices
674 static inline void mat4o_mul_quat(mat4 R, mat4 M, quat q)
675 {
676 quat_mul_vec3(R[0], q, M[0]);
677 quat_mul_vec3(R[1], q, M[1]);
678 quat_mul_vec3(R[2], q, M[2]);
679
680 R[3][0] = R[3][1] = R[3][2] = 0.f;
681 R[3][3] = 1.f;
682 }
683 */
684
685 impl<T: AbsDiffEq> AbsDiffEq for Quaternion<T>
686 where
687 T::Epsilon: Copy,
688 {
689 type Epsilon = T::Epsilon;
690
691 fn default_epsilon() -> T::Epsilon {
692 T::default_epsilon()
693 }
694
695 fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
696 T::abs_diff_eq(&self.w, &other.w, epsilon)
697 && T::abs_diff_eq(&self.x, &other.x, epsilon)
698 && T::abs_diff_eq(&self.y, &other.y, epsilon)
699 && T::abs_diff_eq(&self.z, &other.z, epsilon)
700 }
701 }
702
703 impl<T: UlpsEq> UlpsEq for Quaternion<T>
704 where
705 T::Epsilon: Copy,
706 {
707 fn default_max_ulps() -> u32 {
708 T::default_max_ulps()
709 }
710
711 fn ulps_eq(&self, other: &Self, epsilon: T::Epsilon, max_ulps: u32) -> bool {
712 T::ulps_eq(&self.w, &other.w, epsilon, max_ulps)
713 && T::ulps_eq(&self.x, &other.x, epsilon, max_ulps)
714 && T::ulps_eq(&self.y, &other.y, epsilon, max_ulps)
715 && T::ulps_eq(&self.z, &other.z, epsilon, max_ulps)
716 }
717 }
718
719 impl<T: RelativeEq> RelativeEq for Quaternion<T>
720 where
721 T::Epsilon: Copy,
722 {
723 fn default_max_relative() -> T::Epsilon {
724 T::default_max_relative()
725 }
726
727 fn relative_eq(
728 &self,
729 other: &Self,
730 epsilon: T::Epsilon,
731 max_relative: T::Epsilon,
732 ) -> bool {
733 T::relative_eq(&self.w, &other.w, epsilon, max_relative)
734 && T::relative_eq(&self.x, &other.x, epsilon, max_relative)
735 && T::relative_eq(&self.y, &other.y, epsilon, max_relative)
736 && T::relative_eq(&self.z, &other.z, epsilon, max_relative)
737 }
738 }
739
740 #[cfg(feature = "bytemuck")]
741 unsafe impl<T> bytemuck::Zeroable for Quaternion<T> where T: bytemuck::Zeroable, Vec4<T>: bytemuck::Zeroable {
742 fn zeroed() -> Self {
743 Self {
744 x: T::zeroed(),
745 y: T::zeroed(),
746 z: T::zeroed(),
747 w: T::zeroed(),
748 }
749 }
750 }
751
752 #[cfg(feature = "bytemuck")]
753 unsafe impl<T> bytemuck::Pod for Quaternion<T> where T: bytemuck::Pod {
754 // Nothing here
755 }
756
757 };
758}
759
760
761#[cfg(all(nightly, feature="repr_simd"))]
762pub mod repr_simd {
763 //! `Quaternion`s which are marked `#[repr(simd)]`.
764 use super::*;
765 use super::super::vec::repr_c::{Vec3 as CVec3, Vec4 as CVec4};
766 quaternion_complete_mod!(repr_simd #[repr(simd)]);
767 quaternion_vec3_vec4!(Vec3 Vec4);
768 quaternion_vec3_vec4!(CVec3 CVec4);
769}
770pub mod repr_c {
771 //! `Quaternion`s which are marked `#[repr(C)]`.
772 use super::*;
773 quaternion_complete_mod!(repr_c #[repr(C)]);
774 quaternion_vec3_vec4!(Vec3 Vec4);
775
776 #[cfg(all(nightly, feature="repr_simd"))]
777 use super::super::vec::repr_simd::{Vec3 as SimdVec3, Vec4 as SimdVec4};
778 #[cfg(all(nightly, feature="repr_simd"))]
779 quaternion_vec3_vec4!(SimdVec3 SimdVec4);
780}
781pub use self::repr_c::*;
782
783#[cfg(test)]
784mod tests {
785 use super::Quaternion;
786 use crate::vec::Vec3;
787
788 // Ensures that quaternions generated by our API are normalized.
789 mod is_normalized {
790 use super::*;
791
792 #[test] fn mul_quat() {
793 let a = Quaternion::rotation_3d(5_f32, Vec3::new(2_f32, 3., 5.)).normalized();
794 let b = Quaternion::rotation_3d(3_f32, Vec3::new(1_f32, 5., 20.)).normalized();
795 assert_relative_eq!((a * b).magnitude(), 1.);
796 }
797 #[test] fn rotation_from_to_3d() {
798 let a = Vec3::new(1_f32, 200., 3.);
799 let b = Vec3::new(80_f32, 0., 352.);
800 let q = Quaternion::<f32>::rotation_from_to_3d(a, b);
801 assert_relative_eq!(q.magnitude(), 1.);
802 }
803 #[test] fn rotation_3d() {
804 let v = Vec3::new(1_f32, 200., 3.);
805 let q = Quaternion::rotation_3d(3_f32, v);
806 assert_relative_eq!(q.magnitude(), 1.);
807 }
808 #[test] fn slerp() {
809 let a = Quaternion::rotation_3d(5_f32, Vec3::new(2_f32, 3., 5.)).normalized();
810 let b = Quaternion::rotation_3d(3_f32, Vec3::new(1_f32, 5., 20.)).normalized();
811 let count = 32;
812 for i in 0..(count+1) {
813 let q = Quaternion::slerp(a, b, i as f32 / (count as f32));
814 assert_relative_eq!(q.magnitude(), 1.);
815 }
816 }
817 }
818}