wedged/subspace/
versor.rs

1
2use super::*;
3
4//
5//Applying the versor operations
6//
7
8///
9/// Implemented on versor types in order to apply their transformation to various values
10///
11/// This is mainly intended as a driver for member fuctions of the different versor types
12/// (eg [`Rotor::rot()`], [`Versor::apply()`], etc) and not to be used on its own.
13///
14pub trait VersorMul<Rhs>: Sized {
15    type Output;
16    fn versor_mul(self, rhs: Rhs) -> Self::Output;
17}
18
19macro_rules! impl_versor_mul {
20
21    () => {};
22
23    (
24        @impl
25        $(&$a:lifetime)? $Ty1:ident<T1:$Alloc1:ident, N $(,$G1:ident)*> *
26        $(&$b:lifetime)? $Ty2:ident<T2:$Alloc2:ident, N $(,$G2:ident)*> ;
27        $($rest:tt)*
28    ) => {
29
30        impl<$($a,)? $($b,)? T1, T2, U, N:Dim $(, $G1:Dim)* $(, $G2:Dim)*>
31            VersorMul<$(&$b)? $Ty2<T2,N $(,$G2)?>> for $(&$a)? $Ty1<T1,N $(,$G1)?>
32        where
33            T1: $Alloc1<N $(,$G1)*> + AllRefMul<T2, AllOutput=U>,
34            T2: $Alloc2<N $(,$G2)*>,
35            U: $Alloc2<N $(,$G2)*> + for<'c> Mul<&'c T1, Output=U> + AddGroup,
36        {
37            type Output = $Ty2<U,N $(,$G2)?>;
38            fn versor_mul(self, rhs: $(&$b)? $Ty2<T2,N $(,$G2)?>) -> $Ty2<U,N $(,$G2)?> {
39                use crate::algebra::MultivectorSrc;
40                let shape = rhs.shape();
41                versor_mul_selected(self.odd(), self, rhs, shape)
42            }
43        }
44
45        impl_versor_mul!($($rest)*);
46
47    };
48
49    (
50        $Ty1:ident<T1:$Alloc1:ident, N $(,$G1:ident)*> *
51        $Ty2:ident<T2:$Alloc2:ident, N $(,$G2:ident)*> ;
52        $($rest:tt)*
53    ) => {
54        impl_versor_mul!(
55            @impl     $Ty1<T1:$Alloc1, N $(,$G1)*> * $Ty2<T2:$Alloc2, N $(,$G2)*>;
56            @impl &'a $Ty1<T1:$Alloc1, N $(,$G1)*> * $Ty2<T2:$Alloc2, N $(,$G2)*>;
57            @impl     $Ty1<T1:$Alloc1, N $(,$G1)*> * &'b $Ty2<T2:$Alloc2, N $(,$G2)*>;
58            @impl &'a $Ty1<T1:$Alloc1, N $(,$G1)*> * &'b $Ty2<T2:$Alloc2, N $(,$G2)*>;
59            $($rest)*
60        );
61    }
62
63}
64
65impl_versor_mul!(
66    UnitBlade<T1:AllocBlade,N,G1> * Blade<T2:AllocBlade,N,G2>;
67    UnitBlade<T1:AllocBlade,N,G1> * SimpleBlade<T2:AllocBlade,N,G2>;
68    UnitBlade<T1:AllocBlade,N,G1> * UnitBlade<T2:AllocBlade,N,G2>;
69    UnitBlade<T1:AllocBlade,N,G1> * Even<T2:AllocEven,N>;
70    UnitBlade<T1:AllocBlade,N,G1> * Rotor<T2:AllocEven,N>;
71    UnitBlade<T1:AllocBlade,N,G1> * Odd<T2:AllocOdd,N>;
72    UnitBlade<T1:AllocBlade,N,G1> * Reflector<T2:AllocOdd,N>;
73    UnitBlade<T1:AllocBlade,N,G1> * Multivector<T2:AllocMultivector,N>;
74
75    Rotor<T1:AllocEven,N> * Blade<T2:AllocBlade,N,G2>;
76    Rotor<T1:AllocEven,N> * SimpleBlade<T2:AllocBlade,N,G2>;
77    Rotor<T1:AllocEven,N> * UnitBlade<T2:AllocBlade,N,G2>;
78    Rotor<T1:AllocEven,N> * Even<T2:AllocEven,N>;
79    Rotor<T1:AllocEven,N> * Rotor<T2:AllocEven,N>;
80    Rotor<T1:AllocEven,N> * Odd<T2:AllocOdd,N>;
81    Rotor<T1:AllocEven,N> * Reflector<T2:AllocOdd,N>;
82    Rotor<T1:AllocEven,N> * Multivector<T2:AllocMultivector,N>;
83
84    Reflector<T1:AllocOdd,N> * Blade<T2:AllocBlade,N,G2>;
85    Reflector<T1:AllocOdd,N> * SimpleBlade<T2:AllocBlade,N,G2>;
86    Reflector<T1:AllocOdd,N> * UnitBlade<T2:AllocBlade,N,G2>;
87    Reflector<T1:AllocOdd,N> * Even<T2:AllocEven,N>;
88    Reflector<T1:AllocOdd,N> * Rotor<T2:AllocEven,N>;
89    Reflector<T1:AllocOdd,N> * Odd<T2:AllocOdd,N>;
90    Reflector<T1:AllocOdd,N> * Reflector<T2:AllocOdd,N>;
91    Reflector<T1:AllocOdd,N> * Multivector<T2:AllocMultivector,N>;
92);
93
94macro_rules! impl_non_unit_versor_mul {
95
96    () => {};
97
98    (
99        @impl
100        $(&$a:lifetime)? $Ty1:ident<T1:$Alloc1:ident, N $(,$G1:ident)*> *
101        $(&$b:lifetime)? $Ty2:ident<T2:$Alloc2:ident, N $(,$G2:ident)*>
102        $({$($where:tt)*})?;
103        $($rest:tt)*
104    ) => {
105
106        impl<$($a,)? $($b,)? T1, T2, U, N:Dim $(, $G1:Dim)* $(, $G2:Dim)*>
107            VersorMul<$(&$b)? $Ty2<T2,N $(,$G2)?>> for $(&$a)? $Ty1<T1,N $(,$G1)?>
108        where
109            T1: $Alloc1<N $(,$G1)*> + AllRefMul<T2, AllOutput=U> + AllRefMul<T1, AllOutput=U>,
110            T2: $Alloc2<N $(,$G2)*>,
111            U: $Alloc2<N $(,$G2)*> + for<'c> Mul<&'c T1, Output=U> + AddGroup,
112            U: for<'c> Div<&'c U, Output=U>,
113            $($($where)*)?
114        {
115            type Output = $Ty2<U,N $(,$G2)?>;
116            fn versor_mul(self, rhs: $(&$b)? $Ty2<T2,N $(,$G2)?>) -> $Ty2<U,N $(,$G2)?> {
117                use crate::algebra::MultivectorSrc;
118                let shape = rhs.shape();
119                let norm_sqrd = self.norm_sqrd();
120
121                let mut res: Self::Output = versor_mul_selected(self.odd(), self, rhs, shape);
122                res = res.inv_scale(&norm_sqrd);
123                res
124            }
125        }
126
127        impl_non_unit_versor_mul!($($rest)*);
128
129    };
130
131    (
132        $Ty1:ident<T1:$Alloc1:ident, N $(,$G1:ident)*> *
133        $Ty2:ident<T2:$Alloc2:ident, N $(,$G2:ident)*>
134        $({$($where:tt)*})?;
135        $($rest:tt)*
136    ) => {
137        impl_non_unit_versor_mul!(
138            @impl     $Ty1<T1:$Alloc1, N $(,$G1)*> * $Ty2<T2:$Alloc2, N $(,$G2)*> $({$($where)*})?;
139            @impl &'a $Ty1<T1:$Alloc1, N $(,$G1)*> * $Ty2<T2:$Alloc2, N $(,$G2)*> $({$($where)*})?;
140            @impl     $Ty1<T1:$Alloc1, N $(,$G1)*> * &'b $Ty2<T2:$Alloc2, N $(,$G2)*> $({$($where)*})?;
141            @impl &'a $Ty1<T1:$Alloc1, N $(,$G1)*> * &'b $Ty2<T2:$Alloc2, N $(,$G2)*> $({$($where)*})?;
142            $($rest)*
143        );
144    }
145
146}
147
148impl_non_unit_versor_mul!(
149    Blade<T1:AllocSimpleBlade,N,G1> * Blade<T2:AllocBlade,N,G2>;
150    Blade<T1:AllocSimpleBlade,N,G1> * SimpleBlade<T2:AllocBlade,N,G2>;
151    Blade<T1:AllocSimpleBlade,N,G1> * Even<T2:AllocEven,N>;
152    Blade<T1:AllocSimpleBlade,N,G1> * Odd<T2:AllocOdd,N>;
153    Blade<T1:AllocSimpleBlade,N,G1> * Multivector<T2:AllocMultivector,N>;
154
155    SimpleBlade<T1:AllocBlade,N,G1> * Blade<T2:AllocBlade,N,G2>;
156    SimpleBlade<T1:AllocBlade,N,G1> * SimpleBlade<T2:AllocBlade,N,G2>;
157    SimpleBlade<T1:AllocBlade,N,G1> * Even<T2:AllocEven,N>;
158    SimpleBlade<T1:AllocBlade,N,G1> * Odd<T2:AllocOdd,N>;
159    SimpleBlade<T1:AllocBlade,N,G1> * Multivector<T2:AllocMultivector,N>;
160
161    Even<T1:AllocEven,N> * Blade<T2:AllocBlade,N,G2> {N:ToTypenum, AsTypenum<N>: IsLessOrEqual<U3,Output=True>};
162    Even<T1:AllocEven,N> * SimpleBlade<T2:AllocBlade,N,G2> {N:ToTypenum, AsTypenum<N>: IsLessOrEqual<U3,Output=True>};
163    Even<T1:AllocEven,N> * Even<T2:AllocEven,N> {N:ToTypenum, AsTypenum<N>: IsLessOrEqual<U3,Output=True>};
164    Even<T1:AllocEven,N> * Odd<T2:AllocOdd,N> {N:ToTypenum, AsTypenum<N>: IsLessOrEqual<U3,Output=True>};
165    Even<T1:AllocEven,N> * Multivector<T2:AllocMultivector,N> {N:ToTypenum, AsTypenum<N>: IsLessOrEqual<U3,Output=True>};
166
167    Odd<T1:AllocOdd,N> * Blade<T2:AllocBlade,N,G2> {N:ToTypenum, AsTypenum<N>: IsLessOrEqual<U3,Output=True>};
168    Odd<T1:AllocOdd,N> * SimpleBlade<T2:AllocBlade,N,G2> {N:ToTypenum, AsTypenum<N>: IsLessOrEqual<U3,Output=True>};
169    Odd<T1:AllocOdd,N> * Even<T2:AllocEven,N> {N:ToTypenum, AsTypenum<N>: IsLessOrEqual<U3,Output=True>};
170    Odd<T1:AllocOdd,N> * Odd<T2:AllocOdd,N> {N:ToTypenum, AsTypenum<N>: IsLessOrEqual<U3,Output=True>};
171    Odd<T1:AllocOdd,N> * Multivector<T2:AllocMultivector,N> {N:ToTypenum, AsTypenum<N>: IsLessOrEqual<U3,Output=True>};
172);
173
174macro_rules! versor_versor_mul {
175
176    ($Ty:ident<T:$Alloc:ident, N $(,$G:ident)*>; $($a:lifetime)?; $($b:lifetime)?) => {
177
178        impl<$($a,)? $($b,)? T1, T2, U, N:Dim $(, $G:Dim)*>
179            VersorMul<$(&$b)? $Ty<T2,N $(,$G)?>> for $(&$a)? Versor<T1,N>
180        where
181            T1: AllocVersor<N> + AllRefMul<T2, AllOutput=U>,
182            T2: $Alloc<N $(,$G)*>,
183            U: $Alloc<N $(,$G)*> + for<'c> Mul<&'c T1, Output=U> + AddGroup,
184        {
185            type Output = $Ty<U,N $(,$G)?>;
186            fn versor_mul(self, rhs: $(&$b)? $Ty<T2,N $(,$G)*>) -> $Ty<U,N $(,$G)*> {
187                match self {
188                    Versor::Even(r) => r.versor_mul(rhs),
189                    Versor::Odd(r) => r.versor_mul(rhs),
190                }
191            }
192        }
193
194    };
195
196    () => {};
197    ($Ty:ident<T:$Alloc:ident, N $(,$G:ident)*>; $($rest:tt)*) => {
198        versor_versor_mul!($Ty<T:$Alloc, N $(,$G)*>;   ; );
199        versor_versor_mul!($Ty<T:$Alloc, N $(,$G)*>; 'a; );
200        versor_versor_mul!($Ty<T:$Alloc, N $(,$G)*>;   ; 'b);
201        versor_versor_mul!($Ty<T:$Alloc, N $(,$G)*>; 'a; 'b);
202        versor_versor_mul!($($rest)*);
203    };
204
205}
206
207versor_versor_mul!(
208    Blade<T:AllocBlade,N,G>;
209    SimpleBlade<T:AllocBlade,N,G>;
210    UnitBlade<T:AllocBlade,N,G>;
211    Even<T:AllocEven,N>;
212    Rotor<T:AllocEven,N>;
213    Odd<T:AllocOdd,N>;
214    Reflector<T:AllocOdd,N>;
215    Multivector<T:AllocMultivector,N>;
216);
217
218macro_rules! versor_mul_versor {
219
220    ($Ty:ident<T:$Alloc:ident, N $(,$G:ident)*>; $($a:lifetime)?; $($b:lifetime)?) => {
221
222        impl<$($a,)? $($b,)? T1, T2, U, N:Dim $(, $G:Dim)*>
223            VersorMul<$(&$b)? Versor<T2,N>> for $(&$a)? $Ty<T1,N $(,$G)?>
224        where
225            T1: $Alloc<N $(,$G)*> + AllRefMul<T2, AllOutput=U>,
226            T2: AllocVersor<N>,
227            U: AllocVersor<N> + for<'c> Mul<&'c T1, Output=U> + AddGroup,
228        {
229            type Output = Versor<U,N>;
230            fn versor_mul(self, rhs: $(&$b)? Versor<T2,N>) -> Versor<U,N> {
231                match rhs {
232                    Versor::Even(r) => Versor::Even(self.versor_mul(r)),
233                    Versor::Odd(r) => Versor::Odd(self.versor_mul(r)),
234                }
235            }
236        }
237
238    };
239
240    () => {};
241    ($Ty:ident<T:$Alloc:ident, N $(,$G:ident)*>; $($rest:tt)*) => {
242        versor_mul_versor!($Ty<T:$Alloc, N $(,$G)*>;   ; );
243        versor_mul_versor!($Ty<T:$Alloc, N $(,$G)*>; 'a; );
244        versor_mul_versor!($Ty<T:$Alloc, N $(,$G)*>;   ; 'b);
245        versor_mul_versor!($Ty<T:$Alloc, N $(,$G)*>; 'a; 'b);
246        versor_mul_versor!($($rest)*);
247    };
248
249}
250
251versor_mul_versor!(
252    UnitBlade<T:AllocBlade,N,G>;
253    Rotor<T:AllocEven,N>;
254    Reflector<T:AllocOdd,N>;
255    Versor<T:AllocVersor,N>;
256);
257
258impl<T:AllocBlade<N, U1>, N:Dim> VecN<T,N> {
259    /// Reflects about `self`
260    pub fn reflect<'a,M>(&'a self, m:M) -> <&'a Self as VersorMul<M>>::Output where &'a Self: VersorMul<M> {
261        self.versor_mul(m)
262    }
263}
264
265impl<T:AllocBlade<N, U1>, N:Dim> SimpleVecN<T,N> {
266    /// Reflects about `self`
267    pub fn reflect<'a,M>(&'a self, m:M) -> <&'a Self as VersorMul<M>>::Output where &'a Self: VersorMul<M> {
268        self.versor_mul(m)
269    }
270}
271
272impl<T:AllocBlade<N, U1>, N:Dim> UnitVecN<T,N> {
273    /// Reflects about `self`
274    pub fn reflect<'a,M>(&'a self, m:M) -> <&'a Self as VersorMul<M>>::Output where &'a Self: VersorMul<M> {
275        self.versor_mul(m)
276    }
277}
278
279impl<T:AllocEven<N>, N:Dim> Rotor<T,N> {
280
281    /// Rotates the given input by self
282    pub fn rot<'a,M>(&'a self, m:M) -> <&'a Self as VersorMul<M>>::Output where &'a Self: VersorMul<M> {
283        self.versor_mul(m)
284    }
285
286}
287
288impl<T:AllocOdd<N>, N:Dim> Reflector<T,N> {
289
290    /// Applies the reflection and rotations of self
291    pub fn apply<'a,M>(&'a self, m:M) -> <&'a Self as VersorMul<M>>::Output where &'a Self: VersorMul<M> {
292        self.versor_mul(m)
293    }
294
295}
296
297impl<T:AllocVersor<N>, N:Dim> Versor<T,N> {
298
299    /// Applies the reflection and rotations of self
300    pub fn apply<'a,M>(&'a self, m:M) -> <&'a Self as VersorMul<M>>::Output where &'a Self: VersorMul<M> {
301        self.versor_mul(m)
302    }
303
304}
305
306//
307//Constructions from bivectors / exp and log
308//
309
310impl<T:AllocEven<N>+RefRealField, N:Dim> Rotor<T,N> {
311
312    /// Creates a new `Rotor` rotating in the plane of the input and by the angle given by its [`norm`](Blade::norm())
313    pub fn from_scaled_plane(plane: SimpleBiVecN<T,N>) -> Self where
314        T: AllocBlade<N,U2>
315    {
316        let two = T::one() + T::one();
317        (plane/two).exp()
318    }
319
320    /// Creates a new `Rotor` that rotates in the given plane by the given angle
321    pub fn from_plane_angle(plane: UnitBiVecN<T,N>, angle: T) -> Self where
322        T: AllocBlade<N,U2>
323    {
324
325        //get both the sine and cosine of half the angle
326        let two = T::one() + T::one();
327        let (s, c) = (angle/two).sin_cos();
328
329        //create an even of the form `cos(angle) + plane*sin(angle)`
330        let mut r = Even::from(plane.into_inner() * s);
331        r[0] = c;
332
333        //return
334        r.into_rotor_unchecked()
335
336    }
337
338    ///
339    /// Computes a [bivector](BiVecN) whos [exponential](BiVecN::exp_rotor) is `self`
340    ///
341    /// The result should be a bivector that is the sum of `floor(N/2)` simple bivectors
342    /// with directions and magnitudes corresponding to the planes and angles of the simple rotations
343    /// making up this `Rotor`.
344    ///
345    /// # Uniqueness
346    ///
347    /// As with other non-real logarithms, the result of this function is not unique:
348    ///  - As with complex logarithms, an angle of `2π` can always be added in the direction of
349    ///    the output to get a result with the same exponential.
350    ///  - When `self == -1` and `N >= 3`, there are infinitely many planes in which
351    ///    the rotation could have taken place, so an arbitrary choice is picked.
352    ///  - For isoclinic rotations (ie double rotations where both rotations have the same angle),
353    ///    the choice of rotation planes is also not unique, so again, an arbitrary choice is picked.
354    ///
355    pub fn log(self) -> BiVecN<T,N> where T: AllocBlade<N,U2> {
356        //oooooooh boy
357
358        //so imma do a bad and have this method available for all rotors even though
359        //I don't even know if anyone has a general algorithm for this in all dimensions
360        //oops
361
362        let (n, g) = (self.dim_generic(), <U2 as na::dimension::DimName>::name());
363        match n.value() {
364
365            //weird edge cases
366            0 | 1 => BiVecN::zeroed_generic(n, g),
367
368            //complex numbers
369            2 => {
370                let angle = self.cast_dim_unchecked::<U2>().get_half_angle();
371                BiVec2::new(angle).cast_dim_generic(n)
372            },
373
374            //quaternions
375            3 => self.cast_dim_unchecked::<U3>().get_half_scaled_plane().into_inner().cast_dim_generic(n),
376
377            //magic
378            4 => {
379                let (plane1, plane2) = self.cast_dim_unchecked::<U4>().get_half_scaled_planes();
380                (plane1.into_inner() + plane2.into_inner()).cast_dim_generic(n)
381            }
382
383            //TODO: fill in for 5D. It's basically the same as for 4D but where the quadvector part
384            //has more degrees of freedom
385
386
387            _ => unimplemented!("error: log() currently only implemented up to 4 dimensions")
388
389
390        }
391
392    }
393
394    ///
395    /// Raises this rotor to the power of a real number
396    ///
397    /// Geometrically, this scales the angles of rotation of this rotor by t.
398    ///
399    /// More concretely, this is calulated as `x^t == exp(t*log(x))`. As such,
400    /// whenever [`log()`](Rotor::log()) makes an arbitrary choice for the rotation plane,
401    /// this function does as well.
402    ///
403    pub fn powf(self, t:T) -> Self where T:AllocBlade<N,U2> {
404        (self.log() * t).exp_rotor()
405    }
406
407    /// Smoothly interpolates the rotations of `self` and `other`
408    ///
409    /// Concretely, this is computed as `(other/self)^t * self`
410    pub fn slerp(self, other: Self, t:T) -> Self where T:AllocBlade<N,U2> {
411        //note that the ordering here is very important
412        //the rotations are applied in order from right to left
413        (other / &self).powf(t) * self
414    }
415
416}
417
418impl<T:AllocEven<U2>+RefRealField> Rotor2<T> {
419
420    /// Creates a 2D rotation from an angle
421    pub fn from_angle(angle:T) -> Self
422    {
423        let two = T::one() + T::one();
424        let (s, c) = (angle/two).sin_cos();
425        Even2::new(c, s).into_rotor_unchecked()
426    }
427
428    /// Returns *half* the angle of this `Rotor`'s rotation
429    fn get_half_angle(&self) -> T where T:RefRealField {
430        self.im.clone().atan2(self.re.clone())
431    }
432
433    /// Returns the angle of this `Rotor`'s rotation
434    pub fn get_angle(&self) -> T where T:RefRealField {
435        self.get_half_angle() * (T::one() + T::one())
436    }
437
438}
439
440impl<T:AllocEven<U2>+RefRealField> Rotor3<T> {
441
442    /// Creates a 3D rotation about the given vector axis where the angle is the norm of the vector
443    pub fn from_scaled_axis(scaled_axis: Vec3<T>) -> Self
444    {
445        //TODO: make sure this is actually undual and not dual
446        Self::from_scaled_plane(scaled_axis.undual().into())
447    }
448
449    /// Creates a 3D rotation about the given axis with the given angle
450    pub fn from_axis_angle(axis:UnitVec3<T>, angle:T) -> Self
451    {
452        Self::from_plane_angle(axis.undual(), angle)
453    }
454
455    fn get_half_plane_angle(self) -> Option<(UnitBiVec3<T>, T)> {
456        let [w, x, y, z] = self.into_inner().data;
457
458        let (c, s) = (w, (x.ref_mul(&x)+y.ref_mul(&y)+z.ref_mul(&z)).sqrt());
459        let angle = s.clone().atan2(c);
460
461        if angle.is_zero() {
462            None
463        } else {
464            Some((
465                (BiVec3::new(x, y, z)/&s).into_unit_unchecked(),
466                angle
467            ))
468        }
469    }
470
471    fn get_half_scaled_plane(self) -> SimpleBiVec3<T> {
472        self.get_half_plane_angle().map_or_else(|| Zero::zero(), |(d, a)| SimpleBiVec3::from(d) * a)
473    }
474
475    /// Returns the plane and angle of rotation or `None` if the angle is 0
476    pub fn get_plane_angle(self) -> Option<(UnitBiVec3<T>, T)> {
477        self.get_half_plane_angle().map(|(b,a)| (b, a*(T::one()+T::one())))
478    }
479
480    /// Returns the plane of rotation scaled by the angle of rotation
481    pub fn get_scaled_plane(self) -> SimpleBiVec3<T> {
482        self.get_plane_angle().map_or_else(|| Zero::zero(), |(d, a)| SimpleBiVec3::from(d) * a)
483    }
484
485    /// Returns the axis and angle of rotation or `None` if the angle is 0
486    pub fn get_axis_angle(self) -> Option<(UnitVec3<T>, T)> {
487        self.get_half_plane_angle().map(|(b,a)| (b.dual(), a*(T::one()+T::one())))
488    }
489
490    /// Returns the axis of rotation scaled by the angle of rotation
491    pub fn get_scaled_axis(self) -> Vec3<T> {
492        self.get_plane_angle().map_or_else(|| Zero::zero(), |(d, a)| BiVec3::from(d).dual() * a)
493    }
494
495}
496
497impl<T:AllocEven<U2>+RefRealField> Rotor4<T> {
498
499    fn get_half_plane_angles(self) -> (Option<(UnitBiVec4<T>, T)>, Option<(UnitBiVec4<T>, T)>) {
500
501        // println!("\nlog");
502        // println!("{:+}", self);
503
504        // let to_degrees = T::from_subset(&180.0) / T::pi();
505
506        let two = T::one() + T::one();
507        let [s, b1, b2, b3, b4, b5, b6, q] = self.into_inner().data;
508
509        //if this is just a single rotation, we can just do what we would do for 3D rotations
510        if q.is_zero() {
511
512            let b = BiVec4::new(b1, b2, b3, b4, b5, b6);
513
514            //the RHS 's' is the "scalar" the LHS 's' is the sin
515            let (s, c) = (b.norm(), s);
516            let angle = s.clone().atan2(c);
517
518            // println!("{}", angle.clone() * to_degrees);
519
520            if angle.is_zero() {
521                return (None, None);
522            } else {
523                return (Some(((b/s).into_unit_unchecked(), angle)), None);
524            }
525
526        }
527
528        //using trig identities, we can find the cosine of the sum and differences of the angles
529        let cos_minus = s.ref_add(&q);
530        let cos_plus = s - q;
531
532        // println!("{} {}", cos_plus, cos_minus);
533        // println!("{}° {}°", cos_plus.clone().acos()*&to_degrees, cos_minus.clone().acos()*&to_degrees);
534
535        //next, we find the dual of the bivector part, this has the effect of swapping the
536        //two planes to have the opposite angle
537        let b = BiVec4::new(b1, b2, b3, b4, b5, b6);
538        let b_dual = b.clone().undual();
539
540        // println!("{:+}, {:+}", b, b_dual);
541
542        //then, by adding and subtracting, we end up with two bivectors,
543        //one that has the sine of the angle sum on the sum of the planes
544        //and one that has the sine of the angle difference on the difference of the planes
545        let b_plus = &b - &b_dual;
546        let b_minus = b + b_dual;
547
548        //the sines should come from the norms (divided by sqrt(2))
549        let sin_plus = (b_plus.norm_sqrd()/&two).sqrt();
550        let sin_minus = (b_minus.norm_sqrd()/&two).sqrt();
551
552        // println!("{} {}", b_plus.norm_sqrd(), b_minus.norm_sqrd());
553
554        //normalize (sorta)
555        let b_plus = if sin_plus.is_zero() { None } else { Some(b_plus/&sin_plus) };
556        let b_minus = if sin_minus.is_zero() { None } else { Some(b_minus/&sin_minus) };
557
558        // println!("{} {}", sin_plus, sin_minus);
559        // println!("{}° {}°", sin_plus.clone().asin()*&to_degrees, sin_minus.clone().asin()*&to_degrees);
560
561        //then, we find the angles using atan2
562        let angle_plus = sin_plus.atan2(cos_plus);
563        let angle_minus = sin_minus.atan2(cos_minus);
564
565        // println!("{}° {}°", angle_plus.ref_mul(&to_degrees), angle_minus.ref_mul(&to_degrees));
566
567        //finally, solve for the angles and directions
568        let angle1 = (angle_plus.ref_add(&angle_minus)) / &two;
569        let angle2 = (angle_plus - angle_minus) / &two;
570        // println!("{} {} {}", angle1>=angle2, angle1.ref_mul(&to_degrees), angle2.ref_mul(&to_degrees));
571
572        match (b_plus, b_minus) {
573
574            //since this happens when the Quadvec part is nonzero but the bivec part is
575            //this corresponds to an invalid rotation, so maybe add a panic??
576            (None, None) => (None, None),
577
578            //if we have some an isoclinic rotation (ie the rotation angles are the same)
579            //then, we have to solve for the angles and planes differently
580            (Some(b), None) => {
581
582                // println!("{:+}", b);
583
584                //
585                //since we know the rotation is isoclinic, the plane bivector can be decomposed
586                //by literally just splitting the coordinates in half
587                //
588                //the proof of this basically just boils down to showing that since b is it's own dual
589                //the first half equals the second half. Then, just grind through the algebra to
590                //show that the geometric product of the first half with the second half only has
591                //a quadvector component
592
593                let(dir1, dir2) = b.split_isoclinic();
594                // println!("{:+}, {:+}", dir1, dir2);
595                (
596                    Some((dir1.into_unit_unchecked(), angle1)),
597                    Some((dir2.into_unit_unchecked(), angle2)),
598                )
599
600            },
601            (None, Some(b)) => {
602                // println!("{:+}", b);
603                let(dir1, dir2) = b.split_isoclinic();
604                // println!("{:+}, {:+}", dir1, dir2.ref_neg());
605                (
606                    Some((dir1.into_unit_unchecked(), angle1)),
607                    Some((-dir2.into_unit_unchecked(), angle2)),
608                )
609
610            },
611
612            (Some(b_plus), Some(b_minus)) => {
613
614                // println!("{:+}, {:+}", b_plus, b_minus);
615                let dir1 = (&b_plus + &b_minus) / &two;
616                let dir2 = (b_plus - b_minus) / two;
617
618                // println!("{:+}, {:+}", dir1, dir2);into_iter
619
620                (
621                    Some((dir1.into_unit_unchecked(), angle1)),
622                    Some((dir2.into_unit_unchecked(), angle2)),
623                )
624
625            }
626
627
628        }
629
630    }
631
632    fn get_half_scaled_planes(self) -> (SimpleBiVec4<T>, SimpleBiVec4<T>) {
633        let (x1, x2) = self.get_half_plane_angles();
634        (
635            x1.map_or_else(|| SimpleBiVec4::zeroed(), |(d, a)| d.into_simple() * a),
636            x2.map_or_else(|| SimpleBiVec4::zeroed(), |(d, a)| d.into_simple() * a),
637        )
638    }
639
640    /// Returns both planes and both angles of rotation or `None` for either if they are 0
641    pub fn get_plane_angles(self) -> (Option<(UnitBiVec4<T>, T)>, Option<(UnitBiVec4<T>, T)>) {
642        let (x1, x2) = self.get_half_plane_angles();
643        (
644            x1.map(|(d,a)| (d, a*(T::one()+T::one()))),
645            x2.map(|(d,a)| (d, a*(T::one()+T::one()))),
646        )
647    }
648
649    /// Returns both planes of rotation scaled by their angles of rotation
650    pub fn get_scaled_planes(self) -> (SimpleBiVec4<T>, SimpleBiVec4<T>) {
651        let (x1, x2) = self.get_plane_angles();
652        (
653            x1.map_or_else(|| SimpleBiVec4::zeroed(), |(d, a)| d.into_simple() * a),
654            x2.map_or_else(|| SimpleBiVec4::zeroed(), |(d, a)| d.into_simple() * a),
655        )
656    }
657
658}
659
660impl<T:AllocBlade<N,U2>+RefRealField, N:Dim> SimpleBiVecN<T, N> {
661
662    ///
663    /// Computes the exponential of this bivector as a [`Rotor`]
664    ///
665    /// This will produce a `Rotor` that performs a simple rotation in the plane of `self`
666    /// by an angle **twice** the norm of `self`
667    ///
668    /// This is almost always faster than `[BiVecN::exp_rotor()]`, but can only result in
669    /// simple rotations.
670    ///
671    pub fn exp(self) -> Rotor<T,N> where T:AllocEven<N> {
672        self.into_inner().exp_even_simple().into_rotor_unchecked()
673    }
674}
675
676//
677//Rotation between vectors
678//
679
680impl<T:AllocEven<N>+AllocBlade<N,U1>+RefRealField, N:Dim> Rotor<T,N> {
681
682    #[inline(always)]
683    pub fn rot_between_unit(v1:UnitVecN<T,N>, v2: UnitVecN<T,N>) -> Self where
684        T: AllocBlade<N,U1>
685    { v1.rot_to(v2) }
686
687    #[inline(always)]
688    pub fn rot_between_simple(v1:SimpleVecN<T,N>, v2: SimpleVecN<T,N>) -> Self where
689        T: AllocBlade<N,U1>
690    { v1.rot_to(v2) }
691
692    #[inline(always)]
693    pub fn rot_between(v1:VecN<T,N>, v2: VecN<T,N>) -> Self where
694        T: AllocBlade<N,U1>
695    { v1.rot_to(v2) }
696
697}
698
699impl<T:AllocBlade<N,U1>+RefRealField, N:Dim> UnitVecN<T, N> {
700    pub fn rot_to(self, v2: UnitVecN<T,N>) -> Rotor<T,N> where T:AllocEven<N> {
701
702        //In essence, the algorithm is *almost* just v1*v2. However,
703        //what this would actually give would be a rotor doing *double* the rotation.
704        //so to fix this, we find a vector halfway between v1 and v2 and *then* do the multiplication
705
706        let two = T::one() + T::one();
707        let n = self.dim_generic();
708        let v2 = (self.as_inner() + v2.into_inner()) / &two;
709        let v1 = self;
710
711        //next, we gotta normalize v2
712        match v2.into_simple_vec().try_normalize() {
713            Some(v2) => {
714                //ideally, we'd just do a simple multiplication, and this *does* work,
715                //but it requires an extra trait bound, so we're going to do the worse way
716                //(v1 * v2).unwrap_even()
717                v2.into_blade().mul_even(v1.into_blade()).into_rotor_unchecked()
718            },
719            None => {
720                //if the midpoint is zero, we know the vectors are 180deg apart (so the half angle is 90),
721                //but there are infinitely many solutions for the plane.
722                //hence... we just pick one!
723
724                //now, in order to do this, we need a plane that includes the given vectors
725                //so we first find a basis vector that is not parallel, and then construct a plane
726                //going through it and v1
727
728                for i in 0..n.value() {
729
730                    //the way this is done is somewhat suboptimal, but it's kinda necessary
731                    //unless we want to add an T:Alloc<N,U2> bound to allow for using ^ instead
732                    let ei = Blade::basis_generic(n, v1.grade_generic(), i);
733                    let mut plane = v1.as_inner().mul_even(ei);
734                    plane[0] = T::zero();
735
736                    if let Some(plane) = plane.try_normalize() {
737                        return plane.into_rotor_unchecked();
738                    }
739                }
740
741                //as a special case if everything is broken:
742                panic!("Cannot find rotation between vectors in 1D")
743            },
744        }
745
746    }
747}
748
749impl<T:AllocBlade<N,U1>+RefRealField, N:Dim> SimpleVecN<T, N> {
750    #[inline]
751    pub fn rot_to(self, v2: SimpleVecN<T,N>) -> Rotor<T,N> where T:AllocEven<N> {
752        let n = self.dim_generic();
753        match self.try_normalize() {
754            None => Rotor::from_inner_unchecked(Even::one_generic(n)),
755            Some(v1) => match v2.try_normalize() {
756                None => Rotor::from_inner_unchecked(Even::one_generic(n)),
757                Some(v2) => v1.rot_to(v2)
758            }
759        }
760    }
761}
762
763impl<T:AllocBlade<N,U1>+RefRealField, N:Dim> VecN<T, N> {
764    #[inline(always)]
765    pub fn rot_to(self, v2: VecN<T,N>) -> Rotor<T,N> where T:AllocEven<N> {
766        self.into_simple_unchecked().rot_to(v2.into_simple_unchecked())
767    }
768}
769
770//
771//Reflections
772//
773
774impl<T:AllocOdd<N>+Zero, N:Dim> Reflector<T,N> {
775
776    /// Creates a rotation about the hyperplane normal to the given vector
777    ///
778    /// Concretely, this produces a reflection that flips any components of blades *in* the direction
779    /// of the given vector.
780    ///
781    /// Same as [`Reflector::reflect_unit_normal()`], but performs an extra normalization step
782    /// which may be costly in certain contexts
783    ///
784    pub fn reflect_normal(n: VecN<T,N>) -> Self where T:AllocBlade<N,U1>+RefRealField {
785        n.normalize().into_odd().into_reflector_unchecked()
786    }
787
788    /// Creates a rotation about the hyperplane normal to the given vector
789    ///
790    /// Concretely, this produces a reflection that flips any components of blades *in* the direction
791    /// of the given vector.
792    ///
793    /// Same as [`Reflector::reflect_normal()`], but avoids an extra normalization step
794    /// which may be costly in certain contexts
795    ///
796    pub fn reflect_unit_normal(n: UnitVecN<T,N>) -> Self where T:AllocBlade<N,U1> {
797        n.into_inner().into_odd().into_reflector_unchecked()
798    }
799
800    /// Creates a rotation about the given hyperplane
801    ///
802    /// Concretely, this produces a reflection that flips any components of blades *perpendicular*
803    /// to the given hyperplane
804    ///
805    /// Same as [`Reflector::reflect_unit_hyperplane()`], but performs an extra normalization step
806    /// which may be costly in certain contexts
807    ///
808    pub fn reflect_hyperplane<G:Dim>(plane: Blade<T,N,G>) -> Self where
809        T: AllocBlade<N,G> + AllocBlade<N,U1> + RefRealField,
810        N: DimSub<G,Output=U1>
811    {
812        Self::reflect_normal(plane.dual())
813    }
814
815    /// Creates a rotation about the given hyperplane
816    ///
817    /// Concretely, this produces a reflection that flips any components of blades *perpendicular*
818    /// to the given hyperplane
819    ///
820    /// Same as [`Reflector::reflect_unit_hyperplane()`], but avoids an extra normalization step
821    /// which may be costly in certain contexts
822    ///
823    pub fn reflect_unit_hyperplane<G:Dim>(plane: UnitBlade<T,N,G>) -> Self where
824        T: AllocBlade<N,G> + AllocBlade<N,U1> + ClosedNeg,
825        N: DimSub<G,Output=U1>
826    {
827        Self::reflect_unit_normal(plane.dual())
828    }
829
830}
831
832
833#[cfg(test)]
834mod tests {
835
836    use super::*;
837    use approx::*;
838    use rayon::prelude::*;
839
840    use crate::SHORT_TEST_DIM;
841
842    #[test]
843    fn circle_fractions_2d() {
844
845        for n in 1..=360 {
846
847            let rot32 = Rotor2::from_angle(2.0*std::f32::consts::PI / n as f32);
848            let rot64 = Rotor2::from_angle(2.0*std::f64::consts::PI / n as f64);
849
850            let mut final_rot32 = Rotor2::<f32>::one();
851            let mut final_rot64 = Rotor2::<f64>::one();
852            for _ in 0..n {
853                final_rot32 *= rot32;
854                final_rot64 *= rot64;
855            }
856
857            assert_abs_diff_eq!(-Rotor2::<f32>::one(), final_rot32, epsilon=0.000015);
858            assert_abs_diff_eq!(-Rotor2::<f64>::one(), final_rot64, epsilon=0.0000000000002);
859
860        }
861
862
863    }
864
865    fn double_rot_iter(n:usize) -> impl ParallelIterator<Item=(usize,usize,f64,f64)> {
866        (0..binom(n,2)).into_par_iter()
867        .flat_map(|i| (0..=i).into_par_iter().map(move |j| (i,j)))
868        .flat_map(|(i,j)| (-45i32..45).into_par_iter().map(move |a| (i,j,a)))
869        .flat_map(|(i,j,a)| (-45i32..45).into_par_iter().map(move |b| (i,j,a,b)))
870        .map(|(i,j,a,b)| (i,j, (8.0*a as f64).to_radians(), (8.0*b as f64).to_radians()))
871    }
872
873    // fn double_rot_iter(n:usize) -> impl Iterator<Item=(usize,usize,f64,f64)> {
874    //     (0..binom(n,2)).into_iter()
875    //     .flat_map(|i| (0..=i).into_iter().map(move |j| (i,j)))
876    //     .flat_map(|(i,j)| (-45i32..45).into_iter().map(move |a| (i,j,a)))
877    //     .flat_map(|(i,j,a)| (-45i32..45).into_iter().map(move |b| (i,j,a,b)))
878    //     .map(|(i,j,a,b)| (i,j, (8.0*a as f64).to_radians(), (8.0*b as f64).to_radians()))
879    // }
880
881    #[test]
882    fn double_rot_log() {
883
884
885        for n in 0..=4 {
886
887            let iter = double_rot_iter(n);
888
889            iter.for_each(
890                |(i,j,a,b)| {
891
892                    // if a==b || -a==b || a%180.0==0.0 || b%180.0==0.0 { return; }
893
894                    // println!("\ni={} j={} a={} b={}", i,j,a,b);
895
896                    let b1 = BiVecD::basis(n, i) * a;
897                    let b2 = BiVecD::basis(n, j) * b;
898                    let b = b1 + b2;
899
900                    let rot = b.clone().exp_rotor();
901                    let log = rot.clone().log();
902
903                    // println!("{}: {} = {}", n, b, );
904
905                    assert_abs_diff_eq!(rot, log.exp_rotor(), epsilon=1024.0*f64::EPSILON);
906
907
908                }
909            )
910
911
912        }
913
914    }
915
916    #[test]
917    fn rot_to() {
918
919        for n in 2usize..=4 {
920
921            let s:usize = 3;
922
923            for k in 0..s.pow(2*n as u32) {
924
925                let mut k = k;
926
927                let mut v1 = VecD::zeroed(n);
928                for i in 0..n {
929                    let (q, r) = (k/s, k%s);
930                    v1[i] = r as f64 - (s-1) as f64/2.0;
931                    k = q;
932                }
933
934                let mut v2 = VecD::zeroed(n);
935                for i in 0..n {
936                    let (q, r) = (k/s, k%s);
937                    v2[i] = r as f64 - (s-1) as f64/2.0;
938                    k = q;
939                }
940
941                let rot1 = v1.clone().rot_to(v2.clone());
942                let rot2 = v2.clone().rot_to(v1.clone());
943
944                if v1.norm_sqrd() == 0.0 || v2.norm_sqrd() == 0.0 {
945                    assert_relative_eq!(rot1, Rotor::one_dyn(n));
946                    assert_relative_eq!(rot2, Rotor::one_dyn(n));
947                } else {
948                    assert_relative_eq!(v2.clone().normalize(), rot1.rot(&v1).normalize(), epsilon=1e-10);
949                    assert_relative_eq!(v1.clone().normalize(), rot2.rot(&v2).normalize(), epsilon=1e-10);
950                }
951
952            }
953
954
955        }
956
957
958
959    }
960
961    #[test]
962    fn double_rot_sqrt() {
963
964        for n in 0..=4 {
965
966            let iter = double_rot_iter(n);
967
968            iter.for_each(
969                |(i,j,a,b)| {
970
971                    let b1 = BiVecD::basis(n, i) * a;
972                    let b2 = BiVecD::basis(n, j) * b;
973                    let b = &b1 + &b2;
974
975                    let rot = b.clone().exp_rotor();
976                    let sqrt1 = (b/2.0).exp_rotor();
977                    let sqrt2 = rot.clone().powf(0.5);
978
979                    let eps = 1024.0*f64::EPSILON;
980
981                    //We cannot directly compare sqrt1 and sqrt2 since they could differ by
982                    //a factor of `-1` or some other square root of unity
983
984                    assert_relative_eq!(&sqrt2*&sqrt2, rot, epsilon=eps);
985                    assert_relative_eq!(&sqrt1*&sqrt1, rot, epsilon=eps);
986
987                }
988            )
989
990        }
991
992
993    }
994
995
996    #[test]
997    fn basis_reflection() {
998
999        for n in 0..=SHORT_TEST_DIM {
1000
1001            for i in 0..n {
1002
1003                //create a reflection about one of the axes
1004                let normal = VecD::<f64>::basis(n, i)*2.0;
1005                let unit_normal = normal.clone().into_simple().normalize();
1006                let r = Reflector::reflect_normal(normal.clone());
1007
1008                //compute the input and output directly
1009                let v1 = VecD::from_element(n, 1.0);
1010                let v2 = VecD::from_index_fn(n, |j| if j==i {-1.0} else {1.0});
1011
1012                assert_eq!(r.apply(&v1), v2);
1013                assert_eq!(unit_normal.reflect(&v1), v2);
1014
1015            }
1016
1017        }
1018
1019        dim_name_test_loop!(
1020            {U1 U2 U3 U4 U5 U6}
1021            |$N| for i in 0..$N::dim() {
1022
1023                let normal = VecN::<f64, $N>::basis(i) * 2.0;
1024                let unit_normal = normal.into_simple().normalize();
1025                let plane = if $N::dim()<=2 { normal.undual() } else { PsuedoVecN::<f64, $N>::basis(i) };
1026
1027                let r1 = Reflector::reflect_normal(normal);
1028                let r2 = Reflector::reflect_hyperplane(plane);
1029
1030                let v1 = VecN::<_,$N>::from_element(1.0);
1031                let v2 = VecN::<_,$N>::from_index_fn(|j| if j==i {-1.0} else {1.0});
1032
1033                assert_eq!(r1, r2);
1034                assert_eq!(r1.apply(&v1), v2);
1035                assert_eq!(r2.apply(&v1), v2);
1036                assert_eq!(normal.reflect(&v1), v2);
1037                assert_eq!(unit_normal.reflect(&v1), v2);
1038
1039            }
1040        );
1041
1042    }
1043
1044}
wedged/subspace/versor.rs

wedged/subspace/
versor.rs
