path: root/src/utils.rs

//! Miscellaneous utilities.

use crate::dynamics::RigidBodyHandle;
use na::{Matrix2, Matrix3, Matrix3x2, Point2, Point3, Scalar, SimdRealField, Vector2, Vector3};
use num::Zero;
use simba::simd::SimdValue;

use std::ops::{Add, Mul};
use {
    crate::simd::{SimdBool, SimdFloat},
    na::SimdPartialOrd,
    num::One,
};

// pub(crate) const SIN_10_DEGREES: f32 = 0.17364817766;
// pub(crate) const COS_10_DEGREES: f32 = 0.98480775301;
// pub(crate) const COS_45_DEGREES: f32 = 0.70710678118;
// pub(crate) const SIN_45_DEGREES: f32 = COS_45_DEGREES;
#[cfg(feature = "dim3")]
pub(crate) const COS_1_DEGREES: f32 = 0.99984769515;
pub(crate) const COS_5_DEGREES: f32 = 0.99619469809;
// #[cfg(feature = "dim2")]
pub(crate) const COS_FRAC_PI_8: f32 = 0.92387953251;
#[cfg(feature = "dim2")]
pub(crate) const SIN_FRAC_PI_8: f32 = 0.38268343236;

pub(crate) fn inv(val: f32) -> f32 {
    if val == 0.0 {
        0.0
    } else {
        1.0 / val
    }
}

/// Conditionally swaps each lanes of `a` with those of `b`.
///
/// For each `i in [0..SIMD_WIDTH[`, if `do_swap.extract(i)` is `true` then
/// `a.extract(i)` is swapped with `b.extract(i)`.
pub fn simd_swap(do_swap: SimdBool, a: &mut SimdFloat, b: &mut SimdFloat) {
    let _a = *a;
    *a = b.select(do_swap, *a);
    *b = _a.select(do_swap, *b);
}

/// Trait to copy the sign of each component of one scalar/vector/matrix to another.
pub trait WSign<Rhs>: Sized {
    // See SIMD implementations of copy_sign there: https://stackoverflow.com/a/57872652
    /// Copy the sign of each component of `self` to the corresponding component of `to`.
    fn copy_sign_to(self, to: Rhs) -> Rhs;
}

impl WSign<f32> for f32 {
    fn copy_sign_to(self, to: Self) -> Self {
        let signbit: u32 = (-0.0f32).to_bits();
        f32::from_bits((signbit & self.to_bits()) | ((!signbit) & to.to_bits()))
    }
}

impl<N: Scalar + Copy + WSign<N>> WSign<Vector2<N>> for N {
    fn copy_sign_to(self, to: Vector2<N>) -> Vector2<N> {
        Vector2::new(self.copy_sign_to(to.x), self.copy_sign_to(to.y))
    }
}

impl<N: Scalar + Copy + WSign<N>> WSign<Vector3<N>> for N {
    fn copy_sign_to(self, to: Vector3<N>) -> Vector3<N> {
        Vector3::new(
            self.copy_sign_to(to.x),
            self.copy_sign_to(to.y),