parry2d/utils/
wops.rs

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

parry2d/utils/
wops.rs
