rotation_specialization.rs - source
Description: Source of the Rust file `/Users/erlendbasso/.cargo/registry/src/github.com-1ecc6299db9ec823/nalgebra-0.32.1/src/geometry/rotation_specialization.rs`.
Keywords: rust, rustlang, rust-lang

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    19
    20
    21
    22
    23
    24
    25
    26
    27
    28
    29
    30
    31
    32
    33
    34
    35
    36
    37
    38
    39
    40
    41
    42
    43
    44
    45
    46
    47
    48
    49
    50
    51
    52
    53
    54
    55
    56
    57
    58
    59
    60
    61
    62
    63
    64
    65
    66
    67
    68
    69
    70
    71
    72
    73
    74
    75
    76
    77
    78
    79
    80
    81
    82
    83
    84
    85
    86
    87
    88
    89
    90
    91
    92
    93
    94
    95
    96
    97
    98
    99
    100
    101
    102
    103
    104
    105
    106
    107
    108
    109
    110
    111
    112
    113
    114
    115
    116
    117
    118
    119
    120
    121
    122
    123
    124
    125
    126
    127
    128
    129
    130
    131
    132
    133
    134
    135
    136
    137
    138
    139
    140
    141
    142
    143
    144
    145
    146
    147
    148
    149
    150
    151
    152
    153
    154
    155
    156
    157
    158
    159
    160
    161
    162
    163
    164
    165
    166
    167
    168
    169
    170
    171
    172
    173
    174
    175
    176
    177
    178
    179
    180
    181
    182
    183
    184
    185
    186
    187
    188
    189
    190
    191
    192
    193
    194
    195
    196
    197
    198
    199
    200
    201
    202
    203
    204
    205
    206
    207
    208
    209
    210
    211
    212
    213
    214
    215
    216
    217
    218
    219
    220
    221
    222
    223
    224
    225
    226
    227
    228
    229
    230
    231
    232
    233
    234
    235
    236
    237
    238
    239
    240
    241
    242
    243
    244
    245
    246
    247
    248
    249
    250
    251
    252
    253
    254
    255
    256
    257
    258
    259
    260
    261
    262
    263
    264
    265
    266
    267
    268
    269
    270
    271
    272
    273
    274
    275
    276
    277
    278
    279
    280
    281
    282
    283
    284
    285
    286
    287
    288
    289
    290
    291
    292
    293
    294
    295
    296
    297
    298
    299
    300
    301
    302
    303
    304
    305
    306
    307
    308
    309
    310
    311
    312
    313
    314
    315
    316
    317
    318
    319
    320
    321
    322
    323
    324
    325
    326
    327
    328
    329
    330
    331
    332
    333
    334
    335
    336
    337
    338
    339
    340
    341
    342
    343
    344
    345
    346
    347
    348
    349
    350
    351
    352
    353
    354
    355
    356
    357
    358
    359
    360
    361
    362
    363
    364
    365
    366
    367
    368
    369
    370
    371
    372
    373
    374
    375
    376
    377
    378
    379
    380
    381
    382
    383
    384
    385
    386
    387
    388
    389
    390
    391
    392
    393
    394
    395
    396
    397
    398
    399
    400
    401
    402
    403
    404
    405
    406
    407
    408
    409
    410
    411
    412
    413
    414
    415
    416
    417
    418
    419
    420
    421
    422
    423
    424
    425
    426
    427
    428
    429
    430
    431
    432
    433
    434
    435
    436
    437
    438
    439
    440
    441
    442
    443
    444
    445
    446
    447
    448
    449
    450
    451
    452
    453
    454
    455
    456
    457
    458
    459
    460
    461
    462
    463
    464
    465
    466
    467
    468
    469
    470
    471
    472
    473
    474
    475
    476
    477
    478
    479
    480
    481
    482
    483
    484
    485
    486
    487
    488
    489
    490
    491
    492
    493
    494
    495
    496
    497
    498
    499
    500
    501
    502
    503
    504
    505
    506
    507
    508
    509
    510
    511
    512
    513
    514
    515
    516
    517
    518
    519
    520
    521
    522
    523
    524
    525
    526
    527
    528
    529
    530
    531
    532
    533
    534
    535
    536
    537
    538
    539
    540
    541
    542
    543
    544
    545
    546
    547
    548
    549
    550
    551
    552
    553
    554
    555
    556
    557
    558
    559
    560
    561
    562
    563
    564
    565
    566
    567
    568
    569
    570
    571
    572
    573
    574
    575
    576
    577
    578
    579
    580
    581
    582
    583
    584
    585
    586
    587
    588
    589
    590
    591
    592
    593
    594
    595
    596
    597
    598
    599
    600
    601
    602
    603
    604
    605
    606
    607
    608
    609
    610
    611
    612
    613
    614
    615
    616
    617
    618
    619
    620
    621
    622
    623
    624
    625
    626
    627
    628
    629
    630
    631
    632
    633
    634
    635
    636
    637
    638
    639
    640
    641
    642
    643
    644
    645
    646
    647
    648
    649
    650
    651
    652
    653
    654
    655
    656
    657
    658
    659
    660
    661
    662
    663
    664
    665
    666
    667
    668
    669
    670
    671
    672
    673
    674
    675
    676
    677
    678
    679
    680
    681
    682
    683
    684
    685
    686
    687
    688
    689
    690
    691
    692
    693
    694
    695
    696
    697
    698
    699
    700
    701
    702
    703
    704
    705
    706
    707
    708
    709
    710
    711
    712
    713
    714
    715
    716
    717
    718
    719
    720
    721
    722
    723
    724
    725
    726
    727
    728
    729
    730
    731
    732
    733
    734
    735
    736
    737
    738
    739
    740
    741
    742
    743
    744
    745
    746
    747
    748
    749
    750
    751
    752
    753
    754
    755
    756
    757
    758
    759
    760
    761
    762
    763
    764
    765
    766
    767
    768
    769
    770
    771
    772
    773
    774
    775
    776
    777
    778
    779
    780
    781
    782
    783
    784
    785
    786
    787
    788
    789
    790
    791
    792
    793
    794
    795
    796
    797
    798
    799
    800
    801
    802
    803
    804
    805
    806
    807
    808
    809
    810
    811
    812
    813
    814
    815
    816
    817
    818
    819
    820
    821
    822
    823
    824
    825
    826
    827
    828
    829
    830
    831
    832
    833
    834
    835
    836
    837
    838
    839
    840
    841
    842
    843
    844
    845
    846
    847
    848
    849
    850
    851
    852
    853
    854
    855
    856
    857
    858
    859
    860
    861
    862
    863
    864
    865
    866
    867
    868
    869
    870
    871
    872
    873
    874
    875
    876
    877
    878
    879
    880
    881
    882
    883
    884
    885
    886
    887
    888
    889
    890
    891
    892
    893
    894
    895
    896
    897
    898
    899
    900
    901
    902
    903
    904
    905
    906
    907
    908
    909
    910
    911
    912
    913
    914
    915
    916
    917
    918
    919
    920
    921
    922
    923
    924
    925
    926
    927
    928
    929
    930
    931
    932
    933
    934
    935
    936
    937
    938
    939
    940
    941
    942
    943
    944
    945
    946
    947
    948
    949
    950
    951
    952
    953
    954
    955
    956
    957
    958
    959
    960
    961
    962
    963
    964
    965
    966
    967
    968
    969
    970
    971
    972
    973
    974
    975
    976
    977
    978
    979
    980
    981
    982
    983
    984
    985
    986
    987
    988
    989
    990
    991
    992
    993
    994
    995
    996
    997
    998
    999
    1000
    1001
    1002
    1003
    1004
    1005
    1006
    1007
    1008
    1009
    1010
    1011
    1012
    1013
    1014
    1015
    1016
    1017
    1018
    1019
    1020
    1021
    1022
    1023
    1024
    1025
    1026
    1027
    1028
    1029
    1030
    1031
    1032
    1033
    1034
    1035
    1036
    1037
    1038
    1039

    #[cfg(feature = "arbitrary")]
    use crate::base::storage::Owned;
    #[cfg(feature = "arbitrary")]
    use quickcheck::{Arbitrary, Gen};

    use num::Zero;

    #[cfg(feature = "rand-no-std")]
    use rand::{
        distributions::{uniform::SampleUniform, Distribution, OpenClosed01, Standard, Uniform},
        Rng,
    };

    use simba::scalar::RealField;
    use simba::simd::{SimdBool, SimdRealField};
    use std::ops::Neg;

    use crate::base::dimension::{U1, U2, U3};
    use crate::base::storage::Storage;
    use crate::base::{
        Matrix2, Matrix3, SMatrix, SVector, Unit, UnitVector3, Vector, Vector1, Vector2, Vector3,
    };

    use crate::geometry::{Rotation2, Rotation3, UnitComplex, UnitQuaternion};

    /*
     *
     * 2D Rotation matrix.
     *
     */
    /// # Construction from a 2D rotation angle
    impl<T: SimdRealField> Rotation2<T> {
        /// Builds a 2 dimensional rotation matrix from an angle in radian.
        ///
        /// # Example
        ///
        /// ```
        /// # #[macro_use] extern crate approx;
        /// # use std::f32;
        /// # use nalgebra::{Rotation2, Point2};
        /// let rot = Rotation2::new(f32::consts::FRAC_PI_2);
        ///
        /// assert_relative_eq!(rot * Point2::new(3.0, 4.0), Point2::new(-4.0, 3.0));
        /// ```
        pub fn new(angle: T) -> Self {
            let (sia, coa) = angle.simd_sin_cos();
            Self::from_matrix_unchecked(Matrix2::new(coa.clone(), -sia.clone(), sia, coa))
        }

        /// Builds a 2 dimensional rotation matrix from an angle in radian wrapped in a 1-dimensional vector.
        ///
        ///
        /// This is generally used in the context of generic programming. Using
        /// the `::new(angle)` method instead is more common.
        #[inline]
        pub fn from_scaled_axis<SB: Storage<T, U1>>(axisangle: Vector<T, U1, SB>) -> Self {
            Self::new(axisangle[0].clone())
        }
    }

    /// # Construction from an existing 2D matrix or rotations
    impl<T: SimdRealField> Rotation2<T> {
        /// Builds a rotation from a basis assumed to be orthonormal.
        ///
        /// In order to get a valid rotation matrix, the input must be an
        /// orthonormal basis, i.e., all vectors are normalized, and the are
        /// all orthogonal to each other. These invariants are not checked
        /// by this method.
        pub fn from_basis_unchecked(basis: &[Vector2<T>; 2]) -> Self {
            let mat = Matrix2::from_columns(&basis[..]);
            Self::from_matrix_unchecked(mat)
        }

        /// Builds a rotation matrix by extracting the rotation part of the given transformation `m`.
        ///
        /// This is an iterative method. See `.from_matrix_eps` to provide mover
        /// convergence parameters and starting solution.
        /// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al.
        pub fn from_matrix(m: &Matrix2<T>) -> Self
        where
            T: RealField,
        {
            Self::from_matrix_eps(m, T::default_epsilon(), 0, Self::identity())
        }

        /// Builds a rotation matrix by extracting the rotation part of the given transformation `m`.
        ///
        /// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al.
        ///
        /// # Parameters
        ///
        /// * `m`: the matrix from which the rotational part is to be extracted.
        /// * `eps`: the angular errors tolerated between the current rotation and the optimal one.
        /// * `max_iter`: the maximum number of iterations. Loops indefinitely until convergence if set to `0`.
        /// * `guess`: an estimate of the solution. Convergence will be significantly faster if an initial solution close
        ///           to the actual solution is provided. Can be set to `Rotation2::identity()` if no other
        ///           guesses come to mind.
        pub fn from_matrix_eps(m: &Matrix2<T>, eps: T, mut max_iter: usize, guess: Self) -> Self
        where
            T: RealField,
        {
            if max_iter == 0 {
                max_iter = usize::max_value();
            }

            let mut rot = guess.into_inner();

            for _ in 0..max_iter {
                let axis = rot.column(0).perp(&m.column(0)) + rot.column(1).perp(&m.column(1));
                let denom = rot.column(0).dot(&m.column(0)) + rot.column(1).dot(&m.column(1));

                let angle = axis / (denom.abs() + T::default_epsilon());
                if angle.clone().abs() > eps {
                    rot = Self::new(angle) * rot;
                } else {
                    break;
                }
            }

            Self::from_matrix_unchecked(rot)
        }

        /// The rotation matrix required to align `a` and `b` but with its angle.
        ///
        /// This is the rotation `R` such that `(R * a).angle(b) == 0 && (R * a).dot(b).is_positive()`.
        ///
        /// # Example
        /// ```
        /// # #[macro_use] extern crate approx;
        /// # use nalgebra::{Vector2, Rotation2};
        /// let a = Vector2::new(1.0, 2.0);
        /// let b = Vector2::new(2.0, 1.0);
        /// let rot = Rotation2::rotation_between(&a, &b);
        /// assert_relative_eq!(rot * a, b);
        /// assert_relative_eq!(rot.inverse() * b, a);
        /// ```
        #[inline]
        pub fn rotation_between<SB, SC>(a: &Vector<T, U2, SB>, b: &Vector<T, U2, SC>) -> Self
        where
            T: RealField,
            SB: Storage<T, U2>,
            SC: Storage<T, U2>,
        {
            crate::convert(UnitComplex::rotation_between(a, b).to_rotation_matrix())
        }

        /// The smallest rotation needed to make `a` and `b` collinear and point toward the same
        /// direction, raised to the power `s`.
        ///
        /// # Example
        /// ```
        /// # #[macro_use] extern crate approx;
        /// # use nalgebra::{Vector2, Rotation2};
        /// let a = Vector2::new(1.0, 2.0);
        /// let b = Vector2::new(2.0, 1.0);
        /// let rot2 = Rotation2::scaled_rotation_between(&a, &b, 0.2);
        /// let rot5 = Rotation2::scaled_rotation_between(&a, &b, 0.5);
        /// assert_relative_eq!(rot2 * rot2 * rot2 * rot2 * rot2 * a, b, epsilon = 1.0e-6);
        /// assert_relative_eq!(rot5 * rot5 * a, b, epsilon = 1.0e-6);
        /// ```
        #[inline]
        pub fn scaled_rotation_between<SB, SC>(
            a: &Vector<T, U2, SB>,
            b: &Vector<T, U2, SC>,
            s: T,
        ) -> Self
        where
            T: RealField,
            SB: Storage<T, U2>,
            SC: Storage<T, U2>,
        {
            crate::convert(UnitComplex::scaled_rotation_between(a, b, s).to_rotation_matrix())
        }

        /// The rotation matrix needed to make `self` and `other` coincide.
        ///
        /// The result is such that: `self.rotation_to(other) * self == other`.
        ///
        /// # Example
        /// ```
        /// # #[macro_use] extern crate approx;
        /// # use nalgebra::Rotation2;
        /// let rot1 = Rotation2::new(0.1);
        /// let rot2 = Rotation2::new(1.7);
        /// let rot_to = rot1.rotation_to(&rot2);
        ///
        /// assert_relative_eq!(rot_to * rot1, rot2);
        /// assert_relative_eq!(rot_to.inverse() * rot2, rot1);
        /// ```
        #[inline]
        #[must_use]
        pub fn rotation_to(&self, other: &Self) -> Self {
            other * self.inverse()
        }

        /// Ensure this rotation is an orthonormal rotation matrix. This is useful when repeated
        /// computations might cause the matrix from progressively not being orthonormal anymore.
        #[inline]
        pub fn renormalize(&mut self)
        where
            T: RealField,
        {
            let mut c = UnitComplex::from(self.clone());
            let _ = c.renormalize();

            *self = Self::from_matrix_eps(self.matrix(), T::default_epsilon(), 0, c.into())
        }

        /// Raise the rotation to a given floating power, i.e., returns the rotation with the angle
        /// of `self` multiplied by `n`.
        ///
        /// # Example
        /// ```
        /// # #[macro_use] extern crate approx;
        /// # use nalgebra::Rotation2;
        /// let rot = Rotation2::new(0.78);
        /// let pow = rot.powf(2.0);
        /// assert_relative_eq!(pow.angle(), 2.0 * 0.78);
        /// ```
        #[inline]
        #[must_use]
        pub fn powf(&self, n: T) -> Self {
            Self::new(self.angle() * n)
        }
    }

    /// # 2D angle extraction
    impl<T: SimdRealField> Rotation2<T> {
        /// The rotation angle.
        ///
        /// # Example
        /// ```
        /// # #[macro_use] extern crate approx;
        /// # use nalgebra::Rotation2;
        /// let rot = Rotation2::new(1.78);
        /// assert_relative_eq!(rot.angle(), 1.78);
        /// ```
        #[inline]
        #[must_use]
        pub fn angle(&self) -> T {
            self.matrix()[(1, 0)]
                .clone()
                .simd_atan2(self.matrix()[(0, 0)].clone())
        }

        /// The rotation angle needed to make `self` and `other` coincide.
        ///
        /// # Example
        /// ```
        /// # #[macro_use] extern crate approx;
        /// # use nalgebra::Rotation2;
        /// let rot1 = Rotation2::new(0.1);
        /// let rot2 = Rotation2::new(1.7);
        /// assert_relative_eq!(rot1.angle_to(&rot2), 1.6);
        /// ```
        #[inline]
        #[must_use]
        pub fn angle_to(&self, other: &Self) -> T {
            self.rotation_to(other).angle()
        }

        /// The rotation angle returned as a 1-dimensional vector.
        ///
        /// This is generally used in the context of generic programming. Using
        /// the `.angle()` method instead is more common.
        #[inline]
        #[must_use]
        pub fn scaled_axis(&self) -> SVector<T, 1> {
            Vector1::new(self.angle())
        }
    }

    #[cfg(feature = "rand-no-std")]
    impl<T: SimdRealField> Distribution<Rotation2<T>> for Standard
    where
        T::Element: SimdRealField,
        T: SampleUniform,
    {
        /// Generate a uniformly distributed random rotation.
        #[inline]
        fn sample<R: Rng + ?Sized>(&self, rng: &mut R) -> Rotation2<T> {
            let twopi = Uniform::new(T::zero(), T::simd_two_pi());
            Rotation2::new(rng.sample(twopi))
        }
    }

    #[cfg(feature = "arbitrary")]
    impl<T: SimdRealField + Arbitrary> Arbitrary for Rotation2<T>
    where
        T::Element: SimdRealField,
        Owned<T, U2, U2>: Send,
    {
        #[inline]
        fn arbitrary(g: &mut Gen) -> Self {
            Self::new(T::arbitrary(g))
        }
    }

    /*
     *
     * 3D Rotation matrix.
     *
     */
    /// # Construction from a 3D axis and/or angles
    impl<T: SimdRealField> Rotation3<T>
    where
        T::Element: SimdRealField,
    {
        /// Builds a 3 dimensional rotation matrix from an axis and an angle.
        ///
        /// # Arguments
        ///   * `axisangle` - A vector representing the rotation. Its magnitude is the amount of rotation
        ///   in radian. Its direction is the axis of rotation.
        ///
        /// # Example
        /// ```
        /// # #[macro_use] extern crate approx;
        /// # use std::f32;
        /// # use nalgebra::{Rotation3, Point3, Vector3};
        /// let axisangle = Vector3::y() * f32::consts::FRAC_PI_2;
        /// // Point and vector being transformed in the tests.
        /// let pt = Point3::new(4.0, 5.0, 6.0);
        /// let vec = Vector3::new(4.0, 5.0, 6.0);
        /// let rot = Rotation3::new(axisangle);
        ///
        /// assert_relative_eq!(rot * pt, Point3::new(6.0, 5.0, -4.0), epsilon = 1.0e-6);
        /// assert_relative_eq!(rot * vec, Vector3::new(6.0, 5.0, -4.0), epsilon = 1.0e-6);
        ///
        /// // A zero vector yields an identity.
        /// assert_eq!(Rotation3::new(Vector3::<f32>::zeros()), Rotation3::identity());
        /// ```
        pub fn new<SB: Storage<T, U3>>(axisangle: Vector<T, U3, SB>) -> Self {
            let axisangle = axisangle.into_owned();
            let (axis, angle) = Unit::new_and_get(axisangle);
            Self::from_axis_angle(&axis, angle)
        }

        /// Builds a 3D rotation matrix from an axis scaled by the rotation angle.
        ///
        /// This is the same as `Self::new(axisangle)`.
        ///
        /// # Example
        /// ```
        /// # #[macro_use] extern crate approx;
        /// # use std::f32;
        /// # use nalgebra::{Rotation3, Point3, Vector3};
        /// let axisangle = Vector3::y() * f32::consts::FRAC_PI_2;
        /// // Point and vector being transformed in the tests.
        /// let pt = Point3::new(4.0, 5.0, 6.0);
        /// let vec = Vector3::new(4.0, 5.0, 6.0);
        /// let rot = Rotation3::new(axisangle);
        ///
        /// assert_relative_eq!(rot * pt, Point3::new(6.0, 5.0, -4.0), epsilon = 1.0e-6);
        /// assert_relative_eq!(rot * vec, Vector3::new(6.0, 5.0, -4.0), epsilon = 1.0e-6);
        ///
        /// // A zero vector yields an identity.
        /// assert_eq!(Rotation3::from_scaled_axis(Vector3::<f32>::zeros()), Rotation3::identity());
        /// ```
        pub fn from_scaled_axis<SB: Storage<T, U3>>(axisangle: Vector<T, U3, SB>) -> Self {
            Self::new(axisangle)
        }

        /// Builds a 3D rotation matrix from an axis and a rotation angle.
        ///
        /// # Example
        /// ```
        /// # #[macro_use] extern crate approx;
        /// # use std::f32;
        /// # use nalgebra::{Rotation3, Point3, Vector3};
        /// let axis = Vector3::y_axis();
        /// let angle = f32::consts::FRAC_PI_2;
        /// // Point and vector being transformed in the tests.
        /// let pt = Point3::new(4.0, 5.0, 6.0);
        /// let vec = Vector3::new(4.0, 5.0, 6.0);
        /// let rot = Rotation3::from_axis_angle(&axis, angle);
        ///
        /// assert_eq!(rot.axis().unwrap(), axis);
        /// assert_eq!(rot.angle(), angle);
        /// assert_relative_eq!(rot * pt, Point3::new(6.0, 5.0, -4.0), epsilon = 1.0e-6);
        /// assert_relative_eq!(rot * vec, Vector3::new(6.0, 5.0, -4.0), epsilon = 1.0e-6);
        ///
        /// // A zero vector yields an identity.
        /// assert_eq!(Rotation3::from_scaled_axis(Vector3::<f32>::zeros()), Rotation3::identity());
        /// ```
        pub fn from_axis_angle<SB>(axis: &Unit<Vector<T, U3, SB>>, angle: T) -> Self
        where
            SB: Storage<T, U3>,
        {
            angle.clone().simd_ne(T::zero()).if_else(
                || {
                    let ux = axis.as_ref()[0].clone();
                    let uy = axis.as_ref()[1].clone();
                    let uz = axis.as_ref()[2].clone();
                    let sqx = ux.clone() * ux.clone();
                    let sqy = uy.clone() * uy.clone();
                    let sqz = uz.clone() * uz.clone();
                    let (sin, cos) = angle.simd_sin_cos();
                    let one_m_cos = T::one() - cos.clone();

                    Self::from_matrix_unchecked(SMatrix::<T, 3, 3>::new(
                        sqx.clone() + (T::one() - sqx) * cos.clone(),
                        ux.clone() * uy.clone() * one_m_cos.clone() - uz.clone() * sin.clone(),
                        ux.clone() * uz.clone() * one_m_cos.clone() + uy.clone() * sin.clone(),
                        ux.clone() * uy.clone() * one_m_cos.clone() + uz.clone() * sin.clone(),
                        sqy.clone() + (T::one() - sqy) * cos.clone(),
                        uy.clone() * uz.clone() * one_m_cos.clone() - ux.clone() * sin.clone(),
                        ux.clone() * uz.clone() * one_m_cos.clone() - uy.clone() * sin.clone(),
                        uy * uz * one_m_cos + ux * sin,
                        sqz.clone() + (T::one() - sqz) * cos,
                    ))
                },
                Self::identity,
            )
        }

        /// Creates a new rotation from Euler angles.
        ///
        /// The primitive rotations are applied in order: 1 roll − 2 pitch − 3 yaw.
        ///
        /// # Example
        /// ```
        /// # #[macro_use] extern crate approx;
        /// # use nalgebra::Rotation3;
        /// let rot = Rotation3::from_euler_angles(0.1, 0.2, 0.3);
        /// let euler = rot.euler_angles();
        /// assert_relative_eq!(euler.0, 0.1, epsilon = 1.0e-6);
        /// assert_relative_eq!(euler.1, 0.2, epsilon = 1.0e-6);
        /// assert_relative_eq!(euler.2, 0.3, epsilon = 1.0e-6);
        /// ```
        pub fn from_euler_angles(roll: T, pitch: T, yaw: T) -> Self {
            let (sr, cr) = roll.simd_sin_cos();
            let (sp, cp) = pitch.simd_sin_cos();
            let (sy, cy) = yaw.simd_sin_cos();

            Self::from_matrix_unchecked(SMatrix::<T, 3, 3>::new(
                cy.clone() * cp.clone(),
                cy.clone() * sp.clone() * sr.clone() - sy.clone() * cr.clone(),
                cy.clone() * sp.clone() * cr.clone() + sy.clone() * sr.clone(),
                sy.clone() * cp.clone(),
                sy.clone() * sp.clone() * sr.clone() + cy.clone() * cr.clone(),
                sy * sp.clone() * cr.clone() - cy * sr.clone(),
                -sp,
                cp.clone() * sr,
                cp * cr,
            ))
        }
    }

    /// # Construction from a 3D eye position and target point
    impl<T: SimdRealField> Rotation3<T>
    where
        T::Element: SimdRealField,
    {
        /// Creates a rotation that corresponds to the local frame of an observer standing at the
        /// origin and looking toward `dir`.
        ///
        /// It maps the `z` axis to the direction `dir`.
        ///
        /// # Arguments
        ///   * dir - The look direction, that is, direction the matrix `z` axis will be aligned with.
        ///   * up - The vertical direction. The only requirement of this parameter is to not be
        ///   collinear to `dir`. Non-collinearity is not checked.
        ///
        /// # Example
        /// ```
        /// # #[macro_use] extern crate approx;
        /// # use std::f32;
        /// # use nalgebra::{Rotation3, Vector3};
        /// let dir = Vector3::new(1.0, 2.0, 3.0);
        /// let up = Vector3::y();
        ///
        /// let rot = Rotation3::face_towards(&dir, &up);
        /// assert_relative_eq!(rot * Vector3::z(), dir.normalize());
        /// ```
        #[inline]
        pub fn face_towards<SB, SC>(dir: &Vector<T, U3, SB>, up: &Vector<T, U3, SC>) -> Self
        where
            SB: Storage<T, U3>,
            SC: Storage<T, U3>,
        {
            let zaxis = dir.normalize();
            let xaxis = up.cross(&zaxis).normalize();
            let yaxis = zaxis.cross(&xaxis).normalize();

            Self::from_matrix_unchecked(SMatrix::<T, 3, 3>::new(
                xaxis.x.clone(),
                yaxis.x.clone(),
                zaxis.x.clone(),
                xaxis.y.clone(),
                yaxis.y.clone(),
                zaxis.y.clone(),
                xaxis.z.clone(),
                yaxis.z.clone(),
                zaxis.z.clone(),
            ))
        }

        /// Deprecated: Use [`Rotation3::face_towards`] instead.
        #[deprecated(note = "renamed to `face_towards`")]
        pub fn new_observer_frames<SB, SC>(dir: &Vector<T, U3, SB>, up: &Vector<T, U3, SC>) -> Self
        where
            SB: Storage<T, U3>,
            SC: Storage<T, U3>,
        {
            Self::face_towards(dir, up)
        }

        /// Builds a right-handed look-at view matrix without translation.
        ///
        /// It maps the view direction `dir` to the **negative** `z` axis.
        /// This conforms to the common notion of right handed look-at matrix from the computer
        /// graphics community.
        ///
        /// # Arguments
        ///   * dir - The direction toward which the camera looks.
        ///   * up - A vector approximately aligned with required the vertical axis. The only
        ///   requirement of this parameter is to not be collinear to `dir`.
        ///
        /// # Example
        /// ```
        /// # #[macro_use] extern crate approx;
        /// # use std::f32;
        /// # use nalgebra::{Rotation3, Vector3};
        /// let dir = Vector3::new(1.0, 2.0, 3.0);
        /// let up = Vector3::y();
        ///
        /// let rot = Rotation3::look_at_rh(&dir, &up);
        /// assert_relative_eq!(rot * dir.normalize(), -Vector3::z());
        /// ```
        #[inline]
        pub fn look_at_rh<SB, SC>(dir: &Vector<T, U3, SB>, up: &Vector<T, U3, SC>) -> Self
        where
            SB: Storage<T, U3>,
            SC: Storage<T, U3>,
        {
            Self::face_towards(&dir.neg(), up).inverse()
        }

        /// Builds a left-handed look-at view matrix without translation.
        ///
        /// It maps the view direction `dir` to the **positive** `z` axis.
        /// This conforms to the common notion of left handed look-at matrix from the computer
        /// graphics community.
        ///
        /// # Arguments
        ///   * dir - The direction toward which the camera looks.
        ///   * up - A vector approximately aligned with required the vertical axis. The only
        ///   requirement of this parameter is to not be collinear to `dir`.
        ///
        /// # Example
        /// ```
        /// # #[macro_use] extern crate approx;
        /// # use std::f32;
        /// # use nalgebra::{Rotation3, Vector3};
        /// let dir = Vector3::new(1.0, 2.0, 3.0);
        /// let up = Vector3::y();
        ///
        /// let rot = Rotation3::look_at_lh(&dir, &up);
        /// assert_relative_eq!(rot * dir.normalize(), Vector3::z());
        /// ```
        #[inline]
        pub fn look_at_lh<SB, SC>(dir: &Vector<T, U3, SB>, up: &Vector<T, U3, SC>) -> Self
        where
            SB: Storage<T, U3>,
            SC: Storage<T, U3>,
        {
            Self::face_towards(dir, up).inverse()
        }
    }

    /// # Construction from an existing 3D matrix or rotations
    impl<T: SimdRealField> Rotation3<T>
    where
        T::Element: SimdRealField,
    {
        /// The rotation matrix required to align `a` and `b` but with its angle.
        ///
        /// This is the rotation `R` such that `(R * a).angle(b) == 0 && (R * a).dot(b).is_positive()`.
        ///
        /// # Example
        /// ```
        /// # #[macro_use] extern crate approx;
        /// # use nalgebra::{Vector3, Rotation3};
        /// let a = Vector3::new(1.0, 2.0, 3.0);
        /// let b = Vector3::new(3.0, 1.0, 2.0);
        /// let rot = Rotation3::rotation_between(&a, &b).unwrap();
        /// assert_relative_eq!(rot * a, b, epsilon = 1.0e-6);
        /// assert_relative_eq!(rot.inverse() * b, a, epsilon = 1.0e-6);
        /// ```
        #[inline]
        pub fn rotation_between<SB, SC>(a: &Vector<T, U3, SB>, b: &Vector<T, U3, SC>) -> Option<Self>
        where
            T: RealField,
            SB: Storage<T, U3>,
            SC: Storage<T, U3>,
        {
            Self::scaled_rotation_between(a, b, T::one())
        }

        /// The smallest rotation needed to make `a` and `b` collinear and point toward the same
        /// direction, raised to the power `s`.
        ///
        /// # Example
        /// ```
        /// # #[macro_use] extern crate approx;
        /// # use nalgebra::{Vector3, Rotation3};
        /// let a = Vector3::new(1.0, 2.0, 3.0);
        /// let b = Vector3::new(3.0, 1.0, 2.0);
        /// let rot2 = Rotation3::scaled_rotation_between(&a, &b, 0.2).unwrap();
        /// let rot5 = Rotation3::scaled_rotation_between(&a, &b, 0.5).unwrap();
        /// assert_relative_eq!(rot2 * rot2 * rot2 * rot2 * rot2 * a, b, epsilon = 1.0e-6);
        /// assert_relative_eq!(rot5 * rot5 * a, b, epsilon = 1.0e-6);
        /// ```
        #[inline]
        pub fn scaled_rotation_between<SB, SC>(
            a: &Vector<T, U3, SB>,
            b: &Vector<T, U3, SC>,
            n: T,
        ) -> Option<Self>
        where
            T: RealField,
            SB: Storage<T, U3>,
            SC: Storage<T, U3>,
        {
            // TODO: code duplication with Rotation.
            if let (Some(na), Some(nb)) = (a.try_normalize(T::zero()), b.try_normalize(T::zero())) {
                let c = na.cross(&nb);

                if let Some(axis) = Unit::try_new(c, T::default_epsilon()) {
                    return Some(Self::from_axis_angle(&axis, na.dot(&nb).acos() * n));
                }

                // Zero or PI.
                if na.dot(&nb) < T::zero() {
                    // PI
                    //
                    // The rotation axis is undefined but the angle not zero. This is not a
                    // simple rotation.
                    return None;
                }
            }

            Some(Self::identity())
        }

        /// The rotation matrix needed to make `self` and `other` coincide.
        ///
        /// The result is such that: `self.rotation_to(other) * self == other`.
        ///
        /// # Example
        /// ```
        /// # #[macro_use] extern crate approx;
        /// # use nalgebra::{Rotation3, Vector3};
        /// let rot1 = Rotation3::from_axis_angle(&Vector3::y_axis(), 1.0);
        /// let rot2 = Rotation3::from_axis_angle(&Vector3::x_axis(), 0.1);
        /// let rot_to = rot1.rotation_to(&rot2);
        /// assert_relative_eq!(rot_to * rot1, rot2, epsilon = 1.0e-6);
        /// ```
        #[inline]
        #[must_use]
        pub fn rotation_to(&self, other: &Self) -> Self {
            other * self.inverse()
        }

        /// Raise the rotation to a given floating power, i.e., returns the rotation with the same
        /// axis as `self` and an angle equal to `self.angle()` multiplied by `n`.
        ///
        /// # Example
        /// ```
        /// # #[macro_use] extern crate approx;
        /// # use nalgebra::{Rotation3, Vector3, Unit};
        /// let axis = Unit::new_normalize(Vector3::new(1.0, 2.0, 3.0));
        /// let angle = 1.2;
        /// let rot = Rotation3::from_axis_angle(&axis, angle);
        /// let pow = rot.powf(2.0);
        /// assert_relative_eq!(pow.axis().unwrap(), axis, epsilon = 1.0e-6);
        /// assert_eq!(pow.angle(), 2.4);
        /// ```
        #[inline]
        #[must_use]
        pub fn powf(&self, n: T) -> Self
        where
            T: RealField,
        {
            if let Some(axis) = self.axis() {
                Self::from_axis_angle(&axis, self.angle() * n)
            } else if self.matrix()[(0, 0)] < T::zero() {
                let minus_id = SMatrix::<T, 3, 3>::from_diagonal_element(-T::one());
                Self::from_matrix_unchecked(minus_id)
            } else {
                Self::identity()
            }
        }

        /// Builds a rotation from a basis assumed to be orthonormal.
        ///
        /// In order to get a valid rotation matrix, the input must be an
        /// orthonormal basis, i.e., all vectors are normalized, and the are
        /// all orthogonal to each other. These invariants are not checked
        /// by this method.
        pub fn from_basis_unchecked(basis: &[Vector3<T>; 3]) -> Self {
            let mat = Matrix3::from_columns(&basis[..]);
            Self::from_matrix_unchecked(mat)
        }

        /// Builds a rotation matrix by extracting the rotation part of the given transformation `m`.
        ///
        /// This is an iterative method. See `.from_matrix_eps` to provide mover
        /// convergence parameters and starting solution.
        /// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al.
        pub fn from_matrix(m: &Matrix3<T>) -> Self
        where
            T: RealField,
        {
            Self::from_matrix_eps(m, T::default_epsilon(), 0, Self::identity())
        }

        /// Builds a rotation matrix by extracting the rotation part of the given transformation `m`.
        ///
        /// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al.
        ///
        /// # Parameters
        ///
        /// * `m`: the matrix from which the rotational part is to be extracted.
        /// * `eps`: the angular errors tolerated between the current rotation and the optimal one.
        /// * `max_iter`: the maximum number of iterations. Loops indefinitely until convergence if set to `0`.
        /// * `guess`: a guess of the solution. Convergence will be significantly faster if an initial solution close
        ///           to the actual solution is provided. Can be set to `Rotation3::identity()` if no other
        ///           guesses come to mind.
        pub fn from_matrix_eps(m: &Matrix3<T>, eps: T, mut max_iter: usize, guess: Self) -> Self
        where
            T: RealField,
        {
            if max_iter == 0 {
                max_iter = usize::MAX;
            }

            // Using sqrt(eps) ensures we perturb with something larger than eps; clamp to eps to handle the case of eps > 1.0
            let eps_disturbance = eps.clone().sqrt().max(eps.clone() * eps.clone());
            let mut perturbation_axes = Vector3::x_axis();
            let mut rot = guess.into_inner();

            for _ in 0..max_iter {
                let axis = rot.column(0).cross(&m.column(0))
                    + rot.column(1).cross(&m.column(1))
                    + rot.column(2).cross(&m.column(2));
                let denom = rot.column(0).dot(&m.column(0))
                    + rot.column(1).dot(&m.column(1))
                    + rot.column(2).dot(&m.column(2));

                let axisangle = axis / (denom.abs() + T::default_epsilon());

                if let Some((axis, angle)) = Unit::try_new_and_get(axisangle, eps.clone()) {
                    rot = Rotation3::from_axis_angle(&axis, angle) * rot;
                } else {
                    // Check if stuck in a maximum w.r.t. the norm (m - rot).norm()
                    let mut perturbed = rot.clone();
                    let norm_squared = (m - &rot).norm_squared();
                    let mut new_norm_squared: T;

                    // Perturb until the new norm is significantly different
                    loop {
                        perturbed *=
                            Rotation3::from_axis_angle(&perturbation_axes, eps_disturbance.clone());
                        new_norm_squared = (m - &perturbed).norm_squared();
                        if abs_diff_ne!(
                            norm_squared,
                            new_norm_squared,
                            epsilon = T::default_epsilon()
                        ) {
                            break;
                        }
                    }

                    // If new norm is larger, it's a minimum
                    if norm_squared < new_norm_squared {
                        break;
                    }

                    // If not, continue from perturbed rotation, but use a different axes for the next perturbation
                    perturbation_axes = UnitVector3::new_unchecked(perturbation_axes.yzx());
                    rot = perturbed;
                }
            }

            Self::from_matrix_unchecked(rot)
        }

        /// Ensure this rotation is an orthonormal rotation matrix. This is useful when repeated
        /// computations might cause the matrix from progressively not being orthonormal anymore.
        #[inline]
        pub fn renormalize(&mut self)
        where
            T: RealField,
        {
            let mut c = UnitQuaternion::from(self.clone());
            let _ = c.renormalize();

            *self = Self::from_matrix_eps(self.matrix(), T::default_epsilon(), 0, c.into())
        }
    }

    /// # 3D axis and angle extraction
    impl<T: SimdRealField> Rotation3<T> {
        /// The rotation angle in [0; pi].
        ///
        /// # Example
        /// ```
        /// # #[macro_use] extern crate approx;
        /// # use nalgebra::{Unit, Rotation3, Vector3};
        /// let axis = Unit::new_normalize(Vector3::new(1.0, 2.0, 3.0));
        /// let rot = Rotation3::from_axis_angle(&axis, 1.78);
        /// assert_relative_eq!(rot.angle(), 1.78);
        /// ```
        #[inline]
        #[must_use]
        pub fn angle(&self) -> T {
            ((self.matrix()[(0, 0)].clone()
                + self.matrix()[(1, 1)].clone()
                + self.matrix()[(2, 2)].clone()
                - T::one())
                / crate::convert(2.0))
            .simd_acos()
        }

        /// The rotation axis. Returns `None` if the rotation angle is zero or PI.
        ///
        /// # Example
        /// ```
        /// # #[macro_use] extern crate approx;
        /// # use nalgebra::{Rotation3, Vector3, Unit};
        /// let axis = Unit::new_normalize(Vector3::new(1.0, 2.0, 3.0));
        /// let angle = 1.2;
        /// let rot = Rotation3::from_axis_angle(&axis, angle);
        /// assert_relative_eq!(rot.axis().unwrap(), axis);
        ///
        /// // Case with a zero angle.
        /// let rot = Rotation3::from_axis_angle(&axis, 0.0);
        /// assert!(rot.axis().is_none());
        /// ```
        #[inline]
        #[must_use]
        pub fn axis(&self) -> Option<Unit<Vector3<T>>>
        where
            T: RealField,
        {
            let rotmat = self.matrix();
            let axis = SVector::<T, 3>::new(
                rotmat[(2, 1)].clone() - rotmat[(1, 2)].clone(),
                rotmat[(0, 2)].clone() - rotmat[(2, 0)].clone(),
                rotmat[(1, 0)].clone() - rotmat[(0, 1)].clone(),
            );

            Unit::try_new(axis, T::default_epsilon())
        }

        /// The rotation axis multiplied by the rotation angle.
        ///
        /// # Example
        /// ```
        /// # #[macro_use] extern crate approx;
        /// # use nalgebra::{Rotation3, Vector3, Unit};
        /// let axisangle = Vector3::new(0.1, 0.2, 0.3);
        /// let rot = Rotation3::new(axisangle);
        /// assert_relative_eq!(rot.scaled_axis(), axisangle, epsilon = 1.0e-6);
        /// ```
        #[inline]
        #[must_use]
        pub fn scaled_axis(&self) -> Vector3<T>
        where
            T: RealField,
        {
            if let Some(axis) = self.axis() {
                axis.into_inner() * self.angle()
            } else {
                Vector::zero()
            }
        }

        /// The rotation axis and angle in (0, pi] of this rotation matrix.
        ///
        /// Returns `None` if the angle is zero.
        ///
        /// # Example
        /// ```
        /// # #[macro_use] extern crate approx;
        /// # use nalgebra::{Rotation3, Vector3, Unit};
        /// let axis = Unit::new_normalize(Vector3::new(1.0, 2.0, 3.0));
        /// let angle = 1.2;
        /// let rot = Rotation3::from_axis_angle(&axis, angle);
        /// let axis_angle = rot.axis_angle().unwrap();
        /// assert_relative_eq!(axis_angle.0, axis);
        /// assert_relative_eq!(axis_angle.1, angle);
        ///
        /// // Case with a zero angle.
        /// let rot = Rotation3::from_axis_angle(&axis, 0.0);
        /// assert!(rot.axis_angle().is_none());
        /// ```
        #[inline]
        #[must_use]
        pub fn axis_angle(&self) -> Option<(Unit<Vector3<T>>, T)>
        where
            T: RealField,
        {
            self.axis().map(|axis| (axis, self.angle()))
        }

        /// The rotation angle needed to make `self` and `other` coincide.
        ///
        /// # Example
        /// ```
        /// # #[macro_use] extern crate approx;
        /// # use nalgebra::{Rotation3, Vector3};
        /// let rot1 = Rotation3::from_axis_angle(&Vector3::y_axis(), 1.0);
        /// let rot2 = Rotation3::from_axis_angle(&Vector3::x_axis(), 0.1);
        /// assert_relative_eq!(rot1.angle_to(&rot2), 1.0045657, epsilon = 1.0e-6);
        /// ```
        #[inline]
        #[must_use]
        pub fn angle_to(&self, other: &Self) -> T
        where
            T::Element: SimdRealField,
        {
            self.rotation_to(other).angle()
        }

        /// Creates Euler angles from a rotation.
        ///
        /// The angles are produced in the form (roll, pitch, yaw).
        #[deprecated(note = "This is renamed to use `.euler_angles()`.")]
        pub fn to_euler_angles(self) -> (T, T, T)
        where
            T: RealField,
        {
            self.euler_angles()
        }

        /// Euler angles corresponding to this rotation from a rotation.
        ///
        /// The angles are produced in the form (roll, pitch, yaw).
        ///
        /// # Example
        /// ```
        /// # #[macro_use] extern crate approx;
        /// # use nalgebra::Rotation3;
        /// let rot = Rotation3::from_euler_angles(0.1, 0.2, 0.3);
        /// let euler = rot.euler_angles();
        /// assert_relative_eq!(euler.0, 0.1, epsilon = 1.0e-6);
        /// assert_relative_eq!(euler.1, 0.2, epsilon = 1.0e-6);
        /// assert_relative_eq!(euler.2, 0.3, epsilon = 1.0e-6);
        /// ```
        #[must_use]
        pub fn euler_angles(&self) -> (T, T, T)
        where
            T: RealField,
        {
            // Implementation informed by "Computing Euler angles from a rotation matrix", by Gregory G. Slabaugh
            //  https://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.371.6578
            //  where roll, pitch, yaw angles are referred to as ψ, θ, ϕ,
            if self[(2, 0)].clone().abs() < T::one() {
                let pitch = -self[(2, 0)].clone().asin();
                let theta_cos = pitch.clone().cos();
                let roll = (self[(2, 1)].clone() / theta_cos.clone())
                    .atan2(self[(2, 2)].clone() / theta_cos.clone());
                let yaw =
                    (self[(1, 0)].clone() / theta_cos.clone()).atan2(self[(0, 0)].clone() / theta_cos);
                (roll, pitch, yaw)
            } else if self[(2, 0)].clone() <= -T::one() {
                (
                    self[(0, 1)].clone().atan2(self[(0, 2)].clone()),
                    T::frac_pi_2(),
                    T::zero(),
                )
            } else {
                (
                    -self[(0, 1)].clone().atan2(-self[(0, 2)].clone()),
                    -T::frac_pi_2(),
                    T::zero(),
                )
            }
        }
    }

    #[cfg(feature = "rand-no-std")]
    impl<T: SimdRealField> Distribution<Rotation3<T>> for Standard
    where
        T::Element: SimdRealField,
        OpenClosed01: Distribution<T>,
        T: SampleUniform,
    {
        /// Generate a uniformly distributed random rotation.
        #[inline]
        fn sample<'a, R: Rng + ?Sized>(&self, rng: &mut R) -> Rotation3<T> {
            // James Arvo.
            // Fast random rotation matrices.
            // In D. Kirk, editor, Graphics Gems III, pages 117-120. Academic, New York, 1992.

            // Compute a random rotation around Z
            let twopi = Uniform::new(T::zero(), T::simd_two_pi());
            let theta = rng.sample(&twopi);
            let (ts, tc) = theta.simd_sin_cos();
            let a = SMatrix::<T, 3, 3>::new(
                tc.clone(),
                ts.clone(),
                T::zero(),
                -ts,
                tc,
                T::zero(),
                T::zero(),
                T::zero(),
                T::one(),
            );

            // Compute a random rotation *of* Z
            let phi = rng.sample(&twopi);
            let z = rng.sample(OpenClosed01);
            let (ps, pc) = phi.simd_sin_cos();
            let sqrt_z = z.clone().simd_sqrt();
            let v = Vector3::new(pc * sqrt_z.clone(), ps * sqrt_z, (T::one() - z).simd_sqrt());
            let mut b = v.clone() * v.transpose();
            b += b.clone();
            b -= SMatrix::<T, 3, 3>::identity();

            Rotation3::from_matrix_unchecked(b * a)
        }
    }

    #[cfg(feature = "arbitrary")]
    impl<T: SimdRealField + Arbitrary> Arbitrary for Rotation3<T>
    where
        T::Element: SimdRealField,
        Owned<T, U3, U3>: Send,
        Owned<T, U3>: Send,
    {
        #[inline]
        fn arbitrary(g: &mut Gen) -> Self {
            Self::new(SVector::arbitrary(g))
        }
    }