arris_math/
isometry.rs

1//! Rigid motions.
2
3use nalgebra::{Isometry3, Translation3, UnitQuaternion};
4
5use crate::{Frame, Point3, UnitVec3, Vec3};
6
7/// A rigid motion: a rotation followed by a translation, `p ↦ R p + t`.
8/// Lengths and angles are preserved, so a `Frame` moved by one is still a
9/// frame and geometry moved by one is the same geometry in another place
10/// (`docs/DATA-MODEL.md` §Conventions: a transform is a frame change).
11///
12/// ```
13/// use arris_math::{Isometry, Point3, Vec3, nalgebra::UnitQuaternion};
14/// use core::f64::consts::FRAC_PI_2;
15///
16/// let quarter_turn = UnitQuaternion::from_axis_angle(&Vec3::z_axis(), FRAC_PI_2);
17/// let m = Isometry::new(quarter_turn, Vec3::new(1.0, 0.0, 0.0));
18/// let p = m.apply(Point3::new(1.0, 0.0, 0.0));
19/// assert!((p - Point3::new(1.0, 1.0, 0.0)).norm() < 1e-15);
20/// let back = m.inverse().apply(p);
21/// assert!((back - Point3::new(1.0, 0.0, 0.0)).norm() < 1e-15);
22/// ```
23#[derive(Debug, Clone, Copy, PartialEq)]
24pub struct Isometry {
25    inner: Isometry3<f64>,
26}
27
28impl Isometry {
29    /// `p ↦ rotation · p + translation`.
30    pub fn new(rotation: UnitQuaternion<f64>, translation: Vec3) -> Self {
31        Isometry {
32            inner: Isometry3::from_parts(Translation3::from(translation), rotation),
33        }
34    }
35
36    /// The motion that moves nothing.
37    pub fn identity() -> Self {
38        Isometry {
39            inner: Isometry3::identity(),
40        }
41    }
42
43    /// A pure translation by `v`.
44    pub fn from_translation(v: Vec3) -> Self {
45        Self::new(UnitQuaternion::identity(), v)
46    }
47
48    /// A pure rotation about the origin.
49    pub fn from_rotation(rotation: UnitQuaternion<f64>) -> Self {
50        Self::new(rotation, Vec3::zeros())
51    }
52
53    /// The rotation part.
54    pub fn rotation(&self) -> UnitQuaternion<f64> {
55        self.inner.rotation
56    }
57
58    /// The translation part: where the origin goes.
59    pub fn translation(&self) -> Vec3 {
60        self.inner.translation.vector
61    }
62
63    /// `R p + t`.
64    pub fn apply(&self, p: Point3) -> Point3 {
65        self.inner.transform_point(&p)
66    }
67
68    /// `R v`: a vector is a difference of points, so the translation does
69    /// not act on it.
70    pub fn apply_vec(&self, v: Vec3) -> Vec3 {
71        self.inner.transform_vector(&v)
72    }
73
74    /// `R u`, re-normalised so the result is unit to rounding.
75    pub fn apply_unit(&self, u: UnitVec3) -> UnitVec3 {
76        UnitVec3::new_normalize(self.inner.transform_vector(&u))
77    }
78
79    /// The frame moved by this motion; the same as
80    /// [`Frame::transformed`].
81    pub fn apply_frame(&self, frame: &Frame) -> Frame {
82        frame.transformed(self)
83    }
84
85    /// The motion that is `self` first, then `next`:
86    /// `a.then(&b).apply(p) == b.apply(a.apply(p))` to rounding.
87    pub fn then(&self, next: &Isometry) -> Isometry {
88        Isometry {
89            inner: next.inner * self.inner,
90        }
91    }
92
93    /// The motion that undoes this one: `m.inverse().apply(m.apply(p)) ==
94    /// p` to rounding.
95    pub fn inverse(&self) -> Isometry {
96        Isometry {
97            inner: self.inner.inverse(),
98        }
99    }
100}
101
102impl Default for Isometry {
103    fn default() -> Self {
104        Self::identity()
105    }
106}
arris_math/isometry.rs

arris_math/
isometry.rs
