1 use std::ops::{Div, DivAssign, Mul, MulAssign};
2 
3 use alga::general::RealField;
4 use alga::linear::Rotation as AlgaRotation;
5 
6 use crate::base::allocator::Allocator;
7 use crate::base::dimension::{DimName, U1, U3, U4};
8 use crate::base::{DefaultAllocator, Unit, VectorN};
9 
10 use crate::geometry::{Isometry, Point, Rotation, Translation, UnitQuaternion};
11 
12 // FIXME: there are several cloning of rotations that we could probably get rid of (but we didn't
13 // yet because that would require to add a bound like `where for<'a, 'b> &'a R: Mul<&'b R, Output = R>`
14 // which is quite ugly.
15 
16 /*
17  *
18  * In this file, we provide:
19  * =========================
20  *
21  *
22  * (Operators)
23  *
24  * Isometry × Isometry
25  * Isometry × R
26  *
27  *
28  * Isometry ÷ Isometry
29  * Isometry ÷ R
30  *
31  * Isometry × Point
32  * Isometry × Vector
33  * Isometry × Unit<Vector>
34  *
35  *
36  * Isometry    × Translation
37  * Translation × Isometry
38  * Translation × R           -> Isometry<R>
39  *
40  * NOTE: The following are provided explicitly because we can't have R × Isometry.
41  * Rotation       × Isometry<Rotation>
42  * UnitQuaternion × Isometry<UnitQuaternion>
43  *
44  * Rotation       ÷ Isometry<Rotation>
45  * UnitQuaternion ÷ Isometry<UnitQuaternion>
46  *
47  * Rotation       × Translation -> Isometry<Rotation>
48  * UnitQuaternion × Translation -> Isometry<UnitQuaternion>
49  *
50  *
51  * (Assignment Operators)
52  *
53  * Isometry ×= Translation
54  *
55  * Isometry ×= Isometry
56  * Isometry ×= R
57  *
58  * Isometry ÷= Isometry
59  * Isometry ÷= R
60  *
61  */
62 
63 macro_rules! isometry_binop_impl(
64     ($Op: ident, $op: ident;
65      $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Output: ty;
66      $action: expr; $($lives: tt),*) => {
67         impl<$($lives ,)* N: RealField, D: DimName, R> $Op<$Rhs> for $Lhs
68             where R: AlgaRotation<Point<N, D>>,
69                   DefaultAllocator: Allocator<N, D> {
70             type Output = $Output;
71 
72             #[inline]
73             fn $op($lhs, $rhs: $Rhs) -> Self::Output {
74                 $action
75             }
76         }
77     }
78 );
79 
80 macro_rules! isometry_binop_impl_all(
81     ($Op: ident, $op: ident;
82      $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Output: ty;
83      [val val] => $action_val_val: expr;
84      [ref val] => $action_ref_val: expr;
85      [val ref] => $action_val_ref: expr;
86      [ref ref] => $action_ref_ref: expr;) => {
87         isometry_binop_impl!(
88             $Op, $op;
89             $lhs: $Lhs, $rhs: $Rhs, Output = $Output;
90             $action_val_val; );
91 
92         isometry_binop_impl!(
93             $Op, $op;
94             $lhs: &'a $Lhs, $rhs: $Rhs, Output = $Output;
95             $action_ref_val; 'a);
96 
97         isometry_binop_impl!(
98             $Op, $op;
99             $lhs: $Lhs, $rhs: &'b $Rhs, Output = $Output;
100             $action_val_ref; 'b);
101 
102         isometry_binop_impl!(
103             $Op, $op;
104             $lhs: &'a $Lhs, $rhs: &'b $Rhs, Output = $Output;
105             $action_ref_ref; 'a, 'b);
106     }
107 );
108 
109 macro_rules! isometry_binop_assign_impl_all(
110     ($OpAssign: ident, $op_assign: ident;
111      $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty;
112      [val] => $action_val: expr;
113      [ref] => $action_ref: expr;) => {
114         impl<N: RealField, D: DimName, R> $OpAssign<$Rhs> for $Lhs
115             where R: AlgaRotation<Point<N, D>>,
116                   DefaultAllocator: Allocator<N, D> {
117             #[inline]
118             fn $op_assign(&mut $lhs, $rhs: $Rhs) {
119                 $action_val
120             }
121         }
122 
123         impl<'b, N: RealField, D: DimName, R> $OpAssign<&'b $Rhs> for $Lhs
124             where R: AlgaRotation<Point<N, D>>,
125                   DefaultAllocator: Allocator<N, D> {
126             #[inline]
127             fn $op_assign(&mut $lhs, $rhs: &'b $Rhs) {
128                 $action_ref
129             }
130         }
131     }
132 );
133 
134 // Isometry × Isometry
135 // Isometry ÷ Isometry
136 isometry_binop_impl_all!(
137     Mul, mul;
138     self: Isometry<N, D, R>, rhs: Isometry<N, D, R>, Output = Isometry<N, D, R>;
139     [val val] => &self * &rhs;
140     [ref val] => self * &rhs;
141     [val ref] => &self * rhs;
142     [ref ref] => {
143         let shift = self.rotation.transform_vector(&rhs.translation.vector);
144 
145         Isometry::from_parts(Translation::from(&self.translation.vector + shift),
146                              self.rotation.clone() * rhs.rotation.clone()) // FIXME: too bad we have to clone.
147     };
148 );
149 
150 isometry_binop_impl_all!(
151     Div, div;
152     self: Isometry<N, D, R>, rhs: Isometry<N, D, R>, Output = Isometry<N, D, R>;
153     [val val] => self * rhs.inverse();
154     [ref val] => self * rhs.inverse();
155     [val ref] => self * rhs.inverse();
156     [ref ref] => self * rhs.inverse();
157 );
158 
159 // Isometry ×= Translation
160 isometry_binop_assign_impl_all!(
161     MulAssign, mul_assign;
162     self: Isometry<N, D, R>, rhs: Translation<N, D>;
163     [val] => *self *= &rhs;
164     [ref] => {
165         let shift = self.rotation.transform_vector(&rhs.vector);
166         self.translation.vector += shift;
167     };
168 );
169 
170 // Isometry ×= Isometry
171 // Isometry ÷= Isometry
172 isometry_binop_assign_impl_all!(
173     MulAssign, mul_assign;
174     self: Isometry<N, D, R>, rhs: Isometry<N, D, R>;
175     [val] => *self *= &rhs;
176     [ref] => {
177         let shift = self.rotation.transform_vector(&rhs.translation.vector);
178         self.translation.vector += shift;
179         self.rotation *= rhs.rotation.clone();
180     };
181 );
182 
183 isometry_binop_assign_impl_all!(
184     DivAssign, div_assign;
185     self: Isometry<N, D, R>, rhs: Isometry<N, D, R>;
186     [val] => *self /= &rhs;
187     [ref] => *self *= rhs.inverse();
188 );
189 
190 // Isometry ×= R
191 // Isometry ÷= R
192 isometry_binop_assign_impl_all!(
193     MulAssign, mul_assign;
194     self: Isometry<N, D, R>, rhs: R;
195     [val] => self.rotation *= rhs;
196     [ref] => self.rotation *= rhs.clone();
197 );
198 
199 isometry_binop_assign_impl_all!(
200     DivAssign, div_assign;
201     self: Isometry<N, D, R>, rhs: R;
202     // FIXME: don't invert explicitly?
203     [val] => *self *= rhs.two_sided_inverse();
204     [ref] => *self *= rhs.two_sided_inverse();
205 );
206 
207 // Isometry × R
208 // Isometry ÷ R
209 isometry_binop_impl_all!(
210     Mul, mul;
211     self: Isometry<N, D, R>, rhs: R, Output = Isometry<N, D, R>;
212     [val val] => Isometry::from_parts(self.translation, self.rotation * rhs);
213     [ref val] => Isometry::from_parts(self.translation.clone(), self.rotation.clone() * rhs); // FIXME: do not clone.
214     [val ref] => Isometry::from_parts(self.translation, self.rotation * rhs.clone());
215     [ref ref] => Isometry::from_parts(self.translation.clone(), self.rotation.clone() * rhs.clone());
216 );
217 
218 isometry_binop_impl_all!(
219     Div, div;
220     self: Isometry<N, D, R>, rhs: R, Output = Isometry<N, D, R>;
221     [val val] => Isometry::from_parts(self.translation, self.rotation / rhs);
222     [ref val] => Isometry::from_parts(self.translation.clone(), self.rotation.clone() / rhs);
223     [val ref] => Isometry::from_parts(self.translation, self.rotation / rhs.clone());
224     [ref ref] => Isometry::from_parts(self.translation.clone(), self.rotation.clone() / rhs.clone());
225 );
226 
227 // Isometry × Point
228 isometry_binop_impl_all!(
229     Mul, mul;
230     self: Isometry<N, D, R>, right: Point<N, D>, Output = Point<N, D>;
231     [val val] => self.translation  * self.rotation.transform_point(&right);
232     [ref val] => &self.translation * self.rotation.transform_point(&right);
233     [val ref] => self.translation  * self.rotation.transform_point(right);
234     [ref ref] => &self.translation * self.rotation.transform_point(right);
235 );
236 
237 // Isometry × Vector
238 isometry_binop_impl_all!(
239     Mul, mul;
240     // FIXME: because of `transform_vector`, we cant use a generic storage type for the rhs vector,
241     // i.e., right: Vector<N, D, S> where S: Storage<N, D>.
242     self: Isometry<N, D, R>, right: VectorN<N, D>, Output = VectorN<N, D>;
243     [val val] => self.rotation.transform_vector(&right);
244     [ref val] => self.rotation.transform_vector(&right);
245     [val ref] => self.rotation.transform_vector(right);
246     [ref ref] => self.rotation.transform_vector(right);
247 );
248 
249 // Isometry × Unit<Vector>
250 isometry_binop_impl_all!(
251     Mul, mul;
252     // FIXME: because of `transform_vector`, we cant use a generic storage type for the rhs vector,
253     // i.e., right: Vector<N, D, S> where S: Storage<N, D>.
254     self: Isometry<N, D, R>, right: Unit<VectorN<N, D>>, Output = Unit<VectorN<N, D>>;
255     [val val] => Unit::new_unchecked(self.rotation.transform_vector(right.as_ref()));
256     [ref val] => Unit::new_unchecked(self.rotation.transform_vector(right.as_ref()));
257     [val ref] => Unit::new_unchecked(self.rotation.transform_vector(right.as_ref()));
258     [ref ref] => Unit::new_unchecked(self.rotation.transform_vector(right.as_ref()));
259 );
260 
261 // Isometry × Translation
262 isometry_binop_impl_all!(
263     Mul, mul;
264     self: Isometry<N, D, R>, right: Translation<N, D>, Output = Isometry<N, D, R>;
265     [val val] => &self * &right;
266     [ref val] => self * &right;
267     [val ref] => &self * right;
268     [ref ref] => {
269         let new_tr = &self.translation.vector + self.rotation.transform_vector(&right.vector);
270         Isometry::from_parts(Translation::from(new_tr), self.rotation.clone())
271     };
272 );
273 
274 // Translation × Isometry
275 isometry_binop_impl_all!(
276     Mul, mul;
277     self: Translation<N, D>, right: Isometry<N, D, R>, Output = Isometry<N, D, R>;
278     [val val] => Isometry::from_parts(self * right.translation, right.rotation);
279     [ref val] => Isometry::from_parts(self * &right.translation, right.rotation);
280     [val ref] => Isometry::from_parts(self * &right.translation, right.rotation.clone());
281     [ref ref] => Isometry::from_parts(self * &right.translation, right.rotation.clone());
282 );
283 
284 macro_rules! isometry_from_composition_impl(
285     ($Op: ident, $op: ident;
286      ($R1: ty, $C1: ty),($R2: ty, $C2: ty) $(for $Dims: ident: $DimsBound: ident),*;
287      $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Output: ty;
288      $action: expr; $($lives: tt),*) => {
289         impl<$($lives ,)* N: RealField $(, $Dims: $DimsBound)*> $Op<$Rhs> for $Lhs
290             where DefaultAllocator: Allocator<N, $R1, $C1> +
291                                     Allocator<N, $R2, $C2> {
292             type Output = $Output;
293 
294             #[inline]
295             fn $op($lhs, $rhs: $Rhs) -> Self::Output {
296                 $action
297             }
298         }
299     }
300 );
301 
302 macro_rules! isometry_from_composition_impl_all(
303     ($Op: ident, $op: ident;
304      ($R1: ty, $C1: ty),($R2: ty, $C2: ty) $(for $Dims: ident: $DimsBound: ident),*;
305      $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Output: ty;
306      [val val] => $action_val_val: expr;
307      [ref val] => $action_ref_val: expr;
308      [val ref] => $action_val_ref: expr;
309      [ref ref] => $action_ref_ref: expr;) => {
310 
311         isometry_from_composition_impl!(
312             $Op, $op;
313             ($R1, $C1),($R2, $C2) $(for $Dims: $DimsBound),*;
314             $lhs: $Lhs, $rhs: $Rhs, Output = $Output;
315             $action_val_val; );
316 
317         isometry_from_composition_impl!(
318             $Op, $op;
319             ($R1, $C1),($R2, $C2) $(for $Dims: $DimsBound),*;
320             $lhs: &'a $Lhs, $rhs: $Rhs, Output = $Output;
321             $action_ref_val; 'a);
322 
323         isometry_from_composition_impl!(
324             $Op, $op;
325             ($R1, $C1),($R2, $C2) $(for $Dims: $DimsBound),*;
326             $lhs: $Lhs, $rhs: &'b $Rhs, Output = $Output;
327             $action_val_ref; 'b);
328 
329         isometry_from_composition_impl!(
330             $Op, $op;
331             ($R1, $C1),($R2, $C2) $(for $Dims: $DimsBound),*;
332             $lhs: &'a $Lhs, $rhs: &'b $Rhs, Output = $Output;
333             $action_ref_ref; 'a, 'b);
334     }
335 );
336 
337 // Rotation × Translation
338 isometry_from_composition_impl_all!(
339     Mul, mul;
340     (D, D), (D, U1) for D: DimName;
341     self: Rotation<N, D>, right: Translation<N, D>, Output = Isometry<N, D, Rotation<N, D>>;
342     [val val] => Isometry::from_parts(Translation::from(&self * right.vector),  self);
343     [ref val] => Isometry::from_parts(Translation::from(self * right.vector),   self.clone());
344     [val ref] => Isometry::from_parts(Translation::from(&self * &right.vector), self);
345     [ref ref] => Isometry::from_parts(Translation::from(self * &right.vector),  self.clone());
346 );
347 
348 // UnitQuaternion × Translation
349 isometry_from_composition_impl_all!(
350     Mul, mul;
351     (U4, U1), (U3, U1);
352     self: UnitQuaternion<N>, right: Translation<N, U3>,
353     Output = Isometry<N, U3, UnitQuaternion<N>>;
354     [val val] => Isometry::from_parts(Translation::from(&self *  right.vector), self);
355     [ref val] => Isometry::from_parts(Translation::from( self *  right.vector), self.clone());
356     [val ref] => Isometry::from_parts(Translation::from(&self * &right.vector), self);
357     [ref ref] => Isometry::from_parts(Translation::from( self * &right.vector), self.clone());
358 );
359 
360 // Rotation × Isometry
361 isometry_from_composition_impl_all!(
362     Mul, mul;
363     (D, D), (D, U1) for D: DimName;
364     self: Rotation<N, D>, right: Isometry<N, D, Rotation<N, D>>,
365     Output = Isometry<N, D, Rotation<N, D>>;
366     [val val] => &self * &right;
367     [ref val] =>  self * &right;
368     [val ref] => &self * right;
369     [ref ref] => {
370         let shift = self * &right.translation.vector;
371         Isometry::from_parts(Translation::from(shift), self * &right.rotation)
372     };
373 );
374 
375 // Rotation ÷ Isometry
376 isometry_from_composition_impl_all!(
377     Div, div;
378     (D, D), (D, U1) for D: DimName;
379     self: Rotation<N, D>, right: Isometry<N, D, Rotation<N, D>>,
380     Output = Isometry<N, D, Rotation<N, D>>;
381     // FIXME: don't call inverse explicitly?
382     [val val] => self * right.inverse();
383     [ref val] => self * right.inverse();
384     [val ref] => self * right.inverse();
385     [ref ref] => self * right.inverse();
386 );
387 
388 // UnitQuaternion × Isometry
389 isometry_from_composition_impl_all!(
390     Mul, mul;
391     (U4, U1), (U3, U1);
392     self: UnitQuaternion<N>, right: Isometry<N, U3, UnitQuaternion<N>>,
393     Output = Isometry<N, U3, UnitQuaternion<N>>;
394     [val val] => &self * &right;
395     [ref val] =>  self * &right;
396     [val ref] => &self * right;
397     [ref ref] => {
398         let shift = self * &right.translation.vector;
399         Isometry::from_parts(Translation::from(shift), self * &right.rotation)
400     };
401 );
402 
403 // UnitQuaternion ÷ Isometry
404 isometry_from_composition_impl_all!(
405     Div, div;
406     (U4, U1), (U3, U1);
407     self: UnitQuaternion<N>, right: Isometry<N, U3, UnitQuaternion<N>>,
408     Output = Isometry<N, U3, UnitQuaternion<N>>;
409     // FIXME: don't call inverse explicitly?
410     [val val] => self * right.inverse();
411     [ref val] => self * right.inverse();
412     [val ref] => self * right.inverse();
413     [ref ref] => self * right.inverse();
414 );
415 
416 // Translation × Rotation
417 isometry_from_composition_impl_all!(
418     Mul, mul;
419     (D, D), (D, U1) for D: DimName;
420     self: Translation<N, D>, right: Rotation<N, D>, Output = Isometry<N, D, Rotation<N, D>>;
421     [val val] => Isometry::from_parts(self, right);
422     [ref val] => Isometry::from_parts(self.clone(), right);
423     [val ref] => Isometry::from_parts(self, right.clone());
424     [ref ref] => Isometry::from_parts(self.clone(), right.clone());
425 );
426 
427 // Translation × UnitQuaternion
428 isometry_from_composition_impl_all!(
429     Mul, mul;
430     (U4, U1), (U3, U1);
431     self: Translation<N, U3>, right: UnitQuaternion<N>, Output = Isometry<N, U3, UnitQuaternion<N>>;
432     [val val] => Isometry::from_parts(self, right);
433     [ref val] => Isometry::from_parts(self.clone(), right);
434     [val ref] => Isometry::from_parts(self, right.clone());
435     [ref ref] => Isometry::from_parts(self.clone(), right.clone());
436 );
437