vecmat/transform/
rotation.rs

1#[cfg(feature = "rand")]
2use crate::distr::{Uniform, Unit};
3use crate::{
4    traits::Dot,
5    transform::{Directional, Linear, Reorder, Shift},
6    Complex, Matrix, Quaternion, Transform, Vector,
7};
8#[cfg(feature = "approx")]
9use approx::{abs_diff_eq, AbsDiffEq};
10use core::ops::Neg;
11use num_traits::{Float, FloatConst, Num, NumCast, One};
12#[cfg(feature = "rand")]
13use rand_::{
14    distributions::{uniform::SampleUniform, Distribution, Uniform as RangedUniform},
15    Rng,
16};
17
18// TODO: Use partial specialization when it will be possible.
19/// Two-dimensional rotation.
20#[derive(Clone, Copy, PartialEq, Debug)]
21pub struct Rotation2<T> {
22    comp: Complex<T>,
23}
24
25impl<T> Rotation2<T> {
26    pub fn from_complex(comp: Complex<T>) -> Self {
27        Self { comp }
28    }
29    pub fn into_complex(self) -> Complex<T> {
30        self.comp
31    }
32}
33
34impl<T> From<Complex<T>> for Rotation2<T> {
35    fn from(comp: Complex<T>) -> Self {
36        Self::from_complex(comp)
37    }
38}
39impl<T> From<Rotation2<T>> for Complex<T> {
40    fn from(rot: Rotation2<T>) -> Self {
41        rot.into_complex()
42    }
43}
44
45impl<T> Rotation2<T>
46where
47    T: Float,
48{
49    pub fn new(angle: T) -> Self {
50        Self {
51            comp: Complex::new(angle.cos(), angle.sin()),
52        }
53    }
54    pub fn angle(&self) -> T {
55        self.comp.im().atan2(self.comp.re())
56    }
57}
58
59impl<T> Transform<Vector<T, 2>> for Rotation2<T>
60where
61    T: Neg<Output = T> + Num + Copy,
62{
63    fn identity() -> Self {
64        Self {
65            comp: Complex::one(),
66        }
67    }
68    fn inv(self) -> Self {
69        Self {
70            comp: self.comp.conj(),
71        }
72    }
73    fn apply(&self, pos: Vector<T, 2>) -> Vector<T, 2> {
74        (<Vector<T, 2> as Into<Complex<T>>>::into(pos) * self.into_complex()).into()
75    }
76    fn deriv(&self, _pos: Vector<T, 2>, dir: Vector<T, 2>) -> Vector<T, 2> {
77        self.apply(dir)
78    }
79    fn chain(self, other: Self) -> Self {
80        Self {
81            comp: self.comp * other.comp,
82        }
83    }
84}
85
86impl<T> Directional<Vector<T, 2>> for Rotation2<T>
87where
88    Self: Transform<Vector<T, 2>>,
89{
90    fn apply_dir(&self, pos: Vector<T, 2>, dir: Vector<T, 2>) -> Vector<T, 2> {
91        self.deriv(pos, dir)
92    }
93    fn apply_normal(&self, pos: Vector<T, 2>, normal: Vector<T, 2>) -> Vector<T, 2> {
94        self.apply_dir(pos, normal)
95    }
96}
97
98impl<T> Rotation2<T>
99where
100    T: Neg<Output = T> + Copy,
101{
102    pub fn to_linear(self) -> Linear<T, 2> {
103        Linear::from(Matrix::from([
104            [self.comp.re(), -self.comp.im()],
105            [self.comp.im(), self.comp.re()],
106        ]))
107    }
108}
109
110#[cfg(feature = "rand")]
111impl<T> Distribution<Rotation2<T>> for Uniform
112where
113    RangedUniform<T>: Distribution<T>,
114    T: SampleUniform + Float + FloatConst + NumCast,
115{
116    fn sample<R: Rng + ?Sized>(&self, rng: &mut R) -> Rotation2<T> {
117        Rotation2::new(rng.sample(&RangedUniform::new(
118            T::zero(),
119            T::from(2.0).unwrap() * T::PI(),
120        )))
121    }
122}
123#[cfg(feature = "approx")]
124impl<T> AbsDiffEq for Rotation2<T>
125where
126    T: AbsDiffEq<Epsilon = T> + Copy,
127{
128    type Epsilon = T;
129    fn default_epsilon() -> Self::Epsilon {
130        T::default_epsilon()
131    }
132    fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
133        abs_diff_eq!(self.comp.re(), other.comp.re(), epsilon = epsilon)
134            && abs_diff_eq!(self.comp.im(), other.comp.im(), epsilon = epsilon)
135    }
136}
137
138/// Three-dimensional rotation.
139#[derive(Clone, Copy, PartialEq, Debug)]
140pub struct Rotation3<T> {
141    quat: Quaternion<T>,
142}
143
144impl<T> Rotation3<T> {
145    pub fn from_quaternion(quat: Quaternion<T>) -> Self {
146        Self { quat }
147    }
148    pub fn into_quaternion(self) -> Quaternion<T> {
149        self.quat
150    }
151}
152
153impl<T> From<Quaternion<T>> for Rotation3<T> {
154    fn from(quat: Quaternion<T>) -> Self {
155        Self::from_quaternion(quat)
156    }
157}
158impl<T> From<Rotation3<T>> for Quaternion<T> {
159    fn from(rot: Rotation3<T>) -> Self {
160        rot.into_quaternion()
161    }
162}
163
164impl<T> Rotation3<T>
165where
166    T: Float + NumCast,
167{
168    pub fn new(axis: Vector<T, 3>, angle: T) -> Self {
169        let half = angle / T::from(2.0).unwrap();
170        Self {
171            quat: Quaternion::from_scalar_and_vector3(half.cos(), axis * half.sin()),
172        }
173    }
174    pub fn axis(&self) -> Vector<T, 3> {
175        let (_, ax) = self.quat.into();
176        ax.normalize()
177    }
178    pub fn angle(&self) -> T {
179        let (w, ax) = self.quat.into();
180        T::from(2.0).unwrap() * ax.length().atan2(w)
181    }
182}
183
184impl<T> Transform<Vector<T, 3>> for Rotation3<T>
185where
186    T: Neg<Output = T> + Num + Copy,
187{
188    fn identity() -> Self {
189        Self {
190            quat: Quaternion::one(),
191        }
192    }
193    fn inv(self) -> Self {
194        Self {
195            quat: self.quat.conj(),
196        }
197    }
198    fn apply(&self, pos: Vector<T, 3>) -> Vector<T, 3> {
199        let qpos = Quaternion::from_scalar_and_vector3(T::zero(), pos);
200        let qres = self.quat * qpos * self.quat.conj();
201        let (_, res) = qres.into();
202        res
203    }
204    fn deriv(&self, _pos: Vector<T, 3>, dir: Vector<T, 3>) -> Vector<T, 3> {
205        self.apply(dir)
206    }
207    fn chain(self, other: Self) -> Self {
208        Self {
209            quat: self.quat * other.quat,
210        }
211    }
212}
213
214impl<T> Directional<Vector<T, 3>> for Rotation3<T>
215where
216    Self: Transform<Vector<T, 3>>,
217{
218    fn apply_dir(&self, pos: Vector<T, 3>, dir: Vector<T, 3>) -> Vector<T, 3> {
219        self.deriv(pos, dir)
220    }
221    fn apply_normal(&self, pos: Vector<T, 3>, normal: Vector<T, 3>) -> Vector<T, 3> {
222        self.apply_dir(pos, normal)
223    }
224}
225
226impl<T> Rotation3<T>
227where
228    T: Float + NumCast,
229{
230    pub fn to_linear(self) -> Linear<T, 3> {
231        let t1 = T::one();
232        let t2 = T::from(2.0).unwrap();
233        let q = self.quat;
234        Linear::from(Matrix::from([
235            [
236                t1 - t2 * q.y() * q.y() - t2 * q.z() * q.z(),
237                t2 * q.x() * q.y() - t2 * q.z() * q.w(),
238                t2 * q.x() * q.z() + t2 * q.y() * q.w(),
239            ],
240            [
241                t2 * q.x() * q.y() + t2 * q.z() * q.w(),
242                t1 - t2 * q.x() * q.x() - t2 * q.z() * q.z(),
243                t2 * q.y() * q.z() - t2 * q.x() * q.w(),
244            ],
245            [
246                t2 * q.x() * q.z() - t2 * q.y() * q.w(),
247                t2 * q.y() * q.z() + t2 * q.x() * q.w(),
248                t1 - t2 * q.x() * q.x() - t2 * q.y() * q.y(),
249            ],
250        ]))
251    }
252}
253
254impl<T> Rotation3<T>
255where
256    T: Float + NumCast + FloatConst,
257{
258    /// Continuous version of `look_at_any`.
259    ///
260    /// It hasn't any discontinuities except single point `z = 1`.
261    fn look_at_any_cont(dir: Vector<T, 3>) -> Self {
262        let z = Vector::from((T::zero(), T::zero(), -T::one()));
263        let dot = z.dot(dir);
264        if dot < T::zero() {
265            Self::look_at_any(-dir).chain(Self::new(
266                Vector::from((T::one(), T::zero(), T::zero())),
267                T::PI(),
268            ))
269        } else {
270            let cross = z.cross(dir);
271            let cos = ((T::one() + dot) / T::from(2).unwrap()).sqrt();
272            let sin_mult = T::one() / (T::from(2).unwrap() * cos);
273            Self::from_quaternion((cos, cross * sin_mult).into())
274        }
275    }
276
277    /// Returns any of transformations that rotate `-z`-axis to `dir`.
278    pub fn look_at_any(dir: Vector<T, 3>) -> Self {
279        if dir.z() < T::zero() {
280            Self::look_at_any_cont(dir)
281        } else {
282            Self::look_at_any_cont(-dir).chain(Self::new(
283                Vector::from((T::one(), T::zero(), T::zero())),
284                T::PI(),
285            ))
286        }
287    }
288}
289
290impl<T> Reorder<Rotation2<T>, Vector<T, 2>> for Shift<T, 2>
291where
292    Rotation2<T>: Transform<Vector<T, 2>> + Copy,
293    Self: Transform<Vector<T, 2>>,
294{
295    fn reorder(self, other: Rotation2<T>) -> (Rotation2<T>, Shift<T, 2>) {
296        (other, other.inv().apply(self.into_vector()).into())
297    }
298}
299impl<T> Reorder<Shift<T, 2>, Vector<T, 2>> for Rotation2<T>
300where
301    Self: Transform<Vector<T, 2>>,
302    Shift<T, 2>: Transform<Vector<T, 2>>,
303{
304    fn reorder(self, other: Shift<T, 2>) -> (Shift<T, 2>, Rotation2<T>) {
305        (self.apply(other.into_vector()).into(), self)
306    }
307}
308
309impl<T> Reorder<Rotation3<T>, Vector<T, 3>> for Shift<T, 3>
310where
311    Rotation3<T>: Transform<Vector<T, 3>> + Copy,
312    Self: Transform<Vector<T, 3>>,
313{
314    fn reorder(self, other: Rotation3<T>) -> (Rotation3<T>, Shift<T, 3>) {
315        (other, other.inv().apply(self.into_vector()).into())
316    }
317}
318impl<T> Reorder<Shift<T, 3>, Vector<T, 3>> for Rotation3<T>
319where
320    Self: Transform<Vector<T, 3>>,
321    Shift<T, 3>: Transform<Vector<T, 3>>,
322{
323    fn reorder(self, other: Shift<T, 3>) -> (Shift<T, 3>, Rotation3<T>) {
324        (self.apply(other.into_vector()).into(), self)
325    }
326}
327
328#[cfg(feature = "rand")]
329impl<T> Distribution<Rotation3<T>> for Uniform
330where
331    Unit: Distribution<Vector<T, 3>>,
332    RangedUniform<T>: Distribution<T>,
333    T: SampleUniform + Float + FloatConst + NumCast,
334{
335    fn sample<R: Rng + ?Sized>(&self, rng: &mut R) -> Rotation3<T> {
336        Rotation3::new(
337            rng.sample(&Unit),
338            rng.sample(&RangedUniform::new(
339                T::zero(),
340                T::from(2.0).unwrap() * T::PI(),
341            )),
342        )
343    }
344}
345#[cfg(feature = "approx")]
346impl<T> AbsDiffEq for Rotation3<T>
347where
348    T: AbsDiffEq<Epsilon = T> + Copy,
349{
350    type Epsilon = T;
351    fn default_epsilon() -> Self::Epsilon {
352        T::default_epsilon()
353    }
354    fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
355        abs_diff_eq!(self.quat, other.quat, epsilon = epsilon)
356    }
357}
358
359#[cfg(all(test, feature = "approx", feature = "rand"))]
360mod tests {
361    use super::*;
362    use crate::{distr::Normal, prelude::*, vector::*};
363    use approx::assert_abs_diff_eq;
364    use rand_::SeedableRng;
365    use rand_xorshift::XorShiftRng;
366
367    const SAMPLE_ATTEMPTS: usize = 256;
368    const EPS: f64 = 1e-14;
369
370    mod r2d {
371        use super::*;
372
373        #[test]
374        fn mapping() {
375            let mut rng = XorShiftRng::seed_from_u64(0x2DA);
376            for _ in 0..SAMPLE_ATTEMPTS {
377                let r: Rotation2<f64> = rng.sample(&Uniform);
378                let a: Vector2<f64> = rng.sample(&Normal);
379                let b = r.apply(a);
380                assert_abs_diff_eq!(a.length(), b.length(), epsilon = EPS);
381                assert_abs_diff_eq!(a.dot(b) / a.square_length(), r.angle().cos(), epsilon = EPS);
382            }
383        }
384
385        #[test]
386        fn chaining() {
387            let mut rng = XorShiftRng::seed_from_u64(0x2DB);
388            for _ in 0..SAMPLE_ATTEMPTS {
389                let a: Rotation2<f64> = rng.sample(&Uniform);
390                let b: Rotation2<f64> = rng.sample(&Uniform);
391                let v: Vector2<f64> = rng.sample(&Normal);
392                assert_abs_diff_eq!(a.chain(b).apply(v), a.apply(b.apply(v)), epsilon = EPS);
393            }
394        }
395
396        #[test]
397        fn inversion() {
398            let mut rng = XorShiftRng::seed_from_u64(0x2DC);
399            for _ in 0..SAMPLE_ATTEMPTS {
400                let r: Rotation2<f64> = rng.sample(&Uniform);
401                assert_abs_diff_eq!(r.chain(r.inv()), Rotation2::identity(), epsilon = EPS);
402            }
403        }
404
405        #[test]
406        fn to_linear() {
407            let mut rng = XorShiftRng::seed_from_u64(0x2DD);
408            for _ in 0..SAMPLE_ATTEMPTS {
409                let a: Rotation2<f64> = rng.sample(&Uniform);
410                let b: Rotation2<f64> = rng.sample(&Uniform);
411                let x: Vector2<f64> = rng.sample(&Normal);
412                assert_abs_diff_eq!(a.to_linear().apply(x), a.apply(x), epsilon = EPS);
413                assert_abs_diff_eq!(
414                    a.to_linear().chain(b.to_linear()),
415                    a.chain(b).to_linear(),
416                    epsilon = EPS,
417                );
418            }
419        }
420    }
421
422    mod r3d {
423        use super::*;
424
425        #[test]
426        fn mapping() {
427            let mut rng = XorShiftRng::seed_from_u64(0x3DA);
428            for _ in 0..SAMPLE_ATTEMPTS {
429                let r: Rotation3<f64> = rng.sample(&Uniform);
430                let mut a: Vector3<f64> = rng.sample(&Normal);
431                let mut b = r.apply(a);
432                let (axis, angle) = (r.axis(), r.angle());
433                assert_abs_diff_eq!(a.length(), b.length(), epsilon = EPS);
434                a -= axis * a.dot(axis);
435                b -= axis * b.dot(axis);
436                let aa = a.square_length();
437                assert_abs_diff_eq!(a.dot(b) / aa, angle.cos(), epsilon = EPS);
438                assert_abs_diff_eq!(a.cross(b) / aa, axis * angle.sin(), epsilon = EPS);
439            }
440        }
441
442        #[test]
443        fn chaining() {
444            let mut rng = XorShiftRng::seed_from_u64(0x2DB);
445            for _ in 0..SAMPLE_ATTEMPTS {
446                let a: Rotation3<f64> = rng.sample(&Uniform);
447                let b: Rotation3<f64> = rng.sample(&Uniform);
448                let v: Vector3<f64> = rng.sample(&Normal);
449                assert_abs_diff_eq!(a.chain(b).apply(v), a.apply(b.apply(v)), epsilon = EPS);
450            }
451        }
452
453        #[test]
454        fn inversion() {
455            let mut rng = XorShiftRng::seed_from_u64(0x2DC);
456            for _ in 0..SAMPLE_ATTEMPTS {
457                let r: Rotation3<f64> = rng.sample(&Uniform);
458                assert_abs_diff_eq!(r.chain(r.inv()), Rotation3::identity(), epsilon = EPS);
459            }
460        }
461
462        #[test]
463        fn to_linear() {
464            let mut rng = XorShiftRng::seed_from_u64(0x2DD);
465            for _ in 0..SAMPLE_ATTEMPTS {
466                let a: Rotation3<f64> = rng.sample(&Uniform);
467                let b: Rotation3<f64> = rng.sample(&Uniform);
468                let x: Vector3<f64> = rng.sample(&Normal);
469                assert_abs_diff_eq!(a.to_linear().apply(x), a.apply(x), epsilon = EPS);
470                assert_abs_diff_eq!(
471                    a.to_linear().chain(b.to_linear()),
472                    a.chain(b).to_linear(),
473                    epsilon = EPS,
474                );
475            }
476        }
477
478        #[test]
479        fn look_to_the_direction() {
480            const EPS: f64 = 1e-14;
481            let mut rng = XorShiftRng::seed_from_u64(0xBEC);
482
483            for _ in 0..SAMPLE_ATTEMPTS {
484                let d: Vector<f64, 3> = rng.sample(&Unit);
485                let m = Rotation3::look_at_any(d);
486
487                assert_abs_diff_eq!(m.apply(Vector::from([0.0, 0.0, -1.0])), d, epsilon = EPS);
488            }
489        }
490    }
491}
vecmat/transform/rotation.rs

vecmat/transform/
rotation.rs
