From cc6d1b973002b4d366bc81ec6bf9e8240ad7b404 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 14 Dec 2020 15:51:43 +0100 Subject: 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 +- src/dynamics/solver/position_constraint_wide.rs | 48 +- .../solver/position_ground_constraint_wide.rs | 40 +- src/dynamics/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_detector/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 = Ang