avian3d/math/
drot2.rs

1use core::f64::consts::TAU;
2
3use crate::ops;
4use bevy_math::{DMat2, DVec2, prelude::*};
5
6use bevy::reflect::prelude::*;
7#[cfg(feature = "serialize")]
8use bevy::reflect::{ReflectDeserialize, ReflectSerialize};
9
10/// A double-precision version of [`Rot2`].
11#[derive(Clone, Copy, Debug, PartialEq, Reflect)]
12#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))]
13#[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))]
14#[doc(alias = "drotation", alias = "drotation2d", alias = "drotation_2d")]
15pub struct DRot2 {
16    /// The cosine of the rotation angle.
17    ///
18    /// This is the real part of the unit complex number representing the rotation.
19    pub cos: f64,
20    /// The sine of the rotation angle.
21    ///
22    /// This is the imaginary part of the unit complex number representing the rotation.
23    pub sin: f64,
24}
25
26impl Default for DRot2 {
27    fn default() -> Self {
28        Self::IDENTITY
29    }
30}
31
32impl DRot2 {
33    /// The identity rotation.
34    pub const IDENTITY: Self = Self { cos: 1.0, sin: 0.0 };
35
36    /// A rotation of π radians.
37    /// Corresponds to a half-turn.
38    pub const PI: Self = Self {
39        cos: -1.0,
40        sin: 0.0,
41    };
42
43    /// A counterclockwise rotation of π/2 radians.
44    /// Corresponds to a counterclockwise quarter-turn.
45    pub const FRAC_PI_2: Self = Self { cos: 0.0, sin: 1.0 };
46
47    /// A counterclockwise rotation of π/3 radians.
48    /// Corresponds to a counterclockwise turn by 60°.
49    pub const FRAC_PI_3: Self = Self {
50        cos: 0.5,
51        sin: 0.866_025_4,
52    };
53
54    /// A counterclockwise rotation of π/4 radians.
55    /// Corresponds to a counterclockwise turn by 45°.
56    pub const FRAC_PI_4: Self = Self {
57        cos: core::f64::consts::FRAC_1_SQRT_2,
58        sin: core::f64::consts::FRAC_1_SQRT_2,
59    };
60
61    /// A counterclockwise rotation of π/6 radians.
62    /// Corresponds to a counterclockwise turn by 30°.
63    pub const FRAC_PI_6: Self = Self {
64        cos: 0.866_025_4,
65        sin: 0.5,
66    };
67
68    /// A counterclockwise rotation of π/8 radians.
69    /// Corresponds to a counterclockwise turn by 22.5°.
70    pub const FRAC_PI_8: Self = Self {
71        cos: 0.923_879_5,
72        sin: 0.382_683_43,
73    };
74
75    /// Creates a [`DRot2`] from a counterclockwise angle in radians.
76    /// A negative argument corresponds to a clockwise rotation.
77    ///
78    /// # Note
79    ///
80    /// Angles larger than or equal to 2π (in either direction) loop around to smaller rotations,
81    /// since a full rotation returns an object to its starting orientation.
82    #[inline]
83    pub fn radians(radians: f64) -> Self {
84        #[cfg(feature = "enhanced-determinism")]
85        let (sin, cos) = (libm::sin(radians), libm::cos(radians));
86        #[cfg(not(feature = "enhanced-determinism"))]
87        let (sin, cos) = radians.sin_cos();
88
89        Self::from_sin_cos(sin, cos)
90    }
91
92    /// Creates a [`DRot2`] from a counterclockwise angle in degrees.
93    /// A negative argument corresponds to a clockwise rotation.
94    ///
95    /// # Note
96    ///
97    /// Angles larger than or equal to 360° (in either direction) loop around to smaller rotations,
98    /// since a full rotation returns an object to its starting orientation.
99    #[inline]
100    pub fn degrees(degrees: f64) -> Self {
101        Self::radians(degrees.to_radians())
102    }
103
104    /// Creates a [`DRot2`] from a counterclockwise fraction of a full turn of 360 degrees.
105    /// A negative argument corresponds to a clockwise rotation.
106    ///
107    /// # Note
108    ///
109    /// Angles larger than or equal to 1 turn (in either direction) loop around to smaller rotations,
110    /// since a full rotation returns an object to its starting orientation.
111    #[inline]
112    pub fn turn_fraction(fraction: f64) -> Self {
113        Self::radians(TAU * fraction)
114    }
115
116    /// Creates a [`DRot2`] from the sine and cosine of an angle.
117    ///
118    /// The rotation is only valid if `sin * sin + cos * cos == 1.0`.
119    ///
120    /// # Panics
121    ///
122    /// Panics if `sin * sin + cos * cos != 1.0` with debug assertions enabled.
123    #[inline]
124    pub fn from_sin_cos(sin: f64, cos: f64) -> Self {
125        let rotation = Self { sin, cos };
126        debug_assert!(
127            rotation.is_normalized(),
128            "the given sine and cosine produce an invalid rotation"
129        );
130        rotation
131    }
132
133    /// Returns a corresponding rotation angle in radians in the `(-pi, pi]` range.
134    #[inline]
135    pub fn as_radians(self) -> f64 {
136        #[cfg(feature = "enhanced-determinism")]
137        {
138            libm::atan2(self.sin, self.cos)
139        }
140        #[cfg(not(feature = "enhanced-determinism"))]
141        {
142            f64::atan2(self.sin, self.cos)
143        }
144    }
145
146    /// Returns a corresponding rotation angle in degrees in the `(-180, 180]` range.
147    #[inline]
148    pub fn as_degrees(self) -> f64 {
149        self.as_radians().to_degrees()
150    }
151
152    /// Returns a corresponding rotation angle as a fraction of a full 360 degree turn in the `(-0.5, 0.5]` range.
153    #[inline]
154    pub fn as_turn_fraction(self) -> f64 {
155        self.as_radians() / TAU
156    }
157
158    /// Returns the sine and cosine of the rotation angle.
159    #[inline]
160    pub const fn sin_cos(self) -> (f64, f64) {
161        (self.sin, self.cos)
162    }
163
164    /// Computes the length or norm of the complex number used to represent the rotation.
165    ///
166    /// The length is typically expected to be `1.0`. Unexpectedly denormalized rotations
167    /// can be a result of incorrect construction or floating point error caused by
168    /// successive operations.
169    #[inline]
170    #[doc(alias = "norm")]
171    pub fn length(self) -> f64 {
172        DVec2::new(self.sin, self.cos).length()
173    }
174
175    /// Computes the squared length or norm of the complex number used to represent the rotation.
176    ///
177    /// This is generally faster than [`DRot2::length()`], as it avoids a square
178    /// root operation.
179    ///
180    /// The length is typically expected to be `1.0`. Unexpectedly denormalized rotations
181    /// can be a result of incorrect construction or floating point error caused by
182    /// successive operations.
183    #[inline]
184    #[doc(alias = "norm2")]
185    pub fn length_squared(self) -> f64 {
186        DVec2::new(self.sin, self.cos).length_squared()
187    }
188
189    /// Computes `1.0 / self.length()`.
190    ///
191    /// For valid results, `self` must _not_ have a length of zero.
192    #[inline]
193    pub fn length_recip(self) -> f64 {
194        DVec2::new(self.sin, self.cos).length_recip()
195    }
196
197    /// Returns `self` with a length of `1.0` if possible, and `None` otherwise.
198    ///
199    /// `None` will be returned if the sine and cosine of `self` are both zero (or very close to zero),
200    /// or if either of them is NaN or infinite.
201    ///
202    /// Note that [`DRot2`] should typically already be normalized by design.
203    /// Manual normalization is only needed when successive operations result in
204    /// accumulated floating point error, or if the rotation was constructed
205    /// with invalid values.
206    #[inline]
207    pub fn try_normalize(self) -> Option<Self> {
208        let recip = self.length_recip();
209        if recip.is_finite() && recip > 0.0 {
210            Some(Self::from_sin_cos(self.sin * recip, self.cos * recip))
211        } else {
212            None
213        }
214    }
215
216    /// Returns `self` with a length of `1.0`.
217    ///
218    /// Note that [`DRot2`] should typically already be normalized by design.
219    /// Manual normalization is only needed when successive operations result in
220    /// accumulated floating point error, or if the rotation was constructed
221    /// with invalid values.
222    ///
223    /// # Panics
224    ///
225    /// Panics if `self` has a length of zero, NaN, or infinity when debug assertions are enabled.
226    #[inline]
227    pub fn normalize(self) -> Self {
228        let length_recip = self.length_recip();
229        Self::from_sin_cos(self.sin * length_recip, self.cos * length_recip)
230    }
231
232    /// Returns `self` after an approximate normalization, assuming the value is already nearly normalized.
233    /// Useful for preventing numerical error accumulation.
234    #[inline]
235    pub fn fast_renormalize(self) -> Self {
236        let length_squared = self.length_squared();
237        // Based on a Taylor approximation of the inverse square root
238        let length_recip_approx = 0.5 * (3.0 - length_squared);
239        DRot2 {
240            sin: self.sin * length_recip_approx,
241            cos: self.cos * length_recip_approx,
242        }
243    }
244
245    /// Returns `true` if the rotation is neither infinite nor NaN.
246    #[inline]
247    pub const fn is_finite(self) -> bool {
248        self.sin.is_finite() && self.cos.is_finite()
249    }
250
251    /// Returns `true` if the rotation is NaN.
252    #[inline]
253    pub const fn is_nan(self) -> bool {
254        self.sin.is_nan() || self.cos.is_nan()
255    }
256
257    /// Returns whether `self` has a length of `1.0` or not.
258    ///
259    /// Uses a precision threshold of approximately `1e-4`.
260    #[inline]
261    pub fn is_normalized(self) -> bool {
262        // The allowed length is 1 +/- 1e-4, so the largest allowed
263        // squared length is (1 + 1e-4)^2 = 1.00020001, which makes
264        // the threshold for the squared length approximately 2e-4.
265        (self.length_squared() - 1.0).abs() <= 2e-4
266    }
267
268    /// Returns `true` if the rotation is near [`DRot2::IDENTITY`].
269    #[inline]
270    pub fn is_near_identity(self) -> bool {
271        // Same as `Quat::is_near_identity`, but using sine and cosine
272        let threshold_angle_sin = 0.000_049_692_047; // let threshold_angle = 0.002_847_144_6;
273        self.cos > 0.0 && self.sin.abs() < threshold_angle_sin
274    }
275
276    /// Returns the angle in radians needed to make `self` and `other` coincide.
277    #[inline]
278    pub fn angle_to(self, other: Self) -> f64 {
279        (other * self.inverse()).as_radians()
280    }
281
282    /// Returns the inverse of the rotation. This is also the conjugate
283    /// of the unit complex number representing the rotation.
284    #[inline]
285    #[must_use]
286    #[doc(alias = "conjugate")]
287    pub const fn inverse(self) -> Self {
288        Self {
289            cos: self.cos,
290            sin: -self.sin,
291        }
292    }
293
294    /// Performs a linear interpolation between `self` and `end` based on
295    /// the value `s`, and normalizes the rotation afterwards.
296    ///
297    /// When `s == 0.0`, the result will be equal to `self`.
298    /// When `s == 1.0`, the result will be equal to `end`.
299    ///
300    /// This is slightly more efficient than [`slerp`](Self::slerp), and produces a similar result
301    /// when the difference between the two rotations is small. At larger differences,
302    /// the result resembles a kind of ease-in-out effect.
303    ///
304    /// If you would like the angular velocity to remain constant, consider using [`slerp`](Self::slerp) instead.
305    ///
306    /// # Details
307    ///
308    /// `nlerp` corresponds to computing an angle for a point at position `s` on a line drawn
309    /// between the endpoints of the arc formed by `self` and `end` on a unit circle,
310    /// and normalizing the result afterwards.
311    ///
312    /// Note that if the angles are opposite like 0 and π, the line will pass through the origin,
313    /// and the resulting angle will always be either `self` or `end` depending on `s`.
314    /// If `s` happens to be `0.5` in this case, a valid rotation cannot be computed, and `self`
315    /// will be returned as a fallback.
316    #[inline]
317    pub fn nlerp(self, end: Self, s: f64) -> Self {
318        Self {
319            sin: self.sin.lerp(end.sin, s),
320            cos: self.cos.lerp(end.cos, s),
321        }
322        .try_normalize()
323        // Fall back to the start rotation.
324        // This can happen when `self` and `end` are opposite angles and `s == 0.5`,
325        // because the resulting rotation would be zero, which cannot be normalized.
326        .unwrap_or(self)
327    }
328
329    /// Performs a spherical linear interpolation between `self` and `end`
330    /// based on the value `s`.
331    ///
332    /// This corresponds to interpolating between the two angles at a constant angular velocity.
333    ///
334    /// When `s == 0.0`, the result will be equal to `self`.
335    /// When `s == 1.0`, the result will be equal to `end`.
336    ///
337    /// If you would like the rotation to have a kind of ease-in-out effect, consider
338    /// using the slightly more efficient [`nlerp`](Self::nlerp) instead.
339    #[inline]
340    pub fn slerp(self, end: Self, s: f64) -> Self {
341        self * Self::radians(self.angle_to(end) * s)
342    }
343}
344
345impl From<f64> for DRot2 {
346    /// Creates a [`DRot2`] from a counterclockwise angle in radians.
347    fn from(rotation: f64) -> Self {
348        Self::radians(rotation)
349    }
350}
351
352impl From<DRot2> for DMat2 {
353    /// Creates a [`DMat2`] rotation matrix from a [`DRot2`].
354    fn from(rot: DRot2) -> Self {
355        DMat2::from_cols_array(&[rot.cos, rot.sin, -rot.sin, rot.cos])
356    }
357}
358
359impl core::ops::Mul for DRot2 {
360    type Output = Self;
361
362    fn mul(self, rhs: Self) -> Self::Output {
363        Self {
364            cos: self.cos * rhs.cos - self.sin * rhs.sin,
365            sin: self.sin * rhs.cos + self.cos * rhs.sin,
366        }
367    }
368}
369
370impl core::ops::MulAssign for DRot2 {
371    fn mul_assign(&mut self, rhs: Self) {
372        *self = *self * rhs;
373    }
374}
375
376impl core::ops::Mul<DVec2> for DRot2 {
377    type Output = DVec2;
378
379    /// Rotates a [`DVec2`] by a [`DRot2`].
380    fn mul(self, rhs: DVec2) -> Self::Output {
381        DVec2::new(
382            rhs.x * self.cos - rhs.y * self.sin,
383            rhs.x * self.sin + rhs.y * self.cos,
384        )
385    }
386}
avian3d/math/drot2.rs

avian3d/math/
drot2.rs
