emath/
rot2.rs

1use super::Vec2;
2
3// {s,c} represents the rotation matrix:
4//
5// | c -s |
6// | s  c |
7//
8// `vec2(c,s)` represents where the X axis will end up after rotation.
9//
10/// Represents a rotation in the 2D plane.
11///
12/// A rotation of 𝞃/4 = 90° rotates the X axis to the Y axis.
13///
14/// Normally a [`Rot2`] is normalized (unit-length).
15/// If not, it will also scale vectors.
16#[repr(C)]
17#[derive(Clone, Copy, PartialEq)]
18#[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))]
19#[cfg_attr(feature = "bytemuck", derive(bytemuck::Pod, bytemuck::Zeroable))]
20pub struct Rot2 {
21    /// `angle.sin()`
22    s: f32,
23
24    /// `angle.cos()`
25    c: f32,
26}
27
28/// Identity rotation
29impl Default for Rot2 {
30    /// Identity rotation
31    #[inline]
32    fn default() -> Self {
33        Self { s: 0.0, c: 1.0 }
34    }
35}
36
37impl Rot2 {
38    /// The identity rotation: nothing rotates
39    pub const IDENTITY: Self = Self { s: 0.0, c: 1.0 };
40
41    /// Angle is clockwise in radians.
42    /// A 𝞃/4 = 90° rotation means rotating the X axis to the Y axis.
43    #[inline]
44    pub fn from_angle(angle: f32) -> Self {
45        let (s, c) = angle.sin_cos();
46        Self { s, c }
47    }
48
49    #[inline]
50    pub fn angle(self) -> f32 {
51        self.s.atan2(self.c)
52    }
53
54    /// The factor by which vectors will be scaled.
55    #[inline]
56    pub fn length(self) -> f32 {
57        self.c.hypot(self.s)
58    }
59
60    #[inline]
61    pub fn length_squared(self) -> f32 {
62        self.c.powi(2) + self.s.powi(2)
63    }
64
65    #[inline]
66    pub fn is_finite(self) -> bool {
67        self.c.is_finite() && self.s.is_finite()
68    }
69
70    #[must_use]
71    #[inline]
72    pub fn inverse(self) -> Self {
73        Self {
74            s: -self.s,
75            c: self.c,
76        } / self.length_squared()
77    }
78
79    #[must_use]
80    #[inline]
81    pub fn normalized(self) -> Self {
82        let l = self.length();
83        let ret = Self {
84            c: self.c / l,
85            s: self.s / l,
86        };
87        debug_assert!(
88            ret.is_finite(),
89            "Rot2::normalized produced a non-finite result"
90        );
91        ret
92    }
93}
94
95impl std::fmt::Debug for Rot2 {
96    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
97        if let Some(precision) = f.precision() {
98            write!(
99                f,
100                "Rot2 {{ angle: {:.2$}°, length: {} }}",
101                self.angle().to_degrees(),
102                self.length(),
103                precision
104            )
105        } else {
106            write!(
107                f,
108                "Rot2 {{ angle: {:.1}°, length: {} }}",
109                self.angle().to_degrees(),
110                self.length(),
111            )
112        }
113    }
114}
115
116impl std::ops::Mul<Self> for Rot2 {
117    type Output = Self;
118
119    #[inline]
120    fn mul(self, r: Self) -> Self {
121        /*
122        |lc -ls| * |rc -rs|
123        |ls  lc|   |rs  rc|
124        */
125        Self {
126            c: self.c * r.c - self.s * r.s,
127            s: self.s * r.c + self.c * r.s,
128        }
129    }
130}
131
132/// Rotates (and maybe scales) the vector.
133impl std::ops::Mul<Vec2> for Rot2 {
134    type Output = Vec2;
135
136    #[inline]
137    fn mul(self, v: Vec2) -> Vec2 {
138        Vec2 {
139            x: self.c * v.x - self.s * v.y,
140            y: self.s * v.x + self.c * v.y,
141        }
142    }
143}
144
145/// Scales the rotor.
146impl std::ops::Mul<Rot2> for f32 {
147    type Output = Rot2;
148
149    #[inline]
150    fn mul(self, r: Rot2) -> Rot2 {
151        Rot2 {
152            c: self * r.c,
153            s: self * r.s,
154        }
155    }
156}
157
158/// Scales the rotor.
159impl std::ops::Mul<f32> for Rot2 {
160    type Output = Self;
161
162    #[inline]
163    fn mul(self, r: f32) -> Self {
164        Self {
165            c: self.c * r,
166            s: self.s * r,
167        }
168    }
169}
170
171/// Scales the rotor.
172impl std::ops::Div<f32> for Rot2 {
173    type Output = Self;
174
175    #[inline]
176    fn div(self, r: f32) -> Self {
177        Self {
178            c: self.c / r,
179            s: self.s / r,
180        }
181    }
182}
183
184#[cfg(test)]
185mod test {
186    use super::Rot2;
187    use crate::vec2;
188
189    #[test]
190    fn test_rotation2() {
191        {
192            let angle = std::f32::consts::TAU / 6.0;
193            let rot = Rot2::from_angle(angle);
194            assert!((rot.angle() - angle).abs() < 1e-5);
195            assert!((rot * rot.inverse()).angle().abs() < 1e-5);
196            assert!((rot.inverse() * rot).angle().abs() < 1e-5);
197        }
198
199        {
200            let angle = std::f32::consts::TAU / 4.0;
201            let rot = Rot2::from_angle(angle);
202            assert!(((rot * vec2(1.0, 0.0)) - vec2(0.0, 1.0)).length() < 1e-5);
203        }
204
205        {
206            // Test rotation and scaling
207            let angle = std::f32::consts::TAU / 4.0;
208            let rot = 3.0 * Rot2::from_angle(angle);
209            let rotated = rot * vec2(1.0, 0.0);
210            let expected = vec2(0.0, 3.0);
211            assert!(
212                (rotated - expected).length() < 1e-5,
213                "Expected {rotated:?} to equal {expected:?}. rot: {rot:?}",
214            );
215
216            let undone = rot.inverse() * rot;
217            assert!(undone.angle().abs() < 1e-5);
218            assert!((undone.length() - 1.0).abs() < 1e-5,);
219        }
220    }
221}