parry2d/utils/
wops.rs

1//! Miscellaneous utilities.
2
3use crate::math::{Real, Vector2, Vector3};
4
5#[cfg(feature = "simd-is-enabled")]
6use {
7    crate::math::{SimdBool, SimdReal},
8    simba::simd::SimdValue,
9};
10
11#[cfg(feature = "simd-is-enabled")]
12#[allow(dead_code)]
13/// Conditionally swaps each lanes of `a` with those of `b`.
14///
15/// For each `i in [0..SIMD_WIDTH[`, if `do_swap.extract(i)` is `true` then
16/// `a.extract(i)` is swapped with `b.extract(i)`.
17pub fn simd_swap(do_swap: SimdBool, a: &mut SimdReal, b: &mut SimdReal) {
18    let _a = *a;
19    *a = b.select(do_swap, *a);
20    *b = _a.select(do_swap, *b);
21}
22
23/// Trait to copy the sign of each component of one scalar/vector/matrix to another.
24pub trait WSign<Rhs>: Sized {
25    // See SIMD implementations of copy_sign there: https://stackoverflow.com/a/57872652
26    /// Copy the sign of each component of `self` to the corresponding component of `to`.
27    fn copy_sign_to(self, to: Rhs) -> Rhs;
28}
29
30impl WSign<Real> for Real {
31    fn copy_sign_to(self, to: Self) -> Self {
32        let minus_zero: Real = -0.0;
33        let signbit = minus_zero.to_bits();
34        Real::from_bits((signbit & self.to_bits()) | ((!signbit) & to.to_bits()))
35    }
36}
37
38impl WSign<Vector2> for Real {
39    fn copy_sign_to(self, to: Vector2) -> Vector2 {
40        Vector2::new(self.copy_sign_to(to.x), self.copy_sign_to(to.y))
41    }
42}
43
44impl WSign<Vector3> for Real {
45    fn copy_sign_to(self, to: Vector3) -> Vector3 {
46        Vector3::new(
47            self.copy_sign_to(to.x),
48            self.copy_sign_to(to.y),
49            self.copy_sign_to(to.z),
50        )
51    }
52}
53
54impl WSign<Vector2> for Vector2 {
55    fn copy_sign_to(self, to: Vector2) -> Vector2 {
56        Vector2::new(self.x.copy_sign_to(to.x), self.y.copy_sign_to(to.y))
57    }
58}
59
60impl WSign<Vector3> for Vector3 {
61    fn copy_sign_to(self, to: Vector3) -> Vector3 {
62        Vector3::new(
63            self.x.copy_sign_to(to.x),
64            self.y.copy_sign_to(to.y),
65            self.z.copy_sign_to(to.z),
66        )
67    }
68}
69
70#[cfg(feature = "simd-is-enabled")]
71impl WSign<SimdReal> for SimdReal {
72    fn copy_sign_to(self, to: SimdReal) -> SimdReal {
73        use simba::simd::SimdRealField;
74        to.simd_copysign(self)
75    }
76}
77
78/// Trait to compute the orthonormal basis of a vector.
79pub trait WBasis: Sized {
80    /// The type of the array of orthonormal vectors.
81    type Basis;
82    /// Computes the vectors which, when combined with `self`, form an orthonormal basis.
83    fn orthonormal_basis(self) -> Self::Basis;
84}
85
86impl WBasis for Vector2 {
87    type Basis = [Vector2; 1];
88    fn orthonormal_basis(self) -> [Vector2; 1] {
89        [Vector2::new(-self.y, self.x)]
90    }
91}
92
93impl WBasis for Vector3 {
94    type Basis = [Vector3; 2];
95    // Robust and branchless implementation from Pixar:
96    // https://graphics.pixar.com/library/OrthonormalB/paper.pdf
97    fn orthonormal_basis(self) -> [Vector3; 2] {
98        let sign = self.z.copy_sign_to(1.0);
99        let a = -1.0 / (sign + self.z);
100        let b = self.x * self.y * a;
101
102        [
103            Vector3::new(1.0 + sign * self.x * self.x * a, sign * b, -sign * self.x),
104            Vector3::new(b, sign + self.y * self.y * a, -self.y),
105        ]
106    }
107}
108
109pub(crate) trait WCross<Rhs>: Sized {
110    type Result;
111    fn gcross(&self, rhs: Rhs) -> Self::Result;
112}
113
114impl WCross<Vector3> for Vector3 {
115    type Result = Self;
116
117    fn gcross(&self, rhs: Vector3) -> Self::Result {
118        self.cross(rhs)
119    }
120}
121
122impl WCross<Vector2> for Vector2 {
123    type Result = Real;
124
125    fn gcross(&self, rhs: Vector2) -> Self::Result {
126        self.x * rhs.y - self.y * rhs.x
127    }
128}
129
130impl WCross<Vector2> for Real {
131    type Result = Vector2;
132
133    fn gcross(&self, rhs: Vector2) -> Self::Result {
134        Vector2::new(-rhs.y * *self, rhs.x * *self)
135    }
136}
parry2d/utils/wops.rs

parry2d/utils/
wops.rs
