nalgebra/geometry/
isometry_ops.rs

1// The macros break if the references are taken out, for some reason.
2#![allow(clippy::op_ref)]
3
4use num::{One, Zero};
5use std::ops::{Div, DivAssign, Mul, MulAssign};
6
7use simba::scalar::{ClosedAddAssign, ClosedMulAssign};
8use simba::simd::SimdRealField;
9
10use crate::base::{SVector, Unit};
11use crate::Scalar;
12
13use crate::geometry::{
14    AbstractRotation, Isometry, Point, Rotation, Translation, UnitComplex, UnitQuaternion,
15};
16
17// TODO: there are several cloning of rotations that we could probably get rid of (but we didn't
18// yet because that would require to add a bound like `where for<'a, 'b> &'a R: Mul<&'b R, Output = R>`
19// which is quite ugly.
20
21/*
22 *
23 * In this file, we provide:
24 * =========================
25 *
26 *
27 * (Operators)
28 *
29 * Isometry × Isometry
30 * Isometry × R
31 *
32 *
33 * Isometry ÷ Isometry
34 * Isometry ÷ R
35 *
36 * Isometry × Point
37 * Isometry × Vector
38 * Isometry × Unit<Vector>
39 *
40 *
41 * Isometry    × Translation
42 * Translation × Isometry
43 * Translation × R           -> Isometry<R>
44 *
45 * NOTE: The following are provided explicitly because we can't have R × Isometry.
46 * Rotation       × Isometry<Rotation>
47 * UnitQuaternion × Isometry<UnitQuaternion>
48 *
49 * Rotation       ÷ Isometry<Rotation>
50 * UnitQuaternion ÷ Isometry<UnitQuaternion>
51 *
52 * Rotation       × Translation -> Isometry<Rotation>
53 * UnitQuaternion × Translation -> Isometry<UnitQuaternion>
54 *
55 *
56 * (Assignment Operators)
57 *
58 * Isometry ×= Translation
59 *
60 * Isometry ×= Isometry
61 * Isometry ×= R
62 *
63 * Isometry ÷= Isometry
64 * Isometry ÷= R
65 *
66 */
67
68macro_rules! isometry_binop_impl(
69    ($Op: ident, $op: ident;
70     $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Output: ty;
71     $action: expr; $($lives: tt),*) => {
72        impl<$($lives ,)* T: SimdRealField, R, const D: usize> $Op<$Rhs> for $Lhs
73            where T::Element: SimdRealField,
74                  R: AbstractRotation<T, D>, {
75            type Output = $Output;
76
77            #[inline]
78            fn $op($lhs, $rhs: $Rhs) -> Self::Output {
79                $action
80            }
81        }
82    }
83);
84
85macro_rules! isometry_binop_impl_all(
86    ($Op: ident, $op: ident;
87     $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Output: ty;
88     [val val] => $action_val_val: expr;
89     [ref val] => $action_ref_val: expr;
90     [val ref] => $action_val_ref: expr;
91     [ref ref] => $action_ref_ref: expr;) => {
92        isometry_binop_impl!(
93            $Op, $op;
94            $lhs: $Lhs, $rhs: $Rhs, Output = $Output;
95            $action_val_val; );
96
97        isometry_binop_impl!(
98            $Op, $op;
99            $lhs: &'a $Lhs, $rhs: $Rhs, Output = $Output;
100            $action_ref_val; 'a);
101
102        isometry_binop_impl!(
103            $Op, $op;
104            $lhs: $Lhs, $rhs: &'b $Rhs, Output = $Output;
105            $action_val_ref; 'b);
106
107        isometry_binop_impl!(
108            $Op, $op;
109            $lhs: &'a $Lhs, $rhs: &'b $Rhs, Output = $Output;
110            $action_ref_ref; 'a, 'b);
111    }
112);
113
114macro_rules! isometry_binop_assign_impl_all(
115    ($OpAssign: ident, $op_assign: ident;
116     $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty;
117     [val] => $action_val: expr;
118     [ref] => $action_ref: expr;) => {
119        impl<T: SimdRealField, R, const D: usize> $OpAssign<$Rhs> for $Lhs
120            where T::Element: SimdRealField,
121                  R: AbstractRotation<T, D> {
122            #[inline]
123            fn $op_assign(&mut $lhs, $rhs: $Rhs) {
124                $action_val
125            }
126        }
127
128        impl<'b, T: SimdRealField, R, const D: usize> $OpAssign<&'b $Rhs> for $Lhs
129            where T::Element: SimdRealField,
130                  R: AbstractRotation<T, D> {
131            #[inline]
132            fn $op_assign(&mut $lhs, $rhs: &'b $Rhs) {
133                $action_ref
134            }
135        }
136    }
137);
138
139// Isometry × Isometry
140// Isometry ÷ Isometry
141isometry_binop_impl_all!(
142    Mul, mul;
143    self: Isometry<T, R, D>, rhs: Isometry<T, R, D>, Output = Isometry<T, R, D>;
144    [val val] => &self * &rhs;
145    [ref val] => self * &rhs;
146    [val ref] => &self * rhs;
147    [ref ref] => {
148        let shift = self.rotation.transform_vector(&rhs.translation.vector);
149
150        #[allow(clippy::suspicious_arithmetic_impl)]
151        Isometry::from_parts(Translation::from(&self.translation.vector + shift),
152                             self.rotation.clone() * rhs.rotation.clone()) // TODO: too bad we have to clone.
153    };
154);
155
156isometry_binop_impl_all!(
157    Div, div;
158    self: Isometry<T, R, D>, rhs: Isometry<T, R, D>, Output = Isometry<T, R, D>;
159    [val val] => #[allow(clippy::suspicious_arithmetic_impl)] { self * rhs.inverse() };
160    [ref val] => #[allow(clippy::suspicious_arithmetic_impl)] { self * rhs.inverse() };
161    [val ref] => #[allow(clippy::suspicious_arithmetic_impl)] { self * rhs.inverse() };
162    [ref ref] => #[allow(clippy::suspicious_arithmetic_impl)] { self * rhs.inverse() };
163);
164
165// Isometry ×= Translation
166isometry_binop_assign_impl_all!(
167    MulAssign, mul_assign;
168    self: Isometry<T, R, D>, rhs: Translation<T, D>;
169    [val] => *self *= &rhs;
170    [ref] => #[allow(clippy::suspicious_op_assign_impl)] {
171        let shift = self.rotation.transform_vector(&rhs.vector);
172        self.translation.vector += shift;
173    };
174);
175
176// Isometry ×= Isometry
177// Isometry ÷= Isometry
178isometry_binop_assign_impl_all!(
179    MulAssign, mul_assign;
180    self: Isometry<T, R, D>, rhs: Isometry<T, R, D>;
181    [val] => *self *= &rhs;
182    [ref] => {
183        let shift = self.rotation.transform_vector(&rhs.translation.vector);
184        self.translation.vector += shift;
185        self.rotation *= rhs.rotation.clone();
186    };
187);
188
189isometry_binop_assign_impl_all!(
190    DivAssign, div_assign;
191    self: Isometry<T, R, D>, rhs: Isometry<T, R, D>;
192    [val] => *self /= &rhs;
193    [ref] => #[allow(clippy::suspicious_op_assign_impl)] { *self *= rhs.inverse() };
194);
195
196// Isometry ×= R
197// Isometry ÷= R
198md_assign_impl_all!(
199    MulAssign, mul_assign where T: SimdRealField for T::Element: SimdRealField;
200    (Const<D>, U1), (Const<D>, Const<D>)
201    const D; for; where;
202    self: Isometry<T, Rotation<T, D>, D>, rhs: Rotation<T, D>;
203    [val] => self.rotation *= rhs;
204    [ref] => self.rotation *= rhs.clone();
205);
206
207md_assign_impl_all!(
208    DivAssign, div_assign where T: SimdRealField for T::Element: SimdRealField;
209    (Const<D>, U1), (Const<D>, Const<D>)
210    const D; for; where;
211    self: Isometry<T, Rotation<T, D>, D>, rhs: Rotation<T, D>;
212    // TODO: don't invert explicitly?
213    [val] => #[allow(clippy::suspicious_op_assign_impl)] { *self *= rhs.inverse() };
214    [ref] => #[allow(clippy::suspicious_op_assign_impl)] { *self *= rhs.inverse() };
215);
216
217md_assign_impl_all!(
218    MulAssign, mul_assign where T: SimdRealField for T::Element: SimdRealField;
219    (U3, U3), (U3, U3)
220    const; for; where;
221    self: Isometry<T, UnitQuaternion<T>, 3>, rhs: UnitQuaternion<T>;
222    [val] => self.rotation *= rhs;
223    [ref] => self.rotation *= rhs.clone();
224);
225
226md_assign_impl_all!(
227    DivAssign, div_assign where T: SimdRealField for T::Element: SimdRealField;
228    (U3, U3), (U3, U3)
229    const; for; where;
230    self: Isometry<T, UnitQuaternion<T>, 3>, rhs: UnitQuaternion<T>;
231    // TODO: don't invert explicitly?
232    [val] => #[allow(clippy::suspicious_op_assign_impl)] { *self *= rhs.inverse() };
233    [ref] => #[allow(clippy::suspicious_op_assign_impl)] { *self *= rhs.inverse() };
234);
235
236md_assign_impl_all!(
237    MulAssign, mul_assign where T: SimdRealField for T::Element: SimdRealField;
238    (U2, U2), (U2, U2)
239    const; for; where;
240    self: Isometry<T, UnitComplex<T>, 2>, rhs: UnitComplex<T>;
241    [val] => self.rotation *= rhs;
242    [ref] => self.rotation *= rhs.clone();
243);
244
245md_assign_impl_all!(
246    DivAssign, div_assign where T: SimdRealField for T::Element: SimdRealField;
247    (U2, U2), (U2, U2)
248    const; for; where;
249    self: Isometry<T, UnitComplex<T>, 2>, rhs: UnitComplex<T>;
250    // TODO: don't invert explicitly?
251    [val] => #[allow(clippy::suspicious_op_assign_impl)] { *self *= rhs.inverse() };
252    [ref] => #[allow(clippy::suspicious_op_assign_impl)] { *self *= rhs.inverse() };
253);
254
255// Isometry × Point
256isometry_binop_impl_all!(
257    Mul, mul;
258    self: Isometry<T, R, D>, right: Point<T, D>, Output = Point<T, D>;
259    [val val] => self.translation  * self.rotation.transform_point(&right);
260    [ref val] => &self.translation * self.rotation.transform_point(&right);
261    [val ref] => self.translation  * self.rotation.transform_point(right);
262    [ref ref] => &self.translation * self.rotation.transform_point(right);
263);
264
265// Isometry × Vector
266isometry_binop_impl_all!(
267    Mul, mul;
268    // TODO: because of `transform_vector`, we cant use a generic storage type for the rhs vector,
269    // i.e., right: Vector<T, D, S> where S: Storage<T, D>.
270    self: Isometry<T, R, D>, right: SVector<T, D>, Output = SVector<T, D>;
271    [val val] => self.rotation.transform_vector(&right);
272    [ref val] => self.rotation.transform_vector(&right);
273    [val ref] => self.rotation.transform_vector(right);
274    [ref ref] => self.rotation.transform_vector(right);
275);
276
277// Isometry × Unit<Vector>
278isometry_binop_impl_all!(
279    Mul, mul;
280    // TODO: because of `transform_vector`, we cant use a generic storage type for the rhs vector,
281    // i.e., right: Vector<T, D, S> where S: Storage<T, D>.
282    self: Isometry<T, R, D>, right: Unit<SVector<T, D>>, Output = Unit<SVector<T, D>>;
283    [val val] => Unit::new_unchecked(self.rotation.transform_vector(right.as_ref()));
284    [ref val] => Unit::new_unchecked(self.rotation.transform_vector(right.as_ref()));
285    [val ref] => Unit::new_unchecked(self.rotation.transform_vector(right.as_ref()));
286    [ref ref] => Unit::new_unchecked(self.rotation.transform_vector(right.as_ref()));
287);
288
289// Isometry × Translation
290isometry_binop_impl_all!(
291    Mul, mul;
292    self: Isometry<T, R, D>, right: Translation<T, D>, Output = Isometry<T, R, D>;
293    [val val] => &self * &right;
294    [ref val] => self * &right;
295    [val ref] => &self * right;
296    [ref ref] => {
297        #[allow(clippy::suspicious_arithmetic_impl)]
298        let new_tr = &self.translation.vector + self.rotation.transform_vector(&right.vector);
299        Isometry::from_parts(Translation::from(new_tr), self.rotation.clone())
300    };
301);
302
303// Translation × Isometry
304isometry_binop_impl_all!(
305    Mul, mul;
306    self: Translation<T, D>, right: Isometry<T, R, D>, Output = Isometry<T, R, D>;
307    [val val] => Isometry::from_parts(self * right.translation, right.rotation);
308    [ref val] => Isometry::from_parts(self * &right.translation, right.rotation);
309    [val ref] => Isometry::from_parts(self * &right.translation, right.rotation.clone());
310    [ref ref] => Isometry::from_parts(self * &right.translation, right.rotation.clone());
311);
312
313macro_rules! isometry_from_composition_impl(
314    ($Op: ident, $op: ident;
315     $($Dims: ident),*;
316     $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Output: ty;
317     $action: expr; $($lives: tt),*) => {
318        impl<$($lives ,)* T: SimdRealField $(, const $Dims: usize)*> $Op<$Rhs> for $Lhs
319            where T::Element: SimdRealField {
320            type Output = $Output;
321
322            #[inline]
323            fn $op($lhs, $rhs: $Rhs) -> Self::Output {
324                $action
325            }
326        }
327    }
328);
329
330macro_rules! isometry_from_composition_impl_all(
331    ($Op: ident, $op: ident;
332     $($Dims: ident),*;
333     $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Output: ty;
334     [val val] => $action_val_val: expr;
335     [ref val] => $action_ref_val: expr;
336     [val ref] => $action_val_ref: expr;
337     [ref ref] => $action_ref_ref: expr;) => {
338
339        isometry_from_composition_impl!(
340            $Op, $op;
341            $($Dims),*;
342            $lhs: $Lhs, $rhs: $Rhs, Output = $Output;
343            $action_val_val; );
344
345        isometry_from_composition_impl!(
346            $Op, $op;
347            $($Dims),*;
348            $lhs: &'a $Lhs, $rhs: $Rhs, Output = $Output;
349            $action_ref_val; 'a);
350
351        isometry_from_composition_impl!(
352            $Op, $op;
353            $($Dims),*;
354            $lhs: $Lhs, $rhs: &'b $Rhs, Output = $Output;
355            $action_val_ref; 'b);
356
357        isometry_from_composition_impl!(
358            $Op, $op;
359            $($Dims),*;
360            $lhs: &'a $Lhs, $rhs: &'b $Rhs, Output = $Output;
361            $action_ref_ref; 'a, 'b);
362    }
363);
364
365// Rotation × Translation
366isometry_from_composition_impl_all!(
367    Mul, mul;
368    D;
369    self: Rotation<T, D>, right: Translation<T, D>, Output = Isometry<T, Rotation<T, D>, D>;
370    [val val] => Isometry::from_parts(Translation::from(&self * right.vector),  self);
371    [ref val] => Isometry::from_parts(Translation::from(self * right.vector),   self.clone());
372    [val ref] => Isometry::from_parts(Translation::from(&self * &right.vector), self);
373    [ref ref] => Isometry::from_parts(Translation::from(self * &right.vector),  self.clone());
374);
375
376// UnitQuaternion × Translation
377isometry_from_composition_impl_all!(
378    Mul, mul;
379    ;
380    self: UnitQuaternion<T>, right: Translation<T, 3>,
381    Output = Isometry<T, UnitQuaternion<T>, 3>;
382    [val val] => Isometry::from_parts(Translation::from(&self *  right.vector), self);
383    [ref val] => Isometry::from_parts(Translation::from( self *  right.vector), self.clone());
384    [val ref] => Isometry::from_parts(Translation::from(&self * &right.vector), self);
385    [ref ref] => Isometry::from_parts(Translation::from( self * &right.vector), self.clone());
386);
387
388// Isometry × Rotation
389isometry_from_composition_impl_all!(
390    Mul, mul;
391    D;
392    self: Isometry<T, Rotation<T, D>, D>, rhs: Rotation<T, D>,
393    Output = Isometry<T, Rotation<T, D>, D>;
394    [val val] => Isometry::from_parts(self.translation, self.rotation * rhs);
395    [ref val] => Isometry::from_parts(self.translation.clone(), self.rotation.clone() * rhs);
396    [val ref] => Isometry::from_parts(self.translation, self.rotation * rhs.clone());
397    [ref ref] => Isometry::from_parts(self.translation.clone(), self.rotation.clone() * rhs.clone());
398);
399
400// Rotation × Isometry
401isometry_from_composition_impl_all!(
402    Mul, mul;
403    D;
404    self: Rotation<T, D>, right: Isometry<T, Rotation<T, D>, D>,
405    Output = Isometry<T, Rotation<T, D>, D>;
406    [val val] => &self * &right;
407    [ref val] =>  self * &right;
408    [val ref] => &self * right;
409    [ref ref] => {
410        let shift = self * &right.translation.vector;
411        Isometry::from_parts(Translation::from(shift), self * &right.rotation)
412    };
413);
414
415// Isometry ÷ Rotation
416isometry_from_composition_impl_all!(
417    Div, div;
418    D;
419    self: Isometry<T, Rotation<T, D>, D>, rhs: Rotation<T, D>,
420    Output = Isometry<T, Rotation<T, D>, D>;
421    [val val] => Isometry::from_parts(self.translation, self.rotation / rhs);
422    [ref val] => Isometry::from_parts(self.translation.clone(), self.rotation.clone() / rhs);
423    [val ref] => Isometry::from_parts(self.translation, self.rotation / rhs.clone());
424    [ref ref] => Isometry::from_parts(self.translation.clone(), self.rotation.clone() / rhs.clone());
425);
426
427// Rotation ÷ Isometry
428isometry_from_composition_impl_all!(
429    Div, div;
430    D;
431    self: Rotation<T, D>, right: Isometry<T, Rotation<T, D>, D>,
432    Output = Isometry<T, Rotation<T, D>, D>;
433    // TODO: don't call inverse explicitly?
434    [val val] => #[allow(clippy::suspicious_arithmetic_impl)] { self * right.inverse() };
435    [ref val] => #[allow(clippy::suspicious_arithmetic_impl)] { self * right.inverse() };
436    [val ref] => #[allow(clippy::suspicious_arithmetic_impl)] { self * right.inverse() };
437    [ref ref] => #[allow(clippy::suspicious_arithmetic_impl)] { self * right.inverse() };
438);
439
440// Isometry × UnitQuaternion
441isometry_from_composition_impl_all!(
442    Mul, mul;
443    ;
444    self: Isometry<T, UnitQuaternion<T>, 3>, rhs: UnitQuaternion<T>,
445    Output = Isometry<T, UnitQuaternion<T>, 3>;
446    [val val] => Isometry::from_parts(self.translation, self.rotation * rhs);
447    [ref val] => Isometry::from_parts(self.translation.clone(), self.rotation.clone() * rhs);
448    [val ref] => Isometry::from_parts(self.translation, self.rotation * rhs.clone());
449    [ref ref] => Isometry::from_parts(self.translation.clone(), self.rotation.clone() * rhs.clone());
450);
451
452// UnitQuaternion × Isometry
453isometry_from_composition_impl_all!(
454    Mul, mul;
455    ;
456    self: UnitQuaternion<T>, right: Isometry<T, UnitQuaternion<T>, 3>,
457    Output = Isometry<T, UnitQuaternion<T>, 3>;
458    [val val] => &self * &right;
459    [ref val] =>  self * &right;
460    [val ref] => &self * right;
461    [ref ref] => {
462        let shift = self * &right.translation.vector;
463        Isometry::from_parts(Translation::from(shift), self * &right.rotation)
464    };
465);
466
467// Isometry ÷ UnitQuaternion
468isometry_from_composition_impl_all!(
469    Div, div;
470    ;
471    self: Isometry<T, UnitQuaternion<T>, 3>, rhs: UnitQuaternion<T>,
472    Output = Isometry<T, UnitQuaternion<T>, 3>;
473    [val val] => Isometry::from_parts(self.translation, self.rotation / rhs);
474    [ref val] => Isometry::from_parts(self.translation.clone(), self.rotation.clone() / rhs);
475    [val ref] => Isometry::from_parts(self.translation, self.rotation / rhs.clone());
476    [ref ref] => Isometry::from_parts(self.translation.clone(), self.rotation.clone() / rhs.clone());
477);
478
479// UnitQuaternion ÷ Isometry
480isometry_from_composition_impl_all!(
481    Div, div;
482    ;
483    self: UnitQuaternion<T>, right: Isometry<T, UnitQuaternion<T>, 3>,
484    Output = Isometry<T, UnitQuaternion<T>, 3>;
485    // TODO: don't call inverse explicitly?
486    [val val] => #[allow(clippy::suspicious_arithmetic_impl)] { self * right.inverse() };
487    [ref val] => #[allow(clippy::suspicious_arithmetic_impl)] { self * right.inverse() };
488    [val ref] => #[allow(clippy::suspicious_arithmetic_impl)] { self * right.inverse() };
489    [ref ref] => #[allow(clippy::suspicious_arithmetic_impl)] { self * right.inverse() };
490);
491
492// Translation × Rotation
493isometry_from_composition_impl_all!(
494    Mul, mul;
495    D;
496    self: Translation<T, D>, right: Rotation<T, D>, Output = Isometry<T, Rotation<T, D>, D>;
497    [val val] => Isometry::from_parts(self, right);
498    [ref val] => Isometry::from_parts(self.clone(), right);
499    [val ref] => Isometry::from_parts(self, right.clone());
500    [ref ref] => Isometry::from_parts(self.clone(), right.clone());
501);
502
503// Translation × UnitQuaternion
504isometry_from_composition_impl_all!(
505    Mul, mul;
506    ;
507    self: Translation<T, 3>, right: UnitQuaternion<T>, Output = Isometry<T, UnitQuaternion<T>, 3>;
508    [val val] => Isometry::from_parts(self, right);
509    [ref val] => Isometry::from_parts(self.clone(), right);
510    [val ref] => Isometry::from_parts(self, right.clone());
511    [ref ref] => Isometry::from_parts(self.clone(), right.clone());
512);
513
514// Isometry × UnitComplex
515isometry_from_composition_impl_all!(
516    Mul, mul;
517    ;
518    self: Isometry<T, UnitComplex<T>, 2>, rhs: UnitComplex<T>,
519    Output = Isometry<T, UnitComplex<T>, 2>;
520    [val val] => Isometry::from_parts(self.translation, self.rotation * rhs);
521    [ref val] => Isometry::from_parts(self.translation.clone(), self.rotation.clone() * rhs);
522    [val ref] => Isometry::from_parts(self.translation, self.rotation * rhs.clone());
523    [ref ref] => Isometry::from_parts(self.translation.clone(), self.rotation.clone() * rhs.clone());
524);
525
526// Isometry ÷ UnitComplex
527isometry_from_composition_impl_all!(
528    Div, div;
529    ;
530    self: Isometry<T, UnitComplex<T>, 2>, rhs: UnitComplex<T>,
531    Output = Isometry<T, UnitComplex<T>, 2>;
532    [val val] => Isometry::from_parts(self.translation, self.rotation / rhs);
533    [ref val] => Isometry::from_parts(self.translation.clone(), self.rotation.clone() / rhs);
534    [val ref] => Isometry::from_parts(self.translation, self.rotation / rhs.clone());
535    [ref ref] => Isometry::from_parts(self.translation.clone(), self.rotation.clone() / rhs.clone());
536);
nalgebra/geometry/isometry_ops.rs

nalgebra/geometry/
isometry_ops.rs
