From ecd308338b189ab569816a38a03e3f8b89669dde Mon Sep 17 00:00:00 2001
From: Sébastien Crozet <sebcrozet@dimforge.com>
Date: Sun, 17 Mar 2024 21:20:18 +0100
Subject: feat: start experimenting with a glam/bevy version

---
 src/linalg/linalg_glam.rs      | 279 +++++++++++++++++++++++++
 src/linalg/linalg_glam_simd.rs | 262 +++++++++++++++++++++++
 src/linalg/linalg_nalgebra.rs  | 459 +++++++++++++++++++++++++++++++++++++++++
 src/linalg/mod.rs              |   5 +
 4 files changed, 1005 insertions(+)
 create mode 100644 src/linalg/linalg_glam.rs
 create mode 100644 src/linalg/linalg_glam_simd.rs
 create mode 100644 src/linalg/linalg_nalgebra.rs
 create mode 100644 src/linalg/mod.rs

(limited to 'src/linalg')

diff --git a/src/linalg/linalg_glam.rs b/src/linalg/linalg_glam.rs
new file mode 100644
index 0000000..62ea350
--- /dev/null
+++ b/src/linalg/linalg_glam.rs
@@ -0,0 +1,279 @@
+use crate::utils::{
+    SimdBasis, SimdCapMagnitude, SimdComponent, SimdCross, SimdCrossMatrix, SimdDot, SimdQuat,
+    SimdRealCopy, SimdSign, SimdVec,
+};
+
+use glam::{Mat2, Mat3, Quat, Vec2, Vec3};
+use na::{Matrix1, UnitComplex};
+use parry::math::Real;
+
+use crate::math::*;
+
+/*
+ *
+ * SimdSign
+ *
+ */
+impl SimdSign<Vec2> for Real {
+    fn copy_sign_to(self, to: Vec2) -> Vec2 {
+        Vec2::new(self.copy_sign_to(to.x), self.copy_sign_to(to.y))
+    }
+}
+
+impl SimdSign<Vec3> for Real {
+    fn copy_sign_to(self, to: Vec3) -> Vec3 {
+        Vec3::new(
+            self.copy_sign_to(to.x),
+            self.copy_sign_to(to.y),
+            self.copy_sign_to(to.z),
+        )
+    }
+}
+
+impl SimdSign<Vec2> for Vec2 {
+    fn copy_sign_to(self, to: Vec2) -> Vec2 {
+        Vec2::new(self.x.copy_sign_to(to.x), self.y.copy_sign_to(to.y))
+    }
+}
+
+impl SimdSign<Vec3> for Vec3 {
+    fn copy_sign_to(self, to: Vec3) -> Vec3 {
+        Vec3::new(
+            self.x.copy_sign_to(to.x),
+            self.y.copy_sign_to(to.y),
+            self.z.copy_sign_to(to.z),
+        )
+    }
+}
+
+impl SimdBasis for Vec2 {
+    type Basis = [Vec2; 1];
+    fn orthonormal_basis(self) -> [Vec2; 1] {
+        [Vec2::new(-self.y, self.x)]
+    }
+    fn orthonormal_vector(self) -> Vec2 {
+        Vec2::new(-self.y, self.x)
+    }
+}
+
+impl SimdBasis for Vec3 {
+    type Basis = [Vec3; 2];
+    // Robust and branchless implementation from Pixar:
+    // https://graphics.pixar.com/library/OrthonormalB/paper.pdf
+    fn orthonormal_basis(self) -> [Vec3; 2] {
+        let sign = self.z.copy_sign_to(1.0);
+        let a = -1.0 / (sign + self.z);
+        let b = self.x * self.y * a;
+
+        [
+            Vec3::new(1.0 + sign * self.x * self.x * a, sign * b, -sign * self.x),
+            Vec3::new(b, sign + self.y * self.y * a, -self.y),
+        ]
+    }
+
+    fn orthonormal_vector(self) -> Vec3 {
+        let sign = self.z.copy_sign_to(1.0);
+        let a = -1.0 / (sign + self.z);
+        let b = self.x * self.y * a;
+        Vec3::new(b, sign + self.y * self.y * a, -self.y)
+    }
+}
+
+impl SimdCapMagnitude<Real> for Vec2 {
+    fn simd_cap_magnitude(&self, max: Real) -> Self {
+        use na::SimdPartialOrd;
+        let n = self.length();
+        let scaled = *self * (max / n);
+        let use_scaled = n.simd_gt(max);
+        scaled.select(use_scaled, *self)
+    }
+}
+
+impl SimdVec for Vec2 {
+    type Element = Vec2;
+
+    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())
+    }
+
+    fn component_mul_simd(&self, rhs: &Self) -> Self {
+        *self * *rhs
+    }
+}
+
+impl SimdVec for Vec3 {
+    type Element = Vec3;
+
+    fn horizontal_inf(&self) -> Self::Element {
+        Vec3::new(
+            self.x.min_component(),
+            self.y.min_component(),
+            self.z.min_component(),
+        )
+    }
+
+    fn horizontal_sup(&self) -> Self::Element {
+        Vec3::new(
+            self.x.max_component(),
+            self.y.max_component(),
+            self.z.max_component(),
+        )
+    }
+
+    fn component_mul_simd(&self, rhs: &Self) -> Self {
+        self.component_mul(rhs)
+    }
+}
+
+#[cfg(feature = "dim3")]
+impl SimdCrossMatrix for Vec3 {
+    type CrossMat = Mat3;
+    type CrossMatTr = Mat3;
+
+    #[inline]
+    #[rustfmt::skip]
+    fn gcross_matrix(self) -> Self::CrossMat {
+        Mat3::new(
+            0.0, -self.z, self.y,
+            self.z, 0.0, -self.x,
+            -self.y, self.x, 0.0,
+        )
+    }
+
+    #[inline]
+    #[rustfmt::skip]
+    fn gcross_matrix_tr(self) -> Self::CrossMatTr {
+        Mat3::new(
+            0.0, self.z, -self.y,
+            -self.z, 0.0, self.x,
+            self.y, -self.x, 0.0,
+        )
+    }
+}
+
+impl SimdCrossMatrix for Vec2 {
+    type CrossMat = Vec2;
+    type CrossMatTr = Vec2;
+
+    #[inline]
+    fn gcross_matrix(self) -> Self::CrossMat {
+        Vec2::new(-self.y, self.x)
+    }
+    #[inline]
+    fn gcross_matrix_tr(self) -> Self::CrossMatTr {
+        Vec2::new(-self.y, self.x)
+    }
+}
+
+impl SimdCrossMatrix for Real {
+    type CrossMat = Mat2;
+    type CrossMatTr = Mat2;
+
+    #[inline]
+    fn gcross_matrix(self) -> Mat2 {
+        Mat2::from_cols_array(&[0.0, self, -self, 0.0])
+    }
+
+    #[inline]
+    fn gcross_matrix_tr(self) -> Mat2 {
+        Mat2::from_cols_array(&[0.0, -self, self, 0.0])
+    }
+}
+
+impl SimdCross<Vec3> for Vec3 {
+    type Result = Self;
+
+    fn gcross(&self, rhs: Vec3) -> Self::Result {
+        self.cross(rhs)
+    }
+    #[cfg(feature = "dim3")]
+    fn cross_(&self, rhs: &Self) -> Self {
+        self.cross(*rhs)
+    }
+}
+
+impl SimdCross<Vec2> for Vec2 {
+    type Result = Real;
+
+    fn gcross(&self, rhs: Vec2) -> Self::Result {
+        self.x * rhs.y - self.y * rhs.x
+    }
+
+    #[cfg(feature = "dim3")]
+    fn cross_(&self, _: &Self) -> Self {
+        unreachable!()
+    }
+}
+
+impl SimdCross<Vec2> for Real {
+    type Result = Vec2;
+
+    fn gcross(&self, rhs: Vec2) -> Self::Result {
+        Vec2::new(-rhs.y * *self, rhs.x * *self)
+    }
+
+    #[cfg(feature = "dim3")]
+    fn cross_(&self, _: &Self) -> Self {
+        unreachable!()
+    }
+}
+
+impl SimdDot<Vec3> for Vec3 {
+    type Result = Real;
+
+    fn gdot(&self, rhs: Vec3) -> Self::Result {
+        self.x * rhs.x + self.y * rhs.y + self.z * rhs.z
+    }
+}
+
+impl SimdDot<Vec2> for Vec2 {
+    type Result = Real;
+
+    fn gdot(&self, rhs: Vec2) -> Self::Result {
+        self.x * rhs.x + self.y * rhs.y
+    }
+}
+
+#[cfg(feature = "dim3")]
+impl SimdQuat<Real> for Quat {
+    type Result = Mat3;
+
+    fn diff_conj1_2(&self, rhs: &Self) -> Self::Result {
+        let half = 0.5;
+        let v1 = self.xyz();
+        let v2 = rhs.xyz();
+        let w1 = self.w;
+        let w2 = rhs.w;
+
+        // TODO: this can probably be optimized a lot by unrolling the ops.
+        (v1.outer_product(&v2) + Mat3::from_diagonal_element(w1 * w2)
+            - (v1 * w2 + v2 * w1).gcross_matrix()
+            + v1.gcross_matrix() * v2.gcross_matrix())
+            * half
+    }
+}
+
+impl SimdQuat<Real> for Mat2 {
+    type Result = Real;
+
+    #[inline]
+    fn diff_conj1_2(&self, rhs: &Self) -> Self::Result {
+        let arr_self = self.as_ref();
+        let arr_rhs = rhs.as_ref();
+        (arr_self[1] * arr_rhs[1] + arr_self[0] * arr_rhs[0]) * 2.0
+    }
+}
+
+impl SimdQuat<SimdReal> for SimdMat2 {
+    type Result = SimdReal;
+
+    #[inline]
+    fn diff_conj1_2(&self, rhs: &Self) -> Self::Result {
+        use na::SimdValue;
+        (self[(1, 0)] * rhs[(1, 0)] + self[(0, 0)] * rhs[(0, 0)]) * SimdReal::splat(2.0)
+    }
+}
diff --git a/src/linalg/linalg_glam_simd.rs b/src/linalg/linalg_glam_simd.rs
new file mode 100644
index 0000000..4b37018
--- /dev/null
+++ b/src/linalg/linalg_glam_simd.rs
@@ -0,0 +1,262 @@
+use crate::utils::{
+    SimdBasis, SimdCapMagnitude, SimdComponent, SimdCross, SimdCrossMatrix, SimdDot, SimdQuat,
+    SimdSign, SimdVec,
+};
+
+use glam::{Vec2, Vec3};
+use num::Zero;
+use parry::math::{SimdMat3, SimdReal, SimdVec2, SimdVec3};
+
+use {crate::math::*, na::SimdValue};
+
+/*
+ *
+ * SimdSign
+ *
+ */
+impl SimdSign<SimdVec2> for SimdReal {
+    fn copy_sign_to(self, to: SimdVec2) -> SimdVec2 {
+        SimdVec2::new(self.copy_sign_to(to.x), self.copy_sign_to(to.y))
+    }
+}
+
+impl SimdSign<SimdVec3> for SimdReal {
+    fn copy_sign_to(self, to: SimdVec3) -> SimdVec3 {
+        SimdVec3::new(
+            self.copy_sign_to(to.x),
+            self.copy_sign_to(to.y),
+            self.copy_sign_to(to.z),
+        )
+    }
+}
+
+impl SimdSign<SimdVec2> for SimdVec2 {
+    fn copy_sign_to(self, to: SimdVec2) -> SimdVec2 {
+        SimdVec2::new(self.x.copy_sign_to(to.x), self.y.copy_sign_to(to.y))
+    }
+}
+
+impl SimdSign<SimdVec3> for SimdVec3 {
+    fn copy_sign_to(self, to: SimdVec3) -> SimdVec3 {
+        SimdVec3::new(
+            self.x.copy_sign_to(to.x),
+            self.y.copy_sign_to(to.y),
+            self.z.copy_sign_to(to.z),
+        )
+    }
+}
+
+impl SimdBasis for SimdVec2 {
+    type Basis = [SimdVec2; 1];
+    fn orthonormal_basis(self) -> [SimdVec2; 1] {
+        [SimdVec2::new(-self.y, self.x)]
+    }
+    fn orthonormal_vector(self) -> SimdVec2 {
+        SimdVec2::new(-self.y, self.x)
+    }
+}
+
+impl SimdBasis for SimdVec3 {
+    type Basis = [SimdVec3; 2];
+    // Robust and branchless implementation from Pixar:
+    // https://graphics.pixar.com/library/OrthonormalB/paper.pdf
+    fn orthonormal_basis(self) -> [SimdVec3; 2] {
+        let one = SimdReal::splat(1.0);
+        let sign = self.z.copy_sign_to(one);
+        let a = -one / (sign + self.z);
+        let b = self.x * self.y * a;
+
+        [
+            SimdVec3::new(one + sign * self.x * self.x * a, sign * b, -sign * self.x),
+            SimdVec3::new(b, sign + self.y * self.y * a, -self.y),
+        ]
+    }
+
+    fn orthonormal_vector(self) -> SimdVec3 {
+        let one = SimdReal::splat(1.0);
+        let sign = self.z.copy_sign_to(one);
+        let a = -one / (sign + self.z);
+        let b = self.x * self.y * a;
+        SimdVec3::new(b, sign + self.y * self.y * a, -self.y)
+    }
+}
+
+impl SimdCapMagnitude<SimdReal> for SimdVec2 {
+    fn simd_cap_magnitude(&self, max: SimdReal) -> Self {
+        use na::SimdPartialOrd;
+        let n = self.length();
+        let scaled = *self * (max / n);
+        let use_scaled = n.simd_gt(max);
+        scaled.select(use_scaled, *self)
+    }
+}
+
+impl SimdVec for SimdVec2 {
+    type Element = Vec2;
+
+    fn horizontal_inf(&self) -> Self::Element {
+        Vec2::new(self.x.min_component(), self.y.min_component())
+    }
+
+    fn horizontal_sup(&self) -> Self::Element {
+        Vec2::new(self.x.max_component(), self.y.max_component())
+    }
+
+    fn component_mul_simd(&self, rhs: &Self) -> Self {
+        *self * *rhs
+    }
+}
+
+impl SimdVec for SimdVec3 {
+    type Element = Vec3;
+
+    fn horizontal_inf(&self) -> Self::Element {
+        Vec3::new(
+            self.x.min_component(),
+            self.y.min_component(),
+            self.z.min_component(),
+        )
+    }
+
+    fn horizontal_sup(&self) -> Self::Element {
+        Vec3::new(
+            self.x.max_component(),
+            self.y.max_component(),
+            self.z.max_component(),
+        )
+    }
+
+    fn component_mul_simd(&self, rhs: &Self) -> Self {
+        self.component_mul(rhs)
+    }
+}
+
+impl SimdCrossMatrix for SimdVec3 {
+    type CrossMat = SimdMat3;
+    type CrossMatTr = SimdMat3;
+
+    #[inline]
+    #[rustfmt::skip]
+    fn gcross_matrix(self) -> Self::CrossMat {
+        let zero = SimdReal::splat(0.0);
+        SimdMat3::new(
+            zero, -self.z, self.y,
+            self.z, zero, -self.x,
+            -self.y, self.x, zero,
+        )
+    }
+
+    #[inline]
+    #[rustfmt::skip]
+    fn gcross_matrix_tr(self) -> Self::CrossMatTr {
+        let zero = SimdReal::splat(0.0);
+        SimdMat3::new(
+            zero, self.z, -self.y,
+            -self.z, zero, self.x,
+            self.y, -self.x, zero,
+        )
+    }
+}
+
+impl SimdCrossMatrix for SimdVec2 {
+    type CrossMat = SimdVec2;
+    type CrossMatTr = SimdVec2;
+
+    #[inline]
+    fn gcross_matrix(self) -> Self::CrossMat {
+        SimdVec2::new(-self.y, self.x)
+    }
+    #[inline]
+    fn gcross_matrix_tr(self) -> Self::CrossMatTr {
+        SimdVec2::new(-self.y, self.x)
+    }
+}
+
+impl SimdCrossMatrix for SimdReal {
+    type CrossMat = SimdMat2;
+    type CrossMatTr = SimdMat2;
+
+    #[inline]
+    fn gcross_matrix(self) -> SimdMat2 {
+        let zero = SimdReal::zero();
+        SimdMat2::new(zero, -self, self, zero)
+    }
+
+    #[inline]
+    fn gcross_matrix_tr(self) -> SimdMat2 {
+        let zero = SimdReal::zero();
+        SimdMat2::new(zero, self, -self, zero)
+    }
+}
+
+impl SimdCross<SimdVec3> for SimdVec3 {
+    type Result = Self;
+
+    fn gcross(&self, rhs: SimdVec3) -> Self::Result {
+        self.cross(rhs)
+    }
+    #[cfg(feature = "dim3")]
+    fn cross_(&self, rhs: &Self) -> Self {
+        self.cross(*rhs)
+    }
+}
+
+impl SimdCross<SimdVec2> for SimdVec2 {
+    type Result = SimdReal;
+
+    fn gcross(&self, rhs: SimdVec2) -> Self::Result {
+        self.x * rhs.y - self.y * rhs.x
+    }
+
+    #[cfg(feature = "dim3")]
+    fn cross_(&self, _: &Self) -> Self {
+        unreachable!()
+    }
+}
+
+impl SimdDot<SimdVec3> for SimdVec3 {
+    type Result = SimdReal;
+
+    fn gdot(&self, rhs: SimdVec3) -> Self::Result {
+        self.x * rhs.x + self.y * rhs.y + self.z * rhs.z
+    }
+}
+
+impl SimdDot<SimdVec2> for SimdVec2 {
+    type Result = SimdReal;
+
+    fn gdot(&self, rhs: SimdVec2) -> Self::Result {
+        self.x * rhs.x + self.y * rhs.y
+    }
+}
+
+impl SimdCross<SimdVec2> for SimdReal {
+    type Result = SimdVec2;
+
+    fn gcross(&self, rhs: SimdVec2) -> Self::Result {
+        SimdVec2::new(-rhs.y * *self, rhs.x * *self)
+    }
+
+    #[cfg(feature = "dim3")]
+    fn cross_(&self, rhs: &Self) -> Self {
+        unreachable!()
+    }
+}
+
+impl SimdQuat<SimdReal> for parry::math::SimdQuat {
+    type Result = SimdMat3;
+
+    fn diff_conj1_2(&self, rhs: &Self) -> Self::Result {
+        let half = SimdReal::splat(0.5);
+        let v1 = self.xyz();
+        let v2 = rhs.xyz();
+        let w1 = self.w;
+        let w2 = rhs.w;
+
+        // TODO: this can probably be optimized a lot by unrolling the ops.
+        (v1.outer_product(v2) + SimdMat3::from_diagonal_element(w1 * w2)
+            - (v1 * w2 + v2 * w1).gcross_matrix()
+            + v1.gcross_matrix() * v2.gcross_matrix())
+            * half
+    }
+}
diff --git a/src/linalg/linalg_nalgebra.rs b/src/linalg/linalg_nalgebra.rs
new file mode 100644
index 0000000..bfacf16
--- /dev/null
+++ b/src/linalg/linalg_nalgebra.rs
@@ -0,0 +1,459 @@
+use crate::utils::{
+    simd_inv, SimdAngularInertia, SimdBasis, SimdCapMagnitude, SimdComponent, SimdCross,
+    SimdCrossMatrix, SimdDot, SimdQuat, SimdRealCopy, SimdSign, SimdVec,
+};
+
+use na::{
+    Matrix1, Matrix2, Matrix3, Point2, Point3, RowVector2, Scalar, SimdRealField, UnitComplex,
+    UnitQuaternion, Vector1, Vector2, Vector3,
+};
+use num::Zero;
+use parry::math::{Real, SimdReal};
+use simba::scalar::ClosedMul;
+
+use na::SimdPartialOrd;
+
+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)
+    }
+}
+
+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()
+    }
+}
+
+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)
+    }
+}
+
+impl<N> SimdVec for Vector2<N>
+where
+    N: Scalar + Copy + SimdComponent + ClosedMul,
+    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())
+    }
+
+    fn component_mul_simd(&self, rhs: &Self) -> Self {
+        self.component_mul(rhs)
+    }
+}
+
+impl<N: SimdRealField> SimdCapMagnitude<N> for Vector2<N>
+where
+    N::Element: SimdRealField,
+{
+    fn simd_cap_magnitude(&self, limit: N) -> Self {
+        self.simd_cap_magnitude(limit)
+    }
+}
+
+impl<N: Scalar + Copy + SimdComponent + ClosedMul> 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())
+    }
+
+    fn component_mul_simd(&self, rhs: &Self) -> Self {
+        self.coords.component_mul(&rhs.coords).into()
+    }
+}
+
+impl<N: Scalar + Copy + SimdComponent + ClosedMul> 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(),
+        )
+    }
+
+    fn component_mul_simd(&self, rhs: &Self) -> Self {
+        self.component_mul(rhs)
+    }
+}
+
+impl<N: Scalar + Copy + SimdComponent + ClosedMul> 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(),
+        )
+    }
+
+    fn component_mul_simd(&self, rhs: &Self) -> Self {
+        self.coords.component_mul(&rhs.coords).into()
+    }
+}
+
+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)
+    }
+}
+
+#[cfg(feature = "linalg-nalgebra")]
+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)
+    }
+}
+
+#[cfg(feature = "linalg-nalgebra")]
+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())
+    }
+}
+
+impl SimdCross<Vector3<Real>> for Vector3<Real> {
+    type Result = Self;
+
+    fn gcross(&self, rhs: Vector3<Real>) -> Self::Result {
+        self.cross(rhs)
+    }
+    #[cfg(feature = "dim3")]
+    fn cross_(&self, rhs: &Self) -> Self {
+        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
+    }
+
+    #[cfg(feature = "dim3")]
+    fn cross_(&self, _: &Self) -> Self {
+        unreachable!()
+    }
+}
+
+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)
+    }
+
+    #[cfg(feature = "dim3")]
+    fn cross_(&self, _: &Self) -> Self {
+        unreachable!()
+    }
+}
+
+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)
+    }
+
+    #[cfg(feature = "dim3")]
+    fn cross_(&self, rhs: &Self) -> Self {
+        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)
+    }
+
+    #[cfg(feature = "dim3")]
+    fn cross_(&self, rhs: &Self) -> Self {
+        unreachable!()
+    }
+}
+
+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
+    }
+
+    #[cfg(feature = "dim3")]
+    fn cross_(&self, _: &Self) -> Self {
+        unreachable!()
+    }
+}
+
+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
+    }
+}
+
+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
+    }
+}
diff --git a/src/linalg/mod.rs b/src/linalg/mod.rs
new file mode 100644
index 0000000..4c348ab
--- /dev/null
+++ b/src/linalg/mod.rs
@@ -0,0 +1,5 @@
+#[cfg(feature = "linalg-glam")]
+mod linalg_glam;
+#[cfg(feature = "linalg-glam")]
+mod linalg_glam_simd;
+mod linalg_nalgebra;
-- 
cgit