From cc6d1b973002b4d366bc81ec6bf9e8240ad7b404 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 14 Dec 2020 15:51:43 +0100 Subject: [PATCH] Outsource the Shape trait, wquadtree, and shape types. --- benchmarks3d/all_benchmarks3.rs | 2 +- build/rapier3d/Cargo.toml | 4 +- examples3d/all_examples3.rs | 2 +- src/data/arena.rs | 11 + src/dynamics/mass_properties.rs | 442 ------------ src/dynamics/mass_properties_ball.rs | 30 - src/dynamics/mass_properties_capsule.rs | 39 -- src/dynamics/mass_properties_cone.rs | 29 - src/dynamics/mass_properties_cuboid.rs | 33 - src/dynamics/mass_properties_cylinder.rs | 40 -- src/dynamics/mass_properties_polygon.rs | 146 ---- src/dynamics/mod.rs | 11 +- .../ball_position_constraint_wide.rs | 44 +- .../ball_velocity_constraint_wide.rs | 64 +- .../fixed_velocity_constraint_wide.rs | 76 +-- .../prismatic_velocity_constraint_wide.rs | 114 ++-- .../revolute_velocity_constraint_wide.rs | 60 +- .../solver/position_constraint_wide.rs | 48 +- .../solver/position_ground_constraint_wide.rs | 40 +- .../solver/velocity_constraint_wide.rs | 67 +- .../solver/velocity_ground_constraint_wide.rs | 59 +- src/geometry/ball.rs | 8 +- src/geometry/collider.rs | 18 +- src/geometry/contact.rs | 12 +- .../ball_ball_contact_generator.rs | 8 +- .../contact_generator/contact_dispatcher.rs | 8 +- .../contact_generator/contact_generator.rs | 8 +- .../contact_generator_workspace.rs | 6 +- .../heightfield_shape_contact_generator.rs | 9 +- src/geometry/contact_generator/mod.rs | 2 +- .../serializable_workspace_tag.rs | 2 +- .../trimesh_shape_contact_generator.rs | 32 +- src/geometry/mod.rs | 14 +- src/geometry/narrow_phase.rs | 2 +- .../ball_ball_proximity_detector.rs | 8 +- src/geometry/proximity_detector/mod.rs | 2 +- .../proximity_detector/proximity_detector.rs | 6 +- .../proximity_dispatcher.rs | 10 +- .../trimesh_shape_proximity_detector.rs | 22 +- src/geometry/round_cylinder.rs | 92 --- src/geometry/shape.rs | 393 ----------- src/geometry/trimesh.rs | 203 ------ src/geometry/waabb.rs | 217 ------ src/geometry/wquadtree.rs | 587 ---------------- src/lib.rs | 140 +--- src/pipeline/query_pipeline.rs | 5 +- src/utils.rs | 632 ++---------------- 47 files changed, 444 insertions(+), 3363 deletions(-) delete mode 100644 src/dynamics/mass_properties.rs delete mode 100644 src/dynamics/mass_properties_ball.rs delete mode 100644 src/dynamics/mass_properties_capsule.rs delete mode 100644 src/dynamics/mass_properties_cone.rs delete mode 100644 src/dynamics/mass_properties_cuboid.rs delete mode 100644 src/dynamics/mass_properties_cylinder.rs delete mode 100644 src/dynamics/mass_properties_polygon.rs delete mode 100644 src/geometry/round_cylinder.rs delete mode 100644 src/geometry/shape.rs delete mode 100644 src/geometry/trimesh.rs delete mode 100644 src/geometry/waabb.rs delete mode 100644 src/geometry/wquadtree.rs diff --git a/benchmarks3d/all_benchmarks3.rs b/benchmarks3d/all_benchmarks3.rs index dfbbc51..fa81d87 100644 --- a/benchmarks3d/all_benchmarks3.rs +++ b/benchmarks3d/all_benchmarks3.rs @@ -55,7 +55,7 @@ pub fn main() { ("Heightfield", heightfield3::init_world), ("Stacks", stacks3::init_world), ("Pyramid", pyramid3::init_world), - ("Trimesh", trimesh3::init_world), + ("TriMesh", trimesh3::init_world), ("Joint ball", joint_ball3::init_world), ("Joint fixed", joint_fixed3::init_world), ("Joint revolute", joint_revolute3::init_world), diff --git a/build/rapier3d/Cargo.toml b/build/rapier3d/Cargo.toml index d409035..7d5673b 100644 --- a/build/rapier3d/Cargo.toml +++ b/build/rapier3d/Cargo.toml @@ -15,8 +15,8 @@ edition = "2018" default = [ "dim3" ] dim3 = [ ] parallel = [ "rayon" ] -simd-stable = [ "simba/wide", "simd-is-enabled" ] -simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ] +simd-stable = [ "buckler3d/simd-stable", "simba/wide", "simd-is-enabled" ] +simd-nightly = [ "buckler3d/simd-nightly", "simba/packed_simd", "simd-is-enabled" ] # Do not enable this feature directly. It is automatically # enabled with the "simd-stable" or "simd-nightly" feature. simd-is-enabled = [ ] diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index 4bc15a8..2c8e5c6 100644 --- a/examples3d/all_examples3.rs +++ b/examples3d/all_examples3.rs @@ -84,7 +84,7 @@ pub fn main() { ("Restitution", restitution3::init_world), ("Stacks", stacks3::init_world), ("Sensor", sensor3::init_world), - ("Trimesh", trimesh3::init_world), + ("TriMesh", trimesh3::init_world), ("Keva tower", keva3::init_world), ( "(Debug) add/rm collider", diff --git a/src/data/arena.rs b/src/data/arena.rs index fcec017..a3af45c 100644 --- a/src/data/arena.rs +++ b/src/data/arena.rs @@ -3,6 +3,7 @@ //! See https://github.com/fitzgen/generational-arena/blob/master/src/lib.rs. //! This has been modified to have a fully deterministic deserialization (including for the order of //! Index attribution after a deserialization of the arena. +use buckler::partitioning::IndexedData; use std::cmp; use std::iter::{self, Extend, FromIterator, FusedIterator}; use std::mem; @@ -51,6 +52,16 @@ pub struct Index { generation: u64, } +impl IndexedData for Index { + fn default() -> Self { + Self::from_raw_parts(crate::INVALID_USIZE, crate::INVALID_U64) + } + + fn index(&self) -> usize { + self.into_raw_parts().0 + } +} + impl Index { /// Create a new `Index` from its raw parts. /// diff --git a/src/dynamics/mass_properties.rs b/src/dynamics/mass_properties.rs deleted file mode 100644 index a36ceb5..0000000 --- a/src/dynamics/mass_properties.rs +++ /dev/null @@ -1,442 +0,0 @@ -use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Vector}; -use crate::utils; -use num::Zero; -use std::ops::{Add, AddAssign, Sub, SubAssign}; -#[cfg(feature = "dim3")] -use {na::Matrix3, std::ops::MulAssign}; - -#[derive(Copy, Clone, Debug, PartialEq)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// The local mass properties of a rigid-body. -pub struct MassProperties { - /// The center of mass of a rigid-body expressed in its local-space. - pub local_com: Point, - /// The inverse of the mass of a rigid-body. - /// - /// If this is zero, the rigid-body is assumed to have infinite mass. - pub inv_mass: f32, - /// The inverse of the principal angular inertia of the rigid-body. - /// - /// Components set to zero are assumed to be infinite along the corresponding principal axis. - pub inv_principal_inertia_sqrt: AngVector, - #[cfg(feature = "dim3")] - /// The principal vectors of the local angular inertia tensor of the rigid-body. - pub principal_inertia_local_frame: Rotation, -} - -impl MassProperties { - /// Initializes the mass properties with the given center-of-mass, mass, and angular inertia. - /// - /// The center-of-mass is specified in the local-space of the rigid-body. - #[cfg(feature = "dim2")] - pub fn new(local_com: Point, mass: f32, principal_inertia: f32) -> Self { - let inv_mass = utils::inv(mass); - let inv_principal_inertia_sqrt = utils::inv(principal_inertia.sqrt()); - Self { - local_com, - inv_mass, - inv_principal_inertia_sqrt, - } - } - - /// Initializes the mass properties from the given center-of-mass, mass, and principal angular inertia. - /// - /// The center-of-mass is specified in the local-space of the rigid-body. - /// The principal angular inertia are the angular inertia along the coordinate axes in the local-space - /// of the rigid-body. - #[cfg(feature = "dim3")] - pub fn new(local_com: Point, mass: f32, principal_inertia: AngVector) -> Self { - Self::with_principal_inertia_frame(local_com, mass, principal_inertia, Rotation::identity()) - } - - /// Initializes the mass properties from the given center-of-mass, mass, and principal angular inertia. - /// - /// The center-of-mass is specified in the local-space of the rigid-body. - /// The principal angular inertia are the angular inertia along the coordinate axes defined by - /// the `principal_inertia_local_frame` expressed in the local-space of the rigid-body. - #[cfg(feature = "dim3")] - pub fn with_principal_inertia_frame( - local_com: Point, - mass: f32, - principal_inertia: AngVector, - principal_inertia_local_frame: Rotation, - ) -> Self { - let inv_mass = utils::inv(mass); - let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.sqrt())); - Self { - local_com, - inv_mass, - inv_principal_inertia_sqrt, - principal_inertia_local_frame, - } - } - - /// The world-space center of mass of the rigid-body. - pub fn world_com(&self, pos: &Isometry) -> Point { - pos * self.local_com - } - - #[cfg(feature = "dim2")] - /// The world-space inverse angular inertia tensor of the rigid-body. - pub fn world_inv_inertia_sqrt(&self, _rot: &Rotation) -> AngularInertia { - self.inv_principal_inertia_sqrt - } - - #[cfg(feature = "dim3")] - /// The world-space inverse angular inertia tensor of the rigid-body. - pub fn world_inv_inertia_sqrt(&self, rot: &Rotation) -> AngularInertia { - if !self.inv_principal_inertia_sqrt.is_zero() { - let mut lhs = (rot * self.principal_inertia_local_frame) - .to_rotation_matrix() - .into_inner(); - let rhs = lhs.transpose(); - lhs.column_mut(0) - .mul_assign(self.inv_principal_inertia_sqrt.x); - lhs.column_mut(1) - .mul_assign(self.inv_principal_inertia_sqrt.y); - lhs.column_mut(2) - .mul_assign(self.inv_principal_inertia_sqrt.z); - let inertia = lhs * rhs; - AngularInertia::from_sdp_matrix(inertia) - } else { - AngularInertia::zero() - } - } - - #[cfg(feature = "dim3")] - /// Reconstructs the inverse angular inertia tensor of the rigid body from its principal inertia values and axes. - pub fn reconstruct_inverse_inertia_matrix(&self) -> Matrix3 { - let inv_principal_inertia = self.inv_principal_inertia_sqrt.map(|e| e * e); - self.principal_inertia_local_frame.to_rotation_matrix() - * Matrix3::from_diagonal(&inv_principal_inertia) - * self - .principal_inertia_local_frame - .inverse() - .to_rotation_matrix() - } - - #[cfg(feature = "dim3")] - /// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axes. - pub fn reconstruct_inertia_matrix(&self) -> Matrix3 { - let principal_inertia = self.inv_principal_inertia_sqrt.map(|e| utils::inv(e * e)); - self.principal_inertia_local_frame.to_rotation_matrix() - * Matrix3::from_diagonal(&principal_inertia) - * self - .principal_inertia_local_frame - .inverse() - .to_rotation_matrix() - } - - #[cfg(feature = "dim2")] - pub(crate) fn construct_shifted_inertia_matrix(&self, shift: Vector) -> f32 { - let i = utils::inv(self.inv_principal_inertia_sqrt * self.inv_principal_inertia_sqrt); - - if self.inv_mass != 0.0 { - let mass = 1.0 / self.inv_mass; - i + shift.norm_squared() * mass - } else { - i - } - } - - #[cfg(feature = "dim3")] - pub(crate) fn construct_shifted_inertia_matrix(&self, shift: Vector) -> Matrix3 { - let matrix = self.reconstruct_inertia_matrix(); - - if self.inv_mass != 0.0 { - let mass = 1.0 / self.inv_mass; - let diag = shift.norm_squared(); - let diagm = Matrix3::from_diagonal_element(diag); - matrix + (diagm + shift * shift.transpose()) * mass - } else { - matrix - } - } - - /// Transform each element of the mass properties. - pub fn transform_by(&self, m: &Isometry) -> Self { - // NOTE: we don't apply the parallel axis theorem here - // because the center of mass is also transformed. - Self { - local_com: m * self.local_com, - inv_mass: self.inv_mass, - inv_principal_inertia_sqrt: self.inv_principal_inertia_sqrt, - #[cfg(feature = "dim3")] - principal_inertia_local_frame: m.rotation * self.principal_inertia_local_frame, - } - } -} - -impl Zero for MassProperties { - fn zero() -> Self { - Self { - inv_mass: 0.0, - inv_principal_inertia_sqrt: na::zero(), - #[cfg(feature = "dim3")] - principal_inertia_local_frame: Rotation::identity(), - local_com: Point::origin(), - } - } - - fn is_zero(&self) -> bool { - *self == Self::zero() - } -} - -impl Sub for MassProperties { - type Output = Self; - - #[cfg(feature = "dim2")] - fn sub(self, other: MassProperties) -> Self { - if self.is_zero() || other.is_zero() { - return self; - } - - let m1 = utils::inv(self.inv_mass); - let m2 = utils::inv(other.inv_mass); - let inv_mass = utils::inv(m1 - m2); - - let local_com = (self.local_com * m1 - other.local_com.coords * m2) * inv_mass; - let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com); - let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com); - let inertia = i1 - i2; - // NOTE: we drop the negative eigenvalues that may result from subtraction rounding errors. - let inv_principal_inertia_sqrt = utils::inv(inertia.max(0.0).sqrt()); - - Self { - local_com, - inv_mass, - inv_principal_inertia_sqrt, - } - } - - #[cfg(feature = "dim3")] - fn sub(self, other: MassProperties) -> Self { - if self.is_zero() || other.is_zero() { - return self; - } - - let m1 = utils::inv(self.inv_mass); - let m2 = utils::inv(other.inv_mass); - let inv_mass = utils::inv(m1 - m2); - let local_com = (self.local_com * m1 - other.local_com.coords * m2) * inv_mass; - let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com); - let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com); - let inertia = i1 - i2; - let eigen = inertia.symmetric_eigen(); - let principal_inertia_local_frame = - Rotation::from_matrix_eps(&eigen.eigenvectors, 1.0e-6, 10, na::one()); - let principal_inertia = eigen.eigenvalues; - // NOTE: we drop the negative eigenvalues that may result from subtraction rounding errors. - let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.max(0.0).sqrt())); - - Self { - local_com, - inv_mass, - inv_principal_inertia_sqrt, - principal_inertia_local_frame, - } - } -} - -impl SubAssign for MassProperties { - fn sub_assign(&mut self, rhs: MassProperties) { - *self = *self - rhs - } -} - -impl Add for MassProperties { - type Output = Self; - - #[cfg(feature = "dim2")] - fn add(self, other: MassProperties) -> Self { - if self.is_zero() { - return other; - } else if other.is_zero() { - return self; - } - - let m1 = utils::inv(self.inv_mass); - let m2 = utils::inv(other.inv_mass); - let inv_mass = utils::inv(m1 + m2); - let local_com = (self.local_com * m1 + other.local_com.coords * m2) * inv_mass; - let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com); - let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com); - let inertia = i1 + i2; - let inv_principal_inertia_sqrt = utils::inv(inertia.sqrt()); - - Self { - local_com, - inv_mass, - inv_principal_inertia_sqrt, - } - } - - #[cfg(feature = "dim3")] - fn add(self, other: MassProperties) -> Self { - if self.is_zero() { - return other; - } else if other.is_zero() { - return self; - } - - let m1 = utils::inv(self.inv_mass); - let m2 = utils::inv(other.inv_mass); - let inv_mass = utils::inv(m1 + m2); - let local_com = (self.local_com * m1 + other.local_com.coords * m2) * inv_mass; - let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com); - let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com); - let inertia = i1 + i2; - let eigen = inertia.symmetric_eigen(); - let principal_inertia_local_frame = - Rotation::from_matrix_eps(&eigen.eigenvectors, 1.0e-6, 10, na::one()); - let principal_inertia = eigen.eigenvalues; - let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.sqrt())); - - Self { - local_com, - inv_mass, - inv_principal_inertia_sqrt, - principal_inertia_local_frame, - } - } -} - -impl AddAssign for MassProperties { - fn add_assign(&mut self, rhs: MassProperties) { - *self = *self + rhs - } -} - -impl approx::AbsDiffEq for MassProperties { - type Epsilon = f32; - fn default_epsilon() -> Self::Epsilon { - f32::default_epsilon() - } - - fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool { - #[cfg(feature = "dim2")] - let inertia_is_ok = self - .inv_principal_inertia_sqrt - .abs_diff_eq(&other.inv_principal_inertia_sqrt, epsilon); - - #[cfg(feature = "dim3")] - let inertia_is_ok = self - .reconstruct_inverse_inertia_matrix() - .abs_diff_eq(&other.reconstruct_inverse_inertia_matrix(), epsilon); - - inertia_is_ok - && self.local_com.abs_diff_eq(&other.local_com, epsilon) - && self.inv_mass.abs_diff_eq(&other.inv_mass, epsilon) - && self - .inv_principal_inertia_sqrt - .abs_diff_eq(&other.inv_principal_inertia_sqrt, epsilon) - } -} - -impl approx::RelativeEq for MassProperties { - fn default_max_relative() -> Self::Epsilon { - f32::default_max_relative() - } - - fn relative_eq( - &self, - other: &Self, - epsilon: Self::Epsilon, - max_relative: Self::Epsilon, - ) -> bool { - #[cfg(feature = "dim2")] - let inertia_is_ok = self.inv_principal_inertia_sqrt.relative_eq( - &other.inv_principal_inertia_sqrt, - epsilon, - max_relative, - ); - - #[cfg(feature = "dim3")] - let inertia_is_ok = self.reconstruct_inverse_inertia_matrix().relative_eq( - &other.reconstruct_inverse_inertia_matrix(), - epsilon, - max_relative, - ); - - inertia_is_ok - && self - .local_com - .relative_eq(&other.local_com, epsilon, max_relative) - && self - .inv_mass - .relative_eq(&other.inv_mass, epsilon, max_relative) - } -} - -#[cfg(test)] -mod test { - use super::MassProperties; - use crate::geometry::ColliderBuilder; - use crate::math::{Point, Rotation, Vector}; - use approx::assert_relative_eq; - use num::Zero; - - #[test] - fn mass_properties_add_partial_zero() { - let m1 = MassProperties { - local_com: Point::origin(), - inv_mass: 2.0, - inv_principal_inertia_sqrt: na::zero(), - #[cfg(feature = "dim3")] - principal_inertia_local_frame: Rotation::identity(), - }; - let m2 = MassProperties { - local_com: Point::origin(), - inv_mass: 0.0, - #[cfg(feature = "dim2")] - inv_principal_inertia_sqrt: 1.0, - #[cfg(feature = "dim3")] - inv_principal_inertia_sqrt: Vector::new(1.0, 2.0, 3.0), - #[cfg(feature = "dim3")] - principal_inertia_local_frame: Rotation::identity(), - }; - let result = MassProperties { - local_com: Point::origin(), - inv_mass: 2.0, - #[cfg(feature = "dim2")] - inv_principal_inertia_sqrt: 1.0, - #[cfg(feature = "dim3")] - inv_principal_inertia_sqrt: Vector::new(1.0, 2.0, 3.0), - #[cfg(feature = "dim3")] - principal_inertia_local_frame: Rotation::identity(), - }; - - assert_eq!(m1 + m2, result); - assert_eq!(m2 + m1, result); - } - - #[test] - fn mass_properties_add_sub() { - // Check that addition and subtraction of mass properties behave as expected. - let c1 = ColliderBuilder::capsule_x(1.0, 2.0).build(); - let c2 = ColliderBuilder::capsule_y(3.0, 4.0).build(); - let c3 = ColliderBuilder::ball(5.0).build(); - - let m1 = c1.mass_properties(); - let m2 = c2.mass_properties(); - let m3 = c3.mass_properties(); - let m1m2m3 = m1 + m2 + m3; - - assert_relative_eq!(m1 + m2, m2 + m1, epsilon = 1.0e-6); - assert_relative_eq!(m1m2m3 - m1, m2 + m3, epsilon = 1.0e-6); - assert_relative_eq!(m1m2m3 - m2, m1 + m3, epsilon = 1.0e-6); - assert_relative_eq!(m1m2m3 - m3, m1 + m2, epsilon = 1.0e-6); - assert_relative_eq!(m1m2m3 - (m1 + m2), m3, epsilon = 1.0e-6); - assert_relative_eq!(m1m2m3 - (m1 + m3), m2, epsilon = 1.0e-6); - assert_relative_eq!(m1m2m3 - (m2 + m3), m1, epsilon = 1.0e-6); - assert_relative_eq!(m1m2m3 - m1 - m2, m3, epsilon = 1.0e-6); - assert_relative_eq!(m1m2m3 - m1 - m3, m2, epsilon = 1.0e-6); - assert_relative_eq!(m1m2m3 - m2 - m3, m1, epsilon = 1.0e-6); - assert_relative_eq!( - m1m2m3 - m1 - m2 - m3, - MassProperties::zero(), - epsilon = 1.0e-6 - ); - } -} diff --git a/src/dynamics/mass_properties_ball.rs b/src/dynamics/mass_properties_ball.rs deleted file mode 100644 index ac5790a..0000000 --- a/src/dynamics/mass_properties_ball.rs +++ /dev/null @@ -1,30 +0,0 @@ -use crate::dynamics::MassProperties; -#[cfg(feature = "dim3")] -use crate::math::Vector; -use crate::math::{Point, PrincipalAngularInertia}; - -impl MassProperties { - pub(crate) fn ball_volume_unit_angular_inertia( - radius: f32, - ) -> (f32, PrincipalAngularInertia) { - #[cfg(feature = "dim2")] - { - let volume = std::f32::consts::PI * radius * radius; - let i = radius * radius / 2.0; - (volume, i) - } - #[cfg(feature = "dim3")] - { - let volume = std::f32::consts::PI * radius * radius * radius * 4.0 / 3.0; - let i = radius * radius * 2.0 / 5.0; - - (volume, Vector::repeat(i)) - } - } - - pub(crate) fn from_ball(density: f32, radius: f32) -> Self { - let (vol, unit_i) = Self::ball_volume_unit_angular_inertia(radius); - let mass = vol * density; - Self::new(Point::origin(), mass, unit_i * mass) - } -} diff --git a/src/dynamics/mass_properties_capsule.rs b/src/dynamics/mass_properties_capsule.rs deleted file mode 100644 index 3b1b214..0000000 --- a/src/dynamics/mass_properties_capsule.rs +++ /dev/null @@ -1,39 +0,0 @@ -use crate::dynamics::MassProperties; -#[cfg(feature = "dim3")] -use crate::geometry::Capsule; -use crate::math::Point; - -impl MassProperties { - pub(crate) fn from_capsule(density: f32, a: Point, b: Point, radius: f32) -> Self { - let half_height = (b - a).norm() / 2.0; - let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius); - let (ball_vol, ball_unit_i) = Self::ball_volume_unit_angular_inertia(radius); - let cap_vol = cyl_vol + ball_vol; - let cap_mass = cap_vol * density; - let mut cap_unit_i = cyl_unit_i + ball_unit_i; - let local_com = na::center(&a, &b); - - #[cfg(feature = "dim2")] - { - let h = half_height * 2.0; - let extra = h * h * 0.5 + h * radius * 3.0 / 8.0; - cap_unit_i += extra; - Self::new(local_com, cap_mass, cap_unit_i * cap_mass) - } - - #[cfg(feature = "dim3")] - { - let h = half_height * 2.0; - let extra = h * h * 0.5 + h * radius * 3.0 / 8.0; - cap_unit_i.x += extra; - cap_unit_i.z += extra; - let local_frame = Capsule::new(a, b, radius).rotation_wrt_y(); - Self::with_principal_inertia_frame( - local_com, - cap_mass, - cap_unit_i * cap_mass, - local_frame, - ) - } - } -} diff --git a/src/dynamics/mass_properties_cone.rs b/src/dynamics/mass_properties_cone.rs deleted file mode 100644 index 12f831f..0000000 --- a/src/dynamics/mass_properties_cone.rs +++ /dev/null @@ -1,29 +0,0 @@ -use crate::dynamics::MassProperties; -use crate::math::{Point, PrincipalAngularInertia, Rotation, Vector}; - -impl MassProperties { - pub(crate) fn cone_y_volume_unit_inertia( - half_height: f32, - radius: f32, - ) -> (f32, PrincipalAngularInertia) { - let volume = radius * radius * std::f32::consts::PI * half_height * 2.0 / 3.0; - let sq_radius = radius * radius; - let sq_height = half_height * half_height * 4.0; - let off_principal = sq_radius * 3.0 / 20.0 + sq_height * 3.0 / 5.0; - let principal = sq_radius * 3.0 / 10.0; - - (volume, Vector::new(off_principal, principal, off_principal)) - } - - pub(crate) fn from_cone(density: f32, half_height: f32, radius: f32) -> Self { - let (cyl_vol, cyl_unit_i) = Self::cone_y_volume_unit_inertia(half_height, radius); - let cyl_mass = cyl_vol * density; - - Self::with_principal_inertia_frame( - Point::new(0.0, -half_height / 2.0, 0.0), - cyl_mass, - cyl_unit_i * cyl_mass, - Rotation::identity(), - ) - } -} diff --git a/src/dynamics/mass_properties_cuboid.rs b/src/dynamics/mass_properties_cuboid.rs deleted file mode 100644 index 2d870cf..0000000 --- a/src/dynamics/mass_properties_cuboid.rs +++ /dev/null @@ -1,33 +0,0 @@ -use crate::dynamics::MassProperties; -use crate::math::{Point, PrincipalAngularInertia, Vector}; - -impl MassProperties { - pub(crate) fn cuboid_volume_unit_inertia( - half_extents: Vector, - ) -> (f32, PrincipalAngularInertia) { - #[cfg(feature = "dim2")] - { - let volume = half_extents.x * half_extents.y * 4.0; - let ix = (half_extents.x * half_extents.x) / 3.0; - let iy = (half_extents.y * half_extents.y) / 3.0; - - (volume, ix + iy) - } - - #[cfg(feature = "dim3")] - { - let volume = half_extents.x * half_extents.y * half_extents.z * 8.0; - let ix = (half_extents.x * half_extents.x) / 3.0; - let iy = (half_extents.y * half_extents.y) / 3.0; - let iz = (half_extents.z * half_extents.z) / 3.0; - - (volume, Vector::new(iy + iz, ix + iz, ix + iy)) - } - } - - pub(crate) fn from_cuboid(density: f32, half_extents: Vector) -> Self { - let (vol, unit_i) = Self::cuboid_volume_unit_inertia(half_extents); - let mass = vol * density; - Self::new(Point::origin(), mass, unit_i * mass) - } -} diff --git a/src/dynamics/mass_properties_cylinder.rs b/src/dynamics/mass_properties_cylinder.rs deleted file mode 100644 index 7c8054a..0000000 --- a/src/dynamics/mass_properties_cylinder.rs +++ /dev/null @@ -1,40 +0,0 @@ -use crate::dynamics::MassProperties; -#[cfg(feature = "dim3")] -use crate::math::{Point, Rotation}; -use crate::math::{PrincipalAngularInertia, Vector}; - -impl MassProperties { - pub(crate) fn cylinder_y_volume_unit_inertia( - half_height: f32, - radius: f32, - ) -> (f32, PrincipalAngularInertia) { - #[cfg(feature = "dim2")] - { - Self::cuboid_volume_unit_inertia(Vector::new(radius, half_height)) - } - - #[cfg(feature = "dim3")] - { - let volume = half_height * radius * radius * std::f32::consts::PI * 2.0; - let sq_radius = radius * radius; - let sq_height = half_height * half_height * 4.0; - let off_principal = (sq_radius * 3.0 + sq_height) / 12.0; - - let inertia = Vector::new(off_principal, sq_radius / 2.0, off_principal); - (volume, inertia) - } - } - - #[cfg(feature = "dim3")] - pub(crate) fn from_cylinder(density: f32, half_height: f32, radius: f32) -> Self { - let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius); - let cyl_mass = cyl_vol * density; - - Self::with_principal_inertia_frame( - Point::origin(), - cyl_mass, - cyl_unit_i * cyl_mass, - Rotation::identity(), - ) - } -} diff --git a/src/dynamics/mass_properties_polygon.rs b/src/dynamics/mass_properties_polygon.rs deleted file mode 100644 index 8b0b811..0000000 --- a/src/dynamics/mass_properties_polygon.rs +++ /dev/null @@ -1,146 +0,0 @@ -#![allow(dead_code)] // TODO: remove this - -use crate::dynamics::MassProperties; -use crate::math::Point; - -impl MassProperties { - pub(crate) fn from_polygon(density: f32, vertices: &[Point]) -> MassProperties { - let (area, com) = convex_polygon_area_and_center_of_mass(vertices); - - if area == 0.0 { - return MassProperties::new(com, 0.0, 0.0); - } - - let mut itot = 0.0; - let factor = 1.0 / 6.0; - - let mut iterpeek = vertices.iter().peekable(); - let firstelement = *iterpeek.peek().unwrap(); // store first element to close the cycle in the end with unwrap_or - while let Some(elem) = iterpeek.next() { - let area = triangle_area(&com, elem, iterpeek.peek().unwrap_or(&firstelement)); - - // algorithm adapted from Box2D - let e1 = *elem - com; - let e2 = **(iterpeek.peek().unwrap_or(&firstelement)) - com; - - let ex1 = e1[0]; - let ey1 = e1[1]; - let ex2 = e2[0]; - let ey2 = e2[1]; - - let intx2 = ex1 * ex1 + ex2 * ex1 + ex2 * ex2; - let inty2 = ey1 * ey1 + ey2 * ey1 + ey2 * ey2; - - let ipart = factor * (intx2 + inty2); - - itot += ipart * area; - } - - Self::new(com, area * density, itot * density) - } -} - -fn convex_polygon_area_and_center_of_mass(convex_polygon: &[Point]) -> (f32, Point) { - let geometric_center = convex_polygon - .iter() - .fold(Point::origin(), |e1, e2| e1 + e2.coords) - / convex_polygon.len() as f32; - let mut res = Point::origin(); - let mut areasum = 0.0; - - let mut iterpeek = convex_polygon.iter().peekable(); - let firstelement = *iterpeek.peek().unwrap(); // Stores first element to close the cycle in the end with unwrap_or. - while let Some(elem) = iterpeek.next() { - let (a, b, c) = ( - elem, - iterpeek.peek().unwrap_or(&firstelement), - &geometric_center, - ); - let area = triangle_area(a, b, c); - let center = (a.coords + b.coords + c.coords) / 3.0; - - res += center * area; - areasum += area; - } - - if areasum == 0.0 { - (areasum, geometric_center) - } else { - (areasum, res / areasum) - } -} - -pub fn triangle_area(pa: &Point, pb: &Point, pc: &Point) -> f32 { - // Kahan's formula. - let a = na::distance(pa, pb); - let b = na::distance(pb, pc); - let c = na::distance(pc, pa); - - let (c, b, a) = sort3(&a, &b, &c); - let a = *a; - let b = *b; - let c = *c; - - let sqr = (a + (b + c)) * (c - (a - b)) * (c + (a - b)) * (a + (b - c)); - - sqr.sqrt() * 0.25 -} - -/// Sorts a set of three values in increasing order. -#[inline] -pub fn sort3<'a>(a: &'a f32, b: &'a f32, c: &'a f32) -> (&'a f32, &'a f32, &'a f32) { - let a_b = *a > *b; - let a_c = *a > *c; - let b_c = *b > *c; - - let sa; - let sb; - let sc; - - // Sort the three values. - // FIXME: move this to the utilities? - if a_b { - // a > b - if a_c { - // a > c - sc = a; - - if b_c { - // b > c - sa = c; - sb = b; - } else { - // b <= c - sa = b; - sb = c; - } - } else { - // a <= c - sa = b; - sb = a; - sc = c; - } - } else { - // a < b - if !a_c { - // a <= c - sa = a; - - if b_c { - // b > c - sb = c; - sc = b; - } else { - sb = b; - sc = c; - } - } else { - // a > c - sa = c; - sb = a; - sc = b; - } - } - - (sa, sb, sc) -} diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs index 6967904..28f149a 100644 --- a/src/dynamics/mod.rs +++ b/src/dynamics/mod.rs @@ -7,9 +7,9 @@ pub use self::joint::RevoluteJoint; pub use self::joint::{ BallJoint, FixedJoint, Joint, JointHandle, JointParams, JointSet, PrismaticJoint, }; -pub use self::mass_properties::MassProperties; pub use self::rigid_body::{ActivationStatus, BodyStatus, RigidBody, RigidBodyBuilder}; pub use self::rigid_body_set::{BodyPair, RigidBodyHandle, RigidBodySet}; +pub use buckler::shape::MassProperties; // #[cfg(not(feature = "parallel"))] pub(crate) use self::joint::JointGraphEdge; pub(crate) use self::rigid_body::RigidBodyChanges; @@ -20,15 +20,6 @@ pub(crate) use self::solver::ParallelIslandSolver; mod integration_parameters; mod joint; -mod mass_properties; -mod mass_properties_ball; -mod mass_properties_capsule; -#[cfg(feature = "dim3")] -mod mass_properties_cone; -mod mass_properties_cuboid; -mod mass_properties_cylinder; -#[cfg(feature = "dim2")] -mod mass_properties_polygon; mod rigid_body; mod rigid_body_set; mod solver; diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs index c552d57..f1ca4b6 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs @@ -1,7 +1,7 @@ use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody}; #[cfg(feature = "dim2")] use crate::math::SdpMatrix; -use crate::math::{AngularInertia, Isometry, Point, Rotation, SimdFloat, SIMD_WIDTH}; +use crate::math::{AngularInertia, Isometry, Point, Rotation, SimdReal, SIMD_WIDTH}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use simba::simd::SimdValue; @@ -10,17 +10,17 @@ pub(crate) struct WBallPositionConstraint { position1: [usize; SIMD_WIDTH], position2: [usize; SIMD_WIDTH], - local_com1: Point, - local_com2: Point, + local_com1: Point, + local_com2: Point, - im1: SimdFloat, - im2: SimdFloat, + im1: SimdReal, + im2: SimdReal, - ii1: AngularInertia, - ii2: AngularInertia, + ii1: AngularInertia, + ii2: AngularInertia, - local_anchor1: Point, - local_anchor2: Point, + local_anchor1: Point, + local_anchor2: Point, } impl WBallPositionConstraint { @@ -31,13 +31,13 @@ impl WBallPositionConstraint { ) -> Self { let local_com1 = Point::from(array![|ii| rbs1[ii].mass_properties.local_com; SIMD_WIDTH]); let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]); - let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii1 = AngularInertia::::from( + let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii1 = AngularInertia::::from( array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH], ) .squared(); - let ii2 = AngularInertia::::from( + let ii2 = AngularInertia::::from( array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], ) .squared(); @@ -97,7 +97,7 @@ impl WBallPositionConstraint { }; let inv_lhs = lhs.inverse_unchecked(); - let impulse = inv_lhs * -(err * SimdFloat::splat(params.joint_erp)); + let impulse = inv_lhs * -(err * SimdReal::splat(params.joint_erp)); position1.translation.vector += impulse * self.im1; position2.translation.vector -= impulse * self.im2; @@ -120,11 +120,11 @@ impl WBallPositionConstraint { #[derive(Debug)] pub(crate) struct WBallPositionGroundConstraint { position2: [usize; SIMD_WIDTH], - anchor1: Point, - im2: SimdFloat, - ii2: AngularInertia, - local_anchor2: Point, - local_com2: Point, + anchor1: Point, + im2: SimdReal, + ii2: AngularInertia, + local_anchor2: Point, + local_com2: Point, } impl WBallPositionGroundConstraint { @@ -141,8 +141,8 @@ impl WBallPositionGroundConstraint { } else { cparams[ii].local_anchor1 }; SIMD_WIDTH]); - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii2 = AngularInertia::::from( + let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2 = AngularInertia::::from( array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], ) .squared(); @@ -186,7 +186,7 @@ impl WBallPositionGroundConstraint { }; let inv_lhs = lhs.inverse_unchecked(); - let impulse = inv_lhs * -(err * SimdFloat::splat(params.joint_erp)); + let impulse = inv_lhs * -(err * SimdReal::splat(params.joint_erp)); position2.translation.vector -= impulse * self.im2; let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse)); diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs index b96f3b8..bbb709e 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs @@ -3,7 +3,7 @@ use crate::dynamics::{ BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, }; use crate::math::{ - AngVector, AngularInertia, Isometry, Point, SdpMatrix, SimdFloat, Vector, SIMD_WIDTH, + AngVector, AngularInertia, Isometry, Point, SdpMatrix, SimdReal, Vector, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use simba::simd::SimdValue; @@ -15,16 +15,16 @@ pub(crate) struct WBallVelocityConstraint { joint_id: [JointIndex; SIMD_WIDTH], - rhs: Vector, - pub(crate) impulse: Vector, + rhs: Vector, + pub(crate) impulse: Vector, - gcross1: Vector, - gcross2: Vector, + gcross1: Vector, + gcross2: Vector, - inv_lhs: SdpMatrix, + inv_lhs: SdpMatrix, - im1: SimdFloat, - im2: SimdFloat, + im1: SimdReal, + im2: SimdReal, } impl WBallVelocityConstraint { @@ -37,20 +37,20 @@ impl WBallVelocityConstraint { ) -> Self { let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); - let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii1_sqrt = AngularInertia::::from( + let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii1_sqrt = AngularInertia::::from( array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( + let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::::from( array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -62,8 +62,8 @@ impl WBallVelocityConstraint { let anchor1 = position1 * local_anchor1 - world_com1; let anchor2 = position2 * local_anchor2 - world_com2; - let vel1: Vector = linvel1 + angvel1.gcross(anchor1); - let vel2: Vector = linvel2 + angvel2.gcross(anchor2); + let vel1: Vector = linvel1 + angvel1.gcross(anchor1); + let vel2: Vector = linvel2 + angvel2.gcross(anchor2); let rhs = -(vel1 - vel2); let lhs; @@ -99,7 +99,7 @@ impl WBallVelocityConstraint { mj_lambda2, im1, im2, - impulse: impulse * SimdFloat::splat(params.warmstart_coeff), + impulse: impulse * SimdReal::splat(params.warmstart_coeff), gcross1, gcross2, rhs, @@ -141,7 +141,7 @@ impl WBallVelocityConstraint { } pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda1: DeltaVel = DeltaVel { + let mut mj_lambda1: DeltaVel = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], ), @@ -149,7 +149,7 @@ impl WBallVelocityConstraint { array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], ), }; - let mut mj_lambda2: DeltaVel = DeltaVel { + let mut mj_lambda2: DeltaVel = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], ), @@ -195,11 +195,11 @@ impl WBallVelocityConstraint { pub(crate) struct WBallVelocityGroundConstraint { mj_lambda2: [usize; SIMD_WIDTH], joint_id: [JointIndex; SIMD_WIDTH], - rhs: Vector, - pub(crate) impulse: Vector, - gcross2: Vector, - inv_lhs: SdpMatrix, - im2: SimdFloat, + rhs: Vector, + pub(crate) impulse: Vector, + gcross2: Vector, + inv_lhs: SdpMatrix, + im2: SimdReal, } impl WBallVelocityGroundConstraint { @@ -213,7 +213,7 @@ impl WBallVelocityGroundConstraint { ) -> Self { let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); let local_anchor1 = Point::from( array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH], @@ -221,10 +221,10 @@ impl WBallVelocityGroundConstraint { let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( + let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::::from( array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -237,8 +237,8 @@ impl WBallVelocityGroundConstraint { let anchor1 = position1 * local_anchor1 - world_com1; let anchor2 = position2 * local_anchor2 - world_com2; - let vel1: Vector = linvel1 + angvel1.gcross(anchor1); - let vel2: Vector = linvel2 + angvel2.gcross(anchor2); + let vel1: Vector = linvel1 + angvel1.gcross(anchor1); + let vel2: Vector = linvel2 + angvel2.gcross(anchor2); let rhs = vel2 - vel1; let lhs; @@ -267,7 +267,7 @@ impl WBallVelocityGroundConstraint { joint_id, mj_lambda2, im2, - impulse: impulse * SimdFloat::splat(params.warmstart_coeff), + impulse: impulse * SimdReal::splat(params.warmstart_coeff), gcross2, rhs, inv_lhs, @@ -294,7 +294,7 @@ impl WBallVelocityGroundConstraint { } pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda2: DeltaVel = DeltaVel { + let mut mj_lambda2: DeltaVel = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], ), diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs index 7c87b2c..79c69c6 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs @@ -5,7 +5,7 @@ use crate::dynamics::{ FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, }; use crate::math::{ - AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, SimdFloat, SpacialVector, Vector, + AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, SimdReal, SpacialVector, Vector, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; @@ -24,29 +24,29 @@ pub(crate) struct WFixedVelocityConstraint { joint_id: [JointIndex; SIMD_WIDTH], - impulse: SpacialVector, + impulse: SpacialVector, #[cfg(feature = "dim3")] - inv_lhs: Matrix6, // FIXME: replace by Cholesky. + inv_lhs: Matrix6, // FIXME: replace by Cholesky. #[cfg(feature = "dim3")] - rhs: Vector6, + rhs: Vector6, #[cfg(feature = "dim2")] - inv_lhs: Matrix3, + inv_lhs: Matrix3, #[cfg(feature = "dim2")] - rhs: Vector3, + rhs: Vector3, - im1: SimdFloat, - im2: SimdFloat, + im1: SimdReal, + im2: SimdReal, - ii1: AngularInertia, - ii2: AngularInertia, + ii1: AngularInertia, + ii2: AngularInertia, - ii1_sqrt: AngularInertia, - ii2_sqrt: AngularInertia, + ii1_sqrt: AngularInertia, + ii2_sqrt: AngularInertia, - r1: Vector, - r2: Vector, + r1: Vector, + r2: Vector, } impl WFixedVelocityConstraint { @@ -59,20 +59,20 @@ impl WFixedVelocityConstraint { ) -> Self { let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); - let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii1_sqrt = AngularInertia::::from( + let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii1_sqrt = AngularInertia::::from( array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( + let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::::from( array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -150,7 +150,7 @@ impl WFixedVelocityConstraint { ii2, ii1_sqrt, ii2_sqrt, - impulse: impulse * SimdFloat::splat(params.warmstart_coeff), + impulse: impulse * SimdReal::splat(params.warmstart_coeff), inv_lhs, r1, r2, @@ -203,7 +203,7 @@ impl WFixedVelocityConstraint { } pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda1: DeltaVel = DeltaVel { + let mut mj_lambda1: DeltaVel = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], ), @@ -211,7 +211,7 @@ impl WFixedVelocityConstraint { array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], ), }; - let mut mj_lambda2: DeltaVel = DeltaVel { + let mut mj_lambda2: DeltaVel = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], ), @@ -279,22 +279,22 @@ pub(crate) struct WFixedVelocityGroundConstraint { joint_id: [JointIndex; SIMD_WIDTH], - impulse: SpacialVector, + impulse: SpacialVector, #[cfg(feature = "dim3")] - inv_lhs: Matrix6, // FIXME: replace by Cholesky. + inv_lhs: Matrix6, // FIXME: replace by Cholesky. #[cfg(feature = "dim3")] - rhs: Vector6, + rhs: Vector6, #[cfg(feature = "dim2")] - inv_lhs: Matrix3, + inv_lhs: Matrix3, #[cfg(feature = "dim2")] - rhs: Vector3, + rhs: Vector3, - im2: SimdFloat, - ii2: AngularInertia, - ii2_sqrt: AngularInertia, - r2: Vector, + im2: SimdReal, + ii2: AngularInertia, + ii2_sqrt: AngularInertia, + r2: Vector, } impl WFixedVelocityGroundConstraint { @@ -308,15 +308,15 @@ impl WFixedVelocityGroundConstraint { ) -> Self { let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( + let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::::from( array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -386,7 +386,7 @@ impl WFixedVelocityGroundConstraint { im2, ii2, ii2_sqrt, - impulse: impulse * SimdFloat::splat(params.warmstart_coeff), + impulse: impulse * SimdReal::splat(params.warmstart_coeff), inv_lhs, r2, rhs, @@ -421,7 +421,7 @@ impl WFixedVelocityGroundConstraint { } pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda2: DeltaVel = DeltaVel { + let mut mj_lambda2: DeltaVel = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], ), diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs index bb81f23..c05c08e 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs @@ -5,7 +5,7 @@ use crate::dynamics::{ IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody, }; use crate::math::{ - AngVector, AngularInertia, Isometry, Point, SimdBool, SimdFloat, Vector, SIMD_WIDTH, + AngVector, AngularInertia, Isometry, Point, SimdBool, SimdReal, Vector, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; #[cfg(feature = "dim3")] @@ -28,37 +28,37 @@ pub(crate) struct WPrismaticVelocityConstraint { joint_id: [JointIndex; SIMD_WIDTH], - r1: Vector, - r2: Vector, + r1: Vector, + r2: Vector, #[cfg(feature = "dim3")] - inv_lhs: Matrix5, + inv_lhs: Matrix5, #[cfg(feature = "dim3")] - rhs: Vector5, + rhs: Vector5, #[cfg(feature = "dim3")] - impulse: Vector5, + impulse: Vector5, #[cfg(feature = "dim2")] - inv_lhs: Matrix2, + inv_lhs: Matrix2, #[cfg(feature = "dim2")] - rhs: Vector2, + rhs: Vector2, #[cfg(feature = "dim2")] - impulse: Vector2, + impulse: Vector2, - limits_impulse: SimdFloat, - limits_forcedirs: Option<(Vector, Vector)>, - limits_rhs: SimdFloat, + limits_impulse: SimdReal, + limits_forcedirs: Option<(Vector, Vector)>, + limits_rhs: SimdReal, #[cfg(feature = "dim2")] - basis1: Vector2, + basis1: Vector2, #[cfg(feature = "dim3")] - basis1: Matrix3x2, + basis1: Matrix3x2, - im1: SimdFloat, - im2: SimdFloat, + im1: SimdReal, + im2: SimdReal, - ii1_sqrt: AngularInertia, - ii2_sqrt: AngularInertia, + ii1_sqrt: AngularInertia, + ii2_sqrt: AngularInertia, } impl WPrismaticVelocityConstraint { @@ -71,20 +71,20 @@ impl WPrismaticVelocityConstraint { ) -> Self { let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); - let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii1_sqrt = AngularInertia::::from( + let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii1_sqrt = AngularInertia::::from( array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( + let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::::from( array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -199,14 +199,14 @@ impl WPrismaticVelocityConstraint { // FIXME: we should allow both limits to be active at // the same time + allow predictive constraint activation. - let min_limit = SimdFloat::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]); - let max_limit = SimdFloat::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]); - let lim_impulse = SimdFloat::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]); + let min_limit = SimdReal::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]); + let max_limit = SimdReal::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]); + let lim_impulse = SimdReal::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]); let min_enabled = dist.simd_lt(min_limit); let max_enabled = dist.simd_gt(max_limit); - let _0: SimdFloat = na::zero(); - let _1: SimdFloat = na::one(); + let _0: SimdReal = na::zero(); + let _1: SimdReal = na::one(); let sign = _1.select(min_enabled, (-_1).select(max_enabled, _0)); if sign != _0 { @@ -224,8 +224,8 @@ impl WPrismaticVelocityConstraint { ii1_sqrt, im2, ii2_sqrt, - impulse: impulse * SimdFloat::splat(params.warmstart_coeff), - limits_impulse: limits_impulse * SimdFloat::splat(params.warmstart_coeff), + impulse: impulse * SimdReal::splat(params.warmstart_coeff), + limits_impulse: limits_impulse * SimdReal::splat(params.warmstart_coeff), limits_forcedirs, limits_rhs, basis1, @@ -383,34 +383,34 @@ pub(crate) struct WPrismaticVelocityGroundConstraint { joint_id: [JointIndex; SIMD_WIDTH], - r2: Vector, + r2: Vector, #[cfg(feature = "dim2")] - inv_lhs: Matrix2, + inv_lhs: Matrix2, #[cfg(feature = "dim2")] - rhs: Vector2, + rhs: Vector2, #[cfg(feature = "dim2")] - impulse: Vector2, + impulse: Vector2, #[cfg(feature = "dim3")] - inv_lhs: Matrix5, + inv_lhs: Matrix5, #[cfg(feature = "dim3")] - rhs: Vector5, + rhs: Vector5, #[cfg(feature = "dim3")] - impulse: Vector5, + impulse: Vector5, - limits_impulse: SimdFloat, - limits_rhs: SimdFloat, + limits_impulse: SimdReal, + limits_rhs: SimdReal, - axis2: Vector, + axis2: Vector, #[cfg(feature = "dim2")] - basis1: Vector2, + basis1: Vector2, #[cfg(feature = "dim3")] - basis1: Matrix3x2, - limits_forcedir2: Option>, + basis1: Matrix3x2, + limits_forcedir2: Option>, - im2: SimdFloat, - ii2_sqrt: AngularInertia, + im2: SimdReal, + ii2_sqrt: AngularInertia, } impl WPrismaticVelocityGroundConstraint { @@ -424,15 +424,15 @@ impl WPrismaticVelocityGroundConstraint { ) -> Self { let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( + let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::::from( array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -551,14 +551,14 @@ impl WPrismaticVelocityGroundConstraint { // FIXME: we should allow both limits to be active at // the same time + allow predictive constraint activation. - let min_limit = SimdFloat::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]); - let max_limit = SimdFloat::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]); - let lim_impulse = SimdFloat::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]); + let min_limit = SimdReal::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]); + let max_limit = SimdReal::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]); + let lim_impulse = SimdReal::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]); let use_min = dist.simd_lt(min_limit); let use_max = dist.simd_gt(max_limit); - let _0: SimdFloat = na::zero(); - let _1: SimdFloat = na::one(); + let _0: SimdReal = na::zero(); + let _1: SimdReal = na::one(); let sign = _1.select(use_min, (-_1).select(use_max, _0)); if sign != _0 { @@ -573,8 +573,8 @@ impl WPrismaticVelocityGroundConstraint { mj_lambda2, im2, ii2_sqrt, - impulse: impulse * SimdFloat::splat(params.warmstart_coeff), - limits_impulse: limits_impulse * SimdFloat::splat(params.warmstart_coeff), + impulse: impulse * SimdReal::splat(params.warmstart_coeff), + limits_impulse: limits_impulse * SimdReal::splat(params.warmstart_coeff), basis1, inv_lhs, rhs, diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs index 5eeac18..61122a4 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs @@ -4,7 +4,7 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody, }; -use crate::math::{AngVector, AngularInertia, Isometry, Point, SimdFloat, Vector, SIMD_WIDTH}; +use crate::math::{AngVector, AngularInertia, Isometry, Point, SimdReal, Vector, SIMD_WIDTH}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; @@ -15,20 +15,20 @@ pub(crate) struct WRevoluteVelocityConstraint { joint_id: [JointIndex; SIMD_WIDTH], - r1: Vector, - r2: Vector, + r1: Vector, + r2: Vector, - inv_lhs: Matrix5, - rhs: Vector5, - impulse: Vector5, + inv_lhs: Matrix5, + rhs: Vector5, + impulse: Vector5, - basis1: Matrix3x2, + basis1: Matrix3x2, - im1: SimdFloat, - im2: SimdFloat, + im1: SimdReal, + im2: SimdReal, - ii1_sqrt: AngularInertia, - ii2_sqrt: AngularInertia, + ii1_sqrt: AngularInertia, + ii2_sqrt: AngularInertia, } impl WRevoluteVelocityConstraint { @@ -41,20 +41,20 @@ impl WRevoluteVelocityConstraint { ) -> Self { let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); - let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii1_sqrt = AngularInertia::::from( + let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii1_sqrt = AngularInertia::::from( array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( + let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::::from( array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -115,7 +115,7 @@ impl WRevoluteVelocityConstraint { basis1, im2, ii2_sqrt, - impulse: impulse * SimdFloat::splat(params.warmstart_coeff), + impulse: impulse * SimdReal::splat(params.warmstart_coeff), inv_lhs, rhs, r1, @@ -231,17 +231,17 @@ pub(crate) struct WRevoluteVelocityGroundConstraint { joint_id: [JointIndex; SIMD_WIDTH], - r2: Vector, + r2: Vector, - inv_lhs: Matrix5, - rhs: Vector5, - impulse: Vector5, + inv_lhs: Matrix5, + rhs: Vector5, + impulse: Vector5, - basis1: Matrix3x2, + basis1: Matrix3x2, - im2: SimdFloat, + im2: SimdReal, - ii2_sqrt: AngularInertia, + ii2_sqrt: AngularInertia, } impl WRevoluteVelocityGroundConstraint { @@ -255,15 +255,15 @@ impl WRevoluteVelocityGroundConstraint { ) -> Self { let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( + let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::::from( array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -322,7 +322,7 @@ impl WRevoluteVelocityGroundConstraint { mj_lambda2, im2, ii2_sqrt, - impulse: impulse * SimdFloat::splat(params.warmstart_coeff), + impulse: impulse * SimdReal::splat(params.warmstart_coeff), basis1, inv_lhs, rhs, diff --git a/src/dynamics/solver/position_constraint_wide.rs b/src/dynamics/solver/position_constraint_wide.rs index ecc0fc3..260d495 100644 --- a/src/dynamics/solver/position_constraint_wide.rs +++ b/src/dynamics/solver/position_constraint_wide.rs @@ -2,7 +2,7 @@ use super::AnyPositionConstraint; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::{ContactManifold, KinematicsCategory}; use crate::math::{ - AngularInertia, Isometry, Point, Rotation, SimdFloat, Translation, Vector, MAX_MANIFOLD_POINTS, + AngularInertia, Isometry, Point, Rotation, SimdReal, Translation, Vector, MAX_MANIFOLD_POINTS, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WCross, WDot}; @@ -14,16 +14,16 @@ pub(crate) struct WPositionConstraint { pub rb1: [usize; SIMD_WIDTH], pub rb2: [usize; SIMD_WIDTH], // NOTE: the points are relative to the center of masses. - pub local_p1: [Point; MAX_MANIFOLD_POINTS], - pub local_p2: [Point; MAX_MANIFOLD_POINTS], - pub local_n1: Vector, - pub radius: SimdFloat, - pub im1: SimdFloat, - pub im2: SimdFloat, - pub ii1: AngularInertia, - pub ii2: AngularInertia, - pub erp: SimdFloat, - pub max_linear_correction: SimdFloat, + pub local_p1: [Point; MAX_MANIFOLD_POINTS], + pub local_p2: [Point; MAX_MANIFOLD_POINTS], + pub local_n1: Vector, + pub radius: SimdReal, + pub im1: SimdReal, + pub im2: SimdReal, + pub ii1: AngularInertia, + pub ii2: AngularInertia, + pub erp: SimdReal, + pub max_linear_correction: SimdReal, pub num_contacts: u8, } @@ -38,18 +38,18 @@ impl WPositionConstraint { let rbs1 = array![|ii| bodies.get(manifolds[ii].data.body_pair.body1).unwrap(); SIMD_WIDTH]; let rbs2 = array![|ii| bodies.get(manifolds[ii].data.body_pair.body2).unwrap(); SIMD_WIDTH]; - let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let sqrt_ii1: AngularInertia = + let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let sqrt_ii1: AngularInertia = AngularInertia::from(array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let sqrt_ii2: AngularInertia = + let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let sqrt_ii2: AngularInertia = AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); let local_n1 = Vector::from(array![|ii| manifolds[ii].local_n1; SIMD_WIDTH]); let local_n2 = Vector::from(array![|ii| manifolds[ii].local_n2; SIMD_WIDTH]); - let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]); - let radius2 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]); + let radius1 = SimdReal::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]); + let radius2 = SimdReal::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]); let delta1 = Isometry::from(array![|ii| manifolds[ii].data.delta1; SIMD_WIDTH]); let delta2 = Isometry::from(array![|ii| manifolds[ii].data.delta2; SIMD_WIDTH]); @@ -57,7 +57,7 @@ impl WPositionConstraint { let rb1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - let radius = radius1 + radius2 /*- SimdFloat::splat(params.allowed_linear_error)*/; + let radius = radius1 + radius2 /*- SimdReal::splat(params.allowed_linear_error)*/; for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) { let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH]; @@ -74,8 +74,8 @@ impl WPositionConstraint { im2, ii1: sqrt_ii1.squared(), ii2: sqrt_ii2.squared(), - erp: SimdFloat::splat(params.erp), - max_linear_correction: SimdFloat::splat(params.max_linear_correction), + erp: SimdReal::splat(params.erp), + max_linear_correction: SimdReal::splat(params.max_linear_correction), num_contacts: num_points as u8, }; @@ -119,7 +119,7 @@ impl WPositionConstraint { // Compute jacobians. let mut pos1 = Isometry::from(array![|ii| positions[self.rb1[ii]]; SIMD_WIDTH]); let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]); - let allowed_err = SimdFloat::splat(params.allowed_linear_error); + let allowed_err = SimdReal::splat(params.allowed_linear_error); let target_dist = self.radius - allowed_err; for k in 0..self.num_contacts as usize { @@ -133,7 +133,7 @@ impl WPositionConstraint { let dist = sqdist.simd_sqrt(); let n = dpos / dist; let err = ((dist - target_dist) * self.erp) - .simd_clamp(-self.max_linear_correction, SimdFloat::zero()); + .simd_clamp(-self.max_linear_correction, SimdReal::zero()); let dp1 = p1.coords - pos1.translation.vector; let dp2 = p2.coords - pos2.translation.vector; @@ -173,7 +173,7 @@ impl WPositionConstraint { // Compute jacobians. let mut pos1 = Isometry::from(array![|ii| positions[self.rb1[ii]]; SIMD_WIDTH]); let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]); - let allowed_err = SimdFloat::splat(params.allowed_linear_error); + let allowed_err = SimdReal::splat(params.allowed_linear_error); let target_dist = self.radius - allowed_err; for k in 0..self.num_contacts as usize { @@ -188,7 +188,7 @@ impl WPositionConstraint { // NOTE: only works for the point-point case. let p1 = p2 - n1 * dist; let err = ((dist - target_dist) * self.erp) - .simd_clamp(-self.max_linear_correction, SimdFloat::zero()); + .simd_clamp(-self.max_linear_correction, SimdReal::zero()); let dp1 = p1.coords - pos1.translation.vector; let dp2 = p2.coords - pos2.translation.vector; diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs index 1609709..2531b2e 100644 --- a/src/dynamics/solver/position_ground_constraint_wide.rs +++ b/src/dynamics/solver/position_ground_constraint_wide.rs @@ -2,7 +2,7 @@ use super::AnyPositionConstraint; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::{ContactManifold, KinematicsCategory}; use crate::math::{ - AngularInertia, Isometry, Point, Rotation, SimdFloat, Translation, Vector, MAX_MANIFOLD_POINTS, + AngularInertia, Isometry, Point, Rotation, SimdReal, Translation, Vector, MAX_MANIFOLD_POINTS, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WCross, WDot}; @@ -13,14 +13,14 @@ use simba::simd::{SimdBool as _, SimdComplexField, SimdPartialOrd, SimdValue}; pub(crate) struct WPositionGroundConstraint { pub rb2: [usize; SIMD_WIDTH], // NOTE: the points are relative to the center of masses. - pub p1: [Point; MAX_MANIFOLD_POINTS], - pub local_p2: [Point; MAX_MANIFOLD_POINTS], - pub n1: Vector, - pub radius: SimdFloat, - pub im2: SimdFloat, - pub ii2: AngularInertia, - pub erp: SimdFloat, - pub max_linear_correction: SimdFloat, + pub p1: [Point; MAX_MANIFOLD_POINTS], + pub local_p2: [Point; MAX_MANIFOLD_POINTS], + pub n1: Vector, + pub radius: SimdReal, + pub im2: SimdReal, + pub ii2: AngularInertia, + pub erp: SimdReal, + pub max_linear_correction: SimdReal, pub num_contacts: u8, } @@ -45,8 +45,8 @@ impl WPositionGroundConstraint { } } - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let sqrt_ii2: AngularInertia = + let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let sqrt_ii2: AngularInertia = AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); let local_n1 = Vector::from( @@ -63,15 +63,15 @@ impl WPositionGroundConstraint { array![|ii| if flipped[ii] { manifolds[ii].data.delta1 } else { manifolds[ii].data.delta2 }; SIMD_WIDTH], ); - let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]); - let radius2 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]); + let radius1 = SimdReal::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]); + let radius2 = SimdReal::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]); let coll_pos1 = delta1 * Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]); let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - let radius = radius1 + radius2 /*- SimdFloat::splat(params.allowed_linear_error)*/; + let radius = radius1 + radius2 /*- SimdReal::splat(params.allowed_linear_error)*/; let n1 = coll_pos1 * local_n1; @@ -87,8 +87,8 @@ impl WPositionGroundConstraint { radius, im2, ii2: sqrt_ii2.squared(), - erp: SimdFloat::splat(params.erp), - max_linear_correction: SimdFloat::splat(params.max_linear_correction), + erp: SimdReal::splat(params.erp), + max_linear_correction: SimdReal::splat(params.max_linear_correction), num_contacts: num_points as u8, }; @@ -131,7 +131,7 @@ impl WPositionGroundConstraint { // FIXME: can we avoid most of the multiplications by pos1/pos2? // Compute jacobians. let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]); - let allowed_err = SimdFloat::splat(params.allowed_linear_error); + let allowed_err = SimdReal::splat(params.allowed_linear_error); let target_dist = self.radius - allowed_err; for k in 0..self.num_contacts as usize { @@ -145,7 +145,7 @@ impl WPositionGroundConstraint { let dist = sqdist.simd_sqrt(); let n = dpos / dist; let err = ((dist - target_dist) * self.erp) - .simd_clamp(-self.max_linear_correction, SimdFloat::zero()); + .simd_clamp(-self.max_linear_correction, SimdReal::zero()); let dp2 = p2.coords - pos2.translation.vector; let gcross2 = -dp2.gcross(n); let ii_gcross2 = self.ii2.transform_vector(gcross2); @@ -173,7 +173,7 @@ impl WPositionGroundConstraint { // FIXME: can we avoid most of the multiplications by pos1/pos2? // Compute jacobians. let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]); - let allowed_err = SimdFloat::splat(params.allowed_linear_error); + let allowed_err = SimdReal::splat(params.allowed_linear_error); let target_dist = self.radius - allowed_err; for k in 0..self.num_contacts as usize { @@ -186,7 +186,7 @@ impl WPositionGroundConstraint { // NOTE: this condition does not seem to be useful perfomancewise? if dist.simd_lt(target_dist).any() { let err = ((dist - target_dist) * self.erp) - .simd_clamp(-self.max_linear_correction, SimdFloat::zero()); + .simd_clamp(-self.max_linear_correction, SimdReal::zero()); let dp2 = p2.coords - pos2.translation.vector; let gcross2 = -dp2.gcross(n1); diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index a4efb9a..11e7a9d 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -2,7 +2,7 @@ use super::{AnyVelocityConstraint, DeltaVel}; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{ - AngVector, AngularInertia, Isometry, Point, SimdFloat, Vector, DIM, MAX_MANIFOLD_POINTS, + AngVector, AngularInertia, Isometry, Point, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WBasis, WCross, WDot}; @@ -11,11 +11,11 @@ use simba::simd::{SimdPartialOrd, SimdValue}; #[derive(Copy, Clone, Debug)] pub(crate) struct WVelocityConstraintElementPart { - pub gcross1: AngVector, - pub gcross2: AngVector, - pub rhs: SimdFloat, - pub impulse: SimdFloat, - pub r: SimdFloat, + pub gcross1: AngVector, + pub gcross2: AngVector, + pub rhs: SimdReal, + pub impulse: SimdReal, + pub r: SimdReal, } impl WVelocityConstraintElementPart { @@ -23,9 +23,9 @@ impl WVelocityConstraintElementPart { Self { gcross1: AngVector::zero(), gcross2: AngVector::zero(), - rhs: SimdFloat::zero(), - impulse: SimdFloat::zero(), - r: SimdFloat::zero(), + rhs: SimdReal::zero(), + impulse: SimdReal::zero(), + r: SimdReal::zero(), } } } @@ -47,12 +47,12 @@ impl WVelocityConstraintElement { #[derive(Copy, Clone, Debug)] pub(crate) struct WVelocityConstraint { - pub dir1: Vector, // Non-penetration force direction for the first body. + pub dir1: Vector, // Non-penetration force direction for the first body. pub elements: [WVelocityConstraintElement; MAX_MANIFOLD_POINTS], pub num_contacts: u8, - pub im1: SimdFloat, - pub im2: SimdFloat, - pub limit: SimdFloat, + pub im1: SimdReal, + pub im2: SimdReal, + pub limit: SimdReal, pub mj_lambda1: [usize; SIMD_WIDTH], pub mj_lambda2: [usize; SIMD_WIDTH], pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH], @@ -68,29 +68,29 @@ impl WVelocityConstraint { out_constraints: &mut Vec, push: bool, ) { - let inv_dt = SimdFloat::splat(params.inv_dt()); + let inv_dt = SimdReal::splat(params.inv_dt()); let rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH]; let rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH]; let delta1 = Isometry::from(array![|ii| manifolds[ii].data.delta1; SIMD_WIDTH]); let delta2 = Isometry::from(array![|ii| manifolds[ii].data.delta2; SIMD_WIDTH]); - let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii1: AngularInertia = + let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii1: AngularInertia = AngularInertia::from(array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); let pos1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii2: AngularInertia = + let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2: AngularInertia = AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); @@ -103,14 +103,13 @@ impl WVelocityConstraint { let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - let friction = SimdFloat::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]); - let restitution = SimdFloat::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]); - let restitution_velocity_threshold = - SimdFloat::splat(params.restitution_velocity_threshold); + let friction = SimdReal::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]); + let restitution = SimdReal::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]); + let restitution_velocity_threshold = SimdReal::splat(params.restitution_velocity_threshold); let warmstart_multiplier = - SimdFloat::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]); - let warmstart_coeff = warmstart_multiplier * SimdFloat::splat(params.warmstart_coeff); + SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]); + let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff); for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) { let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH]; @@ -137,10 +136,10 @@ impl WVelocityConstraint { let p2 = coll_pos2 * Point::from(array![|ii| manifold_points[ii][k].local_p2; SIMD_WIDTH]); - let dist = SimdFloat::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); + let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); let impulse = - SimdFloat::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]); + SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]); let dp1 = p1 - world_com1; let dp2 = p2 - world_com2; @@ -153,13 +152,13 @@ impl WVelocityConstraint { let gcross1 = ii1.transform_vector(dp1.gcross(force_dir1)); let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1)); - let r = SimdFloat::splat(1.0) + let r = SimdReal::splat(1.0) / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); let mut rhs = (vel1 - vel2).dot(&force_dir1); let use_restitution = rhs.simd_le(-restitution_velocity_threshold); let rhs_with_restitution = rhs + rhs * restitution; rhs = rhs_with_restitution.select(use_restitution, rhs); - rhs += dist.simd_max(SimdFloat::zero()) * inv_dt; + rhs += dist.simd_max(SimdReal::zero()) * inv_dt; constraint.elements[k].normal_part = WVelocityConstraintElementPart { gcross1, @@ -175,17 +174,17 @@ impl WVelocityConstraint { for j in 0..DIM - 1 { #[cfg(feature = "dim2")] - let impulse = SimdFloat::from( + let impulse = SimdReal::from( array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH], ); #[cfg(feature = "dim3")] - let impulse = SimdFloat::from( + let impulse = SimdReal::from( array![|ii| manifold_points[ii][k].data.tangent_impulse[j]; SIMD_WIDTH], ); let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j])); let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j])); - let r = SimdFloat::splat(1.0) + let r = SimdReal::splat(1.0) / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); let rhs = (vel1 - vel2).dot(&tangents1[j]); @@ -309,7 +308,7 @@ impl WVelocityConstraint { - self.dir1.dot(&mj_lambda2.linear) + elt.gcross2.gdot(mj_lambda2.angular) + elt.rhs; - let new_impulse = (elt.impulse - elt.r * dimpulse).simd_max(SimdFloat::zero()); + let new_impulse = (elt.impulse - elt.r * dimpulse).simd_max(SimdReal::zero()); let dlambda = new_impulse - elt.impulse; elt.impulse = new_impulse; diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index 84c4182..ad16efe 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -2,7 +2,7 @@ use super::{AnyVelocityConstraint, DeltaVel}; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{ - AngVector, AngularInertia, Isometry, Point, SimdFloat, Vector, DIM, MAX_MANIFOLD_POINTS, + AngVector, AngularInertia, Isometry, Point, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WBasis, WCross, WDot}; @@ -11,19 +11,19 @@ use simba::simd::{SimdPartialOrd, SimdValue}; #[derive(Copy, Clone, Debug)] pub(crate) struct WVelocityGroundConstraintElementPart { - pub gcross2: AngVector, - pub rhs: SimdFloat, - pub impulse: SimdFloat, - pub r: SimdFloat, + pub gcross2: AngVector, + pub rhs: SimdReal, + pub impulse: SimdReal, + pub r: SimdReal, } impl WVelocityGroundConstraintElementPart { pub fn zero() -> Self { Self { gcross2: AngVector::zero(), - rhs: SimdFloat::zero(), - impulse: SimdFloat::zero(), - r: SimdFloat::zero(), + rhs: SimdReal::zero(), + impulse: SimdReal::zero(), + r: SimdReal::zero(), } } } @@ -45,11 +45,11 @@ impl WVelocityGroundConstraintElement { #[derive(Copy, Clone, Debug)] pub(crate) struct WVelocityGroundConstraint { - pub dir1: Vector, // Non-penetration force direction for the first body. + pub dir1: Vector, // Non-penetration force direction for the first body. pub elements: [WVelocityGroundConstraintElement; MAX_MANIFOLD_POINTS], pub num_contacts: u8, - pub im2: SimdFloat, - pub limit: SimdFloat, + pub im2: SimdReal, + pub limit: SimdReal, pub mj_lambda2: [usize; SIMD_WIDTH], pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH], pub manifold_contact_id: usize, @@ -64,7 +64,7 @@ impl WVelocityGroundConstraint { out_constraints: &mut Vec, push: bool, ) { - let inv_dt = SimdFloat::splat(params.inv_dt()); + let inv_dt = SimdReal::splat(params.inv_dt()); let mut rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH]; let mut rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH]; let mut flipped = [false; SIMD_WIDTH]; @@ -76,15 +76,15 @@ impl WVelocityGroundConstraint { } } - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii2: AngularInertia = + let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2: AngularInertia = AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let pos1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); @@ -109,14 +109,13 @@ impl WVelocityGroundConstraint { let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - let friction = SimdFloat::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]); - let restitution = SimdFloat::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]); - let restitution_velocity_threshold = - SimdFloat::splat(params.restitution_velocity_threshold); + let friction = SimdReal::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]); + let restitution = SimdReal::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]); + let restitution_velocity_threshold = SimdReal::splat(params.restitution_velocity_threshold); let warmstart_multiplier = - SimdFloat::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]); - let warmstart_coeff = warmstart_multiplier * SimdFloat::splat(params.warmstart_coeff); + SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]); + let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff); for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) { let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH]; @@ -143,10 +142,10 @@ impl WVelocityGroundConstraint { array![|ii| if flipped[ii] { manifold_points[ii][k].local_p1 } else { manifold_points[ii][k].local_p2 }; SIMD_WIDTH], ); - let dist = SimdFloat::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); + let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); let impulse = - SimdFloat::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]); + SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]); let dp1 = p1 - world_com1; let dp2 = p2 - world_com2; @@ -157,12 +156,12 @@ impl WVelocityGroundConstraint { { let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1)); - let r = SimdFloat::splat(1.0) / (im2 + gcross2.gdot(gcross2)); + let r = SimdReal::splat(1.0) / (im2 + gcross2.gdot(gcross2)); let mut rhs = (vel1 - vel2).dot(&force_dir1); let use_restitution = rhs.simd_le(-restitution_velocity_threshold); let rhs_with_restitution = rhs + rhs * restitution; rhs = rhs_with_restitution.select(use_restitution, rhs); - rhs += dist.simd_max(SimdFloat::zero()) * inv_dt; + rhs += dist.simd_max(SimdReal::zero()) * inv_dt; constraint.elements[k].normal_part = WVelocityGroundConstraintElementPart { gcross2, @@ -177,16 +176,16 @@ impl WVelocityGroundConstraint { for j in 0..DIM - 1 { #[cfg(feature = "dim2")] - let impulse = SimdFloat::from( + let impulse = SimdReal::from( array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH], ); #[cfg(feature = "dim3")] - let impulse = SimdFloat::from( + let impulse = SimdReal::from( array![|ii| manifold_points[ii][k].data.tangent_impulse[j]; SIMD_WIDTH], ); let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j])); - let r = SimdFloat::splat(1.0) / (im2 + gcross2.gdot(gcross2)); + let r = SimdReal::splat(1.0) / (im2 + gcross2.gdot(gcross2)); let rhs = -vel2.dot(&tangents1[j]) + vel1.dot(&tangents1[j]); constraint.elements[k].tangent_parts[j] = @@ -274,7 +273,7 @@ impl WVelocityGroundConstraint { let elt = &mut self.elements[i].normal_part; let dimpulse = -self.dir1.dot(&mj_lambda2.linear) + elt.gcross2.gdot(mj_lambda2.angular) + elt.rhs; - let new_impulse = (elt.impulse - elt.r * dimpulse).simd_max(SimdFloat::zero()); + let new_impulse = (elt.impulse - elt.r * dimpulse).simd_max(SimdReal::zero()); let dlambda = new_impulse - elt.impulse; elt.impulse = new_impulse; diff --git a/src/geometry/ball.rs b/src/geometry/ball.rs index 7f4ad03..2d9f7be 100644 --- a/src/geometry/ball.rs +++ b/src/geometry/ball.rs @@ -1,16 +1,16 @@ #[cfg(feature = "simd-is-enabled")] -use crate::math::{Point, SimdFloat}; +use crate::math::{Point, SimdReal}; #[cfg(feature = "simd-is-enabled")] #[derive(Copy, Clone, Debug)] pub(crate) struct WBall { - pub center: Point, - pub radius: SimdFloat, + pub center: Point, + pub radius: SimdReal, } #[cfg(feature = "simd-is-enabled")] impl WBall { - pub fn new(center: Point, radius: SimdFloat) -> Self { + pub fn new(center: Point, radius: SimdReal) -> Self { WBall { center, radius } } } diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 8154554..1275358 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -1,12 +1,13 @@ +use crate::buckler::shape::HalfSpace; use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; -use crate::geometry::{ - Ball, Capsule, Cuboid, HeightField, InteractionGroups, Segment, Shape, ShapeType, Triangle, - Trimesh, -}; -#[cfg(feature = "dim3")] -use crate::geometry::{Cone, Cylinder, RoundCylinder}; +use crate::geometry::InteractionGroups; use crate::math::{AngVector, Isometry, Point, Rotation, Vector}; use buckler::bounding_volume::AABB; +use buckler::shape::{ + Ball, Capsule, Cuboid, HeightField, Segment, Shape, ShapeType, TriMesh, Triangle, +}; +#[cfg(feature = "dim3")] +use buckler::shape::{Cone, Cylinder, RoundCylinder}; use na::Point3; use std::ops::Deref; use std::sync::Arc; @@ -77,7 +78,7 @@ impl ColliderShape { /// Initializes a triangle mesh shape defined by its vertex and index buffers. pub fn trimesh(vertices: Vec>, indices: Vec>) -> Self { - ColliderShape(Arc::new(Trimesh::new(vertices, indices))) + ColliderShape(Arc::new(TriMesh::new(vertices, indices))) } /// Initializes an heightfield shape defined by its set of height and a scale @@ -165,8 +166,9 @@ impl<'de> serde::Deserialize<'de> for ColliderShape { Some(ShapeType::Capsule) => deser::(&mut seq)?, Some(ShapeType::Triangle) => deser::(&mut seq)?, Some(ShapeType::Segment) => deser::(&mut seq)?, - Some(ShapeType::Trimesh) => deser::(&mut seq)?, + Some(ShapeType::TriMesh) => deser::(&mut seq)?, Some(ShapeType::HeightField) => deser::(&mut seq)?, + Some(ShapeType::HalfSpace) => deser::(&mut seq)?, #[cfg(feature = "dim3")] Some(ShapeType::Cylinder) => deser::(&mut seq)?, #[cfg(feature = "dim3")] diff --git a/src/geometry/contact.rs b/src/geometry/contact.rs index 1f0a902..14646d9 100644 --- a/src/geometry/contact.rs +++ b/src/geometry/contact.rs @@ -6,7 +6,7 @@ use crate::geometry::{Collider, ColliderPair, ColliderSet, Contact, ContactManif use crate::math::{Isometry, Point, Vector}; #[cfg(feature = "simd-is-enabled")] use { - crate::math::{SimdFloat, SIMD_WIDTH}, + crate::math::{SimdReal, SIMD_WIDTH}, simba::simd::SimdValue, }; @@ -22,11 +22,11 @@ bitflags::bitflags! { #[cfg(feature = "simd-is-enabled")] pub(crate) struct WContact { - pub local_p1: Point, - pub local_p2: Point, - pub local_n1: Vector, - pub local_n2: Vector, - pub dist: SimdFloat, + pub local_p1: Point, + pub local_p2: Point, + pub local_n1: Vector, + pub local_n2: Vector, + pub dist: SimdReal, pub fid1: [u8; SIMD_WIDTH], pub fid2: [u8; SIMD_WIDTH], } diff --git a/src/geometry/contact_generator/ball_ball_contact_generator.rs b/src/geometry/contact_generator/ball_ball_contact_generator.rs index 7122998..f2ca7af 100644 --- a/src/geometry/contact_generator/ball_ball_contact_generator.rs +++ b/src/geometry/contact_generator/ball_ball_contact_generator.rs @@ -5,12 +5,12 @@ use crate::math::{Point, Vector}; use { crate::geometry::contact_generator::PrimitiveContactGenerationContextSimd, crate::geometry::{WBall, WContact}, - crate::math::{Isometry, SimdFloat, SIMD_WIDTH}, + crate::math::{Isometry, SimdReal, SIMD_WIDTH}, simba::simd::SimdValue, }; #[cfg(feature = "simd-is-enabled")] -fn generate_contacts_simd(ball1: &WBall, ball2: &WBall, pos21: &Isometry) -> WContact { +fn generate_contacts_simd(ball1: &WBall, ball2: &WBall, pos21: &Isometry) -> WContact { let dcenter = ball2.center - ball1.center; let center_dist = dcenter.magnitude(); let normal = dcenter / center_dist; @@ -30,9 +30,9 @@ fn generate_contacts_simd(ball1: &WBall, ball2: &WBall, pos21: &Isometry (ContactPhase, Option) { match (shape1, shape2) { - (ShapeType::Trimesh, _) | (_, ShapeType::Trimesh) => ( + (ShapeType::TriMesh, _) | (_, ShapeType::TriMesh) => ( ContactPhase::NearPhase(ContactGenerator { generate_contacts: super::generate_contacts_trimesh_shape, ..ContactGenerator::default() }), Some(ContactGeneratorWorkspace::from( - TrimeshShapeContactGeneratorWorkspace::new(), + TriMeshShapeContactGeneratorWorkspace::new(), )), ), (ShapeType::HeightField, _) | (_, ShapeType::HeightField) => ( diff --git a/src/geometry/contact_generator/contact_generator.rs b/src/geometry/contact_generator/contact_generator.rs index b1b1be6..06ab265 100644 --- a/src/geometry/contact_generator/contact_generator.rs +++ b/src/geometry/contact_generator/contact_generator.rs @@ -1,11 +1,11 @@ use crate::data::MaybeSerializableData; use crate::geometry::{ Collider, ColliderSet, ContactDispatcher, ContactEvent, ContactManifold, ContactPair, Shape, - SolverFlags, + ShapeType, SolverFlags, }; use crate::math::Isometry; #[cfg(feature = "simd-is-enabled")] -use crate::math::{SimdFloat, SIMD_WIDTH}; +use crate::math::{SimdReal, SIMD_WIDTH}; use crate::pipeline::EventHandler; #[derive(Copy, Clone)] @@ -158,8 +158,8 @@ pub struct PrimitiveContactGenerationContextSimd<'a, 'b> { pub colliders2: [&'a Collider; SIMD_WIDTH], pub shapes1: [&'a dyn Shape; SIMD_WIDTH], pub shapes2: [&'a dyn Shape; SIMD_WIDTH], - pub positions1: &'a Isometry, - pub positions2: &'a Isometry, + pub positions1: &'a Isometry, + pub positions2: &'a Isometry, pub manifolds: &'a mut [&'b mut ContactManifold], pub workspaces: &'a mut [Option<&'b mut (dyn MaybeSerializableData)>], } diff --git a/src/geometry/contact_generator/contact_generator_workspace.rs b/src/geometry/contact_generator/contact_generator_workspace.rs index e89395f..7aac592 100644 --- a/src/geometry/contact_generator/contact_generator_workspace.rs +++ b/src/geometry/contact_generator/contact_generator_workspace.rs @@ -2,7 +2,7 @@ use crate::data::MaybeSerializableData; #[cfg(feature = "dim3")] use crate::geometry::contact_generator::PfmPfmContactManifoldGeneratorWorkspace; use crate::geometry::contact_generator::{ - HeightFieldShapeContactGeneratorWorkspace, TrimeshShapeContactGeneratorWorkspace, + HeightFieldShapeContactGeneratorWorkspace, TriMeshShapeContactGeneratorWorkspace, WorkspaceSerializationTag, }; @@ -81,8 +81,8 @@ impl<'de> serde::Deserialize<'de> for ContactGeneratorWorkspace { Some(WorkspaceSerializationTag::HeightfieldShapeContactGeneratorWorkspace) => { deser::(&mut seq)? } - Some(WorkspaceSerializationTag::TrimeshShapeContactGeneratorWorkspace) => { - deser::(&mut seq)? + Some(WorkspaceSerializationTag::TriMeshShapeContactGeneratorWorkspace) => { + deser::(&mut seq)? } #[cfg(feature = "dim3")] Some(WorkspaceSerializationTag::PfmPfmContactGeneratorWorkspace) => { diff --git a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs index 358ac84..125ac34 100644 --- a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs +++ b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs @@ -5,9 +5,10 @@ use crate::geometry::contact_generator::{ ContactGenerationContext, ContactGeneratorWorkspace, PrimitiveContactGenerationContext, PrimitiveContactGenerator, }; +use crate::geometry::{Collider, ContactManifold, ContactManifoldData}; #[cfg(feature = "dim2")] -use crate::geometry::Capsule; -use crate::geometry::{Collider, ContactManifold, ContactManifoldData, HeightField, Shape}; +use buckler::shape::Capsule; +use buckler::shape::{HeightField, Shape}; #[cfg(feature = "serde-serialize")] use erased_serde::Serialize; @@ -95,7 +96,7 @@ fn do_generate_contacts( #[cfg(feature = "dim3")] let sub_shape1 = *part1; - let sub_detector = match workspace.sub_detectors.entry(i) { + let sub_detector = match workspace.sub_detectors.entry(i as usize) { Entry::Occupied(entry) => { let sub_detector = entry.into_mut(); let manifold = workspace.old_manifolds[sub_detector.manifold_id].take(); @@ -119,7 +120,7 @@ fn do_generate_contacts( collider2, solver_flags, ); - manifolds.push(ContactManifold::with_data((i, 0), manifold_data)); + manifolds.push(ContactManifold::with_data((i as usize, 0), manifold_data)); entry.insert(sub_detector) } diff --git a/src/geometry/contact_generator/mod.rs b/src/geometry/contact_generator/mod.rs index f539d7a..4c0716a 100644 --- a/src/geometry/contact_generator/mod.rs +++ b/src/geometry/contact_generator/mod.rs @@ -25,7 +25,7 @@ pub use self::pfm_pfm_contact_generator::{ // pub use self::polygon_polygon_contact_generator::generate_contacts_polygon_polygon; pub use self::contact_generator_workspace::ContactGeneratorWorkspace; pub use self::trimesh_shape_contact_generator::{ - generate_contacts_trimesh_shape, TrimeshShapeContactGeneratorWorkspace, + generate_contacts_trimesh_shape, TriMeshShapeContactGeneratorWorkspace, }; pub(self) use self::serializable_workspace_tag::WorkspaceSerializationTag; diff --git a/src/geometry/contact_generator/serializable_workspace_tag.rs b/src/geometry/contact_generator/serializable_workspace_tag.rs index 2488c1e..4bdb902 100644 --- a/src/geometry/contact_generator/serializable_workspace_tag.rs +++ b/src/geometry/contact_generator/serializable_workspace_tag.rs @@ -2,7 +2,7 @@ use num_derive::FromPrimitive; #[derive(Copy, Clone, Debug, FromPrimitive)] pub(super) enum WorkspaceSerializationTag { - TrimeshShapeContactGeneratorWorkspace = 0, + TriMeshShapeContactGeneratorWorkspace = 0, #[cfg(feature = "dim3")] PfmPfmContactGeneratorWorkspace, HeightfieldShapeContactGeneratorWorkspace, diff --git a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs index 172069d..209ac42 100644 --- a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs +++ b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs @@ -3,21 +3,21 @@ use crate::data::MaybeSerializableData; use crate::geometry::contact_generator::{ ContactGenerationContext, PrimitiveContactGenerationContext, }; -use crate::geometry::{Collider, ContactManifold, ContactManifoldData, ShapeType, Trimesh}; +use crate::geometry::{Collider, ContactManifold, ContactManifoldData, ShapeType, TriMesh}; #[cfg(feature = "serde-serialize")] use erased_serde::Serialize; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Clone)] -pub struct TrimeshShapeContactGeneratorWorkspace { - interferences: Vec, +pub struct TriMeshShapeContactGeneratorWorkspace { + interferences: Vec, local_aabb2: AABB, - old_interferences: Vec, + old_interferences: Vec, #[cfg_attr(feature = "serde-serialize", serde(skip))] old_manifolds: Vec, } -impl TrimeshShapeContactGeneratorWorkspace { +impl TriMeshShapeContactGeneratorWorkspace { pub fn new() -> Self { Self { interferences: Vec::new(), @@ -40,7 +40,7 @@ pub fn generate_contacts_trimesh_shape(ctxt: &mut ContactGenerationContext) { } fn do_generate_contacts( - trimesh1: &Trimesh, + trimesh1: &TriMesh, collider1: &Collider, collider2: &Collider, ctxt: &mut ContactGenerationContext, @@ -52,14 +52,14 @@ fn do_generate_contacts( ctxt.pair.pair }; - let workspace: &mut TrimeshShapeContactGeneratorWorkspace = ctxt + let workspace: &mut TriMeshShapeContactGeneratorWorkspace = ctxt .pair .generator_workspace .as_mut() - .expect("The TrimeshShapeContactGeneratorWorkspace is missing.") + .expect("The TriMeshShapeContactGeneratorWorkspace is missing.") .0 .downcast_mut() - .expect("Invalid workspace type, expected a TrimeshShapeContactGeneratorWorkspace."); + .expect("Invalid workspace type, expected a TriMeshShapeContactGeneratorWorkspace."); /* * Compute interferences. @@ -97,9 +97,9 @@ fn do_generate_contacts( .iter() .map(|manifold| { if manifold.data.pair.collider1 == ctxt_collider1 { - manifold.subshape_index_pair.0 + manifold.subshape_index_pair.0 as u32 } else { - manifold.subshape_index_pair.1 + manifold.subshape_index_pair.1 as u32 } }) .collect(); @@ -118,7 +118,7 @@ fn do_generate_contacts( workspace.interferences.clear(); trimesh1 - .waabbs() + .quadtree() .intersect_aabb(&local_aabb2, &mut workspace.interferences); workspace.local_aabb2 = local_aabb2; } @@ -134,7 +134,7 @@ fn do_generate_contacts( // TODO: don't redispatch at each frame (we should probably do the same as // the heightfield). for (i, triangle_id) in new_interferences.iter().enumerate() { - if *triangle_id >= trimesh1.num_triangles() { + if *triangle_id >= trimesh1.num_triangles() as u32 { // Because of SIMD padding, the broad-phase may return tiangle indices greater // than the max. continue; @@ -160,7 +160,7 @@ fn do_generate_contacts( ctxt.solver_flags, ); - ContactManifold::with_data((*triangle_id, 0), data) + ContactManifold::with_data((*triangle_id as usize, 0), data) } else { // We already have a manifold for this triangle. old_inter_it.next(); @@ -206,11 +206,11 @@ fn do_generate_contacts( } } -impl MaybeSerializableData for TrimeshShapeContactGeneratorWorkspace { +impl MaybeSerializableData for TriMeshShapeContactGeneratorWorkspace { #[cfg(feature = "serde-serialize")] fn as_serialize(&self) -> Option<(u32, &dyn Serialize)> { Some(( - super::WorkspaceSerializationTag::TrimeshShapeContactGeneratorWorkspace as u32, + super::WorkspaceSerializationTag::TriMeshShapeContactGeneratorWorkspace as u32, self, )) } diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index caa229a..46fe360 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -13,9 +13,6 @@ pub use self::narrow_phase::NarrowPhase; pub use self::polygon::Polygon; pub use self::proximity::ProximityPair; pub use self::proximity_detector::{DefaultProximityDispatcher, ProximityDispatcher}; -#[cfg(feature = "dim3")] -pub use self::round_cylinder::RoundCylinder; -pub use self::trimesh::Trimesh; pub use self::user_callbacks::{ContactPairFilter, PairFilterContext, ProximityPairFilter}; pub use buckler::query::Proximity; @@ -106,11 +103,10 @@ pub(crate) use self::collider_set::RemovedCollider; #[cfg(feature = "simd-is-enabled")] pub(crate) use self::contact::WContact; pub(crate) use self::narrow_phase::ContactManifoldIndex; -pub(crate) use self::waabb::{WRay, WAABB}; -pub(crate) use self::wquadtree::WQuadtree; +pub(crate) use buckler::partitioning::WQuadtree; //pub(crate) use self::z_order::z_cmp_floats; pub use self::interaction_groups::InteractionGroups; -pub use self::shape::{Shape, ShapeType}; +pub use buckler::shape::*; mod ball; mod broad_phase_multi_sap; @@ -125,12 +121,6 @@ mod proximity; mod proximity_detector; pub(crate) mod sat; pub(crate) mod triangle; -mod trimesh; -mod waabb; -mod wquadtree; //mod z_order; mod interaction_groups; -#[cfg(feature = "dim3")] -mod round_cylinder; -mod shape; mod user_callbacks; diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 463dd99..ad2d514 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -20,7 +20,7 @@ use crate::geometry::{ }; use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph}; //#[cfg(feature = "simd-is-enabled")] -//use crate::math::{SimdFloat, SIMD_WIDTH}; +//use crate::math::{SimdReal, SIMD_WIDTH}; use crate::buckler::query::Proximity; use crate::data::pubsub::Subscription; use crate::data::Coarena; diff --git a/src/geometry/proximity_detector/ball_ball_proximity_detector.rs b/src/geometry/proximity_detector/ball_ball_proximity_detector.rs index 2106c9f..65c141c 100644 --- a/src/geometry/proximity_detector/ball_ball_proximity_detector.rs +++ b/src/geometry/proximity_detector/ball_ball_proximity_detector.rs @@ -5,12 +5,12 @@ use crate::math::Point; #[cfg(feature = "simd-is-enabled")] use { crate::geometry::{proximity_detector::PrimitiveProximityDetectionContextSimd, WBall}, - crate::math::{SimdFloat, SIMD_WIDTH}, + crate::math::{SimdReal, SIMD_WIDTH}, simba::simd::SimdValue, }; #[cfg(feature = "simd-is-enabled")] -fn ball_distance_simd(ball1: &WBall, ball2: &WBall) -> SimdFloat { +fn ball_distance_simd(ball1: &WBall, ball2: &WBall) -> SimdReal { let dcenter = ball2.center - ball1.center; let center_dist = dcenter.magnitude(); center_dist - ball1.radius - ball2.radius @@ -22,9 +22,9 @@ pub fn detect_proximity_ball_ball_simd( ) -> [Proximity; SIMD_WIDTH] { let pos_ba = ctxt.positions2.inverse() * ctxt.positions1; let radii_a = - SimdFloat::from(array![|ii| ctxt.shapes1[ii].as_ball().unwrap().radius; SIMD_WIDTH]); + SimdReal::from(array![|ii| ctxt.shapes1[ii].as_ball().unwrap().radius; SIMD_WIDTH]); let radii_b = - SimdFloat::from(array![|ii| ctxt.shapes2[ii].as_ball().unwrap().radius; SIMD_WIDTH]); + SimdReal::from(array![|ii| ctxt.shapes2[ii].as_ball().unwrap().radius; SIMD_WIDTH]); let wball_a = WBall::new(Point::origin(), radii_a); let wball_b = WBall::new(pos_ba.inverse_transform_point(&Point::origin()), radii_b); diff --git a/src/geometry/proximity_detector/mod.rs b/src/geometry/proximity_detector/mod.rs index a99372f..fc904da 100644 --- a/src/geometry/proximity_detector/mod.rs +++ b/src/geometry/proximity_detector/mod.rs @@ -15,7 +15,7 @@ pub use self::proximity_detector::{ }; pub use self::proximity_dispatcher::{DefaultProximityDispatcher, ProximityDispatcher}; pub use self::trimesh_shape_proximity_detector::{ - detect_proximity_trimesh_shape, TrimeshShapeProximityDetectorWorkspace, + detect_proximity_trimesh_shape, TriMeshShapeProximityDetectorWorkspace, }; mod ball_ball_proximity_detector; diff --git a/src/geometry/proximity_detector/proximity_detector.rs b/src/geometry/proximity_detector/proximity_detector.rs index 7c8ad20..ea362de 100644 --- a/src/geometry/proximity_detector/proximity_detector.rs +++ b/src/geometry/proximity_detector/proximity_detector.rs @@ -3,7 +3,7 @@ use crate::geometry::{ }; use crate::math::Isometry; #[cfg(feature = "simd-is-enabled")] -use crate::math::{SimdFloat, SIMD_WIDTH}; +use crate::math::{SimdReal, SIMD_WIDTH}; use crate::pipeline::EventHandler; use std::any::Any; @@ -134,8 +134,8 @@ pub struct PrimitiveProximityDetectionContextSimd<'a, 'b> { pub colliders2: [&'a Collider; SIMD_WIDTH], pub shapes1: [&'a dyn Shape; SIMD_WIDTH], pub shapes2: [&'a dyn Shape; SIMD_WIDTH], - pub positions1: &'a Isometry, - pub positions2: &'a Isometry, + pub positions1: &'a Isometry, + pub positions2: &'a Isometry, pub workspaces: &'a mut [Option<&'b mut (dyn Any + Send + Sync)>], } diff --git a/src/geometry/proximity_detector/proximity_dispatcher.rs b/src/geometry/proximity_detector/proximity_dispatcher.rs index 768aca6..709521e 100644 --- a/src/geometry/proximity_detector/proximity_dispatcher.rs +++ b/src/geometry/proximity_detector/proximity_dispatcher.rs @@ -1,6 +1,6 @@ use crate::geometry::proximity_detector::{ PrimitiveProximityDetector, ProximityDetector, ProximityPhase, - TrimeshShapeProximityDetectorWorkspace, + TriMeshShapeProximityDetectorWorkspace, }; use crate::geometry::ShapeType; use std::any::Any; @@ -113,19 +113,19 @@ impl ProximityDispatcher for DefaultProximityDispatcher { shape2: ShapeType, ) -> (ProximityPhase, Option>) { match (shape1, shape2) { - (ShapeType::Trimesh, _) => ( + (ShapeType::TriMesh, _) => ( ProximityPhase::NearPhase(ProximityDetector { detect_proximity: super::detect_proximity_trimesh_shape, ..ProximityDetector::default() }), - Some(Box::new(TrimeshShapeProximityDetectorWorkspace::new())), + Some(Box::new(TriMeshShapeProximityDetectorWorkspace::new())), ), - (_, ShapeType::Trimesh) => ( + (_, ShapeType::TriMesh) => ( ProximityPhase::NearPhase(ProximityDetector { detect_proximity: super::detect_proximity_trimesh_shape, ..ProximityDetector::default() }), - Some(Box::new(TrimeshShapeProximityDetectorWorkspace::new())), + Some(Box::new(TriMeshShapeProximityDetectorWorkspace::new())), ), _ => { let (gen, workspace) = self.dispatch_primitives(shape1, shape2); diff --git a/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs b/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs index 5366b39..dbb20f5 100644 --- a/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs +++ b/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs @@ -2,15 +2,15 @@ use crate::buckler::bounding_volume::{BoundingVolume, AABB}; use crate::geometry::proximity_detector::{ PrimitiveProximityDetectionContext, ProximityDetectionContext, }; -use crate::geometry::{Collider, Proximity, ShapeType, Trimesh}; +use crate::geometry::{Collider, Proximity, ShapeType, TriMesh}; -pub struct TrimeshShapeProximityDetectorWorkspace { - interferences: Vec, +pub struct TriMeshShapeProximityDetectorWorkspace { + interferences: Vec, local_aabb2: AABB, - old_interferences: Vec, + old_interferences: Vec, } -impl TrimeshShapeProximityDetectorWorkspace { +impl TriMeshShapeProximityDetectorWorkspace { pub fn new() -> Self { Self { interferences: Vec::new(), @@ -34,18 +34,18 @@ pub fn detect_proximity_trimesh_shape(ctxt: &mut ProximityDetectionContext) -> P } fn do_detect_proximity( - trimesh1: &Trimesh, + trimesh1: &TriMesh, collider1: &Collider, collider2: &Collider, ctxt: &mut ProximityDetectionContext, ) -> Proximity { - let workspace: &mut TrimeshShapeProximityDetectorWorkspace = ctxt + let workspace: &mut TriMeshShapeProximityDetectorWorkspace = ctxt .pair .detector_workspace .as_mut() - .expect("The TrimeshShapeProximityDetectorWorkspace is missing.") + .expect("The TriMeshShapeProximityDetectorWorkspace is missing.") .downcast_mut() - .expect("Invalid workspace type, expected a TrimeshShapeProximityDetectorWorkspace."); + .expect("Invalid workspace type, expected a TriMeshShapeProximityDetectorWorkspace."); /* * Compute interferences. @@ -72,7 +72,7 @@ fn do_detect_proximity( workspace.interferences.clear(); trimesh1 - .waabbs() + .quadtree() .intersect_aabb(&local_aabb2, &mut workspace.interferences); workspace.local_aabb2 = local_aabb2; } @@ -86,7 +86,7 @@ fn do_detect_proximity( let shape_type2 = collider2.shape().shape_type(); for triangle_id in new_interferences.iter() { - if *triangle_id >= trimesh1.num_triangles() { + if *triangle_id >= trimesh1.num_triangles() as u32 { // Because of SIMD padding, the broad-phase may return tiangle indices greater // than the max. continue; diff --git a/src/geometry/round_cylinder.rs b/src/geometry/round_cylinder.rs deleted file mode 100644 index 450a9fc..0000000 --- a/src/geometry/round_cylinder.rs +++ /dev/null @@ -1,92 +0,0 @@ -use crate::geometry::Cylinder; -use crate::math::{Isometry, Point, Vector}; -use buckler::query::{ - gjk::VoronoiSimplex, PointProjection, PointQuery, Ray, RayCast, RayIntersection, -}; -use buckler::shape::{FeatureId, SupportMap}; -use na::Unit; - -/// A rounded cylinder. -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Copy, Clone, Debug)] -pub struct RoundCylinder { - /// The cylinder being rounded. - pub cylinder: Cylinder, - /// The rounding radius. - pub border_radius: f32, -} - -impl RoundCylinder { - /// Create sa new cylinder where all its edges and vertices are rounded by a radius of `radius`. - /// - /// This is done by applying a dilation of the given radius to the cylinder. - pub fn new(half_height: f32, radius: f32, border_radius: f32) -> Self { - Self { - cylinder: Cylinder::new(half_height, radius), - border_radius, - } - } -} - -impl SupportMap for RoundCylinder { - fn local_support_point(&self, dir: &Vector) -> Point { - self.local_support_point_toward(&Unit::new_normalize(*dir)) - } - - fn local_support_point_toward(&self, dir: &Unit>) -> Point { - self.cylinder.local_support_point_toward(dir) + **dir * self.border_radius - } - - fn support_point(&self, transform: &Isometry, dir: &Vector) -> Point { - let local_dir = transform.inverse_transform_vector(dir); - transform * self.local_support_point(&local_dir) - } - - fn support_point_toward( - &self, - transform: &Isometry, - dir: &Unit>, - ) -> Point { - let local_dir = Unit::new_unchecked(transform.inverse_transform_vector(dir)); - transform * self.local_support_point_toward(&local_dir) - } -} - -impl RayCast for RoundCylinder { - fn cast_local_ray_and_get_normal( - &self, - ray: &Ray, - max_toi: f32, - solid: bool, - ) -> Option { - buckler::query::details::local_ray_intersection_with_support_map_with_params( - self, - &mut VoronoiSimplex::new(), - ray, - max_toi, - solid, - ) - } -} - -// TODO: if PointQuery had a `project_point_with_normal` method, we could just -// call this and adjust the projected point accordingly. -impl PointQuery for RoundCylinder { - #[inline] - fn project_local_point(&self, point: &Point, solid: bool) -> PointProjection { - buckler::query::details::local_point_projection_on_support_map( - self, - &mut VoronoiSimplex::new(), - point, - solid, - ) - } - - #[inline] - fn project_local_point_and_get_feature( - &self, - point: &Point, - ) -> (PointProjection, FeatureId) { - (self.project_local_point(point, false), FeatureId::Unknown) - } -} diff --git a/src/geometry/shape.rs b/src/geometry/shape.rs deleted file mode 100644 index 80b24c5..0000000 --- a/src/geometry/shape.rs +++ /dev/null @@ -1,393 +0,0 @@ -use crate::dynamics::MassProperties; -use crate::geometry::{Ball, Capsule, Cuboid, HeightField, Segment, Triangle, Trimesh}; -use crate::math::Isometry; -use buckler::bounding_volume::AABB; -use buckler::query::{PointQuery, RayCast}; -use downcast_rs::{impl_downcast, DowncastSync}; -#[cfg(feature = "serde-serialize")] -use erased_serde::Serialize; -use num::Zero; -use num_derive::FromPrimitive; -#[cfg(feature = "dim3")] -use { - crate::geometry::{Cone, Cylinder, RoundCylinder}, - buckler::bounding_volume::BoundingVolume, - buckler::shape::PolygonalFeatureMap, -}; - -#[derive(Copy, Clone, Debug, FromPrimitive)] -/// Enum representing the type of a shape. -pub enum ShapeType { - /// A ball shape. - Ball = 0, - /// A convex polygon shape. - Polygon, - /// A cuboid shape. - Cuboid, - /// A capsule shape. - Capsule, - /// A segment shape. - Segment, - /// A triangle shape. - Triangle, - /// A triangle mesh shape. - Trimesh, - /// A heightfield shape. - HeightField, - #[cfg(feature = "dim3")] - /// A cylindrical shape. - Cylinder, - #[cfg(feature = "dim3")] - /// A cylindrical shape. - Cone, - // /// A custom shape type. - // Custom(u8), - // /// A cuboid with rounded corners. - // RoundedCuboid, - // /// A triangle with rounded corners. - // RoundedTriangle, - // /// A triangle-mesh with rounded corners. - // RoundedTrimesh, - // /// An heightfield with rounded corners. - // RoundedHeightField, - /// A cylinder with rounded corners. - #[cfg(feature = "dim3")] - RoundCylinder, - // /// A cone with rounded corners. - // RoundedCone, -} - -/// Trait implemented by shapes usable by Rapier. -pub trait Shape: RayCast + PointQuery + DowncastSync { - /// Convert this shape as a serializable entity. - #[cfg(feature = "serde-serialize")] - fn as_serialize(&self) -> Option<&dyn Serialize> { - None - } - - // TODO: add a compute_local_aabb method? - - /// Computes the AABB of this shape. - fn compute_aabb(&self, position: &Isometry) -> AABB; - - /// Compute the mass-properties of this shape given its uniform density. - fn mass_properties(&self, density: f32) -> MassProperties; - - /// Gets the type tag of this shape. - fn shape_type(&self) -> ShapeType; - - /// Converts this shape to a polygonal feature-map, if it is one. - #[cfg(feature = "dim3")] - fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { - None - } - - // fn as_rounded(&self) -> Option<&Rounded>> { - // None - // } -} - -impl_downcast!(sync Shape); - -impl dyn Shape { - /// Converts this abstract shape to a ball, if it is one. - pub fn as_ball(&self) -> Option<&Ball> { - self.downcast_ref() - } - - /// Converts this abstract shape to a cuboid, if it is one. - pub fn as_cuboid(&self) -> Option<&Cuboid> { - self.downcast_ref() - } - - /// Converts this abstract shape to a capsule, if it is one. - pub fn as_capsule(&self) -> Option<&Capsule> { - self.downcast_ref() - } - - /// Converts this abstract shape to a triangle, if it is one. - pub fn as_triangle(&self) -> Option<&Triangle> { - self.downcast_ref() - } - - /// Converts this abstract shape to a triangle mesh, if it is one. - pub fn as_trimesh(&self) -> Option<&Trimesh> { - self.downcast_ref() - } - - /// Converts this abstract shape to a heightfield, if it is one. - pub fn as_heightfield(&self) -> Option<&HeightField> { - self.downcast_ref() - } - - /// Converts this abstract shape to a cylinder, if it is one. - #[cfg(feature = "dim3")] - pub fn as_cylinder(&self) -> Option<&Cylinder> { - self.downcast_ref() - } - - /// Converts this abstract shape to a cone, if it is one. - #[cfg(feature = "dim3")] - pub fn as_cone(&self) -> Option<&Cone> { - self.downcast_ref() - } - - /// Converts this abstract shape to a cone, if it is one. - #[cfg(feature = "dim3")] - pub fn as_round_cylinder(&self) -> Option<&RoundCylinder> { - self.downcast_ref() - } -} - -impl Shape for Ball { - #[cfg(feature = "serde-serialize")] - fn as_serialize(&self) -> Option<&dyn Serialize> { - Some(self as &dyn Serialize) - } - - fn compute_aabb(&self, position: &Isometry) -> AABB { - self.aabb(position) - } - - fn mass_properties(&self, density: f32) -> MassProperties { - MassProperties::from_ball(density, self.radius) - } - - fn shape_type(&self) -> ShapeType { - ShapeType::Ball - } -} - -// impl Shape for Polygon { -// #[cfg(feature = "serde-serialize")] -// fn as_serialize(&self) -> Option<&dyn Serialize> { -// Some(self as &dyn Serialize) -// } -// -// fn compute_aabb(&self, position: &Isometry) -> AABB { -// self.aabb(position) -// } -// -// fn mass_properties(&self, _density: f32) -> MassProperties { -// unimplemented!() -// } -// -// fn shape_type(&self) -> ShapeType { -// ShapeType::Polygon -// } -// } - -impl Shape for Cuboid { - #[cfg(feature = "serde-serialize")] - fn as_serialize(&self) -> Option<&dyn Serialize> { - Some(self as &dyn Serialize) - } - - fn compute_aabb(&self, position: &Isometry) -> AABB { - self.aabb(position) - } - - fn mass_properties(&self, density: f32) -> MassProperties { - MassProperties::from_cuboid(density, self.half_extents) - } - - fn shape_type(&self) -> ShapeType { - ShapeType::Cuboid - } - - #[cfg(feature = "dim3")] - fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { - Some((self as &dyn PolygonalFeatureMap, 0.0)) - } -} - -impl Shape for Capsule { - #[cfg(feature = "serde-serialize")] - fn as_serialize(&self) -> Option<&dyn Serialize> { - Some(self as &dyn Serialize) - } - - fn compute_aabb(&self, position: &Isometry) -> AABB { - self.aabb(position) - } - - fn mass_properties(&self, density: f32) -> MassProperties { - MassProperties::from_capsule(density, self.segment.a, self.segment.b, self.radius) - } - - fn shape_type(&self) -> ShapeType { - ShapeType::Capsule - } - - #[cfg(feature = "dim3")] - fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { - Some((&self.segment as &dyn PolygonalFeatureMap, self.radius)) - } -} - -impl Shape for Triangle { - #[cfg(feature = "serde-serialize")] - fn as_serialize(&self) -> Option<&dyn Serialize> { - Some(self as &dyn Serialize) - } - - fn compute_aabb(&self, position: &Isometry) -> AABB { - self.aabb(position) - } - - fn mass_properties(&self, _density: f32) -> MassProperties { - MassProperties::zero() - } - - fn shape_type(&self) -> ShapeType { - ShapeType::Triangle - } - - #[cfg(feature = "dim3")] - fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { - Some((self as &dyn PolygonalFeatureMap, 0.0)) - } -} - -impl Shape for Segment { - #[cfg(feature = "serde-serialize")] - fn as_serialize(&self) -> Option<&dyn Serialize> { - Some(self as &dyn Serialize) - } - - fn compute_aabb(&self, position: &Isometry) -> AABB { - self.aabb(position) - } - - fn mass_properties(&self, _density: f32) -> MassProperties { - MassProperties::zero() - } - - fn shape_type(&self) -> ShapeType { - ShapeType::Segment - } - - #[cfg(feature = "dim3")] - fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { - Some((self as &dyn PolygonalFeatureMap, 0.0)) - } -} - -impl Shape for Trimesh { - #[cfg(feature = "serde-serialize")] - fn as_serialize(&self) -> Option<&dyn Serialize> { - Some(self as &dyn Serialize) - } - - fn compute_aabb(&self, position: &Isometry) -> AABB { - self.aabb(position) - } - - fn mass_properties(&self, _density: f32) -> MassProperties { - MassProperties::zero() - } - - fn shape_type(&self) -> ShapeType { - ShapeType::Trimesh - } -} - -impl Shape for HeightField { - #[cfg(feature = "serde-serialize")] - fn as_serialize(&self) -> Option<&dyn Serialize> { - Some(self as &dyn Serialize) - } - - fn compute_aabb(&self, position: &Isometry) -> AABB { - self.aabb(position) - } - - fn mass_properties(&self, _density: f32) -> MassProperties { - MassProperties::zero() - } - - fn shape_type(&self) -> ShapeType { - ShapeType::HeightField - } -} - -#[cfg(feature = "dim3")] -impl Shape for Cylinder { - #[cfg(feature = "serde-serialize")] - fn as_serialize(&self) -> Option<&dyn Serialize> { - Some(self as &dyn Serialize) - } - - fn compute_aabb(&self, position: &Isometry) -> AABB { - self.aabb(position) - } - - fn mass_properties(&self, density: f32) -> MassProperties { - MassProperties::from_cylinder(density, self.half_height, self.radius) - } - - fn shape_type(&self) -> ShapeType { - ShapeType::Cylinder - } - - #[cfg(feature = "dim3")] - fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { - Some((self as &dyn PolygonalFeatureMap, 0.0)) - } -} - -#[cfg(feature = "dim3")] -impl Shape for Cone { - #[cfg(feature = "serde-serialize")] - fn as_serialize(&self) -> Option<&dyn Serialize> { - Some(self as &dyn Serialize) - } - - fn compute_aabb(&self, position: &Isometry) -> AABB { - self.aabb(position) - } - - fn mass_properties(&self, density: f32) -> MassProperties { - MassProperties::from_cone(density, self.half_height, self.radius) - } - - fn shape_type(&self) -> ShapeType { - ShapeType::Cone - } - - #[cfg(feature = "dim3")] - fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { - Some((self as &dyn PolygonalFeatureMap, 0.0)) - } -} - -#[cfg(feature = "dim3")] -impl Shape for RoundCylinder { - #[cfg(feature = "serde-serialize")] - fn as_serialize(&self) -> Option<&dyn Serialize> { - Some(self as &dyn Serialize) - } - - fn compute_aabb(&self, position: &Isometry) -> AABB { - self.cylinder - .compute_aabb(position) - .loosened(self.border_radius) - } - - fn mass_properties(&self, density: f32) -> MassProperties { - // We ignore the margin here. - self.cylinder.mass_properties(density) - } - - fn shape_type(&self) -> ShapeType { - ShapeType::RoundCylinder - } - - #[cfg(feature = "dim3")] - fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { - Some(( - &self.cylinder as &dyn PolygonalFeatureMap, - self.border_radius, - )) - } -} diff --git a/src/geometry/trimesh.rs b/src/geometry/trimesh.rs deleted file mode 100644 index 02a3b1c..0000000 --- a/src/geometry/trimesh.rs +++ /dev/null @@ -1,203 +0,0 @@ -use crate::geometry::{ - Cuboid, HeightField, PointProjection, Ray, RayIntersection, Triangle, WQuadtree, -}; -use crate::math::{Isometry, Point}; -use buckler::bounding_volume::AABB; -use buckler::query::{PointQuery, RayCast}; -use buckler::shape::FeatureId; -use na::Point3; - -#[derive(Clone)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// A triangle mesh. -pub struct Trimesh { - wquadtree: WQuadtree, - aabb: AABB, - vertices: Vec>, - indices: Vec>, -} - -impl Trimesh { - /// Creates a new triangle mesh from a vertex buffer and an index buffer. - pub fn new(vertices: Vec>, indices: Vec>) -> Self { - assert!( - vertices.len() > 1, - "A triangle mesh must contain at least one point." - ); - assert!( - indices.len() > 1, - "A triangle mesh must contain at least one triangle." - ); - - let aabb = AABB::from_points(&vertices); - let data = indices.iter().enumerate().map(|(i, idx)| { - let aabb = Triangle::new( - vertices[idx[0] as usize], - vertices[idx[1] as usize], - vertices[idx[2] as usize], - ) - .local_aabb(); - (i, aabb) - }); - - let mut wquadtree = WQuadtree::new(); - // NOTE: we apply no dilation factor because we won't - // update this tree dynamically. - wquadtree.clear_and_rebuild(data, 0.0); - - Self { - wquadtree, - aabb, - vertices, - indices, - } - } - - /// Compute the axis-aligned bounding box of this triangle mesh. - pub fn aabb(&self, pos: &Isometry) -> AABB { - self.aabb.transform_by(pos) - } - - pub(crate) fn waabbs(&self) -> &WQuadtree { - &self.wquadtree - } - - /// The number of triangles forming this mesh. - pub fn num_triangles(&self) -> usize { - self.indices.len() - } - - /// An iterator through all the triangles of this mesh. - pub fn triangles(&self) -> impl Iterator + '_ { - self.indices.iter().map(move |ids| { - Triangle::new( - self.vertices[ids.x as usize], - self.vertices[ids.y as usize], - self.vertices[ids.z as usize], - ) - }) - } - - /// Get the `i`-th triangle of this mesh. - pub fn triangle(&self, i: usize) -> Triangle { - let idx = self.indices[i]; - Triangle::new( - self.vertices[idx.x as usize], - self.vertices[idx.y as usize], - self.vertices[idx.z as usize], - ) - } - - /// The vertex buffer of this mesh. - pub fn vertices(&self) -> &[Point] { - &self.vertices[..] - } - - /// The index buffer of this mesh. - pub fn indices(&self) -> &[Point3] { - &self.indices - } - - /// A flat view of the index buffer of this mesh. - pub fn flat_indices(&self) -> &[u32] { - unsafe { - let len = self.indices.len() * 3; - let data = self.indices.as_ptr() as *const u32; - std::slice::from_raw_parts(data, len) - } - } -} - -impl PointQuery for Trimesh { - fn project_local_point(&self, _pt: &Point, _solid: bool) -> PointProjection { - // TODO - unimplemented!() - } - - fn project_local_point_and_get_feature( - &self, - _pt: &Point, - ) -> (PointProjection, FeatureId) { - // TODO - unimplemented!() - } -} - -#[cfg(feature = "dim2")] -impl RayCast for Trimesh { - fn cast_local_ray_and_get_normal( - &self, - _ray: &Ray, - _max_toi: f32, - _solid: bool, - ) -> Option { - // TODO - None - } - - fn intersects_ray(&self, _m: &Isometry, _ray: &Ray, _max_toi: f32) -> bool { - // TODO - false - } -} - -#[cfg(feature = "dim3")] -impl RayCast for Trimesh { - fn cast_local_ray_and_get_normal( - &self, - ray: &Ray, - max_toi: f32, - solid: bool, - ) -> Option { - // FIXME: do a best-first search. - let mut intersections = Vec::new(); - self.wquadtree.cast_ray(&ray, max_toi, &mut intersections); - let mut best: Option = None; - - for inter in intersections { - let tri = self.triangle(inter); - if let Some(inter) = tri.cast_local_ray_and_get_normal(ray, max_toi, solid) { - if let Some(curr) = &mut best { - if curr.toi > inter.toi { - *curr = inter; - } - } else { - best = Some(inter); - } - } - } - - best - } - - fn intersects_local_ray(&self, ray: &Ray, max_toi: f32) -> bool { - // FIXME: do a best-first search. - let mut intersections = Vec::new(); - self.wquadtree.cast_ray(&ray, max_toi, &mut intersections); - - for inter in intersections { - let tri = self.triangle(inter); - if tri.intersects_local_ray(ray, max_toi) { - return true; - } - } - - false - } -} - -#[cfg(feature = "dim3")] -impl From for Trimesh { - fn from(heightfield: HeightField) -> Self { - let (vtx, idx) = heightfield.to_trimesh(); - Trimesh::new(vtx, idx) - } -} - -#[cfg(feature = "dim3")] -impl From for Trimesh { - fn from(cuboid: Cuboid) -> Self { - let (vtx, idx) = cuboid.to_trimesh(); - Trimesh::new(vtx, idx) - } -} diff --git a/src/geometry/waabb.rs b/src/geometry/waabb.rs deleted file mode 100644 index 08f1df1..0000000 --- a/src/geometry/waabb.rs +++ /dev/null @@ -1,217 +0,0 @@ -use crate::geometry::Ray; -use crate::math::{Point, Vector, DIM, SIMD_WIDTH}; -use crate::utils; -use buckler::bounding_volume::AABB; -use num::{One, Zero}; -use { - crate::math::{SimdBool, SimdFloat}, - simba::simd::{SimdPartialOrd, SimdValue}, -}; - -#[derive(Debug, Copy, Clone)] -pub(crate) struct WRay { - pub origin: Point, - pub dir: Vector, -} - -impl WRay { - pub fn splat(ray: Ray) -> Self { - Self { - origin: Point::splat(ray.origin), - dir: Vector::splat(ray.dir), - } - } -} - -#[derive(Debug, Copy, Clone)] -pub(crate) struct WAABB { - pub mins: Point, - pub maxs: Point, -} - -#[cfg(feature = "serde-serialize")] -impl serde::Serialize for WAABB { - fn serialize(&self, serializer: S) -> Result - where - S: serde::Serializer, - { - use serde::ser::SerializeStruct; - - let mins: Point<[f32; SIMD_WIDTH]> = Point::from( - self.mins - .coords - .map(|e| array![|ii| e.extract(ii); SIMD_WIDTH]), - ); - let maxs: Point<[f32; SIMD_WIDTH]> = Point::from( - self.maxs - .coords - .map(|e| array![|ii| e.extract(ii); SIMD_WIDTH]), - ); - - let mut waabb = serializer.serialize_struct("WAABB", 2)?; - waabb.serialize_field("mins", &mins)?; - waabb.serialize_field("maxs", &maxs)?; - waabb.end() - } -} - -#[cfg(feature = "serde-serialize")] -impl<'de> serde::Deserialize<'de> for WAABB { - fn deserialize(deserializer: D) -> Result - where - D: serde::Deserializer<'de>, - { - struct Visitor {}; - impl<'de> serde::de::Visitor<'de> for Visitor { - type Value = WAABB; - fn expecting(&self, formatter: &mut std::fmt::Formatter) -> std::fmt::Result { - write!( - formatter, - "two arrays containing at least {} floats", - SIMD_WIDTH * DIM * 2 - ) - } - - fn visit_seq(self, mut seq: A) -> Result - where - A: serde::de::SeqAccess<'de>, - { - let mins: Point<[f32; SIMD_WIDTH]> = seq - .next_element()? - .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; - let maxs: Point<[f32; SIMD_WIDTH]> = seq - .next_element()? - .ok_or_else(|| serde::de::Error::invalid_length(1, &self))?; - let mins = Point::from(mins.coords.map(|e| SimdFloat::from(e))); - let maxs = Point::from(maxs.coords.map(|e| SimdFloat::from(e))); - Ok(WAABB { mins, maxs }) - } - } - - deserializer.deserialize_struct("WAABB", &["mins", "maxs"], Visitor {}) - } -} - -impl WAABB { - pub fn new_invalid() -> Self { - Self::splat(AABB::new_invalid()) - } - - pub fn splat(aabb: AABB) -> Self { - Self { - mins: Point::splat(aabb.mins), - maxs: Point::splat(aabb.maxs), - } - } - - pub fn dilate_by_factor(&mut self, factor: SimdFloat) { - // If some of the AABBs on this WAABB are invalid, - // don't, dilate them. - let is_valid = self.mins.x.simd_le(self.maxs.x); - let factor = factor.select(is_valid, SimdFloat::zero()); - - // NOTE: we multiply each by factor instead of doing - // (maxs - mins) * factor. That's to avoid overflows (and - // therefore NaNs if this WAABB contains some invalid - // AABBs initialised with f32::MAX - let dilation = self.maxs * factor - self.mins * factor; - self.mins -= dilation; - self.maxs += dilation; - } - - pub fn replace(&mut self, i: usize, aabb: AABB) { - self.mins.replace(i, aabb.mins); - self.maxs.replace(i, aabb.maxs); - } - - pub fn intersects_ray(&self, ray: &WRay, max_toi: SimdFloat) -> SimdBool { - let _0 = SimdFloat::zero(); - let _1 = SimdFloat::one(); - let _infinity = SimdFloat::splat(f32::MAX); - - let mut hit = SimdBool::splat(true); - let mut tmin = SimdFloat::zero(); - let mut tmax = max_toi; - - // TODO: could this be optimized more considering we really just need a boolean answer? - for i in 0usize..DIM { - let is_not_zero = ray.dir[i].simd_ne(_0); - let is_zero_test = - ray.origin[i].simd_ge(self.mins[i]) & ray.origin[i].simd_le(self.maxs[i]); - let is_not_zero_test = { - let denom = _1 / ray.dir[i]; - let mut inter_with_near_plane = - ((self.mins[i] - ray.origin[i]) * denom).select(is_not_zero, -_infinity); - let mut inter_with_far_plane = - ((self.maxs[i] - ray.origin[i]) * denom).select(is_not_zero, _infinity); - - let gt = inter_with_near_plane.simd_gt(inter_with_far_plane); - utils::simd_swap(gt, &mut inter_with_near_plane, &mut inter_with_far_plane); - - tmin = tmin.simd_max(inter_with_near_plane); - tmax = tmax.simd_min(inter_with_far_plane); - - tmin.simd_le(tmax) - }; - - hit = hit & is_not_zero_test.select(is_not_zero, is_zero_test); - } - - hit - } - - #[cfg(feature = "dim2")] - pub fn contains(&self, other: &WAABB) -> SimdBool { - self.mins.x.simd_le(other.mins.x) - & self.mins.y.simd_le(other.mins.y) - & self.maxs.x.simd_ge(other.maxs.x) - & self.maxs.y.simd_ge(other.maxs.y) - } - - #[cfg(feature = "dim3")] - pub fn contains(&self, other: &WAABB) -> SimdBool { - self.mins.x.simd_le(other.mins.x) - & self.mins.y.simd_le(other.mins.y) - & self.mins.z.simd_le(other.mins.z) - & self.maxs.x.simd_ge(other.maxs.x) - & self.maxs.y.simd_ge(other.maxs.y) - & self.maxs.z.simd_ge(other.maxs.z) - } - - #[cfg(feature = "dim2")] - pub fn intersects(&self, other: &WAABB) -> SimdBool { - self.mins.x.simd_le(other.maxs.x) - & other.mins.x.simd_le(self.maxs.x) - & self.mins.y.simd_le(other.maxs.y) - & other.mins.y.simd_le(self.maxs.y) - } - - #[cfg(feature = "dim3")] - pub fn intersects(&self, other: &WAABB) -> SimdBool { - self.mins.x.simd_le(other.maxs.x) - & other.mins.x.simd_le(self.maxs.x) - & self.mins.y.simd_le(other.maxs.y) - & other.mins.y.simd_le(self.maxs.y) - & self.mins.z.simd_le(other.maxs.z) - & other.mins.z.simd_le(self.maxs.z) - } - - pub fn to_merged_aabb(&self) -> AABB { - AABB::new( - self.mins.coords.map(|e| e.simd_horizontal_min()).into(), - self.maxs.coords.map(|e| e.simd_horizontal_max()).into(), - ) - } -} - -impl From<[AABB; SIMD_WIDTH]> for WAABB { - fn from(aabbs: [AABB; SIMD_WIDTH]) -> Self { - let mins = array![|ii| aabbs[ii].mins; SIMD_WIDTH]; - let maxs = array![|ii| aabbs[ii].maxs; SIMD_WIDTH]; - - WAABB { - mins: Point::from(mins), - maxs: Point::from(maxs), - } - } -} diff --git a/src/geometry/wquadtree.rs b/src/geometry/wquadtree.rs deleted file mode 100644 index 6f57ed3..0000000 --- a/src/geometry/wquadtree.rs +++ /dev/null @@ -1,587 +0,0 @@ -use crate::geometry::{ColliderHandle, ColliderSet, Ray, AABB}; -use crate::geometry::{WRay, WAABB}; -use crate::math::Point; -#[cfg(feature = "dim3")] -use crate::math::Vector; -use crate::simd::{SimdFloat, SIMD_WIDTH}; -use buckler::bounding_volume::BoundingVolume; -use simba::simd::{SimdBool, SimdValue}; -use std::collections::VecDeque; -use std::ops::Range; - -pub trait IndexedData: Copy { - fn default() -> Self; - fn index(&self) -> usize; -} - -impl IndexedData for usize { - fn default() -> Self { - u32::MAX as usize - } - - fn index(&self) -> usize { - *self - } -} - -impl IndexedData for ColliderHandle { - fn default() -> Self { - ColliderSet::invalid_handle() - } - - fn index(&self) -> usize { - self.into_raw_parts().0 - } -} - -#[derive(Copy, Clone, Debug, PartialEq, Eq)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -struct NodeIndex { - index: u32, // Index of the addressed node in the `nodes` array. - lane: u8, // SIMD lane of the addressed node. -} - -impl NodeIndex { - fn new(index: u32, lane: u8) -> Self { - Self { index, lane } - } - - fn invalid() -> Self { - Self { - index: u32::MAX, - lane: 0, - } - } -} - -#[derive(Copy, Clone, Debug)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -struct WQuadtreeNode { - waabb: WAABB, - // Index of the nodes of the 4 nodes represented by self. - // If this is a leaf, it contains the proxy ids instead. - children: [u32; 4], - parent: NodeIndex, - leaf: bool, // TODO: pack this with the NodexIndex.lane? - dirty: bool, // TODO: move this to a separate bitvec? -} - -#[derive(Copy, Clone, Debug, PartialEq, Eq)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -struct WQuadtreeProxy { - node: NodeIndex, - data: T, // The collider data. TODO: only set the collider generation here? -} - -impl WQuadtreeProxy { - fn invalid() -> Self { - Self { - node: NodeIndex::invalid(), - data: T::default(), - } - } -} - -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Clone, Debug)] -pub struct WQuadtree { - nodes: Vec, - dirty_nodes: VecDeque, - proxies: Vec>, -} - -// FIXME: this should be generic too. -impl WQuadtree { - pub fn pre_update(&mut self, data: ColliderHandle) { - let id = data.into_raw_parts().0; - let node_id = self.proxies[id].node.index; - let node = &mut self.nodes[node_id as usize]; - if !node.dirty { - node.dirty = true; - self.dirty_nodes.push_back(node_id); - } - } - - pub fn update(&mut self, colliders: &ColliderSet, dilation_factor: f32) { - // Loop on the dirty leaves. - let dilation_factor = SimdFloat::splat(dilation_factor); - - while let Some(id) = self.dirty_nodes.pop_front() { - // NOTE: this will data the case where we reach the root of the tree. - if let Some(node) = self.nodes.get(id as usize) { - // Compute the new WAABB. - let mut new_aabbs = [AABB::new_invalid(); SIMD_WIDTH]; - for (child_id, new_aabb) in node.children.iter().zip(new_aabbs.iter_mut()) { - if node.leaf { - // We are in a leaf: compute the colliders' AABBs. - if let Some(proxy) = self.proxies.get(*child_id as usize) { - let collider = &colliders[proxy.data]; - *new_aabb = collider.compute_aabb(); - } - } else { - // We are in an internal node: compute the children's AABBs. - if let Some(node) = self.nodes.get(*child_id as usize) { - *new_aabb = node.waabb.to_merged_aabb(); - } - } - } - - let node = &mut self.nodes[id as usize]; - let new_waabb = WAABB::from(new_aabbs); - if !node.waabb.contains(&new_waabb).all() { - node.waabb = new_waabb; - node.waabb.dilate_by_factor(dilation_factor); - self.dirty_nodes.push_back(node.parent.index); - } - node.dirty = false; - } - } - } -} - -impl WQuadtree { - pub fn new() -> Self { - WQuadtree { - nodes: Vec::new(), - dirty_nodes: VecDeque::new(), - proxies: Vec::new(), - } - } - - pub fn clear_and_rebuild( - &mut self, - data: impl ExactSizeIterator, - dilation_factor: f32, - ) { - self.nodes.clear(); - self.proxies.clear(); - - // Create proxies. - let mut indices = Vec::with_capacity(data.len()); - let mut aabbs = vec![AABB::new_invalid(); data.len()]; - self.proxies = vec![WQuadtreeProxy::invalid(); data.len()]; - - for (data, aabb) in data { - let index = data.index(); - if index >= self.proxies.len() { - self.proxies.resize(index + 1, WQuadtreeProxy::invalid()); - aabbs.resize(index + 1, AABB::new_invalid()); - } - - self.proxies[index].data = data; - aabbs[index] = aabb; - indices.push(index); - } - - // Build the tree recursively. - let root_node = WQuadtreeNode { - waabb: WAABB::new_invalid(), - children: [1, u32::MAX, u32::MAX, u32::MAX], - parent: NodeIndex::invalid(), - leaf: false, - dirty: false, - }; - - self.nodes.push(root_node); - let root_id = NodeIndex::new(0, 0); - let (_, aabb) = self.do_recurse_build(&mut indices, &aabbs, root_id, dilation_factor); - self.nodes[0].waabb = WAABB::from([ - aabb, - AABB::new_invalid(), - AABB::new_invalid(), - AABB::new_invalid(), - ]); - } - - fn do_recurse_build( - &mut self, - indices: &mut [usize], - aabbs: &[AABB], - parent: NodeIndex, - dilation_factor: f32, - ) -> (u32, AABB) { - if indices.len() <= 4 { - // Leaf case. - let my_id = self.nodes.len(); - let mut my_aabb = AABB::new_invalid(); - let mut leaf_aabbs = [AABB::new_invalid(); 4]; - let mut proxy_ids = [u32::MAX; 4]; - - for (k, id) in indices.iter().enumerate() { - my_aabb.merge(&aabbs[*id]); - leaf_aabbs[k] = aabbs[*id]; - proxy_ids[k] = *id as u32; - self.proxies[*id].node = NodeIndex::new(my_id as u32, k as u8); - } - - let mut node = WQuadtreeNode { - waabb: WAABB::from(leaf_aabbs), - children: proxy_ids, - parent, - leaf: true, - dirty: false, - }; - - node.waabb - .dilate_by_factor(SimdFloat::splat(dilation_factor)); - self.nodes.push(node); - return (my_id as u32, my_aabb); - } - - // Compute the center and variance along each dimension. - // In 3D we compute the variance to not-subdivide the dimension with lowest variance. - // Therefore variance computation is not needed in 2D because we only have 2 dimension - // to split in the first place. - let mut center = Point::origin(); - #[cfg(feature = "dim3")] - let mut variance = Vector::zeros(); - - let denom = 1.0 / (indices.len() as f32); - - for i in &*indices { - let coords = aabbs[*i].center().coords; - center += coords * denom; - #[cfg(feature = "dim3")] - { - variance += coords.component_mul(&coords) * denom; - } - } - - #[cfg(feature = "dim3")] - { - variance = variance - center.coords.component_mul(¢er.coords); - } - - // Find the axis with minimum variance. This is the axis along - // which we are **not** subdividing our set. - #[allow(unused_mut)] // Does not need to be mutable in 2D. - let mut subdiv_dims = [0, 1]; - #[cfg(feature = "dim3")] - { - let min = variance.imin(); - subdiv_dims[0] = (min + 1) % 3; - subdiv_dims[1] = (min + 2) % 3; - } - - // Split the set along the two subdiv_dims dimensions. - // TODO: should we split wrt. the median instead of the average? - // TODO: we should ensure each subslice contains at least 4 elements each (or less if - // indices has less than 16 elements in the first place. - let (left, right) = split_indices_wrt_dim(indices, &aabbs, ¢er, subdiv_dims[0]); - - let (left_bottom, left_top) = split_indices_wrt_dim(left, &aabbs, ¢er, subdiv_dims[1]); - let (right_bottom, right_top) = - split_indices_wrt_dim(right, &aabbs, ¢er, subdiv_dims[1]); - - // println!( - // "Recursing on children: {}, {}, {}, {}", - // left_bottom.len(), - // left_top.len(), - // right_bottom.len(), - // right_top.len() - // ); - - let node = WQuadtreeNode { - waabb: WAABB::new_invalid(), - children: [0; 4], // Will be set after the recursive call - parent, - leaf: false, - dirty: false, - }; - - let id = self.nodes.len() as u32; - self.nodes.push(node); - - // Recurse! - let a = self.do_recurse_build(left_bottom, aabbs, NodeIndex::new(id, 0), dilation_factor); - let b = self.do_recurse_build(left_top, aabbs, NodeIndex::new(id, 1), dilation_factor); - let c = self.do_recurse_build(right_bottom, aabbs, NodeIndex::new(id, 2), dilation_factor); - let d = self.do_recurse_build(right_top, aabbs, NodeIndex::new(id, 3), dilation_factor); - - // Now we know the indices of the grand-nodes. - self.nodes[id as usize].children = [a.0, b.0, c.0, d.0]; - self.nodes[id as usize].waabb = WAABB::from([a.1, b.1, c.1, d.1]); - self.nodes[id as usize] - .waabb - .dilate_by_factor(SimdFloat::splat(dilation_factor)); - - // TODO: will this chain of .merged be properly optimized? - let my_aabb = a.1.merged(&b.1).merged(&c.1).merged(&d.1); - (id, my_aabb) - } - - // FIXME: implement a visitor pattern to merge intersect_aabb - // and intersect_ray into a single method. - pub fn intersect_aabb(&self, aabb: &AABB, out: &mut Vec) { - if self.nodes.is_empty() { - return; - } - - // Special case for the root. - let mut stack = vec![0u32]; - let waabb = WAABB::splat(*aabb); - while let Some(inode) = stack.pop() { - let node = self.nodes[inode as usize]; - let intersections = node.waabb.intersects(&waabb); - let bitmask = intersections.bitmask(); - - for ii in 0..SIMD_WIDTH { - if (bitmask & (1 << ii)) != 0 { - if node.leaf { - // We found a leaf! - // Unfortunately, invalid AABBs return a intersection as well. - if let Some(proxy) = self.proxies.get(node.children[ii] as usize) { - out.push(proxy.data); - } - } else { - // Internal node, visit the child. - // Unfortunately, we have this check because invalid AABBs - // return a intersection as well. - if node.children[ii] as usize <= self.nodes.len() { - stack.push(node.children[ii]); - } - } - } - } - } - } - - pub fn cast_ray(&self, ray: &Ray, max_toi: f32, out: &mut Vec) { - if self.nodes.is_empty() { - return; - } - - // Special case for the root. - let mut stack = vec![0u32]; - let wray = WRay::splat(*ray); - let wmax_toi = SimdFloat::splat(max_toi); - while let Some(inode) = stack.pop() { - let node = self.nodes[inode as usize]; - let hits = node.waabb.intersects_ray(&wray, wmax_toi); - let bitmask = hits.bitmask(); - - for ii in 0..SIMD_WIDTH { - if (bitmask & (1 << ii)) != 0 { - if node.leaf { - // We found a leaf! - // Unfortunately, invalid AABBs return a hit as well. - if let Some(proxy) = self.proxies.get(node.children[ii] as usize) { - out.push(proxy.data); - } - } else { - // Internal node, visit the child. - // Un fortunately, we have this check because invalid AABBs - // return a hit as well. - if node.children[ii] as usize <= self.nodes.len() { - stack.push(node.children[ii]); - } - } - } - } - } - } -} - -#[allow(dead_code)] -struct WQuadtreeIncrementalBuilderStep { - range: Range, - parent: NodeIndex, -} - -#[allow(dead_code)] -struct WQuadtreeIncrementalBuilder { - quadtree: WQuadtree, - to_insert: Vec, - aabbs: Vec, - indices: Vec, -} - -#[allow(dead_code)] -impl WQuadtreeIncrementalBuilder { - pub fn new() -> Self { - Self { - quadtree: WQuadtree::new(), - to_insert: Vec::new(), - aabbs: Vec::new(), - indices: Vec::new(), - } - } - - pub fn update_single_depth(&mut self) { - if let Some(to_insert) = self.to_insert.pop() { - let indices = &mut self.indices[to_insert.range]; - - // Leaf case. - if indices.len() <= 4 { - let id = self.quadtree.nodes.len(); - let mut aabb = AABB::new_invalid(); - let mut leaf_aabbs = [AABB::new_invalid(); 4]; - let mut proxy_ids = [u32::MAX; 4]; - - for (k, id) in indices.iter().enumerate() { - aabb.merge(&self.aabbs[*id]); - leaf_aabbs[k] = self.aabbs[*id]; - proxy_ids[k] = *id as u32; - } - - let node = WQuadtreeNode { - waabb: WAABB::from(leaf_aabbs), - children: proxy_ids, - parent: to_insert.parent, - leaf: true, - dirty: false, - }; - - self.quadtree.nodes[to_insert.parent.index as usize].children - [to_insert.parent.lane as usize] = id as u32; - self.quadtree.nodes[to_insert.parent.index as usize] - .waabb - .replace(to_insert.parent.lane as usize, aabb); - self.quadtree.nodes.push(node); - return; - } - - // Compute the center and variance along each dimension. - // In 3D we compute the variance to not-subdivide the dimension with lowest variance. - // Therefore variance computation is not needed in 2D because we only have 2 dimension - // to split in the first place. - let mut center = Point::origin(); - #[cfg(feature = "dim3")] - let mut variance = Vector::zeros(); - - let denom = 1.0 / (indices.len() as f32); - let mut aabb = AABB::new_invalid(); - - for i in &*indices { - let coords = self.aabbs[*i].center().coords; - aabb.merge(&self.aabbs[*i]); - center += coords * denom; - #[cfg(feature = "dim3")] - { - variance += coords.component_mul(&coords) * denom; - } - } - - #[cfg(feature = "dim3")] - { - variance = variance - center.coords.component_mul(¢er.coords); - } - - // Find the axis with minimum variance. This is the axis along - // which we are **not** subdividing our set. - #[allow(unused_mut)] // Does not need to be mutable in 2D. - let mut subdiv_dims = [0, 1]; - #[cfg(feature = "dim3")] - { - let min = variance.imin(); - subdiv_dims[0] = (min + 1) % 3; - subdiv_dims[1] = (min + 2) % 3; - } - - // Split the set along the two subdiv_dims dimensions. - // TODO: should we split wrt. the median instead of the average? - // TODO: we should ensure each subslice contains at least 4 elements each (or less if - // indices has less than 16 elements in the first place. - let (left, right) = - split_indices_wrt_dim(indices, &self.aabbs, ¢er, subdiv_dims[0]); - - let (left_bottom, left_top) = - split_indices_wrt_dim(left, &self.aabbs, ¢er, subdiv_dims[1]); - let (right_bottom, right_top) = - split_indices_wrt_dim(right, &self.aabbs, ¢er, subdiv_dims[1]); - - let node = WQuadtreeNode { - waabb: WAABB::new_invalid(), - children: [0; 4], // Will be set after the recursive call - parent: to_insert.parent, - leaf: false, - dirty: false, - }; - - let id = self.quadtree.nodes.len() as u32; - self.quadtree.nodes.push(node); - - // Recurse! - let a = left_bottom.len(); - let b = a + left_top.len(); - let c = b + right_bottom.len(); - let d = c + right_top.len(); - self.to_insert.push(WQuadtreeIncrementalBuilderStep { - range: 0..a, - parent: NodeIndex::new(id, 0), - }); - self.to_insert.push(WQuadtreeIncrementalBuilderStep { - range: a..b, - parent: NodeIndex::new(id, 1), - }); - self.to_insert.push(WQuadtreeIncrementalBuilderStep { - range: b..c, - parent: NodeIndex::new(id, 2), - }); - self.to_insert.push(WQuadtreeIncrementalBuilderStep { - range: c..d, - parent: NodeIndex::new(id, 3), - }); - - self.quadtree.nodes[to_insert.parent.index as usize].children - [to_insert.parent.lane as usize] = id as u32; - self.quadtree.nodes[to_insert.parent.index as usize] - .waabb - .replace(to_insert.parent.lane as usize, aabb); - } - } -} - -fn split_indices_wrt_dim<'a>( - indices: &'a mut [usize], - aabbs: &[AABB], - split_point: &Point, - dim: usize, -) -> (&'a mut [usize], &'a mut [usize]) { - let mut icurr = 0; - let mut ilast = indices.len(); - - // The loop condition we can just do 0..indices.len() - // instead of the test icurr < ilast because we know - // we will iterate exactly once per index. - for _ in 0..indices.len() { - let i = indices[icurr]; - let center = aabbs[i].center(); - - if center[dim] > split_point[dim] { - ilast -= 1; - indices.swap(icurr, ilast); - } else { - icurr += 1; - } - } - - if icurr == 0 || icurr == indices.len() { - // We don't want to return one empty set. But - // this can happen if all the coordinates along the - // given dimension are equal. - // In this is the case, we just split in the middle. - let half = indices.len() / 2; - indices.split_at_mut(half) - } else { - indices.split_at_mut(icurr) - } -} - -#[cfg(test)] -mod test { - use crate::geometry::{WQuadtree, AABB}; - use crate::math::{Point, Vector}; - - #[test] - fn multiple_identical_AABB_stack_overflow() { - // A stack overflow was caused during the construction of the - // WAABB tree with more than four AABB with the same center. - let aabb = AABB::new(Point::origin(), Vector::repeat(1.0).into()); - - for k in 0..20 { - let mut tree = WQuadtree::new(); - tree.clear_and_rebuild((0..k).map(|i| (i, aabb)), 0.0); - } - } -} diff --git a/src/lib.rs b/src/lib.rs index ed9eda5..0cd76a1 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -53,14 +53,6 @@ macro_rules! array( #[allow(dead_code)] fn create_arr(mut callback: impl FnMut(usize) -> T) -> [T; SIMD_WIDTH] { [callback(0usize), callback(1usize), callback(2usize), callback(3usize)] - - // [callback(0usize), callback(1usize), callback(2usize), callback(3usize), - // callback(4usize), callback(5usize), callback(6usize), callback(7usize)] - - // [callback(0usize), callback(1usize), callback(2usize), callback(3usize), - // callback(4usize), callback(5usize), callback(6usize), callback(7usize), - // callback(8usize), callback(9usize), callback(10usize), callback(11usize), - // callback(12usize), callback(13usize), callback(14usize), callback(15usize)] } create_arr($callback) @@ -136,134 +128,4 @@ pub mod dynamics; pub mod geometry; pub mod pipeline; pub mod utils; - -#[cfg(feature = "dim2")] -/// Math primitives used throughout Rapier. -pub mod math { - pub use super::simd::*; - use na::{Isometry2, Matrix2, Point2, Translation2, UnitComplex, Vector2, Vector3, U1, U2}; - - /// The dimension of the physics simulated by this crate. - pub const DIM: usize = 2; - /// The maximum number of point a contact manifold can hold. - pub const MAX_MANIFOLD_POINTS: usize = 2; - /// The dimension of the physics simulated by this crate, given as a type-level-integer. - pub type Dim = U2; - /// The maximum number of angular degrees of freedom of a rigid body given as a type-level-integer. - pub type AngDim = U1; - /// A 2D isometry, i.e., a rotation followed by a translation. - pub type Isometry = Isometry2; - /// A 2D vector. - pub type Vector = Vector2; - /// A scalar used for angular velocity. - /// - /// This is called `AngVector` for coherence with the 3D version of this crate. - pub type AngVector = N; - /// A 2D point. - pub type Point = Point2; - /// A 2D rotation expressed as an unit complex number. - pub type Rotation = UnitComplex; - /// A 2D translation. - pub type Translation = Translation2; - /// The angular inertia of a rigid body. - pub type AngularInertia = N; - /// The principal angular inertia of a rigid body. - pub type PrincipalAngularInertia = N; - /// A matrix that represent the cross product with a given vector. - pub type CrossMatrix = Vector2; - /// A 2x2 matrix. - pub type Matrix = Matrix2; - /// A vector with a dimension equal to the maximum number of degrees of freedom of a rigid body. - pub type SpacialVector = Vector3; - /// A 2D symmetric-definite-positive matrix. - pub type SdpMatrix = crate::utils::SdpMatrix2; -} - -#[cfg(feature = "dim3")] -/// Math primitives used throughout Rapier. -pub mod math { - pub use super::simd::*; - use na::{Isometry3, Matrix3, Point3, Translation3, UnitQuaternion, Vector3, Vector6, U3}; - - /// The dimension of the physics simulated by this crate. - pub const DIM: usize = 3; - /// The maximum number of point a contact manifold can hold. - pub const MAX_MANIFOLD_POINTS: usize = 4; - /// The dimension of the physics simulated by this crate, given as a type-level-integer. - pub type Dim = U3; - /// The maximum number of angular degrees of freedom of a rigid body given as a type-level-integer. - pub type AngDim = U3; - /// A 3D isometry, i.e., a rotation followed by a translation. - pub type Isometry = Isometry3; - /// A 3D vector. - pub type Vector = Vector3; - /// An axis-angle vector used for angular velocity. - pub type AngVector = Vector3; - /// A 3D point. - pub type Point = Point3; - /// A 3D rotation expressed as an unit quaternion. - pub type Rotation = UnitQuaternion; - /// A 3D translation. - pub type Translation = Translation3; - /// The angular inertia of a rigid body. - pub type AngularInertia = crate::utils::SdpMatrix3; - /// The principal angular inertia of a rigid body. - pub type PrincipalAngularInertia = Vector3; - /// A matrix that represent the cross product with a given vector. - pub type CrossMatrix = Matrix3; - /// A 3x3 matrix. - pub type Matrix = Matrix3; - /// A vector with a dimension equal to the maximum number of degrees of freedom of a rigid body. - pub type SpacialVector = Vector6; - /// A 3D symmetric-definite-positive matrix. - pub type SdpMatrix = crate::utils::SdpMatrix3; -} - -#[cfg(not(feature = "simd-is-enabled"))] -mod simd { - use simba::simd::{AutoBoolx4, AutoF32x4}; - /// The number of lanes of a SIMD number. - pub const SIMD_WIDTH: usize = 4; - /// SIMD_WIDTH - 1 - pub const SIMD_LAST_INDEX: usize = 3; - /// A SIMD float with SIMD_WIDTH lanes. - pub type SimdFloat = AutoF32x4; - /// A SIMD bool with SIMD_WIDTH lanes. - pub type SimdBool = AutoBoolx4; -} - -#[cfg(feature = "simd-is-enabled")] -mod simd { - #[allow(unused_imports)] - #[cfg(feature = "simd-nightly")] - use simba::simd::{f32x16, f32x4, f32x8, m32x16, m32x4, m32x8, u8x16, u8x4, u8x8}; - #[cfg(feature = "simd-stable")] - use simba::simd::{WideBoolF32x4, WideF32x4}; - - /// The number of lanes of a SIMD number. - pub const SIMD_WIDTH: usize = 4; - /// SIMD_WIDTH - 1 - pub const SIMD_LAST_INDEX: usize = 3; - #[cfg(not(feature = "simd-nightly"))] - /// A SIMD float with SIMD_WIDTH lanes. - pub type SimdFloat = WideF32x4; - #[cfg(not(feature = "simd-nightly"))] - /// A SIMD bool with SIMD_WIDTH lanes. - pub type SimdBool = WideBoolF32x4; - #[cfg(feature = "simd-nightly")] - /// A SIMD float with SIMD_WIDTH lanes. - pub type SimdFloat = f32x4; - #[cfg(feature = "simd-nightly")] - /// A bool float with SIMD_WIDTH lanes. - pub type SimdBool = m32x4; - - // pub const SIMD_WIDTH: usize = 8; - // pub const SIMD_LAST_INDEX: usize = 7; - // pub type SimdFloat = f32x8; - // pub type SimdBool = m32x8; - - // pub const SIMD_WIDTH: usize = 16; - // pub const SIMD_LAST_INDEX: usize = 15; - // pub type SimdFloat = f32x16; - // pub type SimdBool = m32x16; -} +pub use buckler::math; diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index dee4312..11fac1c 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -47,7 +47,10 @@ impl QueryPipeline { } } - self.quadtree.update(colliders, self.dilation_factor); + self.quadtree.update( + |handle| colliders[*handle].compute_aabb(), + self.dilation_factor, + ); } /// Find the closest intersection between a ray and a set of collider. diff --git a/src/utils.rs b/src/utils.rs index 6557b74..4089631 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -7,7 +7,7 @@ use simba::simd::SimdValue; use std::ops::{Add, Mul}; use { - crate::simd::{SimdBool, SimdFloat}, + crate::math::{AngularInertia, SimdBool, SimdReal}, na::SimdPartialOrd, num::One, }; @@ -32,16 +32,6 @@ pub(crate) fn inv(val: f32) -> f32 { } } -/// 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: Sized { // See SIMD implementations of copy_sign there: https://stackoverflow.com/a/57872652 @@ -88,8 +78,8 @@ impl> WSign> for Vector3 { } } -impl WSign for SimdFloat { - fn copy_sign_to(self, to: SimdFloat) -> SimdFloat { +impl WSign for SimdReal { + fn copy_sign_to(self, to: SimdReal) -> SimdReal { to.simd_copysign(self) } } @@ -112,7 +102,7 @@ impl WComponent for f32 { } } -impl WComponent for SimdFloat { +impl WComponent for SimdReal { type Element = f32; fn min_component(self) -> Self::Element { @@ -328,22 +318,22 @@ impl WDot for f32 { } } -impl WCrossMatrix for Vector3 { - type CrossMat = Matrix3; +impl WCrossMatrix for Vector3 { + type CrossMat = Matrix3; #[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(), + SimdReal::zero(), -self.z, self.y, + self.z, SimdReal::zero(), -self.x, + -self.y, self.x, SimdReal::zero(), ) } } -impl WCrossMatrix for Vector2 { - type CrossMat = Vector2; +impl WCrossMatrix for Vector2 { + type CrossMat = Vector2; #[inline] fn gcross_matrix(self) -> Self::CrossMat { @@ -351,24 +341,24 @@ impl WCrossMatrix for Vector2 { } } -impl WCross> for Vector3 { - type Result = Vector3; +impl WCross> for Vector3 { + type Result = Vector3; fn gcross(&self, rhs: Self) -> Self::Result { self.cross(&rhs) } } -impl WCross> for SimdFloat { - type Result = Vector2; +impl WCross> for SimdReal { + type Result = Vector2; - fn gcross(&self, rhs: Vector2) -> Self::Result { + fn gcross(&self, rhs: Vector2) -> Self::Result { Vector2::new(-rhs.y * *self, rhs.x * *self) } } -impl WCross> for Vector2 { - type Result = SimdFloat; +impl WCross> for Vector2 { + type Result = SimdReal; fn gcross(&self, rhs: Self) -> Self::Result { let yx = Vector2::new(rhs.y, rhs.x); @@ -377,26 +367,26 @@ impl WCross> for Vector2 { } } -impl WDot> for Vector3 { - type Result = SimdFloat; +impl WDot> for Vector3 { + type Result = SimdReal; - fn gdot(&self, rhs: Vector3) -> Self::Result { + fn gdot(&self, rhs: Vector3) -> Self::Result { self.x * rhs.x + self.y * rhs.y + self.z * rhs.z } } -impl WDot> for Vector2 { - type Result = SimdFloat; +impl WDot> for Vector2 { + type Result = SimdReal; - fn gdot(&self, rhs: Vector2) -> Self::Result { + fn gdot(&self, rhs: Vector2) -> Self::Result { self.x * rhs.x + self.y * rhs.y } } -impl WDot for SimdFloat { - type Result = SimdFloat; +impl WDot for SimdReal { + type Result = SimdReal; - fn gdot(&self, rhs: SimdFloat) -> Self::Result { + fn gdot(&self, rhs: SimdReal) -> Self::Result { *self * rhs } } @@ -446,26 +436,26 @@ impl WAngularInertia for f32 { } } -impl WAngularInertia for SimdFloat { - type AngVector = SimdFloat; - type LinVector = Vector2; - type AngMatrix = SimdFloat; +impl WAngularInertia for SimdReal { + type AngVector = SimdReal; + type LinVector = Vector2; + type AngMatrix = SimdReal; fn inverse(&self) -> Self { - let zero = ::zero(); + let zero = ::zero(); let is_zero = self.simd_eq(zero); - (::one() / *self).select(is_zero, zero) + (::one() / *self).select(is_zero, zero) } - fn transform_lin_vector(&self, pt: Vector2) -> Vector2 { + fn transform_lin_vector(&self, pt: Vector2) -> Vector2 { pt * *self } - fn transform_vector(&self, pt: SimdFloat) -> SimdFloat { + fn transform_vector(&self, pt: SimdReal) -> SimdReal { *self * pt } - fn squared(&self) -> SimdFloat { + fn squared(&self) -> SimdReal { *self * *self } @@ -478,325 +468,8 @@ impl WAngularInertia for SimdFloat { } } -/// A 2x2 symmetric-definite-positive matrix. -#[derive(Copy, Clone, Debug, PartialEq)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -pub struct SdpMatrix2 { - /// The component at the first row and first column of this matrix. - pub m11: N, - /// The component at the first row and second column of this matrix. - pub m12: N, - /// The component at the second row and second column of this matrix. - pub m22: N, -} - -impl SdpMatrix2 { - /// A new SDP 2x2 matrix with the given components. - /// - /// Because the matrix is symmetric, only the lower off-diagonal component is required. - pub fn new(m11: N, m12: N, m22: N) -> Self { - Self { m11, m12, m22 } - } - - /// Build an `SdpMatrix2` structure from a plain matrix, assuming it is SDP. - /// - /// No check is performed to ensure `mat` is actually SDP. - pub fn from_sdp_matrix(mat: na::Matrix2) -> Self { - Self { - m11: mat.m11, - m12: mat.m12, - m22: mat.m22, - } - } - - /// Create a new SDP matrix filled with zeros. - pub fn zero() -> Self { - Self { - m11: N::zero(), - m12: N::zero(), - m22: N::zero(), - } - } - - /// Create a new SDP matrix with its diagonal filled with `val`, and its off-diagonal elements set to zero. - pub fn diagonal(val: N) -> Self { - Self { - m11: val, - m12: N::zero(), - m22: val, - } - } - - /// Adds `val` to the diagonal components of `self`. - pub fn add_diagonal(&mut self, elt: N) -> Self { - Self { - m11: self.m11 + elt, - m12: self.m12, - m22: self.m22 + elt, - } - } - - /// Compute the inverse of this SDP matrix without performing any inversibility check. - 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 } - } - - /// Convert this SDP matrix to a regular matrix representation. - pub fn into_matrix(self) -> Matrix2 { - Matrix2::new(self.m11, self.m12, self.m12, self.m22) - } -} - -impl Add> for SdpMatrix2 { - type Output = Self; - - fn add(self, rhs: SdpMatrix2) -> Self { - Self::new(self.m11 + rhs.m11, self.m12 + rhs.m12, self.m22 + rhs.m22) - } -} - -impl Mul> for SdpMatrix2 { - type Output = Vector2; - - fn mul(self, rhs: Vector2) -> Self::Output { - Vector2::new( - self.m11 * rhs.x + self.m12 * rhs.y, - self.m12 * rhs.x + self.m22 * rhs.y, - ) - } -} - -/// A 3x3 symmetric-definite-positive matrix. -#[derive(Copy, Clone, Debug, PartialEq)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -pub struct SdpMatrix3 { - /// The component at the first row and first column of this matrix. - pub m11: N, - /// The component at the first row and second column of this matrix. - pub m12: N, - /// The component at the first row and third column of this matrix. - pub m13: N, - /// The component at the second row and second column of this matrix. - pub m22: N, - /// The component at the second row and third column of this matrix. - pub m23: N, - /// The component at the third row and third column of this matrix. - pub m33: N, -} - -impl SdpMatrix3 { - /// A new SDP 3x3 matrix with the given components. - /// - /// Because the matrix is symmetric, only the lower off-diagonal components is required. - pub fn new(m11: N, m12: N, m13: N, m22: N, m23: N, m33: N) -> Self { - Self { - m11, - m12, - m13, - m22, - m23, - m33, - } - } - - /// Build an `SdpMatrix3` structure from a plain matrix, assuming it is SDP. - /// - /// No check is performed to ensure `mat` is actually SDP. - pub fn from_sdp_matrix(mat: na::Matrix3) -> Self { - Self { - m11: mat.m11, - m12: mat.m12, - m13: mat.m13, - m22: mat.m22, - m23: mat.m23, - m33: mat.m33, - } - } - - /// Create a new SDP matrix filled with zeros. - pub fn zero() -> Self { - Self { - m11: N::zero(), - m12: N::zero(), - m13: N::zero(), - m22: N::zero(), - m23: N::zero(), - m33: N::zero(), - } - } - - /// Create a new SDP matrix with its diagonal filled with `val`, and its off-diagonal elements set to zero. - pub fn diagonal(val: N) -> Self { - Self { - m11: val, - m12: N::zero(), - m13: N::zero(), - m22: val, - m23: N::zero(), - m33: val, - } - } - - /// Are all components of this matrix equal to zero? - 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() - } - - /// Compute the inverse of this SDP matrix without performing any inversibility check. - 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, - } - } - - /// Compute the quadratic form `m.transpose() * self * m`. - pub fn quadform3x2(&self, m: &Matrix3x2) -> SdpMatrix2 { - 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 } - } - - /// Compute the quadratic form `m.transpose() * self * m`. - pub fn quadform(&self, m: &Matrix3) -> 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, - } - } - - /// Adds `elt` to the diagonal components of `self`. - 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> Add> for SdpMatrix3 { - type Output = SdpMatrix3; - - fn add(self, rhs: SdpMatrix3) -> 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 Mul> for SdpMatrix3 { - type Output = Vector3; - - fn mul(self, rhs: Vector3) -> 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 Mul> for SdpMatrix3 { - type Output = Matrix3; - - fn mul(self, rhs: Matrix3) -> 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 Mul> for SdpMatrix3 { - type Output = Matrix3x2; - - fn mul(self, rhs: Matrix3x2) -> 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 for SdpMatrix3 { +#[cfg(feature = "dim3")] +impl WAngularInertia for AngularInertia { type AngVector = Vector3; type LinVector = Vector3; type AngMatrix = Matrix3; @@ -812,7 +485,7 @@ impl WAngularInertia for SdpMatrix3 { if determinant.is_zero() { Self::zero() } else { - SdpMatrix3 { + AngularInertia { m11: minor_m12_m23 / determinant, m12: -minor_m11_m23 / determinant, m13: minor_m11_m22 / determinant, @@ -824,7 +497,7 @@ impl WAngularInertia for SdpMatrix3 { } fn squared(&self) -> Self { - SdpMatrix3 { + AngularInertia { 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, @@ -860,10 +533,11 @@ impl WAngularInertia for SdpMatrix3 { } } -impl WAngularInertia for SdpMatrix3 { - type AngVector = Vector3; - type LinVector = Vector3; - type AngMatrix = Matrix3; +#[cfg(feature = "dim3")] +impl WAngularInertia for AngularInertia { + type AngVector = Vector3; + type LinVector = Vector3; + type AngMatrix = Matrix3; fn inverse(&self) -> Self { let minor_m12_m23 = self.m22 * self.m33 - self.m23 * self.m23; @@ -873,11 +547,11 @@ impl WAngularInertia for SdpMatrix3 { let determinant = self.m11 * minor_m12_m23 - self.m12 * minor_m11_m23 + self.m13 * minor_m11_m22; - let zero = ::zero(); + let zero = ::zero(); let is_zero = determinant.simd_eq(zero); - let inv_det = (::one() / determinant).select(is_zero, zero); + let inv_det = (::one() / determinant).select(is_zero, zero); - SdpMatrix3 { + AngularInertia { m11: minor_m12_m23 * inv_det, m12: -minor_m11_m23 * inv_det, m13: minor_m11_m22 * inv_det, @@ -887,11 +561,11 @@ impl WAngularInertia for SdpMatrix3 { } } - fn transform_lin_vector(&self, v: Vector3) -> Vector3 { + fn transform_lin_vector(&self, v: Vector3) -> Vector3 { self.transform_vector(v) } - fn transform_vector(&self, v: Vector3) -> Vector3 { + fn transform_vector(&self, v: Vector3) -> Vector3 { 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; @@ -899,7 +573,7 @@ impl WAngularInertia for SdpMatrix3 { } fn squared(&self) -> Self { - SdpMatrix3 { + AngularInertia { 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, @@ -910,7 +584,7 @@ impl WAngularInertia for SdpMatrix3 { } #[rustfmt::skip] - fn into_matrix(self) -> Matrix3 { + fn into_matrix(self) -> Matrix3 { Matrix3::new( self.m11, self.m12, self.m13, self.m12, self.m22, self.m23, @@ -919,7 +593,7 @@ impl WAngularInertia for SdpMatrix3 { } #[rustfmt::skip] - fn transform_matrix(&self, m: &Matrix3) -> Matrix3 { + fn transform_matrix(&self, m: &Matrix3) -> Matrix3 { 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; @@ -940,206 +614,6 @@ impl WAngularInertia for SdpMatrix3 { } } -impl From<[SdpMatrix3; 4]> for SdpMatrix3 -where - T: From<[f32; 4]>, -{ - fn from(data: [SdpMatrix3; 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; 8]> for SdpMatrix3 { - fn from(data: [SdpMatrix3; 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; 16]> for SdpMatrix3 { - fn from(data: [SdpMatrix3; 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, - ]), - } - } -} - // This is an RAII structure that enables flushing denormal numbers // to zero, and automatically reseting previous flags once it is dropped. #[derive(Clone, Debug, PartialEq, Eq)]