diff options
Diffstat (limited to 'src/dynamics')
53 files changed, 5997 insertions, 4158 deletions
diff --git a/src/dynamics/ccd/mod.rs b/src/dynamics/ccd/mod.rs index 84807fa..a73ca85 100644 --- a/src/dynamics/ccd/mod.rs +++ b/src/dynamics/ccd/mod.rs @@ -1,3 +1,6 @@ +// TODO: not sure why it complains about PredictedImpacts being unused, +// making it private or pub(crate) triggers a different error. +#[allow(unused_imports)] pub use self::ccd_solver::{CCDSolver, PredictedImpacts}; pub use self::toi_entry::TOIEntry; diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 6d3acc5..d07527f 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -1,4 +1,5 @@ use crate::math::Real; +use std::num::NonZeroUsize; /// Parameters for a time-step of the physics engine. #[derive(Copy, Clone, Debug)] @@ -43,15 +44,10 @@ pub struct IntegrationParameters { pub max_penetration_correction: Real, /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`). pub prediction_distance: Real, - /// Maximum number of iterations performed to solve non-penetration and joint constraints (default: `4`). - pub max_velocity_iterations: usize, - /// Maximum number of iterations performed to solve friction constraints (default: `8`). - pub max_velocity_friction_iterations: usize, - /// Maximum number of iterations performed to remove the energy introduced by penetration corrections (default: `1`). - pub max_stabilization_iterations: usize, - /// If `false`, friction and non-penetration constraints will be solved in the same loop. Otherwise, - /// non-penetration constraints are solved first, and friction constraints are solved after (default: `true`). - pub interleave_restitution_and_friction_resolution: bool, + /// Number of iterations performed to solve friction constraints at solver iteration (default: `2`). + pub num_friction_iteration_per_solver_iteration: usize, + /// The number of solver iterations run by the constraints solver for calculating forces (default: `4`). + pub num_solver_iterations: NonZeroUsize, /// Minimum number of dynamic bodies in each active island (default: `128`). pub min_island_size: usize, /// Maximum number of substeps performed by the solver (default: `1`). @@ -151,17 +147,15 @@ impl Default for IntegrationParameters { Self { dt: 1.0 / 60.0, min_ccd_dt: 1.0 / 60.0 / 100.0, - erp: 0.8, - damping_ratio: 0.25, + erp: 0.6, + damping_ratio: 1.0, joint_erp: 1.0, joint_damping_ratio: 1.0, - allowed_linear_error: 0.001, // 0.005 + allowed_linear_error: 0.001, max_penetration_correction: Real::MAX, prediction_distance: 0.002, - max_velocity_iterations: 4, - max_velocity_friction_iterations: 8, - max_stabilization_iterations: 1, - interleave_restitution_and_friction_resolution: true, // Enabling this makes a big difference for 2D stability. + num_friction_iteration_per_solver_iteration: 2, + num_solver_iterations: NonZeroUsize::new(4).unwrap(), // TODO: what is the optimal value for min_island_size? // It should not be too big so that we don't end up with // huge islands that don't fit in cache. diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs index ef5d50a..4d14147 100644 --- a/src/dynamics/island_manager.rs +++ b/src/dynamics/island_manager.rs @@ -4,7 +4,7 @@ use crate::dynamics::{ }; use crate::geometry::{ColliderSet, NarrowPhase}; use crate::math::Real; -use crate::utils::WDot; +use crate::utils::SimdDot; /// Structure responsible for maintaining the set of active rigid-bodies, and /// putting non-moving rigid-bodies to sleep to save computation times. @@ -14,6 +14,7 @@ pub struct IslandManager { pub(crate) active_dynamic_set: Vec<RigidBodyHandle>, pub(crate) active_kinematic_set: Vec<RigidBodyHandle>, pub(crate) active_islands: Vec<usize>, + pub(crate) active_islands_additional_solver_iterations: Vec<usize>, active_set_timestamp: u32, #[cfg_attr(feature = "serde-serialize", serde(skip))] can_sleep: Vec<RigidBodyHandle>, // Workspace. @@ -28,6 +29,7 @@ impl IslandManager { active_dynamic_set: vec![], active_kinematic_set: vec![], active_islands: vec![], + active_islands_additional_solver_iterations: vec![], active_set_timestamp: 0, can_sleep: vec![], stack: vec![], @@ -127,6 +129,10 @@ impl IslandManager { &self.active_dynamic_set[island_range] } + pub(crate) fn active_island_additional_solver_iterations(&self, island_id: usize) -> usize { + self.active_islands_additional_solver_iterations[island_id] + } + #[inline(always)] pub(crate) fn iter_active_bodies<'a>(&'a self) -> impl Iterator<Item = RigidBodyHandle> + 'a { self.active_dynamic_set @@ -232,12 +238,21 @@ impl IslandManager { // let t = instant::now(); // Propagation of awake state and awake island computation through the // traversal of the interaction graph. + self.active_islands_additional_solver_iterations.clear(); self.active_islands.clear(); self.active_islands.push(0); // The max avoid underflow when the stack is empty. let mut island_marker = self.stack.len().max(1) - 1; + // NOTE: islands containing a body with non-standard number of iterations won’t + // be merged with another island, unless another island with standard + // iterations number already started before and got continued due to the + // `min_island_size`. That could be avoided by pushing bodies with non-standard + // iterations on top of the stack (and other bodies on the back). Not sure it’s + // worth it though. + let mut additional_solver_iterations = 0; + while let Some(handle) = self.stack.pop() { let rb = bodies.index_mut_internal(handle); @@ -248,16 +263,23 @@ impl IslandManager { } if self.stack.len() < island_marker { - if self.active_dynamic_set.len() - *self.active_islands.last().unwrap() - >= min_island_size + if additional_solver_iterations != rb.additional_solver_iterations + || self.active_dynamic_set.len() - *self.active_islands.last().unwrap() + >= min_island_size { // We are starting a new island. + self.active_islands_additional_solver_iterations + .push(additional_solver_iterations); self.active_islands.push(self.active_dynamic_set.len()); + additional_solver_iterations = 0; } island_marker = self.stack.len(); } + additional_solver_iterations = + additional_solver_iterations.max(rb.additional_solver_iterations); + // Transmit the active state to all the rigid-bodies with colliders // in contact or joined with this collider. push_contacting_bodies(&rb.colliders, colliders, narrow_phase, &mut self.stack); @@ -281,6 +303,8 @@ impl IslandManager { self.active_dynamic_set.push(handle); } + self.active_islands_additional_solver_iterations + .push(additional_solver_iterations); self.active_islands.push(self.active_dynamic_set.len()); // println!( // "Extraction: {}, num islands: {}", diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index 5096e15..83e27be 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -1,7 +1,7 @@ use crate::dynamics::solver::MotorParameters; use crate::dynamics::{FixedJoint, MotorModel, PrismaticJoint, RevoluteJoint, RopeJoint}; use crate::math::{Isometry, Point, Real, Rotation, UnitVector, Vector, SPATIAL_DIM}; -use crate::utils::{WBasis, WReal}; +use crate::utils::{SimdBasis, SimdRealCopy}; #[cfg(feature = "dim3")] use crate::dynamics::SphericalJoint; @@ -121,7 +121,7 @@ pub struct JointLimits<N> { pub impulse: N, } -impl<N: WReal> Default for JointLimits<N> { +impl<N: SimdRealCopy> Default for JointLimits<N> { fn default() -> Self { Self { min: -N::splat(Real::MAX), @@ -131,6 +131,16 @@ impl<N: WReal> Default for JointLimits<N> { } } +impl<N: SimdRealCopy> From<[N; 2]> for JointLimits<N> { + fn from(value: [N; 2]) -> Self { + Self { + min: value[0], + max: value[1], + impulse: N::splat(0.0), + } + } +} + /// A joint’s motor along one of its degrees of freedom. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq)] @@ -210,14 +220,23 @@ pub struct GenericJoint { /// The degrees-of-freedoms motorised by this joint. pub motor_axes: JointAxesMask, /// The coupled degrees of freedom of this joint. + /// + /// Note that coupling degrees of freedoms (DoF) changes the interpretation of the coupled joint’s limits and motors. + /// If multiple linear DoF are limited/motorized, only the limits/motor configuration for the first + /// coupled linear DoF is applied to all coupled linear DoF. Similarly, if multiple angular DoF are limited/motorized + /// only the limits/motor configuration for the first coupled angular DoF is applied to all coupled angular DoF. pub coupled_axes: JointAxesMask, /// The limits, along each degrees of freedoms of this joint. /// /// Note that the limit must also be explicitly enabled by the `limit_axes` bitmask. + /// For coupled degrees of freedoms (DoF), only the first linear (resp. angular) coupled DoF limit and `limit_axis` + /// bitmask is applied to the coupled linear (resp. angular) axes. pub limits: [JointLimits<Real>; SPATIAL_DIM], /// The motors, along each degrees of freedoms of this joint. /// - /// Note that the mostor must also be explicitly enabled by the `motors` bitmask. + /// Note that the motor must also be explicitly enabled by the `motor_axes` bitmask. + /// For coupled degrees of freedoms (DoF), only the first linear (resp. angular) coupled DoF motor and `motor_axes` + /// bitmask is applied to the coupled linear (resp. angular) axes. pub motors: [JointMotor; SPATIAL_DIM], /// Are contacts between the attached rigid-bodies enabled? pub contacts_enabled: bool, diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs index f21f950..faae1f6 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs @@ -214,7 +214,7 @@ impl ImpulseJointSet { // // .map(|e| &mut e.weight) // } - #[cfg(not(feature = "parallel"))] + // #[cfg(not(feature = "parallel"))] pub(crate) fn joints_mut(&mut self) -> &mut [JointGraphEdge] { &mut self.joint_graph.graph.edges[..] } diff --git a/src/dynamics/joint/mod.rs b/src/dynamics/joint/mod.rs index 93cb0ab..423d4c2 100644 --- a/src/dynamics/joint/mod.rs +++ b/src/dynamics/joint/mod.rs @@ -6,6 +6,7 @@ pub use self::multibody_joint::*; pub use self::prismatic_joint::*; pub use self::revolute_joint::*; pub use self::rope_joint::*; +pub use self::spring_joint::*; #[cfg(feature = "dim3")] pub use self::spherical_joint::*; @@ -21,3 +22,4 @@ mod rope_joint; #[cfg(feature = "dim3")] mod spherical_joint; +mod spring_joint; diff --git a/src/dynamics/joint/multibody_joint/mod.rs b/src/dynamics/joint/multibody_joint/mod.rs index a701350..c789004 100644 --- a/src/dynamics/joint/multibody_joint/mod.rs +++ b/src/dynamics/joint/multibody_joint/mod.rs @@ -2,7 +2,9 @@ pub use self::multibody::Multibody; pub use self::multibody_joint::MultibodyJoint; -pub use self::multibody_joint_set::{MultibodyIndex, MultibodyJointHandle, MultibodyJointSet}; +pub use self::multibody_joint_set::{ + MultibodyIndex, MultibodyJointHandle, MultibodyJointSet, MultibodyLinkId, +}; pub use self::multibody_link::MultibodyLink; pub use self::unit_multibody_joint::{unit_joint_limit_constraint, unit_joint_motor_constraint}; diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index 63e87e2..c3092ae 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -1,16 +1,13 @@ use super::multibody_link::{MultibodyLink, MultibodyLinkVec}; use super::multibody_workspace::MultibodyWorkspace; -use crate::dynamics::{ - solver::AnyJointVelocityConstraint, IntegrationParameters, RigidBodyHandle, RigidBodySet, - RigidBodyType, RigidBodyVelocity, -}; +use crate::dynamics::{RigidBodyHandle, RigidBodySet, RigidBodyType, RigidBodyVelocity}; #[cfg(feature = "dim3")] use crate::math::Matrix; use crate::math::{ AngDim, AngVector, Dim, Isometry, Jacobian, Point, Real, Vector, ANG_DIM, DIM, SPATIAL_DIM, }; use crate::prelude::MultibodyJoint; -use crate::utils::{IndexMut2, WAngularInertia, WCross, WCrossMatrix}; +use crate::utils::{IndexMut2, SimdAngularInertia, SimdCross, SimdCrossMatrix}; use na::{self, DMatrix, DVector, DVectorView, DVectorViewMut, Dyn, OMatrix, SMatrix, SVector, LU}; #[repr(C)] @@ -372,6 +369,7 @@ impl Multibody { self.accelerations.fill(0.0); + // Eqn 42 to 45 for i in 0..self.links.len() { let link = &self.links[i]; let rb = &bodies[link.rigid_body]; @@ -400,7 +398,7 @@ impl Multibody { } acc.linvel += rb.vels.angvel.gcross(rb.vels.angvel.gcross(link.shift23)); - acc.linvel += self.workspace.accs[i].angvel.gcross(link.shift23); + acc.linvel += acc.angvel.gcross(link.shift23); self.workspace.accs[i] = acc; @@ -728,7 +726,7 @@ impl Multibody { /// The generalized velocity at the multibody_joint of the given link. #[inline] - pub(crate) fn joint_velocity(&self, link: &MultibodyLink) -> DVectorView<Real> { + pub fn joint_velocity(&self, link: &MultibodyLink) -> DVectorView<Real> { let ndofs = link.joint().ndofs(); DVectorView::from_slice( &self.velocities.as_slice()[link.assembly_id..link.assembly_id + ndofs], @@ -829,8 +827,10 @@ impl Multibody { } } + // TODO: make a version that doesn’t write back to bodies and doesn’t update the jacobians + // (i.e. just something used by the velocity solver’s small steps). /// Apply forward-kinematics to this multibody and its related rigid-bodies. - pub fn forward_kinematics(&mut self, bodies: &mut RigidBodySet, update_mass_props: bool) { + pub fn forward_kinematics(&mut self, bodies: &mut RigidBodySet, update_rb_mass_props: bool) { // Special case for the root, which has no parent. { let link = &mut self.links[0]; @@ -839,7 +839,7 @@ impl Multibody { if let Some(rb) = bodies.get_mut_internal(link.rigid_body) { rb.pos.next_position = link.local_to_world; - if update_mass_props { + if update_rb_mass_props { rb.mprops.update_world_mass_properties(&link.local_to_world); } } @@ -873,7 +873,7 @@ impl Multibody { "A rigid-body that is not at the root of a multibody must be dynamic." ); - if update_mass_props { + if update_rb_mass_props { link_rb .mprops .update_world_mass_properties(&link.local_to_world); @@ -951,40 +951,4 @@ impl Multibody { .sum(); (num_constraints, num_constraints) } - - #[inline] - pub(crate) fn generate_internal_constraints( - &self, - params: &IntegrationParameters, - j_id: &mut usize, - jacobians: &mut DVector<Real>, - out: &mut Vec<AnyJointVelocityConstraint>, - mut insert_at: Option<usize>, - ) { - if !cfg!(feature = "parallel") { - let num_constraints: usize = self - .links - .iter() - .map(|l| l.joint().num_velocity_constraints()) - .sum(); - - let required_jacobian_len = *j_id + num_constraints * self.ndofs * 2; - if jacobians.nrows() < required_jacobian_len { - jacobians.resize_vertically_mut(required_jacobian_len, 0.0); - } - } - - for link in self.links.iter() { - link.joint().velocity_constraints( - params, - self, - link, - 0, - j_id, - jacobians, - out, - &mut insert_at, - ); - } - } } diff --git a/src/dynamics/joint/multibody_joint/multibody_joint.rs b/src/dynamics/joint/multibody_joint/multibody_joint.rs index da650e6..62fc434 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint.rs @@ -1,4 +1,4 @@ -use crate::dynamics::solver::AnyJointVelocityConstraint; +use crate::dynamics::solver::JointGenericOneBodyConstraint; use crate::dynamics::{ joint, FixedJointBuilder, GenericJoint, IntegrationParameters, Multibody, MultibodyLink, RigidBodyVelocity, @@ -254,15 +254,15 @@ impl MultibodyJoint { params: &IntegrationParameters, multibody: &Multibody, link: &MultibodyLink, - dof_id: usize, - j_id: &mut usize, + mut j_id: usize, jacobians: &mut DVector<Real>, - constraints: &mut Vec<AnyJointVelocityConstraint>, - insert_at: &mut Option<usize>, - ) { + constraints: &mut [JointGenericOneBodyConstraint], + ) -> usize { + let j_id = &mut j_id; let locked_bits = self.data.locked_axes.bits(); let limit_bits = self.data.limit_axes.bits(); let motor_bits = self.data.motor_axes.bits(); + let mut num_constraints = 0; let mut curr_free_dof = 0; for i in 0..DIM { @@ -281,11 +281,11 @@ impl MultibodyJoint { &self.data.motors[i], self.coords[i], limits, - dof_id + curr_free_dof, + curr_free_dof, j_id, jacobians, constraints, - insert_at, + &mut num_constraints, ); } @@ -296,11 +296,11 @@ impl MultibodyJoint { link, [self.data.limits[i].min, self.data.limits[i].max], self.coords[i], - dof_id + curr_free_dof, + curr_free_dof, j_id, jacobians, constraints, - insert_at, + &mut num_constraints, ); } curr_free_dof += 1; @@ -331,11 +331,11 @@ impl MultibodyJoint { link, limits, self.coords[i], - dof_id + curr_free_dof, + curr_free_dof, j_id, jacobians, constraints, - insert_at, + &mut num_constraints, ); Some(limits) } else { @@ -350,15 +350,17 @@ impl MultibodyJoint { &self.data.motors[i], self.coords[i], limits, - dof_id + curr_free_dof, + curr_free_dof, j_id, jacobians, constraints, - insert_at, + &mut num_constraints, ); } curr_free_dof += 1; } } + + num_constraints } } diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs index a4b338a..a428c8b 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs @@ -53,13 +53,24 @@ impl IndexedData for MultibodyJointHandle { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq, Eq)] -pub struct MultibodyJointLink { - pub graph_id: RigidBodyGraphIndex, +/// Indexes usable to get a multibody link from a `MultibodyJointSet`. +/// +/// ```rust +/// // With: +/// // multibody_joint_set: MultibodyJointSet +/// // multibody_link_id: MultibodyL |