parry2d/utils/
wops.rs

1//! Miscellaneous utilities.
2
3use crate::math::Real;
4use na::{Scalar, SimdRealField, Vector2, Vector3};
5
6#[cfg(feature = "simd-is-enabled")]
7use {
8    crate::simd::{SimdBool, SimdReal},
9    simba::simd::SimdValue,
10};
11
12#[cfg(feature = "simd-is-enabled")]
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<N: Scalar + Copy + WSign<N>> WSign<Vector2<N>> for N {
39    fn copy_sign_to(self, to: Vector2<N>) -> Vector2<N> {
40        Vector2::new(self.copy_sign_to(to.x), self.copy_sign_to(to.y))
41    }
42}
43
44impl<N: Scalar + Copy + WSign<N>> WSign<Vector3<N>> for N {
45    fn copy_sign_to(self, to: Vector3<N>) -> Vector3<N> {
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<N: Scalar + Copy + WSign<N>> WSign<Vector2<N>> for Vector2<N> {
55    fn copy_sign_to(self, to: Vector2<N>) -> Vector2<N> {
56        Vector2::new(self.x.copy_sign_to(to.x), self.y.copy_sign_to(to.y))
57    }
58}
59
60impl<N: Scalar + Copy + WSign<N>> WSign<Vector3<N>> for Vector3<N> {
61    fn copy_sign_to(self, to: Vector3<N>) -> Vector3<N> {
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        to.simd_copysign(self)
74    }
75}
76
77/// Trait to compute the orthonormal basis of a vector.
78pub trait WBasis: Sized {
79    /// The type of the array of orthonormal vectors.
80    type Basis;
81    /// Computes the vectors which, when combined with `self`, form an orthonormal basis.
82    fn orthonormal_basis(self) -> Self::Basis;
83}
84
85impl<N: SimdRealField + Copy> WBasis for Vector2<N> {
86    type Basis = [Vector2<N>; 1];
87    fn orthonormal_basis(self) -> [Vector2<N>; 1] {
88        [Vector2::new(-self.y, self.x)]
89    }
90}
91
92impl<N: SimdRealField + Copy + WSign<N>> WBasis for Vector3<N> {
93    type Basis = [Vector3<N>; 2];
94    // Robust and branchless implementation from Pixar:
95    // https://graphics.pixar.com/library/OrthonormalB/paper.pdf
96    fn orthonormal_basis(self) -> [Vector3<N>; 2] {
97        let sign = self.z.copy_sign_to(N::one());
98        let a = -N::one() / (sign + self.z);
99        let b = self.x * self.y * a;
100
101        [
102            Vector3::new(
103                N::one() + sign * self.x * self.x * a,
104                sign * b,
105                -sign * self.x,
106            ),
107            Vector3::new(b, sign + self.y * self.y * a, -self.y),
108        ]
109    }
110}
111
112pub(crate) trait WCross<Rhs>: Sized {
113    type Result;
114    fn gcross(&self, rhs: Rhs) -> Self::Result;
115}
116
117impl WCross<Vector3<Real>> for Vector3<Real> {
118    type Result = Self;
119
120    fn gcross(&self, rhs: Vector3<Real>) -> Self::Result {
121        self.cross(&rhs)
122    }
123}
124
125impl WCross<Vector2<Real>> for Vector2<Real> {
126    type Result = Real;
127
128    fn gcross(&self, rhs: Vector2<Real>) -> Self::Result {
129        self.x * rhs.y - self.y * rhs.x
130    }
131}
132
133impl WCross<Vector2<Real>> for Real {
134    type Result = Vector2<Real>;
135
136    fn gcross(&self, rhs: Vector2<Real>) -> Self::Result {
137        Vector2::new(-rhs.y * *self, rhs.x * *self)
138    }
139}
140
141#[cfg(feature = "simd-is-enabled")]
142impl WCross<Vector3<SimdReal>> for Vector3<SimdReal> {
143    type Result = Vector3<SimdReal>;
144
145    fn gcross(&self, rhs: Self) -> Self::Result {
146        self.cross(&rhs)
147    }
148}
149
150#[cfg(feature = "simd-is-enabled")]
151impl WCross<Vector2<SimdReal>> for SimdReal {
152    type Result = Vector2<SimdReal>;
153
154    fn gcross(&self, rhs: Vector2<SimdReal>) -> Self::Result {
155        Vector2::new(-rhs.y * *self, rhs.x * *self)
156    }
157}
158
159#[cfg(feature = "simd-is-enabled")]
160impl WCross<Vector2<SimdReal>> for Vector2<SimdReal> {
161    type Result = SimdReal;
162
163    fn gcross(&self, rhs: Self) -> Self::Result {
164        let yx = Vector2::new(rhs.y, rhs.x);
165        let prod = self.component_mul(&yx);
166        prod.x - prod.y
167    }
168}
parry2d/utils/wops.rs

parry2d/utils/
wops.rs
