rapier3d/
utils.rs

1//! Miscellaneous utilities.
2
3use na::{
4    Matrix1, Matrix2, Matrix3, RowVector2, Scalar, SimdRealField, UnitComplex, UnitQuaternion,
5    Vector1, Vector2, Vector3,
6};
7use num::Zero;
8use simba::simd::SimdValue;
9use std::ops::IndexMut;
10
11use parry::utils::SdpMatrix3;
12use {
13    crate::math::{Real, SimdReal},
14    na::SimdPartialOrd,
15    num::One,
16};
17
18/// The trait for real numbers used by Rapier.
19///
20/// This includes `f32`, `f64` and their related SIMD types.
21pub trait SimdRealCopy: SimdRealField<Element = Real> + Copy {}
22impl SimdRealCopy for Real {}
23impl SimdRealCopy for SimdReal {}
24
25const INV_EPSILON: Real = 1.0e-20;
26
27pub(crate) fn inv(val: Real) -> Real {
28    if (-INV_EPSILON..=INV_EPSILON).contains(&val) {
29        0.0
30    } else {
31        1.0 / val
32    }
33}
34
35pub(crate) fn simd_inv<N: SimdRealCopy>(val: N) -> N {
36    let eps = N::splat(INV_EPSILON);
37    N::zero().select(val.simd_gt(-eps) & val.simd_lt(eps), N::one() / val)
38}
39
40/// Trait to copy the sign of each component of one scalar/vector/matrix to another.
41pub trait SimdSign<Rhs>: Sized {
42    // See SIMD implementations of copy_sign there: https://stackoverflow.com/a/57872652
43    /// Copy the sign of each component of `self` to the corresponding component of `to`.
44    fn copy_sign_to(self, to: Rhs) -> Rhs;
45}
46
47impl SimdSign<Real> for Real {
48    fn copy_sign_to(self, to: Self) -> Self {
49        const MINUS_ZERO: Real = -0.0;
50        let signbit = MINUS_ZERO.to_bits();
51        Real::from_bits((signbit & self.to_bits()) | ((!signbit) & to.to_bits()))
52    }
53}
54
55impl<N: Scalar + Copy + SimdSign<N>> SimdSign<Vector2<N>> for N {
56    fn copy_sign_to(self, to: Vector2<N>) -> Vector2<N> {
57        Vector2::new(self.copy_sign_to(to.x), self.copy_sign_to(to.y))
58    }
59}
60
61impl<N: Scalar + Copy + SimdSign<N>> SimdSign<Vector3<N>> for N {
62    fn copy_sign_to(self, to: Vector3<N>) -> Vector3<N> {
63        Vector3::new(
64            self.copy_sign_to(to.x),
65            self.copy_sign_to(to.y),
66            self.copy_sign_to(to.z),
67        )
68    }
69}
70
71impl<N: Scalar + Copy + SimdSign<N>> SimdSign<Vector2<N>> for Vector2<N> {
72    fn copy_sign_to(self, to: Vector2<N>) -> Vector2<N> {
73        Vector2::new(self.x.copy_sign_to(to.x), self.y.copy_sign_to(to.y))
74    }
75}
76
77impl<N: Scalar + Copy + SimdSign<N>> SimdSign<Vector3<N>> for Vector3<N> {
78    fn copy_sign_to(self, to: Vector3<N>) -> Vector3<N> {
79        Vector3::new(
80            self.x.copy_sign_to(to.x),
81            self.y.copy_sign_to(to.y),
82            self.z.copy_sign_to(to.z),
83        )
84    }
85}
86
87impl SimdSign<SimdReal> for SimdReal {
88    fn copy_sign_to(self, to: SimdReal) -> SimdReal {
89        to.simd_copysign(self)
90    }
91}
92
93/// Trait to compute the orthonormal basis of a vector.
94pub trait SimdBasis: Sized {
95    /// The type of the array of orthonormal vectors.
96    type Basis;
97    /// Computes the vectors which, when combined with `self`, form an orthonormal basis.
98    fn orthonormal_basis(self) -> Self::Basis;
99    /// Computes a vector orthogonal to `self` with a unit length (if `self` has a unit length).
100    fn orthonormal_vector(self) -> Self;
101}
102
103impl<N: SimdRealCopy> SimdBasis for Vector2<N> {
104    type Basis = [Vector2<N>; 1];
105    fn orthonormal_basis(self) -> [Vector2<N>; 1] {
106        [Vector2::new(-self.y, self.x)]
107    }
108    fn orthonormal_vector(self) -> Vector2<N> {
109        Vector2::new(-self.y, self.x)
110    }
111}
112
113impl<N: SimdRealCopy + SimdSign<N>> SimdBasis for Vector3<N> {
114    type Basis = [Vector3<N>; 2];
115    // Robust and branchless implementation from Pixar:
116    // https://graphics.pixar.com/library/OrthonormalB/paper.pdf
117    fn orthonormal_basis(self) -> [Vector3<N>; 2] {
118        let sign = self.z.copy_sign_to(N::one());
119        let a = -N::one() / (sign + self.z);
120        let b = self.x * self.y * a;
121
122        [
123            Vector3::new(
124                N::one() + sign * self.x * self.x * a,
125                sign * b,
126                -sign * self.x,
127            ),
128            Vector3::new(b, sign + self.y * self.y * a, -self.y),
129        ]
130    }
131
132    fn orthonormal_vector(self) -> Vector3<N> {
133        let sign = self.z.copy_sign_to(N::one());
134        let a = -N::one() / (sign + self.z);
135        let b = self.x * self.y * a;
136        Vector3::new(b, sign + self.y * self.y * a, -self.y)
137    }
138}
139
140pub(crate) trait SimdCrossMatrix: Sized {
141    type CrossMat;
142    type CrossMatTr;
143
144    fn gcross_matrix(self) -> Self::CrossMat;
145    fn gcross_matrix_tr(self) -> Self::CrossMatTr;
146}
147
148impl<N: SimdRealCopy> SimdCrossMatrix for Vector3<N> {
149    type CrossMat = Matrix3<N>;
150    type CrossMatTr = Matrix3<N>;
151
152    #[inline]
153    #[rustfmt::skip]
154    fn gcross_matrix(self) -> Self::CrossMat {
155        Matrix3::new(
156            N::zero(), -self.z, self.y,
157            self.z, N::zero(), -self.x,
158            -self.y, self.x, N::zero(),
159        )
160    }
161
162    #[inline]
163    #[rustfmt::skip]
164    fn gcross_matrix_tr(self) -> Self::CrossMatTr {
165        Matrix3::new(
166            N::zero(), self.z, -self.y,
167            -self.z, N::zero(), self.x,
168            self.y, -self.x, N::zero(),
169        )
170    }
171}
172
173impl<N: SimdRealCopy> SimdCrossMatrix for Vector2<N> {
174    type CrossMat = RowVector2<N>;
175    type CrossMatTr = Vector2<N>;
176
177    #[inline]
178    fn gcross_matrix(self) -> Self::CrossMat {
179        RowVector2::new(-self.y, self.x)
180    }
181    #[inline]
182    fn gcross_matrix_tr(self) -> Self::CrossMatTr {
183        Vector2::new(-self.y, self.x)
184    }
185}
186impl SimdCrossMatrix for Real {
187    type CrossMat = Matrix2<Real>;
188    type CrossMatTr = Matrix2<Real>;
189
190    #[inline]
191    fn gcross_matrix(self) -> Matrix2<Real> {
192        Matrix2::new(0.0, -self, self, 0.0)
193    }
194
195    #[inline]
196    fn gcross_matrix_tr(self) -> Matrix2<Real> {
197        Matrix2::new(0.0, self, -self, 0.0)
198    }
199}
200
201impl SimdCrossMatrix for SimdReal {
202    type CrossMat = Matrix2<SimdReal>;
203    type CrossMatTr = Matrix2<SimdReal>;
204
205    #[inline]
206    fn gcross_matrix(self) -> Matrix2<SimdReal> {
207        Matrix2::new(SimdReal::zero(), -self, self, SimdReal::zero())
208    }
209
210    #[inline]
211    fn gcross_matrix_tr(self) -> Matrix2<SimdReal> {
212        Matrix2::new(SimdReal::zero(), self, -self, SimdReal::zero())
213    }
214}
215
216pub(crate) trait SimdCross<Rhs>: Sized {
217    type Result;
218    fn gcross(&self, rhs: Rhs) -> Self::Result;
219}
220
221impl SimdCross<Vector3<Real>> for Vector3<Real> {
222    type Result = Self;
223
224    fn gcross(&self, rhs: Vector3<Real>) -> Self::Result {
225        self.cross(&rhs)
226    }
227}
228
229impl SimdCross<Vector2<Real>> for Vector2<Real> {
230    type Result = Real;
231
232    fn gcross(&self, rhs: Vector2<Real>) -> Self::Result {
233        self.x * rhs.y - self.y * rhs.x
234    }
235}
236
237impl SimdCross<Vector2<Real>> for Real {
238    type Result = Vector2<Real>;
239
240    fn gcross(&self, rhs: Vector2<Real>) -> Self::Result {
241        Vector2::new(-rhs.y * *self, rhs.x * *self)
242    }
243}
244
245pub(crate) trait SimdDot<Rhs>: Sized {
246    type Result;
247    fn gdot(&self, rhs: Rhs) -> Self::Result;
248}
249
250impl<N: SimdRealCopy> SimdDot<Vector3<N>> for Vector3<N> {
251    type Result = N;
252
253    fn gdot(&self, rhs: Vector3<N>) -> Self::Result {
254        self.x * rhs.x + self.y * rhs.y + self.z * rhs.z
255    }
256}
257
258impl<N: SimdRealCopy> SimdDot<Vector2<N>> for Vector2<N> {
259    type Result = N;
260
261    fn gdot(&self, rhs: Vector2<N>) -> Self::Result {
262        self.x * rhs.x + self.y * rhs.y
263    }
264}
265
266impl<N: SimdRealCopy> SimdDot<Vector1<N>> for N {
267    type Result = N;
268
269    fn gdot(&self, rhs: Vector1<N>) -> Self::Result {
270        *self * rhs.x
271    }
272}
273
274impl<N: SimdRealCopy> SimdDot<N> for N {
275    type Result = N;
276
277    fn gdot(&self, rhs: N) -> Self::Result {
278        *self * rhs
279    }
280}
281
282impl<N: SimdRealCopy> SimdDot<N> for Vector1<N> {
283    type Result = N;
284
285    fn gdot(&self, rhs: N) -> Self::Result {
286        self.x * rhs
287    }
288}
289
290impl SimdCross<Vector3<SimdReal>> for Vector3<SimdReal> {
291    type Result = Vector3<SimdReal>;
292
293    fn gcross(&self, rhs: Self) -> Self::Result {
294        self.cross(&rhs)
295    }
296}
297
298impl SimdCross<Vector2<SimdReal>> for SimdReal {
299    type Result = Vector2<SimdReal>;
300
301    fn gcross(&self, rhs: Vector2<SimdReal>) -> Self::Result {
302        Vector2::new(-rhs.y * *self, rhs.x * *self)
303    }
304}
305
306impl SimdCross<Vector2<SimdReal>> for Vector2<SimdReal> {
307    type Result = SimdReal;
308
309    fn gcross(&self, rhs: Self) -> Self::Result {
310        let yx = Vector2::new(rhs.y, rhs.x);
311        let prod = self.component_mul(&yx);
312        prod.x - prod.y
313    }
314}
315
316/// Trait implemented by quaternions.
317pub trait SimdQuat<N> {
318    /// The result of quaternion differentiation.
319    type Result;
320
321    /// Compute the differential of `inv(q1) * q2`.
322    fn diff_conj1_2(&self, rhs: &Self) -> Self::Result;
323}
324
325impl<N: SimdRealCopy> SimdQuat<N> for UnitComplex<N> {
326    type Result = Matrix1<N>;
327
328    fn diff_conj1_2(&self, rhs: &Self) -> Self::Result {
329        let two: N = N::splat(2.0);
330        Matrix1::new((self.im * rhs.im + self.re * rhs.re) * two)
331    }
332}
333
334impl<N: SimdRealCopy> SimdQuat<N> for UnitQuaternion<N> {
335    type Result = Matrix3<N>;
336
337    fn diff_conj1_2(&self, rhs: &Self) -> Self::Result {
338        let half = N::splat(0.5);
339        let v1 = self.imag();
340        let v2 = rhs.imag();
341        let w1 = self.w;
342        let w2 = rhs.w;
343
344        // TODO: this can probably be optimized a lot by unrolling the ops.
345        (v1 * v2.transpose() + Matrix3::from_diagonal_element(w1 * w2)
346            - (v1 * w2 + v2 * w1).cross_matrix()
347            + v1.cross_matrix() * v2.cross_matrix())
348            * half
349    }
350}
351
352pub(crate) trait SimdAngularInertia<N> {
353    type AngVector;
354    type AngMatrix;
355    fn inverse(&self) -> Self;
356    fn transform_vector(&self, pt: Self::AngVector) -> Self::AngVector;
357    fn squared(&self) -> Self;
358    fn into_matrix(self) -> Self::AngMatrix;
359}
360
361impl<N: SimdRealCopy> SimdAngularInertia<N> for N {
362    type AngVector = N;
363    type AngMatrix = N;
364
365    fn inverse(&self) -> Self {
366        simd_inv(*self)
367    }
368
369    fn transform_vector(&self, pt: N) -> N {
370        pt * *self
371    }
372
373    fn squared(&self) -> N {
374        *self * *self
375    }
376
377    fn into_matrix(self) -> Self::AngMatrix {
378        self
379    }
380}
381
382impl SimdAngularInertia<Real> for SdpMatrix3<Real> {
383    type AngVector = Vector3<Real>;
384    type AngMatrix = Matrix3<Real>;
385
386    fn inverse(&self) -> Self {
387        let minor_m12_m23 = self.m22 * self.m33 - self.m23 * self.m23;
388        let minor_m11_m23 = self.m12 * self.m33 - self.m13 * self.m23;
389        let minor_m11_m22 = self.m12 * self.m23 - self.m13 * self.m22;
390
391        let determinant =
392            self.m11 * minor_m12_m23 - self.m12 * minor_m11_m23 + self.m13 * minor_m11_m22;
393
394        if determinant.is_zero() {
395            Self::zero()
396        } else {
397            SdpMatrix3 {
398                m11: minor_m12_m23 / determinant,
399                m12: -minor_m11_m23 / determinant,
400                m13: minor_m11_m22 / determinant,
401                m22: (self.m11 * self.m33 - self.m13 * self.m13) / determinant,
402                m23: (self.m13 * self.m12 - self.m23 * self.m11) / determinant,
403                m33: (self.m11 * self.m22 - self.m12 * self.m12) / determinant,
404            }
405        }
406    }
407
408    fn squared(&self) -> Self {
409        SdpMatrix3 {
410            m11: self.m11 * self.m11 + self.m12 * self.m12 + self.m13 * self.m13,
411            m12: self.m11 * self.m12 + self.m12 * self.m22 + self.m13 * self.m23,
412            m13: self.m11 * self.m13 + self.m12 * self.m23 + self.m13 * self.m33,
413            m22: self.m12 * self.m12 + self.m22 * self.m22 + self.m23 * self.m23,
414            m23: self.m12 * self.m13 + self.m22 * self.m23 + self.m23 * self.m33,
415            m33: self.m13 * self.m13 + self.m23 * self.m23 + self.m33 * self.m33,
416        }
417    }
418
419    fn transform_vector(&self, v: Vector3<Real>) -> Vector3<Real> {
420        let x = self.m11 * v.x + self.m12 * v.y + self.m13 * v.z;
421        let y = self.m12 * v.x + self.m22 * v.y + self.m23 * v.z;
422        let z = self.m13 * v.x + self.m23 * v.y + self.m33 * v.z;
423        Vector3::new(x, y, z)
424    }
425
426    #[rustfmt::skip]
427    fn into_matrix(self) -> Matrix3<Real> {
428        Matrix3::new(
429            self.m11, self.m12, self.m13,
430            self.m12, self.m22, self.m23,
431            self.m13, self.m23, self.m33,
432        )
433    }
434}
435
436impl SimdAngularInertia<SimdReal> for SdpMatrix3<SimdReal> {
437    type AngVector = Vector3<SimdReal>;
438    type AngMatrix = Matrix3<SimdReal>;
439
440    fn inverse(&self) -> Self {
441        let minor_m12_m23 = self.m22 * self.m33 - self.m23 * self.m23;
442        let minor_m11_m23 = self.m12 * self.m33 - self.m13 * self.m23;
443        let minor_m11_m22 = self.m12 * self.m23 - self.m13 * self.m22;
444
445        let determinant =
446            self.m11 * minor_m12_m23 - self.m12 * minor_m11_m23 + self.m13 * minor_m11_m22;
447
448        let zero = <SimdReal>::zero();
449        let is_zero = determinant.simd_eq(zero);
450        let inv_det = (<SimdReal>::one() / determinant).select(is_zero, zero);
451
452        SdpMatrix3 {
453            m11: minor_m12_m23 * inv_det,
454            m12: -minor_m11_m23 * inv_det,
455            m13: minor_m11_m22 * inv_det,
456            m22: (self.m11 * self.m33 - self.m13 * self.m13) * inv_det,
457            m23: (self.m13 * self.m12 - self.m23 * self.m11) * inv_det,
458            m33: (self.m11 * self.m22 - self.m12 * self.m12) * inv_det,
459        }
460    }
461
462    fn transform_vector(&self, v: Vector3<SimdReal>) -> Vector3<SimdReal> {
463        let x = self.m11 * v.x + self.m12 * v.y + self.m13 * v.z;
464        let y = self.m12 * v.x + self.m22 * v.y + self.m23 * v.z;
465        let z = self.m13 * v.x + self.m23 * v.y + self.m33 * v.z;
466        Vector3::new(x, y, z)
467    }
468
469    fn squared(&self) -> Self {
470        SdpMatrix3 {
471            m11: self.m11 * self.m11 + self.m12 * self.m12 + self.m13 * self.m13,
472            m12: self.m11 * self.m12 + self.m12 * self.m22 + self.m13 * self.m23,
473            m13: self.m11 * self.m13 + self.m12 * self.m23 + self.m13 * self.m33,
474            m22: self.m12 * self.m12 + self.m22 * self.m22 + self.m23 * self.m23,
475            m23: self.m12 * self.m13 + self.m22 * self.m23 + self.m23 * self.m33,
476            m33: self.m13 * self.m13 + self.m23 * self.m23 + self.m33 * self.m33,
477        }
478    }
479
480    #[rustfmt::skip]
481    fn into_matrix(self) -> Matrix3<SimdReal> {
482        Matrix3::new(
483            self.m11, self.m12, self.m13,
484            self.m12, self.m22, self.m23,
485            self.m13, self.m23, self.m33,
486        )
487    }
488}
489
490// This is an RAII structure that enables flushing denormal numbers
491// to zero, and automatically resetting previous flags once it is dropped.
492#[derive(Clone, Debug, PartialEq, Eq)]
493pub(crate) struct FlushToZeroDenormalsAreZeroFlags {
494    original_flags: u32,
495}
496
497impl FlushToZeroDenormalsAreZeroFlags {
498    #[cfg(not(all(
499        not(feature = "enhanced-determinism"),
500        any(target_arch = "x86_64", target_arch = "x86"),
501        target_feature = "sse"
502    )))]
503    pub fn flush_denormal_to_zero() -> Self {
504        Self { original_flags: 0 }
505    }
506
507    #[cfg(all(
508        not(feature = "enhanced-determinism"),
509        any(target_arch = "x86", target_arch = "x86_64"),
510        target_feature = "sse"
511    ))]
512    #[allow(deprecated)] // will address that later.
513    pub fn flush_denormal_to_zero() -> Self {
514        unsafe {
515            #[cfg(target_arch = "x86")]
516            use std::arch::x86::{_MM_FLUSH_ZERO_ON, _mm_getcsr, _mm_setcsr};
517            #[cfg(target_arch = "x86_64")]
518            use std::arch::x86_64::{_MM_FLUSH_ZERO_ON, _mm_getcsr, _mm_setcsr};
519
520            // Flush denormals & underflows to zero as this as a significant impact on the solver's performances.
521            // To enable this we need to set the bit 15 (given by _MM_FLUSH_ZERO_ON) and the bit 6 (for denormals-are-zero).
522            // See https://software.intel.com/content/www/us/en/develop/articles/x87-and-sse-floating-point-assists-in-ia-32-flush-to-zero-ftz-and-denormals-are-zero-daz.html
523            let original_flags = _mm_getcsr();
524            _mm_setcsr(original_flags | _MM_FLUSH_ZERO_ON | (1 << 6));
525            Self { original_flags }
526        }
527    }
528}
529
530#[cfg(all(
531    not(feature = "enhanced-determinism"),
532    any(target_arch = "x86", target_arch = "x86_64"),
533    target_feature = "sse"
534))]
535impl Drop for FlushToZeroDenormalsAreZeroFlags {
536    #[allow(deprecated)] // will address that later.
537    fn drop(&mut self) {
538        #[cfg(target_arch = "x86")]
539        unsafe {
540            std::arch::x86::_mm_setcsr(self.original_flags)
541        }
542        #[cfg(target_arch = "x86_64")]
543        unsafe {
544            std::arch::x86_64::_mm_setcsr(self.original_flags)
545        }
546    }
547}
548
549/// This is an RAII structure that disables floating point exceptions while
550/// it is alive, so that operations which generate NaNs and infinite values
551/// intentionally will not trip an exception when debugging problematic
552/// code that is generating NaNs and infinite values erroneously.
553#[derive(Clone, Debug, PartialEq, Eq)]
554pub(crate) struct DisableFloatingPointExceptionsFlags {
555    #[cfg(feature = "debug-disable-legitimate-fe-exceptions")]
556    // We can't get a precise size for this, because it's of type
557    // `fenv_t`, which is a definition that doesn't exist in rust
558    // (not even in the libc crate, as of the time of writing.)
559    // But since the state is intended to be stored on the stack,
560    // 256 bytes should be more than enough.
561    original_flags: [u8; 256],
562}
563
564#[cfg(feature = "debug-disable-legitimate-fe-exceptions")]
565extern "C" {
566    fn feholdexcept(env: *mut std::ffi::c_void);
567    fn fesetenv(env: *const std::ffi::c_void);
568}
569
570impl DisableFloatingPointExceptionsFlags {
571    #[cfg(not(feature = "debug-disable-legitimate-fe-exceptions"))]
572    #[allow(dead_code)]
573    /// Disables floating point exceptions as long as this object is not dropped.
574    pub fn disable_floating_point_exceptions() -> Self {
575        Self {}
576    }
577
578    #[cfg(feature = "debug-disable-legitimate-fe-exceptions")]
579    /// Disables floating point exceptions as long as this object is not dropped.
580    pub fn disable_floating_point_exceptions() -> Self {
581        unsafe {
582            let mut original_flags = [0; 256];
583            feholdexcept(original_flags.as_mut_ptr() as *mut _);
584            Self { original_flags }
585        }
586    }
587}
588
589#[cfg(feature = "debug-disable-legitimate-fe-exceptions")]
590impl Drop for DisableFloatingPointExceptionsFlags {
591    fn drop(&mut self) {
592        unsafe {
593            fesetenv(self.original_flags.as_ptr() as *const _);
594        }
595    }
596}
597
598pub(crate) fn select_other<T: PartialEq>(pair: (T, T), elt: T) -> T {
599    if pair.0 == elt { pair.1 } else { pair.0 }
600}
601
602/// Methods for simultaneously indexing a container with two distinct indices.
603pub trait IndexMut2<I>: IndexMut<I> {
604    /// Gets mutable references to two distinct elements of the container.
605    ///
606    /// Panics if `i == j`.
607    fn index_mut2(&mut self, i: usize, j: usize) -> (&mut Self::Output, &mut Self::Output);
608
609    /// Gets a mutable reference to one element, and immutable reference to a second one.
610    ///
611    /// Panics if `i == j`.
612    #[inline]
613    fn index_mut_const(&mut self, i: usize, j: usize) -> (&mut Self::Output, &Self::Output) {
614        let (a, b) = self.index_mut2(i, j);
615        (a, &*b)
616    }
617}
618
619impl<T> IndexMut2<usize> for Vec<T> {
620    #[inline]
621    fn index_mut2(&mut self, i: usize, j: usize) -> (&mut T, &mut T) {
622        assert!(i != j, "Unable to index the same element twice.");
623        assert!(i < self.len() && j < self.len(), "Index out of bounds.");
624
625        unsafe {
626            let a = &mut *(self.get_unchecked_mut(i) as *mut _);
627            let b = &mut *(self.get_unchecked_mut(j) as *mut _);
628            (a, b)
629        }
630    }
631}
632
633impl<T> IndexMut2<usize> for [T] {
634    #[inline]
635    fn index_mut2(&mut self, i: usize, j: usize) -> (&mut T, &mut T) {
636        assert!(i != j, "Unable to index the same element twice.");
637        assert!(i < self.len() && j < self.len(), "Index out of bounds.");
638
639        unsafe {
640            let a = &mut *(self.get_unchecked_mut(i) as *mut _);
641            let b = &mut *(self.get_unchecked_mut(j) as *mut _);
642            (a, b)
643        }
644    }
645}
646
647/// Calculate the difference with smallest absolute value between the two given values.
648pub fn smallest_abs_diff_between_sin_angles<N: SimdRealCopy>(a: N, b: N) -> N {
649    // Select the smallest path among the two angles to reach the target.
650    let s_err = a - b;
651    let sgn = s_err.simd_signum();
652    let s_err_complement = s_err - sgn * N::splat(2.0);
653    let s_err_is_smallest = s_err.simd_abs().simd_lt(s_err_complement.simd_abs());
654    s_err.select(s_err_is_smallest, s_err_complement)
655}
656
657/// Calculate the difference with smallest absolute value between the two given angles.
658pub fn smallest_abs_diff_between_angles<N: SimdRealCopy>(a: N, b: N) -> N {
659    // Select the smallest path among the two angles to reach the target.
660    let s_err = a - b;
661    let sgn = s_err.simd_signum();
662    let s_err_complement = s_err - sgn * N::simd_two_pi();
663    let s_err_is_smallest = s_err.simd_abs().simd_lt(s_err_complement.simd_abs());
664    s_err.select(s_err_is_smallest, s_err_complement)
665}
666
667/// Helpers around serialization.
668#[cfg(feature = "serde-serialize")]
669pub mod serde {
670    use serde::{Deserialize, Serialize};
671    use std::iter::FromIterator;
672
673    /// Serializes to a `Vec<(K, V)>`.
674    ///
675    /// Useful for [`std::collections::HashMap`] with a non-string key,
676    /// which is unsupported by [`serde_json`](https://docs.rs/serde_json/).
677    pub fn serialize_to_vec_tuple<
678        'a,
679        S: serde::Serializer,
680        T: IntoIterator<Item = (&'a K, &'a V)>,
681        K: Serialize + 'a,
682        V: Serialize + 'a,
683    >(
684        target: T,
685        s: S,
686    ) -> Result<S::Ok, S::Error> {
687        let container: Vec<_> = target.into_iter().collect();
688        serde::Serialize::serialize(&container, s)
689    }
690
691    /// Deserializes from a `Vec<(K, V)>`.
692    ///
693    /// Useful for [`std::collections::HashMap`] with a non-string key,
694    /// which is unsupported by [`serde_json`](https://docs.rs/serde_json/).
695    pub fn deserialize_from_vec_tuple<
696        'de,
697        D: serde::Deserializer<'de>,
698        T: FromIterator<(K, V)>,
699        K: Deserialize<'de>,
700        V: Deserialize<'de>,
701    >(
702        d: D,
703    ) -> Result<T, D::Error> {
704        let hashmap_as_vec: Vec<(K, V)> = Deserialize::deserialize(d)?;
705        Ok(T::from_iter(hashmap_as_vec))
706    }
707
708    #[cfg(test)]
709    mod test {
710        use std::collections::HashMap;
711
712        /// This test uses serde_json because json doesn't support non string
713        /// keys in hashmaps, which requires a custom serialization.
714        #[test]
715        fn serde_json_hashmap() {
716            #[derive(Serialize, Deserialize, PartialEq, Eq, Debug)]
717            struct Test {
718                #[cfg_attr(
719                    feature = "serde-serialize",
720                    serde(
721                        serialize_with = "crate::utils::serde::serialize_to_vec_tuple",
722                        deserialize_with = "crate::utils::serde::deserialize_from_vec_tuple"
723                    )
724                )]
725                pub map: HashMap<usize, String>,
726            }
727
728            let s = Test {
729                map: [(42, "Forty-Two".to_string())].into(),
730            };
731            let j = serde_json::to_string(&s).unwrap();
732            assert_eq!(&j, "{\"map\":[[42,\"Forty-Two\"]]}");
733            let p: Test = serde_json::from_str(&j).unwrap();
734            assert_eq!(&p, &s);
735        }
736    }
737}
rapier3d/utils.rs

rapier3d/
utils.rs
