use crate::dynamics::RigidBodyHandle;
#[cfg(all(feature = "enhanced-determinism", feature = "serde-serialize"))]
use indexmap::IndexMap as HashMap;
use na::{Matrix2, Matrix3, Matrix3x2, Point2, Point3, Scalar, SimdRealField, Vector2, Vector3};
use num::Zero;
use simba::simd::SimdValue;
#[cfg(all(not(feature = "enhanced-determinism"), feature = "serde-serialize"))]
use std::collections::HashMap;
use std::ops::{Add, Mul};
use {
crate::simd::{SimdBool, SimdFloat},
na::SimdPartialOrd,
num::One,
};
#[cfg(feature = "dim3")]
pub(crate) const COS_1_DEGREES: f32 = 0.99984769515;
pub(crate) const COS_5_DEGREES: f32 = 0.99619469809;
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
}
}
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);
}
pub trait WSign<Rhs>: Sized {
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),
self.copy_sign_to(to.z),
)
}
}
impl<N: Scalar + Copy + WSign<N>> WSign<Vector2<N>> for Vector2<N> {
fn copy_sign_to(self, to: Vector2<N>) -> Vector2<N> {
Vector2::new(self.x.copy_sign_to(to.x), self.y.copy_sign_to(to.y))
}
}
impl<N: Scalar + Copy + WSign<N>> WSign<Vector3<N>> for Vector3<N> {
fn copy_sign_to(self, to: Vector3<N>) -> Vector3<N> {
Vector3::new(
self.x.copy_sign_to(to.x),
self.y.copy_sign_to(to.y),
self.z.copy_sign_to(to.z),
)
}
}
impl WSign<SimdFloat> for SimdFloat {
fn copy_sign_to(self, to: SimdFloat) -> SimdFloat {
to.simd_copysign(self)
}
}
pub(crate) trait WComponent: Sized {
type Element;
fn min_component(self) -> Self::Element;
fn max_component(self) -> Self::Element;
}
impl WComponent for f32 {
type Element = f32;
fn min_component(self) -> Self::Element {
self
}
fn max_component(self) -> Self::Element {
self
}
}
impl WComponent for SimdFloat {
type Element = f32;
fn min_component(self) -> Self::Element {
self.simd_horizontal_min()
}
fn max_component(self) -> Self::Element {
self.simd_horizontal_max()
}
}
pub trait WBasis: Sized {
type Basis;
fn orthonormal_basis(self) -> Self::Basis;
}
impl<N: SimdRealField> WBasis for Vector2<N> {
type Basis = [Vector2<N>; 1];
fn orthonormal_basis(self) -> [Vector2<N>; 1] {
[Vector2::new(-self.y, self.x)]
}
}
impl<N: SimdRealField + WSign<N>> WBasis for Vector3<N> {
type Basis = [Vector3<N>; 2];
fn orthonormal_basis(self) -> [Vector3<N>; 2] {
let sign = self.z.copy_sign_to(N::one());
let a = -N::one() / (sign + self.z);
let b = self.x * self.y * a;
[
Vector3::new(
N::one() + sign * self.x * self.x * a,
sign * b,
-sign * self.x,
),
Vector3::new(b, sign + self.y * self.y * a, -self.y),
]
}
}
pub(crate) trait WVec: Sized {
type Element;
fn horizontal_inf(&self) -> Self::Element;
fn horizontal_sup(&self) -> Self::Element;
}
impl<N: Scalar + Copy + WComponent> WVec for Vector2<N>
where
N::Element: Scalar,
{
type Element = Vector2<N::Element>;
fn horizontal_inf(&self) -> Self::Element {
Vector2::new(self.x.min_component(), self.y.min_component())
}
fn horizontal_sup(&self) -> Self::Element {
Vector2::new(self.x.max_component(), self.y.max_component())
}
}
impl<N: Scalar + Copy + WComponent> WVec for Point2<N>
where
N::Element: Scalar,
{
type Element = Point2<N::Element>;
fn horizontal_inf(&self) -> Self::Element {
Point2::new(self.x.min_component(), self.y.min_component())
}
fn horizontal_sup(&self) -> Self::Element {
Point2::new(self.x.max_component(), self.y.max_component())
}
}
impl<N: Scalar + Copy + WComponent> WVec for Vector3<N>
where
N::Element: Scalar,
{
type Element = Vector3<N::Element>;
fn horizontal_inf(&self) -> Self::Element {
Vector3::new(
self.x.min_component(),
self.y.min_component(),
self.z.min_component(),
)
}
fn horizontal_sup(&self) -> Self::Element {
Vector3::new(
self.x.max_component(),
self.y.max_component(),
self.z.max_component(),
)
}
}
impl<N: Scalar + Copy + WComponent> WVec for Point3<N>
where
N::Element: Scalar,
{
type Element = Point3<N::Element>;
fn horizontal_inf(&self) -> Self::Element {
Point3::new(
self.x.min_component(),
self.y.min_component(),
self.z.min_component(),
)
}
fn horizontal_sup(&self) -> Self::Element {
Point3::new(
self.x.max_component(),
self.y.max_component(),
self.z.max_component(),
)
}
}
pub(crate) trait WCrossMatrix: Sized {
type CrossMat;
fn gcross_matrix(self) -> Self::CrossMat;
}
impl WCrossMatrix for Vector3<f32> {
type CrossMat = Matrix3<f32>;
#[inline]
#[rustfmt::skip]
fn gcross_matrix(self) -> Self::CrossMat {
Matrix3::new(
0.0, -self.z, self.y,
self.z, 0.0, -self.x,
-self.y, self.x, 0.0,
)
}
}
impl WCrossMatrix for Vector2<f32> {
type CrossMat = Vector2<f32>;
#[inline]
fn gcross_matrix(self) -> Self::CrossMat {
Vector2::new(-self.y, self.x)
}
}
pub(crate) trait WCross<Rhs>: Sized {
type Result;
fn gcross(&self, rhs: Rhs) -> Self::Result;
}
impl WCross<Vector3<f32>> for Vector3<f32> {
type Result = Self;
fn gcross(&self, rhs: Vector3<f32>) -> Self::Result {
self.cross(&rhs)
}
}
impl WCross<Vector2<f32>> for Vector2<f32> {
type Result = f32;
fn gcross(&self, rhs: Vector2<f32>) -> Self::Result {
self.x * rhs.y - self.y * rhs.x
}
}
impl WCross<Vector2<f32>> for f32 {
type Result = Vector2<f32>;
fn gcross(&self, rhs: Vector2<f32>) -> Self::Result {
Vector2::new(-rhs.y * *self, rhs.x * *self)
}
}
pub(crate) trait WDot<Rhs>: Sized {
type Result;
fn gdot(&self, rhs: Rhs) -> Self::Result;
}
impl WDot<Vector3<f32>> for Vector3<f32> {
type Result = f32;
fn gdot(&self, rhs: Vector3<f32>) -> Self::Result {
self.x * rhs.x + self.y * rhs.y + self.z * rhs.z
}
}
impl WDot<Vector2<f32>> for Vector2<f32> {
type Result = f32;
fn gdot(&self, rhs: Vector2<f32>) -> Self::Result {
self.x * rhs.x + self.y * rhs.y
}
}
impl WDot<f32> for f32 {
type Result = f32;
fn gdot(&self, rhs: f32) -> Self::Result {
*self * rhs
}
}
impl WCrossMatrix for Vector3<SimdFloat> {
type CrossMat = Matrix3<SimdFloat>;
#[inline]
#[rustfmt::skip]
fn gcross_matrix(self) -> Self::CrossMat {
Matrix3::new(
SimdFloat::zero(), -self.z, self.y,
self.z, SimdFloat::zero(), -self.x,
-self.y, self.x, SimdFloat::zero(),
)
}
}
impl WCrossMatrix for Vector2<SimdFloat> {
type CrossMat = Vector2<SimdFloat>;
#[inline]
fn gcross_matrix(self) -> Self::CrossMat {
Vector2::new(-self.y, self.x)
}
}
impl WCross<Vector3<SimdFloat>> for Vector3<SimdFloat> {
type Result = Vector3<SimdFloat>;
fn gcross(&self, rhs: Self) -> Self::Result {
self.cross(&rhs)
}
}
impl WCross<Vector2<SimdFloat>> for SimdFloat {
type Result = Vector2<SimdFloat>;
fn gcross(&self, rhs: Vector2<SimdFloat>) -> Self::Result {
Vector2::new(-rhs.y * *self, rhs.x * *self)
}
}
impl WCross<Vector2<SimdFloat>> for Vector2<SimdFloat> {
type Result = SimdFloat;
fn gcross(&self, rhs: Self) -> Self::Result {
let yx = Vector2::new(rhs.y, rhs.x);
let prod = self.component_mul(&yx);
prod.x - prod.y
}
}
impl WDot<Vector3<SimdFloat>> for Vector3<SimdFloat> {
type Result = SimdFloat;
fn gdot(&self, rhs: Vector3<SimdFloat>) -> Self::Result {
self.x * rhs.x + self.y * rhs.y + self.z * rhs.z
}
}
impl WDot<Vector2<SimdFloat>> for Vector2<SimdFloat> {
type Result = SimdFloat;
fn gdot(&self, rhs: Vector2<SimdFloat>) -> Self::Result {
self.x * rhs.x + self.y * rhs.y
}
}
impl WDot<SimdFloat> for SimdFloat {
type Result = SimdFloat;
fn gdot(&self, rhs: SimdFloat) -> Self::Result {
*self * rhs
}
}
pub(crate) trait WAngularInertia<N> {
type AngVector;
type LinVector;
type AngMatrix;
fn inverse(&self) -> Self;
fn transform_lin_vector(&self, pt: Self::LinVector) -> Self::LinVector;
fn transform_vector(&self, pt: Self::AngVector) -> Self::AngVector;
fn squared(&self) -> Self;
fn transform_matrix(&self, mat: &Self::AngMatrix) -> Self::AngMatrix;
fn into_matrix(self) -> Self::AngMatrix;
}
impl WAngularInertia<f32> for f32 {
type AngVector = f32;
type LinVector = Vector2<f32>;
type AngMatrix = f32;
fn inverse(&self) -> Self {
if *self != 0.0 {
1.0 / *self
} else {
0.0
}
}
fn transform_lin_vector(&self, pt: Vector2<f32>) -> Vector2<f32> {
*self * pt
}
fn transform_vector(&self, pt: f32) -> f32 {
*self * pt
}
fn squared(&self) -> f32 {
*self * *self
}
fn transform_matrix(&self, mat: &Self::AngMatrix) -> Self::AngMatrix {
mat * *self
}
fn into_matrix(self) -> Self::AngMatrix {
self
}
}
impl WAngularInertia<SimdFloat> for SimdFloat {
type AngVector = SimdFloat;
type LinVector = Vector2<SimdFloat>;
type AngMatrix = SimdFloat;
fn inverse(&self) -> Self {
let zero = <SimdFloat>::zero();
let is_zero = self.simd_eq(zero);
(<SimdFloat>::one() / *self).select(is_zero, zero)
}
fn transform_lin_vector(&self, pt: Vector2<SimdFloat>) -> Vector2<SimdFloat> {
pt * *self
}
fn transform_vector(&self, pt: SimdFloat) -> SimdFloat {
*self * pt
}
fn squared(&self) -> SimdFloat {
*self * *self
}
fn transform_matrix(&self, mat: &Self::AngMatrix) -> Self::AngMatrix {
*mat * *self
}
fn into_matrix(self) -> Self::AngMatrix {
self
}
}
#[derive(Copy, Clone, Debug, PartialEq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct SdpMatrix2<N> {
pub m11: N,
pub m12: N,
pub m22: N,
}
impl<N: SimdRealField> SdpMatrix2<N> {
pub fn new(m11: N, m12: N, m22: N) -> Self {
Self { m11, m12, m22 }
}
pub fn from_sdp_matrix(mat: na::Matrix2<N>) -> Self {
Self {
m11: mat.m11,
m12: mat.m12,
m22: mat.m22,
}
}
pub fn zero() -> Self {
Self {
m11: N::zero(),
m12: N::zero(),
m22: N::zero(),
}
}
pub fn diagonal(val: N) -> Self {
Self {
m11: val,
m12: N::zero(),
m22: val,
}
}
pub fn add_diagonal(&mut self, elt: N) -> Self {
Self {
m11: self.m11 + elt,
m12: self.m12,
m22: self.m22 + elt,
}
}
pub fn inverse_unchecked(&self) -> Self {
let determinant = self.m11 * self.m22 - self.m12 * self.m12;
let m11 = self.m22 / determinant;
let m12 = -self.m12 / determinant;
let m22 = self.m11 / determinant;
Self { m11, m12, m22 }
}
pub fn into_matrix(self) -> Matrix2<N> {
Matrix2::new(self.m11, self.m12, self.m12, self.m22)
}
}
impl<N: SimdRealField> Add<SdpMatrix2<N>> for SdpMatrix2<N> {
type Output = Self;
fn add(self, rhs: SdpMatrix2<N>) -> Self {
Self::new(self.m11 + rhs.m11, self.m12 + rhs.m12, self.m22 + rhs.m22)
}
}
impl<N: SimdRealField> Mul<Vector2<N>> for SdpMatrix2<N> {
type Output = Vector2<N>;
fn mul(self, rhs: Vector2<N>) -> Self::Output {
Vector2::new(
self.m11 * rhs.x + self.m12 * rhs.y,
self.m12 * rhs.x + self.m22 * rhs.y,
)
}
}
#[derive(Copy, Clone, Debug, PartialEq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct SdpMatrix3<N> {
pub m11: N,
pub m12: N,
pub m13: N,
pub m22: N,
pub m23: N,
pub m33: N,
}
impl<N: SimdRealField> SdpMatrix3<N> {
pub fn new(m11: N, m12: N, m13: N, m22: N, m23: N, m33: N) -> Self {
Self {
m11,
m12,
m13,
m22,
m23,
m33,
}
}
pub fn from_sdp_matrix(mat: na::Matrix3<N>) -> Self {
Self {
m11: mat.m11,
m12: mat.m12,
m13: mat.m13,
m22: mat.m22,
m23: mat.m23,
m33: mat.m33,
}
}
pub fn zero() -> Self {
Self {
m11: N::zero(),
m12: N::zero(),
m13: N::zero(),
m22: N::zero(),
m23: N::zero(),
m33: N::zero(),
}
}
pub fn diagonal(val: N) -> Self {
Self {
m11: val,
m12: N::zero(),
m13: N::zero(),
m22: val,
m23: N::zero(),
m33: val,
}
}
pub fn is_zero(&self) -> bool {
self.m11.is_zero()
&& self.m12.is_zero()
&& self.m13.is_zero()
&& self.m22.is_zero()
&& self.m23.is_zero()
&& self.m33.is_zero()
}
pub fn inverse_unchecked(&self) -> Self {
let minor_m12_m23 = self.m22 * self.m33 - self.m23 * self.m23;
let minor_m11_m23 = self.m12 * self.m33 - self.m13 * self.m23;
let minor_m11_m22 = self.m12 * self.m23 - self.m13 * self.m22;
let determinant =
self.m11 * minor_m12_m23 - self.m12 * minor_m11_m23 + self.m13 * minor_m11_m22;
let inv_det = N::one() / determinant;
SdpMatrix3 {
m11: minor_m12_m23 * inv_det,
m12: -minor_m11_m23 * inv_det,
m13: minor_m11_m22 * inv_det,
m22: (self.m11 * self.m33 - self.m13 * self.m13) * inv_det,
m23: (self.m13 * self.m12 - self.m23 * self.m11) * inv_det,
m33: (self.m11 * self.m22 - self.m12 * self.m12) * inv_det,
}
}
pub fn quadform3x2(&self, m: &Matrix3x2<N>) -> SdpMatrix2<N> {
let x0 = self.m11 * m.m11 + self.m12 * m.m21 + self.m13 * m.m31;
let y0 = self.m12 * m.m11 + self.m22 * m.m21 + self.m23 * m.m31;
let z0 = self.m13 * m.m11 + self.m23 * m.m21 + self.m33 * m.m31;
let x1 = self.m11 * m.m12 + self.m12 * m.m22 + self.m13 * m.m32;
let y1 = self.m12 * m.m12 + self.m22 * m.m22 + self.m23 * m.m32;
let z1 = self.m13 * m.m12 + self.m23 * m.m22 + self.m33 * m.m32;
let m11 = m.m11 * x0 + m.m21 * y0 + m.m31 * z0;
let m12 = m.m11 * x1 + m.m21 * y1 + m.m31 * z1;
let m22 = m.m12 * x1 + m.m22 * y1 + m.m32 * z1;
SdpMatrix2 { m11, m12, m22 }
}
pub fn quadform(&self, m: &Matrix3<N>) -> Self {
let x0 = self.m11 * m.m11 + self.m12 * m.m21 + self.m13 * m.m31;
let y0 = self.m12 * m.m11 + self.m22 * m.m21 + self.m23 * m.m31;
let z0 = self.m13 * m.m11 + self.m23 * m.m21 + self.m33 * m.m31;
let x1 = self.m11 * m.m12 + self.m12 * m.m22 + self.m13 * m.m32;
let y1 = self.m12 * m.m12 + self.m22 * m.m22 + self.m23 * m.m32;
let z1 = self.m13 * m.m12 + self.m23 * m.m22 + self.m33 * m.m32;
let x2 = self.m11 * m.m13 + self.m12 * m.m23 + self.m13 * m.m33;
let y2 = self.m12 * m.m13 + self.m22 * m.m23 + self.m23 * m.m33;
let z2 = self.m13 * m.m13 + self.m23 * m.m23 + self.m33 * m.m33;
let m11 = m.m11 * x0 + m.m21 * y0 + m.m31 * z0;
let m12 = m.m11 * x1 + m.m21 * y1 + m.m31 * z1;
let m13 = m.m11 * x2 + m.m21 * y2 + m.m31 * z2;
let m22 = m.m12 * x1 + m.m22 * y1 + m.m32 * z1;
let m23 = m.m12 * x2 + m.m22 * y2 + m.m32 * z2;
let m33 = m.m13 * x2 + m.m23 * y2 + m.m33 * z2;
Self {
m11,
m12,
m13,
m22,
m23,
m33,
}
}
pub fn add_diagonal(&self, elt: N) -> Self {
Self {
m11: self.m11 + elt,
m12: self.m12,
m13: self.m13,
m22: self.m22 + elt,
m23: self.m23,
m33: self.m33 + elt,
}
}
}
impl<N: Add<N>> Add<SdpMatrix3<N>> for SdpMatrix3<N> {
type Output = SdpMatrix3<N::Output>;
fn add(self, rhs: SdpMatrix3<N>) -> Self::Output {
SdpMatrix3 {
m11: self.m11 + rhs.m11,
m12: self.m12 + rhs.m12,
m13: self.m13 + rhs.m13,
m22: self.m22 + rhs.m22,
m23: self.m23 + rhs.m23,
m33: self.m33 + rhs.m33,
}
}
}
impl<N: SimdRealField> Mul<Vector3<N>> for SdpMatrix3<N> {
type Output = Vector3<N>;
fn mul(self, rhs: Vector3<N>) -> Self::Output {
let x = self.m11 * rhs.x + self.m12 * rhs.y + self.m13 * rhs.z;
let y = self.m12 * rhs.x + self.m22 * rhs.y + self.m23 * rhs.z;
let z = self.m13 * rhs.x + self.m23 * rhs.y + self.m33 * rhs.z;
Vector3::new(x, y, z)
}
}
impl<N: SimdRealField> Mul<Matrix3<N>> for SdpMatrix3<N> {
type Output = Matrix3<N>;
fn mul(self, rhs: Matrix3<N>) -> Self::Output {
let x0 = self.m11 * rhs.m11 + self.m12 * rhs.m21 + self.m13 * rhs.m31;
let y0 = self.m12 * rhs.m11 + self.m22 * rhs.m21 + self.m23 * rhs.m31;
let z0 = self.m13 * rhs.m11 + self.m23 * rhs.m21 + self.m33 * rhs.m31;
let x1 = self.m11 * rhs.m12 + self.m12 * rhs.m22 + self.m13 * rhs.m32;
let y1 = self.m12 * rhs.m12 + self.m22 * rhs.m22 + self.m23 * rhs.m32;
let z1 = self.m13 * rhs.m12 + self.m23 * rhs.m22 + self.m33 * rhs.m32;
let x2 = self.m11 * rhs.m13 + self.m12 * rhs.m23 + self.m13 * rhs.m33;
let y2 = self.m12 * rhs.m13 + self.m22 * rhs.m23 + self.m23 * rhs.m33;
let z2 = self.m13 * rhs.m13 + self.m23 * rhs.m23 + self.m33 * rhs.m33;
Matrix3::new(x0, x1, x2, y0, y1, y2, z0, z1, z2)
}
}
impl<N: SimdRealField> Mul<Matrix3x2<N>> for SdpMatrix3<N> {
type Output = Matrix3x2<N>;
fn mul(self, rhs: Matrix3x2<N>) -> Self::Output {
let x0 = self.m11 * rhs.m11 + self.m12 * rhs.m21 + self.m13 * rhs.m31;
let y0 = self.m12 * rhs.m11 + self.m22 * rhs.m21 + self.m23 * rhs.m31;
let z0 = self.m13 * rhs.m11 + self.m23 * rhs.m21 + self.m33 * rhs.m31;
let x1 = self.m11 * rhs.m12 + self.m12 * rhs.m22 + self.m13 * rhs.m32;
let y1 = self.m12 * rhs.m12 + self.m22 * rhs.m22 + self.m23 * rhs.m32;
let z1 = self.m13 * rhs.m12 + self.m23 * rhs.m22 + self.m33 * rhs.m32;
Matrix3x2::new(x0, x1, y0, y1, z0, z1)
}
}
impl WAngularInertia<f32> for SdpMatrix3<f32> {
type AngVector = Vector3<f32>;
type LinVector = Vector3<f32>;
type AngMatrix = Matrix3<f32>;
fn inverse(&self) -> Self {
let minor_m12_m23 = self.m22 * self.m33 - self.m23 * self.m23;
let minor_m11_m23 = self.m12 * self.m33 - self.m13 * self.m23;
let minor_m11_m22 = self.m12 * self.m23 - self.m13 * self.m22;
let determinant =
self.m11 * minor_m12_m23 - self.m12 * minor_m11_m23 + self.m13 * minor_m11_m22;
if determinant.is_zero() {
Self::zero()
} else {
SdpMatrix3 {
m11: minor_m12_m23 / determinant,
m12: -minor_m11_m23 / determinant,
m13: minor_m11_m22 / determinant,
m22: (self.m11 * self.m33 - self.m13 * self.m13) / determinant,
m23: (self.m13 * self.m12 - self.m23 * self.m11) / determinant,
m33: (self.m11 * self.m22 - self.m12 * self.m12) / determinant,
}
}
}
fn squared(&self) -> Self {
SdpMatrix3 {
m11: self.m11 * self.m11 + self.m12 * self.m12 + self.m13 * self.m13,
m12: self.m11 * self.m12 + self.m12 * self.m22 + self.m13 * self.m23,
m13: self.m11 * self.m13 + self.m12 * self.m23 + self.m13 * self.m33,
m22: self.m12 * self.m12 + self.m22 * self.m22 + self.m23 * self.m23,
m23: self.m12 * self.m13 + self.m22 * self.m23 + self.m23 * self.m33,
m33: self.m13 * self.m13 + self.m23 * self.m23 + self.m33 * self.m33,
}
}
fn transform_lin_vector(&self, v: Vector3<f32>) -> Vector3<f32> {
self.transform_vector(v)
}
fn transform_vector(&self, v: Vector3<f32>) -> Vector3<f32> {
let x = self.m11 * v.x + self.m12 * v.y + self.m13 * v.z;
let y = self.m12 * v.x + self.m22 * v.y + self.m23 * v.z;
let z = self.m13 * v.x + self.m23 * v.y + self.m33 * v.z;
Vector3::new(x, y, z)
}
#[rustfmt::skip]
fn into_matrix(self) -> Matrix3<f32> {
Matrix3::new(
self.m11, self.m12, self.m13,
self.m12, self.m22, self.m23,
self.m13, self.m23, self.m33,
)
}
#[rustfmt::skip]
fn transform_matrix(&self, m: &Matrix3<f32>) -> Matrix3<f32> {
*self * *m
}
}
impl WAngularInertia<SimdFloat> for SdpMatrix3<SimdFloat> {
type AngVector = Vector3<SimdFloat>;
type LinVector = Vector3<SimdFloat>;
type AngMatrix = Matrix3<SimdFloat>;
fn inverse(&self) -> Self {
let minor_m12_m23 = self.m22 * self.m33 - self.m23 * self.m23;
let minor_m11_m23 = self.m12 * self.m33 - self.m13 * self.m23;
let minor_m11_m22 = self.m12 * self.m23 - self.m13 * self.m22;
let determinant =
self.m11 * minor_m12_m23 - self.m12 * minor_m11_m23 + self.m13 * minor_m11_m22;
let zero = <SimdFloat>::zero();
let is_zero = determinant.simd_eq(zero);
let inv_det = (<SimdFloat>::one() / determinant).select(is_zero, zero);
SdpMatrix3 {
m11: minor_m12_m23 * inv_det,
m12: -minor_m11_m23 * inv_det,
m13: minor_m11_m22 * inv_det,
m22: (self.m11 * self.m33 - self.m13 * self.m13) * inv_det,
m23: (self.m13 * self.m12 - self.m23 * self.m11) * inv_det,
m33: (self.m11 * self.m22 - self.m12 * self.m12) * inv_det,
}
}
fn transform_lin_vector(&self, v: Vector3<SimdFloat>) -> Vector3<SimdFloat> {
self.transform_vector(v)
}
fn transform_vector(&self, v: Vector3<SimdFloat>) -> Vector3<SimdFloat> {
let x = self.m11 * v.x + self.m12 * v.y + self.m13 * v.z;
let y = self.m12 * v.x + self.m22 * v.y + self.m23 * v.z;
let z = self.m13 * v.x + self.m23 * v.y + self.m33 * v.z;
Vector3::new(x, y, z)
}
fn squared(&self) -> Self {
SdpMatrix3 {
m11: self.m11 * self.m11 + self.m12 * self.m12 + self.m13 * self.m13,
m12: self.m11 * self.m12 + self.m12 * self.m22 + self.m13 * self.m23,
m13: self.m11 * self.m13 + self.m12 * self.m23 + self.m13 * self.m33,
m22: self.m12 * self.m12 + self.m22 * self.m22 + self.m23 * self.m23,
m23: self.m12 * self.m13 + self.m22 * self.m23 + self.m23 * self.m33,
m33: self.m13 * self.m13 + self.m23 * self.m23 + self.m33 * self.m33,
}
}
#[rustfmt::skip]
fn into_matrix(self) -> Matrix3<SimdFloat> {
Matrix3::new(
self.m11, self.m12, self.m13,
self.m12, self.m22, self.m23,
self.m13, self.m23, self.m33,
)
}
#[rustfmt::skip]
fn transform_matrix(&self, m: &Matrix3<SimdFloat>) -> Matrix3<SimdFloat> {
let x0 = self.m11 * m.m11 + self.m12 * m.m21 + self.m13 * m.m31;
let y0 = self.m12 * m.m11 + self.m22 * m.m21 + self.m23 * m.m31;
let z0 = self.m13 * m.m11 + self.m23 * m.m21 + self.m33 * m.m31;
let x1 = self.m11 * m.m12 + self.m12 * m.m22 + self.m13 * m.m32;
let y1 = self.m12 * m.m12 + self.m22 * m.m22 + self.m23 * m.m32;
let z1 = self.m13 * m.m12 + self.m23 * m.m22 + self.m33 * m.m32;
let x2 = self.m11 * m.m13 + self.m12 * m.m23 + self.m13 * m.m33;
let y2 = self.m12 * m.m13 + self.m22 * m.m23 + self.m23 * m.m33;
let z2 = self.m13 * m.m13 + self.m23 * m.m23 + self.m33 * m.m33;
Matrix3::new(
x0, x1, x2,
y0, y1, y2,
z0, z1, z2,
)
}
}
impl<T> From<[SdpMatrix3<f32>; 4]> for SdpMatrix3<T>
where
T: From<[f32; 4]>,
{
fn from(data: [SdpMatrix3<f32>; 4]) -> Self {
SdpMatrix3 {
m11: T::from([data[0].m11, data[1].m11, data[2].m11, data[3].m11]),
m12: T::from([data[0].m12, data[1].m12, data[2].m12, data[3].m12]),
m13: T::from([data[0].m13, data[1].m13, data[2].m13, data[3].m13]),
m22: T::from([data[0].m22, data[1].m22, data[2].m22, data[3].m22]),
m23: T::from([data[0].m23, data[1].m23, data[2].m23, data[3].m23]),
m33: T::from([data[0].m33, data[1].m33, data[2].m33, data[3].m33]),
}
}
}
#[cfg(feature = "simd-nightly")]
impl From<[SdpMatrix3<f32>; 8]> for SdpMatrix3<simba::simd::f32x8> {
fn from(data: [SdpMatrix3<f32>; 8]) -> Self {
SdpMatrix3 {
m11: simba::simd::f32x8::from([
data[0].m11,
data[1].m11,
data[2].m11,
data[3].m11,
data[4].m11,
data[5].m11,
data[6].m11,
data[7].m11,
]),
m12: simba::simd::f32x8::from([
data[0].m12,
data[1].m12,
data[2].m12,
data[3].m12,
data[4].m12,
data[5].m12,
data[6].m12,
data[7].m12,
]),
m13: simba::simd::f32x8::from([
data[0].m13,
data[1].m13,
data[2].m13,
data[3].m13,
data[4].m13,
data[5].m13,
data[6].m13,
data[7].m13,
]),
m22: simba::simd::f32x8::from([
data[0].m22,
data[1].m22,
data[2].m22,
data[3].m22,
data[4].m22,
data[5].m22,
data[6].m22,
data[7].m22,
]),
m23: simba::simd::f32x8::from([
data[0].m23,
data[1].m23,
data[2].m23,
data[3].m23,
data[4].m23,
data[5].m23,
data[6].m23,
data[7].m23,
]),
m33: simba::simd::f32x8::from([
data[0].m33,
data[1].m33,
data[2].m33,
data[3].m33,
data[4].m33,
data[5].m33,
data[6].m33,
data[7].m33,
]),
}
}
}
#[cfg(feature = "simd-nightly")]
impl From<[SdpMatrix3<f32>; 16]> for SdpMatrix3<simba::simd::f32x16> {
fn from(data: [SdpMatrix3<f32>; 16]) -> Self {
SdpMatrix3 {
m11: simba::simd::f32x16::from([
data[0].m11,
data[1].m11,
data[2].m11,
data[3].m11,
data[4].m11,
data[5].m11,
data[6].m11,
data[7].m11,
data[8].m11,
data[9].m11,
data[10].m11,
data[11].m11,
data[12].m11,
data[13].m11,
data[14].m11,
data[15].m11,
]),
m12: simba::simd::f32x16::from([
data[0].m12,
data[1].m12,
data[2].m12,
data[3].m12,
data[4].m12,
data[5].m12,
data[6].m12,
data[7].m12,
data[8].m12,
data[9].m12,
data[10].m12,
data[11].m12,
data[12].m12,
data[13].m12,
data[14].m12,
data[15].m12,
]),
m13: simba::simd::f32x16::from([
data[0].m13,
data[1].m13,
data[2].m13,
data[3].m13,
data[4].m13,
data[5].m13,
data[6].m13,
data[7].m13,
data[8].m13,
data[9].m13,
data[10].m13,
data[11].m13,
data[12].m13,
data[13].m13,
data[14].m13,
data[15].m13,
]),
m22: simba::simd::f32x16::from([
data[0].m22,
data[1].m22,
data[2].m22,
data[3].m22,
data[4].m22,
data[5].m22,
data[6].m22,
data[7].m22,
data[8].m22,
data[9].m22,
data[10].m22,
data[11].m22,
data[12].m22,
data[13].m22,
data[14].m22,
data[15].m22,
]),
m23: simba::simd::f32x16::from([
data[0].m23,
data[1].m23,
data[2].m23,
data[3].m23,
data[4].m23,
data[5].m23,
data[6].m23,
data[7].m23,
data[8].m23,
data[9].m23,
data[10].m23,
data[11].m23,
data[12].m23,
data[13].m23,
data[14].m23,
data[15].m23,
]),
m33: simba::simd::f32x16::from([
data[0].m33,
data[1].m33,
data[2].m33,
data[3].m33,
data[4].m33,
data[5].m33,
data[6].m33,
data[7].m33,
data[8].m33,
data[9].m33,
data[10].m33,
data[11].m33,
data[12].m33,
data[13].m33,
data[14].m33,
data[15].m33,
]),
}
}
}
#[derive(Clone, Debug, PartialEq, Eq)]
pub(crate) struct FlushToZeroDenormalsAreZeroFlags {
original_flags: u32,
}
impl FlushToZeroDenormalsAreZeroFlags {
#[cfg(not(all(
not(feature = "enhanced-determinism"),
any(target_arch = "x86_64", target_arch = "x86"),
target_feature = "sse"
)))]
pub fn flush_denormal_to_zero() -> Self {
Self { original_flags: 0 }
}
#[cfg(all(
not(feature = "enhanced-determinism"),
any(target_arch = "x86", target_arch = "x86_64"),
target_feature = "sse"
))]
pub fn flush_denormal_to_zero() -> Self {
unsafe {
#[cfg(target_arch = "x86")]
use std::arch::x86::{_mm_getcsr, _mm_setcsr, _MM_FLUSH_ZERO_ON};
#[cfg(target_arch = "x86_64")]
use std::arch::x86_64::{_mm_getcsr, _mm_setcsr, _MM_FLUSH_ZERO_ON};
let original_flags = _mm_getcsr();
_mm_setcsr(original_flags | _MM_FLUSH_ZERO_ON | (1 << 6));
Self { original_flags }
}
}
}
#[cfg(all(
not(feature = "enhanced-determinism"),
any(target_arch = "x86", target_arch = "x86_64"),
target_feature = "sse"
))]
impl Drop for FlushToZeroDenormalsAreZeroFlags {
fn drop(&mut self) {
#[cfg(target_arch = "x86")]
unsafe {
std::arch::x86::_mm_setcsr(self.original_flags)
}
#[cfg(target_arch = "x86_64")]
unsafe {
std::arch::x86_64::_mm_setcsr(self.original_flags)
}
}
}
#[cfg(feature = "serde-serialize")]
pub(crate) fn serialize_hashmap_capacity<S: serde::Serializer, K, V, H: std::hash::BuildHasher>(
map: &HashMap<K, V, H>,
s: S,
) -> Result<S::Ok, S::Error> {
s.serialize_u64(map.capacity() as u64)
}
#[cfg(feature = "serde-serialize")]
pub(crate) fn deserialize_hashmap_capacity<
'de,
D: serde::Deserializer<'de>,
K,
V,
H: std::hash::BuildHasher + Default,
>(
d: D,
) -> Result<HashMap<K, V, H>, D::Error> {
struct CapacityVisitor;
impl<'de> serde::de::Visitor<'de> for CapacityVisitor {
type Value = u64;
fn expecting(&self, formatter: &mut std::fmt::Formatter) -> std::fmt::Result {
write!(formatter, "an integer between 0 and 2^64")
}
fn visit_u64<E: serde::de::Error>(self, val: u64) -> Result<Self::Value, E> {
Ok(val)
}
}
let capacity = d.deserialize_u64(CapacityVisitor)? as usize;
Ok(HashMap::with_capacity_and_hasher(
capacity,
Default::default(),
))
}
#[cfg(feature = "enhanced-determinism")]
pub(crate) type FxHashMap32<K, V> =
indexmap::IndexMap<K, V, std::hash::BuildHasherDefault<FxHasher32>>;
const K: u32 = 0x9e3779b9;
pub(crate) struct FxHasher32 {
hash: u32,
}
impl Default for FxHasher32 {
#[inline]
fn default() -> FxHasher32 {
FxHasher32 { hash: 0 }
}
}
impl FxHasher32 {
#[inline]
fn add_to_hash(&mut self, i: u32) {
use std::ops::BitXor;
self.hash = self.hash.rotate_left(5).bitxor(i).wrapping_mul(K);
}
}
impl std::hash::Hasher for FxHasher32 {
#[inline]
fn write(&mut self, mut bytes: &[u8]) {
use std::convert::TryInto;
let read_u32 = |bytes: &[u8]| u32::from_ne_bytes(bytes[..4].try_into().unwrap());
let mut hash = FxHasher32 { hash: self.hash };
assert!(std::mem::size_of::<u32>() <= 8);
while bytes.len() >= std::mem::size_of::<u32>() {
hash.add_to_hash(read_u32(bytes) as u32);
bytes = &bytes[std::mem::size_of::<u32>()..];
}
if (std::mem::size_of::<u32>() > 4) && (bytes.len() >= 4) {
hash.add_to_hash(u32::from_ne_bytes(bytes[..4].try_into().unwrap()) as u32);
bytes = &bytes[4..];
}
if (std::mem::size_of::<u32>() > 2) && bytes.len() >= 2 {
hash.add_to_hash(u16::from_ne_bytes(bytes[..2].try_into().unwrap()) as u32);
bytes = &bytes[2..];
}
if (std::mem::size_of::<u32>() > 1) && bytes.len() >= 1 {
hash.add_to_hash(bytes[0] as u32);
}
self.hash = hash.hash;
}
#[inline]
fn write_u8(&mut self, i: u8) {
self.add_to_hash(i as u32);
}
#[inline]
fn write_u16(&mut self, i: u16) {
self.add_to_hash(i as u32);
}
#[inline]
fn write_u32(&mut self, i: u32) {
self.add_to_hash(i as u32);
}
#[inline]
fn write_u64(&mut self, i: u64) {
self.add_to_hash(i as u32);
self.add_to_hash((i >> 32) as u32);
}
#[inline]
fn write_usize(&mut self, i: usize) {
self.add_to_hash(i as u32);
}
#[inline]
fn finish(&self) -> u64 {
self.hash as u64
}
}
pub(crate) fn other_handle(
pair: (RigidBodyHandle, RigidBodyHandle),
handle: RigidBodyHandle,
) -> RigidBodyHandle {
if pair.0 == handle {
pair.1
} else {
pair.0
}
}