aboutsummaryrefslogtreecommitdiff
path: root/src/utils.rs
diff options
context:
space:
mode:
Diffstat (limited to 'src/utils.rs')
-rw-r--r--src/utils.rs534
1 files changed, 117 insertions, 417 deletions
diff --git a/src/utils.rs b/src/utils.rs
index 6521b0f..e443130 100644
--- a/src/utils.rs
+++ b/src/utils.rs
@@ -1,26 +1,79 @@
//! Miscellaneous utilities.
-use na::{
- Matrix1, Matrix2, Matrix3, Point2, Point3, RowVector2, Scalar, SimdRealField, UnitComplex,
- UnitQuaternion, Vector1, Vector2, Vector3,
-};
+// FIXME: move to linalg/traits
+
+use na::SimdRealField;
use num::Zero;
use simba::simd::SimdValue;
-use std::ops::IndexMut;
+use std::fmt::Debug;
+use std::ops::{Add, AddAssign, Index, IndexMut, Mul, MulAssign, Sub, SubAssign};
use parry::utils::SdpMatrix3;
-use {
- crate::math::{Real, SimdReal},
- na::SimdPartialOrd,
- num::One,
-};
+use simba::scalar::{ClosedAdd, ClosedSub};
+use {crate::math::*, na::SimdPartialOrd, num::One};
/// The trait for real numbers used by Rapier.
///
/// This includes `f32`, `f64` and their related SIMD types.
-pub trait SimdRealCopy: SimdRealField<Element = Real> + Copy {}
-impl SimdRealCopy for Real {}
-impl SimdRealCopy for SimdReal {}
+pub trait SimdRealCopy: SimdRealField<Element = Real> + Copy + Default {
+ type Vector: Copy
+ + Debug
+ + Default
+ + SimdSign<Self::Vector>
+ + SimdBasis
+ + SimdVec
+ + SimdCrossMatrix
+ + SimdCross<Self::Vector, Result = Self::AngVector>
+ + SimdDot<Self::Vector, Result = Self>
+ + ClosedAdd
+ + ClosedSub
+ + Mul<Self, Output = Self::Vector>
+ + MulAssign<Self>;
+ #[cfg(feature = "dim3")]
+ type Vector2: Copy
+ + Default
+ + Debug
+ + From<[Self; 2]>
+ + ClosedSub
+ + SimdCapMagnitude<Self>
+ + Index<usize, Output = Self>;
+ type AngVector: Copy
+ + Default
+ + Debug
+ + SimdDot<Self::AngVector, Result = Self>
+ + Add<Self::AngVector, Output = Self::AngVector>
+ + Sub<Self::AngVector, Output = Self::AngVector>
+ + AddAssign<Self::AngVector>
+ + SubAssign<Self::AngVector>
+ + Mul<Self, Output = Self::AngVector>
+ + MulAssign<Self>;
+ type Point: Copy + Default + Debug + SimdVec;
+ type Isometry: Copy + IsometryOps<Self::Vector, Self::Rotation>;
+ type Matrix: Copy;
+ type Rotation: Copy + SimdQuat<Self>;
+}
+
+impl SimdRealCopy for Real {
+ type Vector = Vector;
+ #[cfg(feature = "dim3")]
+ type Vector2 = Vector2;
+ type AngVector = AngVector;
+ type Point = Point;
+ type Isometry = Isometry;
+ type Matrix = Matrix;
+ type Rotation = Rotation;
+}
+
+impl SimdRealCopy for SimdReal {
+ type Vector = SimdVector;
+ #[cfg(feature = "dim3")]
+ type Vector2 = SimdVector2;
+ type AngVector = SimdAngVector;
+ type Point = SimdPoint;
+ type Isometry = SimdIsometry;
+ type Matrix = SimdMatrix;
+ type Rotation = SimdRotation;
+}
const INV_EPSILON: Real = 1.0e-20;
@@ -52,44 +105,6 @@ impl SimdSign<Real> for Real {
}
}
-impl<N: Scalar + Copy + SimdSign<N>> SimdSign<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 + SimdSign<N>> SimdSign<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 + SimdSign<N>> SimdSign<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 + SimdSign<N>> SimdSign<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 SimdSign<SimdReal> for SimdReal {
- fn copy_sign_to(self, to: SimdReal) -> SimdReal {
- to.simd_copysign(self)
- }
-}
-
pub(crate) trait SimdComponent: Sized {
type Element;
@@ -97,28 +112,6 @@ pub(crate) trait SimdComponent: Sized {
fn max_component(self) -> Self::Element;
}
-impl SimdComponent for Real {
- type Element = Real;
-
- fn min_component(self) -> Self::Element {
- self
- }
- fn max_component(self) -> Self::Element {
- self
- }
-}
-
-impl SimdComponent for SimdReal {
- type Element = Real;
-
- fn min_component(self) -> Self::Element {
- self.simd_horizontal_min()
- }
- fn max_component(self) -> Self::Element {
- self.simd_horizontal_max()
- }
-}
-
/// Trait to compute the orthonormal basis of a vector.
pub trait SimdBasis: Sized {
/// The type of the array of orthonormal vectors.
@@ -129,124 +122,16 @@ pub trait SimdBasis: Sized {
fn orthonormal_vector(self) -> Self;
}
-impl<N: SimdRealCopy> SimdBasis for Vector2<N> {
- type Basis = [Vector2<N>; 1];
- fn orthonormal_basis(self) -> [Vector2<N>; 1] {
- [Vector2::new(-self.y, self.x)]
- }
- fn orthonormal_vector(self) -> Vector2<N> {
- Vector2::new(-self.y, self.x)
- }
-}
-
-impl<N: SimdRealCopy + SimdSign<N>> SimdBasis for Vector3<N> {
- type Basis = [Vector3<N>; 2];
- // Robust and branchless implementation from Pixar:
- // https://graphics.pixar.com/library/OrthonormalB/paper.pdf
- 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),
- ]
- }
-
- fn orthonormal_vector(self) -> Vector3<N> {
- 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(b, sign + self.y * self.y * a, -self.y)
- }
-}
-
pub(crate) trait SimdVec: Sized {
type Element;
fn horizontal_inf(&self) -> Self::Element;
fn horizontal_sup(&self) -> Self::Element;
+ fn component_mul_simd(&self, rhs: &Self) -> Self;
}
-impl<N: Scalar + Copy + SimdComponent> SimdVec 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 + SimdComponent> SimdVec 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 + SimdComponent> SimdVec 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 + SimdComponent> SimdVec 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 SimdCapMagnitude<N>: Sized {
+ fn simd_cap_magnitude(&self, limit: N) -> Self;
}
pub(crate) trait SimdCrossMatrix: Sized {
@@ -257,101 +142,13 @@ pub(crate) trait SimdCrossMatrix: Sized {
fn gcross_matrix_tr(self) -> Self::CrossMatTr;
}
-impl<N: SimdRealCopy> SimdCrossMatrix for Vector3<N> {
- type CrossMat = Matrix3<N>;
- type CrossMatTr = Matrix3<N>;
-
- #[inline]
- #[rustfmt::skip]
- fn gcross_matrix(self) -> Self::CrossMat {
- Matrix3::new(
- N::zero(), -self.z, self.y,
- self.z, N::zero(), -self.x,
- -self.y, self.x, N::zero(),
- )
- }
-
- #[inline]
- #[rustfmt::skip]
- fn gcross_matrix_tr(self) -> Self::CrossMatTr {
- Matrix3::new(
- N::zero(), self.z, -self.y,
- -self.z, N::zero(), self.x,
- self.y, -self.x, N::zero(),
- )
- }
-}
-
-impl<N: SimdRealCopy> SimdCrossMatrix for Vector2<N> {
- type CrossMat = RowVector2<N>;
- type CrossMatTr = Vector2<N>;
-
- #[inline]
- fn gcross_matrix(self) -> Self::CrossMat {
- RowVector2::new(-self.y, self.x)
- }
- #[inline]
- fn gcross_matrix_tr(self) -> Self::CrossMatTr {
- Vector2::new(-self.y, self.x)
- }
-}
-impl SimdCrossMatrix for Real {
- type CrossMat = Matrix2<Real>;
- type CrossMatTr = Matrix2<Real>;
-
- #[inline]
- fn gcross_matrix(self) -> Matrix2<Real> {
- Matrix2::new(0.0, -self, self, 0.0)
- }
-
- #[inline]
- fn gcross_matrix_tr(self) -> Matrix2<Real> {
- Matrix2::new(0.0, self, -self, 0.0)
- }
-}
-
-impl SimdCrossMatrix for SimdReal {
- type CrossMat = Matrix2<SimdReal>;
- type CrossMatTr = Matrix2<SimdReal>;
-
- #[inline]
- fn gcross_matrix(self) -> Matrix2<SimdReal> {
- Matrix2::new(SimdReal::zero(), -self, self, SimdReal::zero())
- }
-
- #[inline]
- fn gcross_matrix_tr(self) -> Matrix2<SimdReal> {
- Matrix2::new(SimdReal::zero(), self, -self, SimdReal::zero())
- }
-}
-
pub(crate) trait SimdCross<Rhs>: Sized {
type Result;
fn gcross(&self, rhs: Rhs) -> Self::Result;
-}
-
-impl SimdCross<Vector3<Real>> for Vector3<Real> {
- type Result = Self;
-
- fn gcross(&self, rhs: Vector3<Real>) -> Self::Result {
- self.cross(&rhs)
- }
-}
-
-impl SimdCross<Vector2<Real>> for Vector2<Real> {
- type Result = Real;
-
- fn gcross(&self, rhs: Vector2<Real>) -> Self::Result {
- self.x * rhs.y - self.y * rhs.x
- }
-}
-
-impl SimdCross<Vector2<Real>> for Real {
- type Result = Vector2<Real>;
-
- fn gcross(&self, rhs: Vector2<Real>) -> Self::Result {
- Vector2::new(-rhs.y * *self, rhs.x * *self)
- }
+ // This method is here only to get the correct generic return
+ // type for the 3D cross product when used in generic code.
+ #[cfg(feature = "dim3")]
+ fn cross_(&self, rhs: &Self) -> Self;
}
pub(crate) trait SimdDot<Rhs>: Sized {
@@ -359,72 +156,6 @@ pub(crate) trait SimdDot<Rhs>: Sized {
fn gdot(&self, rhs: Rhs) -> Self::Result;
}
-impl<N: SimdRealCopy> SimdDot<Vector3<N>> for Vector3<N> {
- type Result = N;
-
- fn gdot(&self, rhs: Vector3<N>) -> Self::Result {
- self.x * rhs.x + self.y * rhs.y + self.z * rhs.z
- }
-}
-
-impl<N: SimdRealCopy> SimdDot<Vector2<N>> for Vector2<N> {
- type Result = N;
-
- fn gdot(&self, rhs: Vector2<N>) -> Self::Result {
- self.x * rhs.x + self.y * rhs.y
- }
-}
-
-impl<N: SimdRealCopy> SimdDot<Vector1<N>> for N {
- type Result = N;
-
- fn gdot(&self, rhs: Vector1<N>) -> Self::Result {
- *self * rhs.x
- }
-}
-
-impl<N: SimdRealCopy> SimdDot<N> for N {
- type Result = N;
-
- fn gdot(&self, rhs: N) -> Self::Result {
- *self * rhs
- }
-}
-
-impl<N: SimdRealCopy> SimdDot<N> for Vector1<N> {
- type Result = N;
-
- fn gdot(&self, rhs: N) -> Self::Result {
- self.x * rhs
- }
-}
-
-impl SimdCross<Vector3<SimdReal>> for Vector3<SimdReal> {
- type Result = Vector3<SimdReal>;
-
- fn gcross(&self, rhs: Self) -> Self::Result {
- self.cross(&rhs)
- }
-}
-
-impl SimdCross<Vector2<SimdReal>> for SimdReal {
- type Result = Vector2<SimdReal>;
-
- fn gcross(&self, rhs: Vector2<SimdReal>) -> Self::Result {
- Vector2::new(-rhs.y * *self, rhs.x * *self)
- }
-}
-
-impl SimdCross<Vector2<SimdReal>> for Vector2<SimdReal> {
- type Result = SimdReal;
-
- 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
- }
-}
-
/// Trait implemented by quaternions.
pub trait SimdQuat<N> {
/// The result of quaternion differentiation.
@@ -434,33 +165,6 @@ pub trait SimdQuat<N> {
fn diff_conj1_2(&self, rhs: &Self) -> Self::Result;
}
-impl<N: SimdRealCopy> SimdQuat<N> for UnitComplex<N> {
- type Result = Matrix1<N>;
-
- fn diff_conj1_2(&self, rhs: &Self) -> Self::Result {
- let two: N = N::splat(2.0);
- Matrix1::new((self.im * rhs.im + self.re * rhs.re) * two)
- }
-}
-
-impl<N: SimdRealCopy> SimdQuat<N> for UnitQuaternion<N> {
- type Result = Matrix3<N>;
-
- fn diff_conj1_2(&self, rhs: &Self) -> Self::Result {
- let half = N::splat(0.5);
- let v1 = self.imag();
- let v2 = rhs.imag();
- let w1 = self.w;
- let w2 = rhs.w;
-
- // TODO: this can probably be optimized a lot by unrolling the ops.
- (v1 * v2.transpose() + Matrix3::from_diagonal_element(w1 * w2)
- - (v1 * w2 + v2 * w1).cross_matrix()
- + v1.cross_matrix() * v2.cross_matrix())
- * half
- }
-}
-
pub(crate) trait SimdAngularInertia<N> {
type AngVector;
type LinVector;
@@ -473,39 +177,11 @@ pub(crate) trait SimdAngularInertia<N> {
fn into_matrix(self) -> Self::AngMatrix;
}
-impl<N: SimdRealCopy> SimdAngularInertia<N> for N {
- type AngVector = N;
- type LinVector = Vector2<N>;
- type AngMatrix = N;
-
- fn inverse(&self) -> Self {
- simd_inv(*self)
- }
-
- fn transform_lin_vector(&self, pt: Vector2<N>) -> Vector2<N> {
- pt * *self
- }
- fn transform_vector(&self, pt: N) -> N {
- pt * *self
- }
-
- fn squared(&self) -> N {
- *self * *self
- }
-
- fn transform_matrix(&self, mat: &Self::AngMatrix) -> Self::AngMatrix {
- *mat * *self
- }
-
- fn into_matrix(self) -> Self::AngMatrix {
- self
- }
-}
-
+#[cfg(feature = "dim3")]
impl SimdAngularInertia<Real> for SdpMatrix3<Real> {
- type AngVector = Vector3<Real>;
- type LinVector = Vector3<Real>;
- type AngMatrix = Matrix3<Real>;
+ type AngVector = Vector;
+ type LinVector = Vector;
+ type AngMatrix = Matrix;
fn inverse(&self) -> Self {
let minor_m12_m23 = self.m22 * self.m33 - self.m23 * self.m23;
@@ -540,20 +216,20 @@ impl SimdAngularInertia<Real> for SdpMatrix3<Real> {
}
}
- fn transform_lin_vector(&self, v: Vector3<Real>) -> Vector3<Real> {
+ fn transform_lin_vector(&self, v: Vector) -> Vector {
self.transform_vector(v)
}
- fn transform_vector(&self, v: Vector3<Real>) -> Vector3<Real> {
+ fn transform_vector(&self, v: Vector) -> Vector {
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)
+ Vector::new(x, y, z)
}
#[rustfmt::skip]
- fn into_matrix(self) -> Matrix3<Real> {
- Matrix3::new(
+ fn into_matrix(self) -> Matrix {
+ Matrix::new(
self.m11, self.m12, self.m13,
self.m12, self.m22, self.m23,
self.m13, self.m23, self.m33,
@@ -561,15 +237,16 @@ impl SimdAngularInertia<Real> for SdpMatrix3<Real> {
}
#[rustfmt::skip]
- fn transform_matrix(&self, m: &Matrix3<Real>) -> Matrix3<Real> {
+ fn transform_matrix(&self, m: &Matrix) -> Matrix {
*self * *m
}
}
+#[cfg(feature = "dim3")]
impl SimdAngularInertia<SimdReal> for SdpMatrix3<SimdReal> {
- type AngVector = Vector3<SimdReal>;
- type LinVector = Vector3<SimdReal>;
- type AngMatrix = Matrix3<SimdReal>;
+ type AngVector = SimdVector;
+ type LinVector = SimdVector;
+ type AngMatrix = SimdMatrix;
fn inverse(&self) -> Self {
let minor_m12_m23 = self.m22 * self.m33 - self.m23 * self.m23;
@@ -593,15 +270,15 @@ impl SimdAngularInertia<SimdReal> for SdpMatrix3<SimdReal> {
}
}
- fn transform_lin_vector(&self, v: Vector3<SimdReal>) -> Vector3<SimdReal> {
+ fn transform_lin_vector(&self, v: SimdVector) -> SimdVector {
self.transform_vector(v)
}
- fn transform_vector(&self, v: Vector3<SimdReal>) -> Vector3<SimdReal> {
+ fn transform_vector(&self, v: SimdVector) -> SimdVector {
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)
+ SimdVector::new(x, y, z)
}
fn squared(&self) -> Self {
@@ -616,8 +293,8 @@ impl SimdAngularInertia<SimdReal> for SdpMatrix3<SimdReal> {
}
#[rustfmt::skip]
- fn into_matrix(self) -> Matrix3<SimdReal> {
- Matrix3::new(
+ fn into_matrix(self) -> SimdMatrix {
+ SimdMatrix::new(
self.m11, self.m12, self.m13,
self.m12, self.m22, self.m23,
self.m13, self.m23, self.m33,
@@ -625,7 +302,8 @@ impl SimdAngularInertia<SimdReal> for SdpMatrix3<SimdReal> {
}
#[rustfmt::skip]
- fn transform_matrix(&self, m: &Matrix3<SimdReal>) -> Matrix3<SimdReal> {
+ #[cfg(feature = "linalg-nalgebra")]
+ fn transform_matrix(&self, m: &SimdMatrix) -> SimdMatrix {
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;
@@ -638,7 +316,29 @@ impl SimdAngularInertia<SimdReal> for SdpMatrix3<SimdReal> {
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(
+ SimdMatrix::new(
+ x0, x1, x2,
+ y0, y1, y2,
+ z0, z1, z2,
+ )
+ }
+
+ #[rustfmt::skip]
+ #[cfg(feature = "linalg-glam")]
+ fn transform_matrix(&self, m: &SimdMatrix) -> SimdMatrix {
+ let x0 = self.m11 * m[(0, 0)] + self.m12 * m[(1, 0)] + self.m13 * m[(2, 0)];
+ let y0 = self.m12 * m[(0, 0)] + self.m22 * m[(1, 0)] + self.m23 * m[(2, 0)];
+ let z0 = self.m13 * m[(0, 0)] + self.m23 * m[(1, 0)] + self.m33 * m[(2, 0)];
+
+ let x1 = self.m11 * m[(0, 1)] + self.m12 * m[(1, 1)] + self.m13 * m[(2, 1)];
+ let y1 = self.m12 * m[(0, 1)] + self.m22 * m[(1, 1)] + self.m23 * m[(2, 1)];
+ let z1 = self.m13 * m[(0, 1)] + self.m23 * m[(1, 1)] + self.m33 * m[(2, 1)];
+
+ let x2 = self.m11 * m[(0, 2)] + self.m12 * m[(1, 2)] + self.m13 * m[(2, 2)];
+ let y2 = self.m12 * m[(0, 2)] + self.m22 * m[(1, 2)] + self.m23 * m[(2, 2)];
+ let z2 = self.m13 * m[(0, 2)] + self.m23 * m[(1, 2)] + self.m33 * m[(2, 2)];
+
+ SimdMatrix::new(
x0, x1, x2,
y0, y1, y2,
z0, z1, z2,
generated by cgit (git 2.34.1) at 2026-01-16 08:23:42 +0000