nalgebra/geometry/rotation_specialization.rs
1#[cfg(feature = "arbitrary")]
2use crate::base::storage::Owned;
3#[cfg(feature = "arbitrary")]
4use quickcheck::{Arbitrary, Gen};
5
6use num::Zero;
7
8#[cfg(feature = "rand-no-std")]
9use rand::{
10 distributions::{uniform::SampleUniform, Distribution, OpenClosed01, Standard, Uniform},
11 Rng,
12};
13
14use simba::scalar::RealField;
15use simba::simd::{SimdBool, SimdRealField};
16use std::ops::Neg;
17
18use crate::base::dimension::{U1, U2, U3};
19use crate::base::storage::Storage;
20use crate::base::{Matrix2, Matrix3, SMatrix, SVector, Unit, Vector, Vector1, Vector2, Vector3};
21
22use crate::geometry::{Rotation2, Rotation3, UnitComplex, UnitQuaternion};
23
24/*
25 *
26 * 2D Rotation matrix.
27 *
28 */
29/// # Construction from a 2D rotation angle
30impl<T: SimdRealField> Rotation2<T> {
31 /// Builds a 2 dimensional rotation matrix from an angle in radian.
32 ///
33 /// # Example
34 ///
35 /// ```
36 /// # #[macro_use] extern crate approx;
37 /// # use std::f32;
38 /// # use nalgebra::{Rotation2, Point2};
39 /// let rot = Rotation2::new(f32::consts::FRAC_PI_2);
40 ///
41 /// assert_relative_eq!(rot * Point2::new(3.0, 4.0), Point2::new(-4.0, 3.0));
42 /// ```
43 pub fn new(angle: T) -> Self {
44 let (sia, coa) = angle.simd_sin_cos();
45 Self::from_matrix_unchecked(Matrix2::new(coa.clone(), -sia.clone(), sia, coa))
46 }
47
48 /// Builds a 2 dimensional rotation matrix from an angle in radian wrapped in a 1-dimensional vector.
49 ///
50 ///
51 /// This is generally used in the context of generic programming. Using
52 /// the `::new(angle)` method instead is more common.
53 #[inline]
54 pub fn from_scaled_axis<SB: Storage<T, U1>>(axisangle: Vector<T, U1, SB>) -> Self {
55 Self::new(axisangle[0].clone())
56 }
57}
58
59/// # Construction from an existing 2D matrix or rotations
60impl<T: SimdRealField> Rotation2<T> {
61 /// Builds a rotation from a basis assumed to be orthonormal.
62 ///
63 /// In order to get a valid rotation matrix, the input must be an
64 /// orthonormal basis, i.e., all vectors are normalized, and the are
65 /// all orthogonal to each other. These invariants are not checked
66 /// by this method.
67 pub fn from_basis_unchecked(basis: &[Vector2<T>; 2]) -> Self {
68 let mat = Matrix2::from_columns(&basis[..]);
69 Self::from_matrix_unchecked(mat)
70 }
71
72 /// Builds a rotation matrix by extracting the rotation part of the given transformation `m`.
73 ///
74 /// This is an iterative method. See `.from_matrix_eps` to provide mover
75 /// convergence parameters and starting solution.
76 /// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al.
77 pub fn from_matrix(m: &Matrix2<T>) -> Self
78 where
79 T: RealField,
80 {
81 Self::from_matrix_eps(m, T::default_epsilon(), 0, Self::identity())
82 }
83
84 /// Builds a rotation matrix by extracting the rotation part of the given transformation `m`.
85 ///
86 /// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al.
87 ///
88 /// # Parameters
89 ///
90 /// * `m`: the matrix from which the rotational part is to be extracted.
91 /// * `eps`: the angular errors tolerated between the current rotation and the optimal one.
92 /// * `max_iter`: the maximum number of iterations. Loops indefinitely until convergence if set to `0`.
93 /// * `guess`: an estimate of the solution. Convergence will be significantly faster if an initial solution close
94 /// to the actual solution is provided. Can be set to `Rotation2::identity()` if no other
95 /// guesses come to mind.
96 pub fn from_matrix_eps(m: &Matrix2<T>, eps: T, mut max_iter: usize, guess: Self) -> Self
97 where
98 T: RealField,
99 {
100 if max_iter == 0 {
101 max_iter = usize::max_value();
102 }
103
104 let mut rot = guess.into_inner();
105
106 for _ in 0..max_iter {
107 let axis = rot.column(0).perp(&m.column(0)) + rot.column(1).perp(&m.column(1));
108 let denom = rot.column(0).dot(&m.column(0)) + rot.column(1).dot(&m.column(1));
109
110 let angle = axis / (denom.abs() + T::default_epsilon());
111 if angle.clone().abs() > eps {
112 rot = Self::new(angle) * rot;
113 } else {
114 break;
115 }
116 }
117
118 Self::from_matrix_unchecked(rot)
119 }
120
121 /// The rotation matrix required to align `a` and `b` but with its angle.
122 ///
123 /// This is the rotation `R` such that `(R * a).angle(b) == 0 && (R * a).dot(b).is_positive()`.
124 ///
125 /// # Example
126 /// ```
127 /// # #[macro_use] extern crate approx;
128 /// # use nalgebra::{Vector2, Rotation2};
129 /// let a = Vector2::new(1.0, 2.0);
130 /// let b = Vector2::new(2.0, 1.0);
131 /// let rot = Rotation2::rotation_between(&a, &b);
132 /// assert_relative_eq!(rot * a, b);
133 /// assert_relative_eq!(rot.inverse() * b, a);
134 /// ```
135 #[inline]
136 pub fn rotation_between<SB, SC>(a: &Vector<T, U2, SB>, b: &Vector<T, U2, SC>) -> Self
137 where
138 T: RealField,
139 SB: Storage<T, U2>,
140 SC: Storage<T, U2>,
141 {
142 crate::convert(UnitComplex::rotation_between(a, b).to_rotation_matrix())
143 }
144
145 /// The smallest rotation needed to make `a` and `b` collinear and point toward the same
146 /// direction, raised to the power `s`.
147 ///
148 /// # Example
149 /// ```
150 /// # #[macro_use] extern crate approx;
151 /// # use nalgebra::{Vector2, Rotation2};
152 /// let a = Vector2::new(1.0, 2.0);
153 /// let b = Vector2::new(2.0, 1.0);
154 /// let rot2 = Rotation2::scaled_rotation_between(&a, &b, 0.2);
155 /// let rot5 = Rotation2::scaled_rotation_between(&a, &b, 0.5);
156 /// assert_relative_eq!(rot2 * rot2 * rot2 * rot2 * rot2 * a, b, epsilon = 1.0e-6);
157 /// assert_relative_eq!(rot5 * rot5 * a, b, epsilon = 1.0e-6);
158 /// ```
159 #[inline]
160 pub fn scaled_rotation_between<SB, SC>(
161 a: &Vector<T, U2, SB>,
162 b: &Vector<T, U2, SC>,
163 s: T,
164 ) -> Self
165 where
166 T: RealField,
167 SB: Storage<T, U2>,
168 SC: Storage<T, U2>,
169 {
170 crate::convert(UnitComplex::scaled_rotation_between(a, b, s).to_rotation_matrix())
171 }
172
173 /// The rotation matrix needed to make `self` and `other` coincide.
174 ///
175 /// The result is such that: `self.rotation_to(other) * self == other`.
176 ///
177 /// # Example
178 /// ```
179 /// # #[macro_use] extern crate approx;
180 /// # use nalgebra::Rotation2;
181 /// let rot1 = Rotation2::new(0.1);
182 /// let rot2 = Rotation2::new(1.7);
183 /// let rot_to = rot1.rotation_to(&rot2);
184 ///
185 /// assert_relative_eq!(rot_to * rot1, rot2);
186 /// assert_relative_eq!(rot_to.inverse() * rot2, rot1);
187 /// ```
188 #[inline]
189 #[must_use]
190 pub fn rotation_to(&self, other: &Self) -> Self {
191 other * self.inverse()
192 }
193
194 /// Ensure this rotation is an orthonormal rotation matrix. This is useful when repeated
195 /// computations might cause the matrix from progressively not being orthonormal anymore.
196 #[inline]
197 pub fn renormalize(&mut self)
198 where
199 T: RealField,
200 {
201 let mut c = UnitComplex::from(self.clone());
202 let _ = c.renormalize();
203
204 *self = Self::from_matrix_eps(self.matrix(), T::default_epsilon(), 0, c.into())
205 }
206
207 /// Raise the rotation to a given floating power, i.e., returns the rotation with the angle
208 /// of `self` multiplied by `n`.
209 ///
210 /// # Example
211 /// ```
212 /// # #[macro_use] extern crate approx;
213 /// # use nalgebra::Rotation2;
214 /// let rot = Rotation2::new(0.78);
215 /// let pow = rot.powf(2.0);
216 /// assert_relative_eq!(pow.angle(), 2.0 * 0.78);
217 /// ```
218 #[inline]
219 #[must_use]
220 pub fn powf(&self, n: T) -> Self {
221 Self::new(self.angle() * n)
222 }
223}
224
225/// # 2D angle extraction
226impl<T: SimdRealField> Rotation2<T> {
227 /// The rotation angle.
228 ///
229 /// # Example
230 /// ```
231 /// # #[macro_use] extern crate approx;
232 /// # use nalgebra::Rotation2;
233 /// let rot = Rotation2::new(1.78);
234 /// assert_relative_eq!(rot.angle(), 1.78);
235 /// ```
236 #[inline]
237 #[must_use]
238 pub fn angle(&self) -> T {
239 self.matrix()[(1, 0)]
240 .clone()
241 .simd_atan2(self.matrix()[(0, 0)].clone())
242 }
243
244 /// The rotation angle needed to make `self` and `other` coincide.
245 ///
246 /// # Example
247 /// ```
248 /// # #[macro_use] extern crate approx;
249 /// # use nalgebra::Rotation2;
250 /// let rot1 = Rotation2::new(0.1);
251 /// let rot2 = Rotation2::new(1.7);
252 /// assert_relative_eq!(rot1.angle_to(&rot2), 1.6);
253 /// ```
254 #[inline]
255 #[must_use]
256 pub fn angle_to(&self, other: &Self) -> T {
257 self.rotation_to(other).angle()
258 }
259
260 /// The rotation angle returned as a 1-dimensional vector.
261 ///
262 /// This is generally used in the context of generic programming. Using
263 /// the `.angle()` method instead is more common.
264 #[inline]
265 #[must_use]
266 pub fn scaled_axis(&self) -> SVector<T, 1> {
267 Vector1::new(self.angle())
268 }
269}
270
271#[cfg(feature = "rand-no-std")]
272impl<T: SimdRealField> Distribution<Rotation2<T>> for Standard
273where
274 T::Element: SimdRealField,
275 T: SampleUniform,
276{
277 /// Generate a uniformly distributed random rotation.
278 #[inline]
279 fn sample<R: Rng + ?Sized>(&self, rng: &mut R) -> Rotation2<T> {
280 let twopi = Uniform::new(T::zero(), T::simd_two_pi());
281 Rotation2::new(rng.sample(twopi))
282 }
283}
284
285#[cfg(feature = "arbitrary")]
286impl<T: SimdRealField + Arbitrary> Arbitrary for Rotation2<T>
287where
288 T::Element: SimdRealField,
289 Owned<T, U2, U2>: Send,
290{
291 #[inline]
292 fn arbitrary(g: &mut Gen) -> Self {
293 Self::new(T::arbitrary(g))
294 }
295}
296
297/*
298 *
299 * 3D Rotation matrix.
300 *
301 */
302/// # Construction from a 3D axis and/or angles
303impl<T: SimdRealField> Rotation3<T>
304where
305 T::Element: SimdRealField,
306{
307 /// Builds a 3 dimensional rotation matrix from an axis and an angle.
308 ///
309 /// # Arguments
310 /// * `axisangle` - A vector representing the rotation. Its magnitude is the amount of rotation
311 /// in radian. Its direction is the axis of rotation.
312 ///
313 /// # Example
314 /// ```
315 /// # #[macro_use] extern crate approx;
316 /// # use std::f32;
317 /// # use nalgebra::{Rotation3, Point3, Vector3};
318 /// let axisangle = Vector3::y() * f32::consts::FRAC_PI_2;
319 /// // Point and vector being transformed in the tests.
320 /// let pt = Point3::new(4.0, 5.0, 6.0);
321 /// let vec = Vector3::new(4.0, 5.0, 6.0);
322 /// let rot = Rotation3::new(axisangle);
323 ///
324 /// assert_relative_eq!(rot * pt, Point3::new(6.0, 5.0, -4.0), epsilon = 1.0e-6);
325 /// assert_relative_eq!(rot * vec, Vector3::new(6.0, 5.0, -4.0), epsilon = 1.0e-6);
326 ///
327 /// // A zero vector yields an identity.
328 /// assert_eq!(Rotation3::new(Vector3::<f32>::zeros()), Rotation3::identity());
329 /// ```
330 pub fn new<SB: Storage<T, U3>>(axisangle: Vector<T, U3, SB>) -> Self {
331 let axisangle = axisangle.into_owned();
332 let (axis, angle) = Unit::new_and_get(axisangle);
333 Self::from_axis_angle(&axis, angle)
334 }
335
336 /// Builds a 3D rotation matrix from an axis scaled by the rotation angle.
337 ///
338 /// This is the same as `Self::new(axisangle)`.
339 ///
340 /// # Example
341 /// ```
342 /// # #[macro_use] extern crate approx;
343 /// # use std::f32;
344 /// # use nalgebra::{Rotation3, Point3, Vector3};
345 /// let axisangle = Vector3::y() * f32::consts::FRAC_PI_2;
346 /// // Point and vector being transformed in the tests.
347 /// let pt = Point3::new(4.0, 5.0, 6.0);
348 /// let vec = Vector3::new(4.0, 5.0, 6.0);
349 /// let rot = Rotation3::new(axisangle);
350 ///
351 /// assert_relative_eq!(rot * pt, Point3::new(6.0, 5.0, -4.0), epsilon = 1.0e-6);
352 /// assert_relative_eq!(rot * vec, Vector3::new(6.0, 5.0, -4.0), epsilon = 1.0e-6);
353 ///
354 /// // A zero vector yields an identity.
355 /// assert_eq!(Rotation3::from_scaled_axis(Vector3::<f32>::zeros()), Rotation3::identity());
356 /// ```
357 pub fn from_scaled_axis<SB: Storage<T, U3>>(axisangle: Vector<T, U3, SB>) -> Self {
358 Self::new(axisangle)
359 }
360
361 /// Builds a 3D rotation matrix from an axis and a rotation angle.
362 ///
363 /// # Example
364 /// ```
365 /// # #[macro_use] extern crate approx;
366 /// # use std::f32;
367 /// # use nalgebra::{Rotation3, Point3, Vector3};
368 /// let axis = Vector3::y_axis();
369 /// let angle = f32::consts::FRAC_PI_2;
370 /// // Point and vector being transformed in the tests.
371 /// let pt = Point3::new(4.0, 5.0, 6.0);
372 /// let vec = Vector3::new(4.0, 5.0, 6.0);
373 /// let rot = Rotation3::from_axis_angle(&axis, angle);
374 ///
375 /// assert_eq!(rot.axis().unwrap(), axis);
376 /// assert_eq!(rot.angle(), angle);
377 /// assert_relative_eq!(rot * pt, Point3::new(6.0, 5.0, -4.0), epsilon = 1.0e-6);
378 /// assert_relative_eq!(rot * vec, Vector3::new(6.0, 5.0, -4.0), epsilon = 1.0e-6);
379 ///
380 /// // A zero vector yields an identity.
381 /// assert_eq!(Rotation3::from_scaled_axis(Vector3::<f32>::zeros()), Rotation3::identity());
382 /// ```
383 pub fn from_axis_angle<SB>(axis: &Unit<Vector<T, U3, SB>>, angle: T) -> Self
384 where
385 SB: Storage<T, U3>,
386 {
387 angle.clone().simd_ne(T::zero()).if_else(
388 || {
389 let ux = axis.as_ref()[0].clone();
390 let uy = axis.as_ref()[1].clone();
391 let uz = axis.as_ref()[2].clone();
392 let sqx = ux.clone() * ux.clone();
393 let sqy = uy.clone() * uy.clone();
394 let sqz = uz.clone() * uz.clone();
395 let (sin, cos) = angle.simd_sin_cos();
396 let one_m_cos = T::one() - cos.clone();
397
398 Self::from_matrix_unchecked(SMatrix::<T, 3, 3>::new(
399 sqx.clone() + (T::one() - sqx) * cos.clone(),
400 ux.clone() * uy.clone() * one_m_cos.clone() - uz.clone() * sin.clone(),
401 ux.clone() * uz.clone() * one_m_cos.clone() + uy.clone() * sin.clone(),
402 ux.clone() * uy.clone() * one_m_cos.clone() + uz.clone() * sin.clone(),
403 sqy.clone() + (T::one() - sqy) * cos.clone(),
404 uy.clone() * uz.clone() * one_m_cos.clone() - ux.clone() * sin.clone(),
405 ux.clone() * uz.clone() * one_m_cos.clone() - uy.clone() * sin.clone(),
406 uy * uz * one_m_cos + ux * sin,
407 sqz.clone() + (T::one() - sqz) * cos,
408 ))
409 },
410 Self::identity,
411 )
412 }
413
414 /// Creates a new rotation from Euler angles.
415 ///
416 /// The primitive rotations are applied in order: 1 roll − 2 pitch − 3 yaw.
417 ///
418 /// # Example
419 /// ```
420 /// # #[macro_use] extern crate approx;
421 /// # use nalgebra::Rotation3;
422 /// let rot = Rotation3::from_euler_angles(0.1, 0.2, 0.3);
423 /// let euler = rot.euler_angles();
424 /// assert_relative_eq!(euler.0, 0.1, epsilon = 1.0e-6);
425 /// assert_relative_eq!(euler.1, 0.2, epsilon = 1.0e-6);
426 /// assert_relative_eq!(euler.2, 0.3, epsilon = 1.0e-6);
427 /// ```
428 pub fn from_euler_angles(roll: T, pitch: T, yaw: T) -> Self {
429 let (sr, cr) = roll.simd_sin_cos();
430 let (sp, cp) = pitch.simd_sin_cos();
431 let (sy, cy) = yaw.simd_sin_cos();
432
433 Self::from_matrix_unchecked(SMatrix::<T, 3, 3>::new(
434 cy.clone() * cp.clone(),
435 cy.clone() * sp.clone() * sr.clone() - sy.clone() * cr.clone(),
436 cy.clone() * sp.clone() * cr.clone() + sy.clone() * sr.clone(),
437 sy.clone() * cp.clone(),
438 sy.clone() * sp.clone() * sr.clone() + cy.clone() * cr.clone(),
439 sy * sp.clone() * cr.clone() - cy * sr.clone(),
440 -sp,
441 cp.clone() * sr,
442 cp * cr,
443 ))
444 }
445}
446
447/// # Construction from a 3D eye position and target point
448impl<T: SimdRealField> Rotation3<T>
449where
450 T::Element: SimdRealField,
451{
452 /// Creates a rotation that corresponds to the local frame of an observer standing at the
453 /// origin and looking toward `dir`.
454 ///
455 /// It maps the `z` axis to the direction `dir`.
456 ///
457 /// # Arguments
458 /// * dir - The look direction, that is, direction the matrix `z` axis will be aligned with.
459 /// * up - The vertical direction. The only requirement of this parameter is to not be
460 /// collinear to `dir`. Non-collinearity is not checked.
461 ///
462 /// # Example
463 /// ```
464 /// # #[macro_use] extern crate approx;
465 /// # use std::f32;
466 /// # use nalgebra::{Rotation3, Vector3};
467 /// let dir = Vector3::new(1.0, 2.0, 3.0);
468 /// let up = Vector3::y();
469 ///
470 /// let rot = Rotation3::face_towards(&dir, &up);
471 /// assert_relative_eq!(rot * Vector3::z(), dir.normalize());
472 /// ```
473 #[inline]
474 pub fn face_towards<SB, SC>(dir: &Vector<T, U3, SB>, up: &Vector<T, U3, SC>) -> Self
475 where
476 SB: Storage<T, U3>,
477 SC: Storage<T, U3>,
478 {
479 let zaxis = dir.normalize();
480 let xaxis = up.cross(&zaxis).normalize();
481 let yaxis = zaxis.cross(&xaxis).normalize();
482
483 Self::from_matrix_unchecked(SMatrix::<T, 3, 3>::new(
484 xaxis.x.clone(),
485 yaxis.x.clone(),
486 zaxis.x.clone(),
487 xaxis.y.clone(),
488 yaxis.y.clone(),
489 zaxis.y.clone(),
490 xaxis.z.clone(),
491 yaxis.z.clone(),
492 zaxis.z.clone(),
493 ))
494 }
495
496 /// Deprecated: Use [`Rotation3::face_towards`] instead.
497 #[deprecated(note = "renamed to `face_towards`")]
498 pub fn new_observer_frames<SB, SC>(dir: &Vector<T, U3, SB>, up: &Vector<T, U3, SC>) -> Self
499 where
500 SB: Storage<T, U3>,
501 SC: Storage<T, U3>,
502 {
503 Self::face_towards(dir, up)
504 }
505
506 /// Builds a right-handed look-at view matrix without translation.
507 ///
508 /// It maps the view direction `dir` to the **negative** `z` axis.
509 /// This conforms to the common notion of right handed look-at matrix from the computer
510 /// graphics community.
511 ///
512 /// # Arguments
513 /// * dir - The direction toward which the camera looks.
514 /// * up - A vector approximately aligned with required the vertical axis. The only
515 /// requirement of this parameter is to not be collinear to `dir`.
516 ///
517 /// # Example
518 /// ```
519 /// # #[macro_use] extern crate approx;
520 /// # use std::f32;
521 /// # use nalgebra::{Rotation3, Vector3};
522 /// let dir = Vector3::new(1.0, 2.0, 3.0);
523 /// let up = Vector3::y();
524 ///
525 /// let rot = Rotation3::look_at_rh(&dir, &up);
526 /// assert_relative_eq!(rot * dir.normalize(), -Vector3::z());
527 /// ```
528 #[inline]
529 pub fn look_at_rh<SB, SC>(dir: &Vector<T, U3, SB>, up: &Vector<T, U3, SC>) -> Self
530 where
531 SB: Storage<T, U3>,
532 SC: Storage<T, U3>,
533 {
534 Self::face_towards(&dir.neg(), up).inverse()
535 }
536
537 /// Builds a left-handed look-at view matrix without translation.
538 ///
539 /// It maps the view direction `dir` to the **positive** `z` axis.
540 /// This conforms to the common notion of left handed look-at matrix from the computer
541 /// graphics community.
542 ///
543 /// # Arguments
544 /// * dir - The direction toward which the camera looks.
545 /// * up - A vector approximately aligned with required the vertical axis. The only
546 /// requirement of this parameter is to not be collinear to `dir`.
547 ///
548 /// # Example
549 /// ```
550 /// # #[macro_use] extern crate approx;
551 /// # use std::f32;
552 /// # use nalgebra::{Rotation3, Vector3};
553 /// let dir = Vector3::new(1.0, 2.0, 3.0);
554 /// let up = Vector3::y();
555 ///
556 /// let rot = Rotation3::look_at_lh(&dir, &up);
557 /// assert_relative_eq!(rot * dir.normalize(), Vector3::z());
558 /// ```
559 #[inline]
560 pub fn look_at_lh<SB, SC>(dir: &Vector<T, U3, SB>, up: &Vector<T, U3, SC>) -> Self
561 where
562 SB: Storage<T, U3>,
563 SC: Storage<T, U3>,
564 {
565 Self::face_towards(dir, up).inverse()
566 }
567}
568
569/// # Construction from an existing 3D matrix or rotations
570impl<T: SimdRealField> Rotation3<T>
571where
572 T::Element: SimdRealField,
573{
574 /// The rotation matrix required to align `a` and `b` but with its angle.
575 ///
576 /// This is the rotation `R` such that `(R * a).angle(b) == 0 && (R * a).dot(b).is_positive()`.
577 ///
578 /// # Example
579 /// ```
580 /// # #[macro_use] extern crate approx;
581 /// # use nalgebra::{Vector3, Rotation3};
582 /// let a = Vector3::new(1.0, 2.0, 3.0);
583 /// let b = Vector3::new(3.0, 1.0, 2.0);
584 /// let rot = Rotation3::rotation_between(&a, &b).unwrap();
585 /// assert_relative_eq!(rot * a, b, epsilon = 1.0e-6);
586 /// assert_relative_eq!(rot.inverse() * b, a, epsilon = 1.0e-6);
587 /// ```
588 #[inline]
589 pub fn rotation_between<SB, SC>(a: &Vector<T, U3, SB>, b: &Vector<T, U3, SC>) -> Option<Self>
590 where
591 T: RealField,
592 SB: Storage<T, U3>,
593 SC: Storage<T, U3>,
594 {
595 Self::scaled_rotation_between(a, b, T::one())
596 }
597
598 /// The smallest rotation needed to make `a` and `b` collinear and point toward the same
599 /// direction, raised to the power `s`.
600 ///
601 /// # Example
602 /// ```
603 /// # #[macro_use] extern crate approx;
604 /// # use nalgebra::{Vector3, Rotation3};
605 /// let a = Vector3::new(1.0, 2.0, 3.0);
606 /// let b = Vector3::new(3.0, 1.0, 2.0);
607 /// let rot2 = Rotation3::scaled_rotation_between(&a, &b, 0.2).unwrap();
608 /// let rot5 = Rotation3::scaled_rotation_between(&a, &b, 0.5).unwrap();
609 /// assert_relative_eq!(rot2 * rot2 * rot2 * rot2 * rot2 * a, b, epsilon = 1.0e-6);
610 /// assert_relative_eq!(rot5 * rot5 * a, b, epsilon = 1.0e-6);
611 /// ```
612 #[inline]
613 pub fn scaled_rotation_between<SB, SC>(
614 a: &Vector<T, U3, SB>,
615 b: &Vector<T, U3, SC>,
616 n: T,
617 ) -> Option<Self>
618 where
619 T: RealField,
620 SB: Storage<T, U3>,
621 SC: Storage<T, U3>,
622 {
623 // TODO: code duplication with Rotation.
624 if let (Some(na), Some(nb)) = (a.try_normalize(T::zero()), b.try_normalize(T::zero())) {
625 let c = na.cross(&nb);
626
627 if let Some(axis) = Unit::try_new(c, T::default_epsilon()) {
628 return Some(Self::from_axis_angle(&axis, na.dot(&nb).acos() * n));
629 }
630
631 // Zero or PI.
632 if na.dot(&nb) < T::zero() {
633 // PI
634 //
635 // The rotation axis is undefined but the angle not zero. This is not a
636 // simple rotation.
637 return None;
638 }
639 }
640
641 Some(Self::identity())
642 }
643
644 /// The rotation matrix needed to make `self` and `other` coincide.
645 ///
646 /// The result is such that: `self.rotation_to(other) * self == other`.
647 ///
648 /// # Example
649 /// ```
650 /// # #[macro_use] extern crate approx;
651 /// # use nalgebra::{Rotation3, Vector3};
652 /// let rot1 = Rotation3::from_axis_angle(&Vector3::y_axis(), 1.0);
653 /// let rot2 = Rotation3::from_axis_angle(&Vector3::x_axis(), 0.1);
654 /// let rot_to = rot1.rotation_to(&rot2);
655 /// assert_relative_eq!(rot_to * rot1, rot2, epsilon = 1.0e-6);
656 /// ```
657 #[inline]
658 #[must_use]
659 pub fn rotation_to(&self, other: &Self) -> Self {
660 other * self.inverse()
661 }
662
663 /// Raise the rotation to a given floating power, i.e., returns the rotation with the same
664 /// axis as `self` and an angle equal to `self.angle()` multiplied by `n`.
665 ///
666 /// # Example
667 /// ```
668 /// # #[macro_use] extern crate approx;
669 /// # use nalgebra::{Rotation3, Vector3, Unit};
670 /// let axis = Unit::new_normalize(Vector3::new(1.0, 2.0, 3.0));
671 /// let angle = 1.2;
672 /// let rot = Rotation3::from_axis_angle(&axis, angle);
673 /// let pow = rot.powf(2.0);
674 /// assert_relative_eq!(pow.axis().unwrap(), axis, epsilon = 1.0e-6);
675 /// assert_eq!(pow.angle(), 2.4);
676 /// ```
677 #[inline]
678 #[must_use]
679 pub fn powf(&self, n: T) -> Self
680 where
681 T: RealField,
682 {
683 if let Some(axis) = self.axis() {
684 Self::from_axis_angle(&axis, self.angle() * n)
685 } else if self.matrix()[(0, 0)] < T::zero() {
686 let minus_id = SMatrix::<T, 3, 3>::from_diagonal_element(-T::one());
687 Self::from_matrix_unchecked(minus_id)
688 } else {
689 Self::identity()
690 }
691 }
692
693 /// Builds a rotation from a basis assumed to be orthonormal.
694 ///
695 /// In order to get a valid rotation matrix, the input must be an
696 /// orthonormal basis, i.e., all vectors are normalized, and the are
697 /// all orthogonal to each other. These invariants are not checked
698 /// by this method.
699 pub fn from_basis_unchecked(basis: &[Vector3<T>; 3]) -> Self {
700 let mat = Matrix3::from_columns(&basis[..]);
701 Self::from_matrix_unchecked(mat)
702 }
703
704 /// Builds a rotation matrix by extracting the rotation part of the given transformation `m`.
705 ///
706 /// This is an iterative method. See `.from_matrix_eps` to provide mover
707 /// convergence parameters and starting solution.
708 /// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al.
709 pub fn from_matrix(m: &Matrix3<T>) -> Self
710 where
711 T: RealField,
712 {
713 Self::from_matrix_eps(m, T::default_epsilon(), 0, Self::identity())
714 }
715
716 /// Builds a rotation matrix by extracting the rotation part of the given transformation `m`.
717 ///
718 /// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al.
719 ///
720 /// # Parameters
721 ///
722 /// * `m`: the matrix from which the rotational part is to be extracted.
723 /// * `eps`: the angular errors tolerated between the current rotation and the optimal one.
724 /// * `max_iter`: the maximum number of iterations. Loops indefinitely until convergence if set to `0`.
725 /// * `guess`: a guess of the solution. Convergence will be significantly faster if an initial solution close
726 /// to the actual solution is provided. Can be set to `Rotation3::identity()` if no other
727 /// guesses come to mind.
728 pub fn from_matrix_eps(m: &Matrix3<T>, eps: T, mut max_iter: usize, guess: Self) -> Self
729 where
730 T: RealField,
731 {
732 if max_iter == 0 {
733 max_iter = usize::max_value();
734 }
735
736 let mut rot = guess.into_inner();
737
738 for _ in 0..max_iter {
739 let axis = rot.column(0).cross(&m.column(0))
740 + rot.column(1).cross(&m.column(1))
741 + rot.column(2).cross(&m.column(2));
742 let denom = rot.column(0).dot(&m.column(0))
743 + rot.column(1).dot(&m.column(1))
744 + rot.column(2).dot(&m.column(2));
745
746 let axisangle = axis / (denom.abs() + T::default_epsilon());
747
748 if let Some((axis, angle)) = Unit::try_new_and_get(axisangle, eps.clone()) {
749 rot = Rotation3::from_axis_angle(&axis, angle) * rot;
750 } else {
751 break;
752 }
753 }
754
755 Self::from_matrix_unchecked(rot)
756 }
757
758 /// Ensure this rotation is an orthonormal rotation matrix. This is useful when repeated
759 /// computations might cause the matrix from progressively not being orthonormal anymore.
760 #[inline]
761 pub fn renormalize(&mut self)
762 where
763 T: RealField,
764 {
765 let mut c = UnitQuaternion::from(self.clone());
766 let _ = c.renormalize();
767
768 *self = Self::from_matrix_eps(self.matrix(), T::default_epsilon(), 0, c.into())
769 }
770}
771
772/// # 3D axis and angle extraction
773impl<T: SimdRealField> Rotation3<T> {
774 /// The rotation angle in [0; pi].
775 ///
776 /// # Example
777 /// ```
778 /// # #[macro_use] extern crate approx;
779 /// # use nalgebra::{Unit, Rotation3, Vector3};
780 /// let axis = Unit::new_normalize(Vector3::new(1.0, 2.0, 3.0));
781 /// let rot = Rotation3::from_axis_angle(&axis, 1.78);
782 /// assert_relative_eq!(rot.angle(), 1.78);
783 /// ```
784 #[inline]
785 #[must_use]
786 pub fn angle(&self) -> T {
787 ((self.matrix()[(0, 0)].clone()
788 + self.matrix()[(1, 1)].clone()
789 + self.matrix()[(2, 2)].clone()
790 - T::one())
791 / crate::convert(2.0))
792 .simd_acos()
793 }
794
795 /// The rotation axis. Returns `None` if the rotation angle is zero or PI.
796 ///
797 /// # Example
798 /// ```
799 /// # #[macro_use] extern crate approx;
800 /// # use nalgebra::{Rotation3, Vector3, Unit};
801 /// let axis = Unit::new_normalize(Vector3::new(1.0, 2.0, 3.0));
802 /// let angle = 1.2;
803 /// let rot = Rotation3::from_axis_angle(&axis, angle);
804 /// assert_relative_eq!(rot.axis().unwrap(), axis);
805 ///
806 /// // Case with a zero angle.
807 /// let rot = Rotation3::from_axis_angle(&axis, 0.0);
808 /// assert!(rot.axis().is_none());
809 /// ```
810 #[inline]
811 #[must_use]
812 pub fn axis(&self) -> Option<Unit<Vector3<T>>>
813 where
814 T: RealField,
815 {
816 let rotmat = self.matrix();
817 let axis = SVector::<T, 3>::new(
818 rotmat[(2, 1)].clone() - rotmat[(1, 2)].clone(),
819 rotmat[(0, 2)].clone() - rotmat[(2, 0)].clone(),
820 rotmat[(1, 0)].clone() - rotmat[(0, 1)].clone(),
821 );
822
823 Unit::try_new(axis, T::default_epsilon())
824 }
825
826 /// The rotation axis multiplied by the rotation angle.
827 ///
828 /// # Example
829 /// ```
830 /// # #[macro_use] extern crate approx;
831 /// # use nalgebra::{Rotation3, Vector3, Unit};
832 /// let axisangle = Vector3::new(0.1, 0.2, 0.3);
833 /// let rot = Rotation3::new(axisangle);
834 /// assert_relative_eq!(rot.scaled_axis(), axisangle, epsilon = 1.0e-6);
835 /// ```
836 #[inline]
837 #[must_use]
838 pub fn scaled_axis(&self) -> Vector3<T>
839 where
840 T: RealField,
841 {
842 if let Some(axis) = self.axis() {
843 axis.into_inner() * self.angle()
844 } else {
845 Vector::zero()
846 }
847 }
848
849 /// The rotation axis and angle in ]0, pi] of this rotation matrix.
850 ///
851 /// Returns `None` if the angle is zero.
852 ///
853 /// # Example
854 /// ```
855 /// # #[macro_use] extern crate approx;
856 /// # use nalgebra::{Rotation3, Vector3, Unit};
857 /// let axis = Unit::new_normalize(Vector3::new(1.0, 2.0, 3.0));
858 /// let angle = 1.2;
859 /// let rot = Rotation3::from_axis_angle(&axis, angle);
860 /// let axis_angle = rot.axis_angle().unwrap();
861 /// assert_relative_eq!(axis_angle.0, axis);
862 /// assert_relative_eq!(axis_angle.1, angle);
863 ///
864 /// // Case with a zero angle.
865 /// let rot = Rotation3::from_axis_angle(&axis, 0.0);
866 /// assert!(rot.axis_angle().is_none());
867 /// ```
868 #[inline]
869 #[must_use]
870 pub fn axis_angle(&self) -> Option<(Unit<Vector3<T>>, T)>
871 where
872 T: RealField,
873 {
874 self.axis().map(|axis| (axis, self.angle()))
875 }
876
877 /// The rotation angle needed to make `self` and `other` coincide.
878 ///
879 /// # Example
880 /// ```
881 /// # #[macro_use] extern crate approx;
882 /// # use nalgebra::{Rotation3, Vector3};
883 /// let rot1 = Rotation3::from_axis_angle(&Vector3::y_axis(), 1.0);
884 /// let rot2 = Rotation3::from_axis_angle(&Vector3::x_axis(), 0.1);
885 /// assert_relative_eq!(rot1.angle_to(&rot2), 1.0045657, epsilon = 1.0e-6);
886 /// ```
887 #[inline]
888 #[must_use]
889 pub fn angle_to(&self, other: &Self) -> T
890 where
891 T::Element: SimdRealField,
892 {
893 self.rotation_to(other).angle()
894 }
895
896 /// Creates Euler angles from a rotation.
897 ///
898 /// The angles are produced in the form (roll, pitch, yaw).
899 #[deprecated(note = "This is renamed to use `.euler_angles()`.")]
900 pub fn to_euler_angles(self) -> (T, T, T)
901 where
902 T: RealField,
903 {
904 self.euler_angles()
905 }
906
907 /// Euler angles corresponding to this rotation from a rotation.
908 ///
909 /// The angles are produced in the form (roll, pitch, yaw).
910 ///
911 /// # Example
912 /// ```
913 /// # #[macro_use] extern crate approx;
914 /// # use nalgebra::Rotation3;
915 /// let rot = Rotation3::from_euler_angles(0.1, 0.2, 0.3);
916 /// let euler = rot.euler_angles();
917 /// assert_relative_eq!(euler.0, 0.1, epsilon = 1.0e-6);
918 /// assert_relative_eq!(euler.1, 0.2, epsilon = 1.0e-6);
919 /// assert_relative_eq!(euler.2, 0.3, epsilon = 1.0e-6);
920 /// ```
921 #[must_use]
922 pub fn euler_angles(&self) -> (T, T, T)
923 where
924 T: RealField,
925 {
926 // Implementation informed by "Computing Euler angles from a rotation matrix", by Gregory G. Slabaugh
927 // https://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.371.6578
928 if self[(2, 0)].clone().abs() < T::one() {
929 let yaw = -self[(2, 0)].clone().asin();
930 let roll = (self[(2, 1)].clone() / yaw.clone().cos())
931 .atan2(self[(2, 2)].clone() / yaw.clone().cos());
932 let pitch = (self[(1, 0)].clone() / yaw.clone().cos())
933 .atan2(self[(0, 0)].clone() / yaw.clone().cos());
934 (roll, yaw, pitch)
935 } else if self[(2, 0)].clone() <= -T::one() {
936 (
937 self[(0, 1)].clone().atan2(self[(0, 2)].clone()),
938 T::frac_pi_2(),
939 T::zero(),
940 )
941 } else {
942 (
943 -self[(0, 1)].clone().atan2(-self[(0, 2)].clone()),
944 -T::frac_pi_2(),
945 T::zero(),
946 )
947 }
948 }
949}
950
951#[cfg(feature = "rand-no-std")]
952impl<T: SimdRealField> Distribution<Rotation3<T>> for Standard
953where
954 T::Element: SimdRealField,
955 OpenClosed01: Distribution<T>,
956 T: SampleUniform,
957{
958 /// Generate a uniformly distributed random rotation.
959 #[inline]
960 fn sample<'a, R: Rng + ?Sized>(&self, rng: &mut R) -> Rotation3<T> {
961 // James Arvo.
962 // Fast random rotation matrices.
963 // In D. Kirk, editor, Graphics Gems III, pages 117-120. Academic, New York, 1992.
964
965 // Compute a random rotation around Z
966 let twopi = Uniform::new(T::zero(), T::simd_two_pi());
967 let theta = rng.sample(&twopi);
968 let (ts, tc) = theta.simd_sin_cos();
969 let a = SMatrix::<T, 3, 3>::new(
970 tc.clone(),
971 ts.clone(),
972 T::zero(),
973 -ts,
974 tc,
975 T::zero(),
976 T::zero(),
977 T::zero(),
978 T::one(),
979 );
980
981 // Compute a random rotation *of* Z
982 let phi = rng.sample(&twopi);
983 let z = rng.sample(OpenClosed01);
984 let (ps, pc) = phi.simd_sin_cos();
985 let sqrt_z = z.clone().simd_sqrt();
986 let v = Vector3::new(pc * sqrt_z.clone(), ps * sqrt_z, (T::one() - z).simd_sqrt());
987 let mut b = v.clone() * v.transpose();
988 b += b.clone();
989 b -= SMatrix::<T, 3, 3>::identity();
990
991 Rotation3::from_matrix_unchecked(b * a)
992 }
993}
994
995#[cfg(feature = "arbitrary")]
996impl<T: SimdRealField + Arbitrary> Arbitrary for Rotation3<T>
997where
998 T::Element: SimdRealField,
999 Owned<T, U3, U3>: Send,
1000 Owned<T, U3>: Send,
1001{
1002 #[inline]
1003 fn arbitrary(g: &mut Gen) -> Self {
1004 Self::new(SVector::arbitrary(g))
1005 }
1006}