parry3d_f64/utils/
wops.rs

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

parry3d_f64/utils/
wops.rs
