nalgebra/geometry/
quaternion_ops.rs

1// The macros break if the references are taken out, for some reason.
2#![allow(clippy::op_ref)]
3
4/*
5 * This file provides:
6 * ===================
7 *
8 *
9 * (Quaternion)
10 *
11 * Index<usize>
12 * IndexMut<usize>
13 * Quaternion × Quaternion
14 * Quaternion + Quaternion
15 * Quaternion - Quaternion
16 * -Quaternion
17 * Quaternion × Scalar
18 * Quaternion ÷ Scalar
19 * Scalar × Quaternion
20 *
21 * (Unit Quaternion)
22 * UnitQuaternion × UnitQuaternion
23 * UnitQuaternion × Rotation       -> UnitQuaternion
24 * Rotation       × UnitQuaternion -> UnitQuaternion
25 *
26 * UnitQuaternion ÷ UnitQuaternion
27 * UnitQuaternion ÷ Rotation       -> UnitQuaternion
28 * Rotation       ÷ UnitQuaternion -> UnitQuaternion
29 *
30 *
31 * UnitQuaternion × Point
32 * UnitQuaternion × Vector
33 * UnitQuaternion × Unit<Vector>
34 *
35 * NOTE: -UnitQuaternion is already provided by `Unit<T>`.
36 *
37 *
38 * (Assignment Operators)
39 *
40 * Quaternion ×= Scalar
41 * Quaternion ×= Quaternion
42 * Quaternion += Quaternion
43 * Quaternion -= Quaternion
44 *
45 * UnitQuaternion ×= UnitQuaternion
46 * UnitQuaternion ×= Rotation
47 *
48 * UnitQuaternion ÷= UnitQuaternion
49 * UnitQuaternion ÷= Rotation
50 *
51 * TODO: Rotation ×= UnitQuaternion
52 * TODO: Rotation ÷= UnitQuaternion
53 *
54 */
55
56use std::ops::{
57    Add, AddAssign, Div, DivAssign, Index, IndexMut, Mul, MulAssign, Neg, Sub, SubAssign,
58};
59
60use crate::base::dimension::U3;
61use crate::base::storage::Storage;
62use crate::base::{Const, Scalar, Unit, Vector, Vector3};
63use crate::SimdRealField;
64
65use crate::geometry::{Point3, Quaternion, Rotation, UnitQuaternion};
66
67impl<T: Scalar> Index<usize> for Quaternion<T> {
68    type Output = T;
69
70    #[inline]
71    fn index(&self, i: usize) -> &Self::Output {
72        &self.coords[i]
73    }
74}
75
76impl<T: Scalar> IndexMut<usize> for Quaternion<T> {
77    #[inline]
78    fn index_mut(&mut self, i: usize) -> &mut T {
79        &mut self.coords[i]
80    }
81}
82
83macro_rules! quaternion_op_impl(
84    ($Op: ident, $op: ident;
85     $($Storage: ident: $StoragesBound: ident $(<$($BoundParam: ty),*>)*),*;
86     $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Result: ty;
87     $action: expr; $($lives: tt),*) => {
88        impl<$($lives ,)* T: SimdRealField $(, $Storage: $StoragesBound $(<$($BoundParam),*>)*)*> $Op<$Rhs> for $Lhs
89            where T::Element: SimdRealField {
90            type Output = $Result;
91
92            #[inline]
93            fn $op($lhs, $rhs: $Rhs) -> Self::Output {
94                $action
95            }
96        }
97    }
98);
99
100// Quaternion + Quaternion
101quaternion_op_impl!(
102    Add, add;
103    ;
104    self: &'a Quaternion<T>, rhs: &'b Quaternion<T>, Output = Quaternion<T>;
105    Quaternion::from(&self.coords + &rhs.coords);
106    'a, 'b);
107
108quaternion_op_impl!(
109    Add, add;
110    ;
111    self: &'a Quaternion<T>, rhs: Quaternion<T>, Output = Quaternion<T>;
112    Quaternion::from(&self.coords + rhs.coords);
113    'a);
114
115quaternion_op_impl!(
116    Add, add;
117    ;
118    self: Quaternion<T>, rhs: &'b Quaternion<T>, Output = Quaternion<T>;
119    Quaternion::from(self.coords + &rhs.coords);
120    'b);
121
122quaternion_op_impl!(
123    Add, add;
124    ;
125    self: Quaternion<T>, rhs: Quaternion<T>, Output = Quaternion<T>;
126    Quaternion::from(self.coords + rhs.coords); );
127
128// Quaternion - Quaternion
129quaternion_op_impl!(
130    Sub, sub;
131    ;
132    self: &'a Quaternion<T>, rhs: &'b Quaternion<T>, Output = Quaternion<T>;
133    Quaternion::from(&self.coords - &rhs.coords);
134    'a, 'b);
135
136quaternion_op_impl!(
137    Sub, sub;
138    ;
139    self: &'a Quaternion<T>, rhs: Quaternion<T>, Output = Quaternion<T>;
140    Quaternion::from(&self.coords - rhs.coords);
141    'a);
142
143quaternion_op_impl!(
144    Sub, sub;
145    ;
146    self: Quaternion<T>, rhs: &'b Quaternion<T>, Output = Quaternion<T>;
147    Quaternion::from(self.coords - &rhs.coords);
148    'b);
149
150quaternion_op_impl!(
151    Sub, sub;
152    ;
153    self: Quaternion<T>, rhs: Quaternion<T>, Output = Quaternion<T>;
154    Quaternion::from(self.coords - rhs.coords); );
155
156// Quaternion × Quaternion
157quaternion_op_impl!(
158    Mul, mul;
159    ;
160    self: &'a Quaternion<T>, rhs: &'b Quaternion<T>, Output = Quaternion<T>;
161    Quaternion::new(
162        self[3].clone() * rhs[3].clone() - self[0].clone() * rhs[0].clone() - self[1].clone() * rhs[1].clone() - self[2].clone() * rhs[2].clone(),
163        self[3].clone() * rhs[0].clone() + self[0].clone() * rhs[3].clone() + self[1].clone() * rhs[2].clone() - self[2].clone() * rhs[1].clone(),
164        self[3].clone() * rhs[1].clone() - self[0].clone() * rhs[2].clone() + self[1].clone() * rhs[3].clone() + self[2].clone() * rhs[0].clone(),
165        self[3].clone() * rhs[2].clone() + self[0].clone() * rhs[1].clone() - self[1].clone() * rhs[0].clone() + self[2].clone() * rhs[3].clone());
166    'a, 'b);
167
168quaternion_op_impl!(
169    Mul, mul;
170    ;
171    self: &'a Quaternion<T>, rhs: Quaternion<T>, Output = Quaternion<T>;
172    self * &rhs;
173    'a);
174
175quaternion_op_impl!(
176    Mul, mul;
177    ;
178    self: Quaternion<T>, rhs: &'b Quaternion<T>, Output = Quaternion<T>;
179    &self * rhs;
180    'b);
181
182quaternion_op_impl!(
183    Mul, mul;
184    ;
185    self: Quaternion<T>, rhs: Quaternion<T>, Output = Quaternion<T>;
186    &self * &rhs; );
187
188// UnitQuaternion × UnitQuaternion
189quaternion_op_impl!(
190    Mul, mul;
191    ;
192    self: &'a UnitQuaternion<T>, rhs: &'b UnitQuaternion<T>, Output = UnitQuaternion<T>;
193    UnitQuaternion::new_unchecked(self.quaternion() * rhs.quaternion());
194    'a, 'b);
195
196quaternion_op_impl!(
197    Mul, mul;
198    ;
199    self: &'a UnitQuaternion<T>, rhs: UnitQuaternion<T>, Output = UnitQuaternion<T>;
200    self * &rhs;
201    'a);
202
203quaternion_op_impl!(
204    Mul, mul;
205    ;
206    self: UnitQuaternion<T>, rhs: &'b UnitQuaternion<T>, Output = UnitQuaternion<T>;
207    &self * rhs;
208    'b);
209
210quaternion_op_impl!(
211    Mul, mul;
212    ;
213    self: UnitQuaternion<T>, rhs: UnitQuaternion<T>, Output = UnitQuaternion<T>;
214    &self * &rhs; );
215
216// UnitQuaternion ÷ UnitQuaternion
217quaternion_op_impl!(
218    Div, div;
219    ;
220    self: &'a UnitQuaternion<T>, rhs: &'b UnitQuaternion<T>, Output = UnitQuaternion<T>;
221    #[allow(clippy::suspicious_arithmetic_impl)] { self * rhs.inverse() };
222    'a, 'b);
223
224quaternion_op_impl!(
225    Div, div;
226    ;
227    self: &'a UnitQuaternion<T>, rhs: UnitQuaternion<T>, Output = UnitQuaternion<T>;
228    self / &rhs;
229    'a);
230
231quaternion_op_impl!(
232    Div, div;
233    ;
234    self: UnitQuaternion<T>, rhs: &'b UnitQuaternion<T>, Output = UnitQuaternion<T>;
235    &self / rhs;
236    'b);
237
238quaternion_op_impl!(
239    Div, div;
240    ;
241    self: UnitQuaternion<T>, rhs: UnitQuaternion<T>, Output = UnitQuaternion<T>;
242    &self / &rhs; );
243
244// UnitQuaternion × Rotation
245quaternion_op_impl!(
246    Mul, mul;
247    ;
248    self: &'a UnitQuaternion<T>, rhs: &'b Rotation<T, 3>,
249    Output = UnitQuaternion<T>;
250    // TODO: can we avoid the conversion from a rotation matrix?
251    self * UnitQuaternion::<T>::from_rotation_matrix(rhs);
252    'a, 'b);
253
254quaternion_op_impl!(
255    Mul, mul;
256    ;
257    self: &'a UnitQuaternion<T>, rhs: Rotation<T, 3>,
258    Output = UnitQuaternion<T>;
259    self * UnitQuaternion::<T>::from_rotation_matrix(&rhs);
260    'a);
261
262quaternion_op_impl!(
263    Mul, mul;
264    ;
265    self: UnitQuaternion<T>, rhs: &'b Rotation<T, 3>,
266    Output = UnitQuaternion<T>;
267    self * UnitQuaternion::<T>::from_rotation_matrix(rhs);
268    'b);
269
270quaternion_op_impl!(
271    Mul, mul;
272    ;
273    self: UnitQuaternion<T>, rhs: Rotation<T, 3>,
274    Output = UnitQuaternion<T>;
275    self * UnitQuaternion::<T>::from_rotation_matrix(&rhs); );
276
277// UnitQuaternion ÷ Rotation
278quaternion_op_impl!(
279    Div, div;
280    ;
281    self: &'a UnitQuaternion<T>, rhs: &'b Rotation<T, 3>,
282    Output = UnitQuaternion<T>;
283    // TODO: can we avoid the conversion to a rotation matrix?
284    self / UnitQuaternion::<T>::from_rotation_matrix(rhs);
285    'a, 'b);
286
287quaternion_op_impl!(
288    Div, div;
289    ;
290    self: &'a UnitQuaternion<T>, rhs: Rotation<T, 3>,
291    Output = UnitQuaternion<T>;
292    self / UnitQuaternion::<T>::from_rotation_matrix(&rhs);
293    'a);
294
295quaternion_op_impl!(
296    Div, div;
297    ;
298    self: UnitQuaternion<T>, rhs: &'b Rotation<T, 3>,
299    Output = UnitQuaternion<T>;
300    self / UnitQuaternion::<T>::from_rotation_matrix(rhs);
301    'b);
302
303quaternion_op_impl!(
304    Div, div;
305    ;
306    self: UnitQuaternion<T>, rhs: Rotation<T, 3>,
307    Output = UnitQuaternion<T>;
308    self / UnitQuaternion::<T>::from_rotation_matrix(&rhs); );
309
310// Rotation × UnitQuaternion
311quaternion_op_impl!(
312    Mul, mul;
313    ;
314    self: &'a Rotation<T, 3>, rhs: &'b UnitQuaternion<T>,
315    Output = UnitQuaternion<T>;
316    // TODO: can we avoid the conversion from a rotation matrix?
317    UnitQuaternion::<T>::from_rotation_matrix(self) * rhs;
318    'a, 'b);
319
320quaternion_op_impl!(
321    Mul, mul;
322    ;
323    self: &'a Rotation<T, 3>, rhs: UnitQuaternion<T>,
324    Output = UnitQuaternion<T>;
325    UnitQuaternion::<T>::from_rotation_matrix(self) * rhs;
326    'a);
327
328quaternion_op_impl!(
329    Mul, mul;
330    ;
331    self: Rotation<T, 3>, rhs: &'b UnitQuaternion<T>,
332    Output = UnitQuaternion<T>;
333    UnitQuaternion::<T>::from_rotation_matrix(&self) * rhs;
334    'b);
335
336quaternion_op_impl!(
337    Mul, mul;
338    ;
339    self: Rotation<T, 3>, rhs: UnitQuaternion<T>,
340    Output = UnitQuaternion<T>;
341    UnitQuaternion::<T>::from_rotation_matrix(&self) * rhs; );
342
343// Rotation ÷ UnitQuaternion
344quaternion_op_impl!(
345    Div, div;
346    ;
347    self: &'a Rotation<T, 3>, rhs: &'b UnitQuaternion<T>,
348    Output = UnitQuaternion<T>;
349    // TODO: can we avoid the conversion from a rotation matrix?
350    UnitQuaternion::<T>::from_rotation_matrix(self) / rhs;
351    'a, 'b);
352
353quaternion_op_impl!(
354    Div, div;
355    ;
356    self: &'a Rotation<T, 3>, rhs: UnitQuaternion<T>,
357    Output = UnitQuaternion<T>;
358    UnitQuaternion::<T>::from_rotation_matrix(self) / rhs;
359    'a);
360
361quaternion_op_impl!(
362    Div, div;
363    ;
364    self: Rotation<T, 3>, rhs: &'b UnitQuaternion<T>,
365    Output = UnitQuaternion<T>;
366    UnitQuaternion::<T>::from_rotation_matrix(&self) / rhs;
367    'b);
368
369quaternion_op_impl!(
370    Div, div;
371    ;
372    self: Rotation<T, 3>, rhs: UnitQuaternion<T>,
373    Output = UnitQuaternion<T>;
374    UnitQuaternion::<T>::from_rotation_matrix(&self) / rhs; );
375
376// UnitQuaternion × Vector
377quaternion_op_impl!(
378    Mul, mul;
379    SB: Storage<T, Const<3>> ;
380    self: &'a UnitQuaternion<T>, rhs: &'b Vector<T, Const<3>, SB>,
381    Output = Vector3<T>;
382    {
383        let two: T = crate::convert(2.0f64);
384        let t = self.as_ref().vector().cross(rhs) * two;
385        let cross = self.as_ref().vector().cross(&t);
386
387        t * self.as_ref().scalar() + cross + rhs
388    };
389    'a, 'b);
390
391quaternion_op_impl!(
392    Mul, mul;
393    SB: Storage<T, Const<3>> ;
394    self: &'a UnitQuaternion<T>, rhs: Vector<T, U3, SB>,
395    Output = Vector3<T>;
396    self * &rhs;
397    'a);
398
399quaternion_op_impl!(
400    Mul, mul;
401    SB: Storage<T, Const<3>> ;
402    self: UnitQuaternion<T>, rhs: &'b Vector<T, U3, SB>,
403    Output = Vector3<T>;
404    &self * rhs;
405    'b);
406
407quaternion_op_impl!(
408    Mul, mul;
409    SB: Storage<T, Const<3>> ;
410    self: UnitQuaternion<T>, rhs: Vector<T, U3, SB>,
411    Output = Vector3<T>;
412    &self * &rhs; );
413
414// UnitQuaternion × Point
415quaternion_op_impl!(
416    Mul, mul;
417    ;
418    self: &'a UnitQuaternion<T>, rhs: &'b Point3<T>,
419    Output = Point3<T>;
420    Point3::from(self * &rhs.coords);
421    'a, 'b);
422
423quaternion_op_impl!(
424    Mul, mul;
425    ;
426    self: &'a UnitQuaternion<T>, rhs: Point3<T>,
427    Output = Point3<T>;
428    Point3::from(self * rhs.coords);
429    'a);
430
431quaternion_op_impl!(
432    Mul, mul;
433    ;
434    self: UnitQuaternion<T>, rhs: &'b Point3<T>,
435    Output = Point3<T>;
436    Point3::from(self * &rhs.coords);
437    'b);
438
439quaternion_op_impl!(
440    Mul, mul;
441    ;
442    self: UnitQuaternion<T>, rhs: Point3<T>,
443    Output = Point3<T>;
444    Point3::from(self * rhs.coords); );
445
446// UnitQuaternion × Unit<Vector>
447quaternion_op_impl!(
448    Mul, mul;
449    SB: Storage<T, Const<3>> ;
450    self: &'a UnitQuaternion<T>, rhs: &'b Unit<Vector<T, U3, SB>>,
451    Output = Unit<Vector3<T>>;
452    Unit::new_unchecked(self * rhs.as_ref());
453    'a, 'b);
454
455quaternion_op_impl!(
456    Mul, mul;
457    SB: Storage<T, Const<3>> ;
458    self: &'a UnitQuaternion<T>, rhs: Unit<Vector<T, U3, SB>>,
459    Output = Unit<Vector3<T>>;
460    Unit::new_unchecked(self * rhs.into_inner());
461    'a);
462
463quaternion_op_impl!(
464    Mul, mul;
465    SB: Storage<T, Const<3>> ;
466    self: UnitQuaternion<T>, rhs: &'b Unit<Vector<T, U3, SB>>,
467    Output = Unit<Vector3<T>>;
468    Unit::new_unchecked(self * rhs.as_ref());
469    'b);
470
471quaternion_op_impl!(
472    Mul, mul;
473    SB: Storage<T, Const<3>> ;
474    self: UnitQuaternion<T>, rhs: Unit<Vector<T, U3, SB>>,
475    Output = Unit<Vector3<T>>;
476    Unit::new_unchecked(self * rhs.into_inner()); );
477
478macro_rules! scalar_op_impl(
479    ($($Op: ident, $op: ident, $OpAssign: ident, $op_assign: ident);* $(;)*) => {$(
480        impl<T: SimdRealField> $Op<T> for Quaternion<T>
481         where T::Element: SimdRealField {
482            type Output = Quaternion<T>;
483
484            #[inline]
485            fn $op(self, n: T) -> Self::Output {
486                Quaternion::from(self.coords.$op(n))
487            }
488        }
489
490        impl<'a, T: SimdRealField> $Op<T> for &'a Quaternion<T>
491         where T::Element: SimdRealField {
492            type Output = Quaternion<T>;
493
494            #[inline]
495            fn $op(self, n: T) -> Self::Output {
496                Quaternion::from((&self.coords).$op(n))
497            }
498        }
499
500        impl<T: SimdRealField> $OpAssign<T> for Quaternion<T>
501         where T::Element: SimdRealField {
502
503            #[inline]
504            fn $op_assign(&mut self, n: T) {
505                self.coords.$op_assign(n)
506            }
507        }
508    )*}
509);
510
511scalar_op_impl!(
512    Mul, mul, MulAssign, mul_assign;
513    Div, div, DivAssign, div_assign;
514);
515
516macro_rules! left_scalar_mul_impl(
517    ($($T: ty),* $(,)*) => {$(
518        impl Mul<Quaternion<$T>> for $T {
519            type Output = Quaternion<$T>;
520
521            #[inline]
522            fn mul(self, right: Quaternion<$T>) -> Self::Output {
523                Quaternion::from(self * right.coords)
524            }
525        }
526
527        impl<'b> Mul<&'b Quaternion<$T>> for $T {
528            type Output = Quaternion<$T>;
529
530            #[inline]
531            fn mul(self, right: &'b Quaternion<$T>) -> Self::Output {
532                Quaternion::from(self * &right.coords)
533            }
534        }
535    )*}
536);
537
538left_scalar_mul_impl!(f32, f64);
539
540impl<T: SimdRealField> Neg for Quaternion<T>
541where
542    T::Element: SimdRealField,
543{
544    type Output = Quaternion<T>;
545
546    #[inline]
547    fn neg(self) -> Self::Output {
548        Self::Output::from(-self.coords)
549    }
550}
551
552impl<'a, T: SimdRealField> Neg for &'a Quaternion<T>
553where
554    T::Element: SimdRealField,
555{
556    type Output = Quaternion<T>;
557
558    #[inline]
559    fn neg(self) -> Self::Output {
560        Self::Output::from(-&self.coords)
561    }
562}
563
564macro_rules! quaternion_op_impl(
565    ($OpAssign: ident, $op_assign: ident;
566     $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty $(=> $VDimA: ty, $VDimB: ty)*;
567     $action: expr; $($lives: tt),*) => {
568        impl<$($lives ,)* T: SimdRealField> $OpAssign<$Rhs> for $Lhs
569            where T::Element: SimdRealField {
570
571            #[inline]
572            fn $op_assign(&mut $lhs, $rhs: $Rhs) {
573                $action
574            }
575        }
576    }
577);
578
579// Quaternion += Quaternion
580quaternion_op_impl!(
581    AddAssign, add_assign;
582    self: Quaternion<T>, rhs: &'b Quaternion<T>;
583    self.coords += &rhs.coords;
584    'b);
585
586quaternion_op_impl!(
587    AddAssign, add_assign;
588    self: Quaternion<T>, rhs: Quaternion<T>;
589    self.coords += rhs.coords; );
590
591// Quaternion -= Quaternion
592quaternion_op_impl!(
593    SubAssign, sub_assign;
594    self: Quaternion<T>, rhs: &'b Quaternion<T>;
595    self.coords -= &rhs.coords;
596    'b);
597
598quaternion_op_impl!(
599    SubAssign, sub_assign;
600    self: Quaternion<T>, rhs: Quaternion<T>;
601    self.coords -= rhs.coords; );
602
603// Quaternion ×= Quaternion
604quaternion_op_impl!(
605    MulAssign, mul_assign;
606    self: Quaternion<T>, rhs: &'b Quaternion<T>;
607    {
608        let res = &*self * rhs;
609        // TODO: will this be optimized away?
610        self.coords.copy_from(&res.coords);
611    };
612    'b);
613
614quaternion_op_impl!(
615    MulAssign, mul_assign;
616    self: Quaternion<T>, rhs: Quaternion<T>;
617    *self *= &rhs; );
618
619// UnitQuaternion ×= UnitQuaternion
620quaternion_op_impl!(
621    MulAssign, mul_assign;
622    self: UnitQuaternion<T>, rhs: &'b UnitQuaternion<T>;
623    {
624        let res = &*self * rhs;
625        self.as_mut_unchecked().coords.copy_from(&res.as_ref().coords);
626    };
627    'b);
628
629quaternion_op_impl!(
630    MulAssign, mul_assign;
631    self: UnitQuaternion<T>, rhs: UnitQuaternion<T>;
632    *self *= &rhs; );
633
634// UnitQuaternion ÷= UnitQuaternion
635quaternion_op_impl!(
636    DivAssign, div_assign;
637    self: UnitQuaternion<T>, rhs: &'b UnitQuaternion<T>;
638    {
639        let res = &*self / rhs;
640        self.as_mut_unchecked().coords.copy_from(&res.as_ref().coords);
641    };
642    'b);
643
644quaternion_op_impl!(
645    DivAssign, div_assign;
646    self: UnitQuaternion<T>, rhs: UnitQuaternion<T>;
647    *self /= &rhs; );
648
649// UnitQuaternion ×= Rotation
650quaternion_op_impl!(
651    MulAssign, mul_assign;
652    self: UnitQuaternion<T>, rhs: &'b Rotation<T, 3>;
653    {
654        let res = &*self * rhs;
655        self.as_mut_unchecked().coords.copy_from(&res.as_ref().coords);
656    };
657    'b);
658
659quaternion_op_impl!(
660    MulAssign, mul_assign;
661    self: UnitQuaternion<T>, rhs: Rotation<T, 3>;
662    *self *= &rhs; );
663
664// UnitQuaternion ÷= Rotation
665quaternion_op_impl!(
666    DivAssign, div_assign;
667    self: UnitQuaternion<T>, rhs: &'b Rotation<T, 3>;
668    {
669        let res = &*self / rhs;
670        self.as_mut_unchecked().coords.copy_from(&res.as_ref().coords);
671    };
672    'b);
673
674quaternion_op_impl!(
675    DivAssign, div_assign;
676    self: UnitQuaternion<T>, rhs: Rotation<T, 3>;
677    *self /= &rhs; );
nalgebra/geometry/quaternion_ops.rs

nalgebra/geometry/
quaternion_ops.rs
