From 9b87f06a856c4d673642e210f8b0986cfdbac3af Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 21 Jan 2024 21:02:23 +0100 Subject: feat: implement new "small-steps" solver + joint improvements --- src/control/ray_cast_vehicle_controller.rs | 2 +- src/dynamics/ccd/mod.rs | 3 + src/dynamics/integration_parameters.rs | 26 +- src/dynamics/island_manager.rs | 30 +- src/dynamics/joint/generic_joint.rs | 25 +- .../joint/impulse_joint/impulse_joint_set.rs | 2 +- src/dynamics/joint/mod.rs | 2 + src/dynamics/joint/multibody_joint/mod.rs | 4 +- src/dynamics/joint/multibody_joint/multibody.rs | 56 +- .../joint/multibody_joint/multibody_joint.rs | 30 +- .../joint/multibody_joint/multibody_joint_set.rs | 46 +- .../joint/multibody_joint/unit_multibody_joint.rs | 51 +- src/dynamics/joint/rope_joint.rs | 104 +- src/dynamics/joint/spring_joint.rs | 172 +++ src/dynamics/mod.rs | 6 +- src/dynamics/rigid_body.rs | 41 +- src/dynamics/rigid_body_components.rs | 45 +- src/dynamics/solver/categorization.rs | 32 +- .../contact_constraint/contact_constraints_set.rs | 528 ++++++++ .../generic_one_body_constraint.rs | 292 ++++ .../generic_one_body_constraint_element.rs | 147 ++ .../generic_two_body_constraint.rs | 433 ++++++ .../generic_two_body_constraint_element.rs | 367 +++++ src/dynamics/solver/contact_constraint/mod.rs | 29 + .../contact_constraint/one_body_constraint.rs | 382 ++++++ .../one_body_constraint_element.rs | 232 ++++ .../contact_constraint/one_body_constraint_simd.rs | 372 ++++++ .../contact_constraint/two_body_constraint.rs | 470 +++++++ .../two_body_constraint_element.rs | 271 ++++ .../contact_constraint/two_body_constraint_simd.rs | 370 ++++++ src/dynamics/solver/delta_vel.rs | 57 - src/dynamics/solver/generic_velocity_constraint.rs | 378 ------ .../solver/generic_velocity_constraint_element.rs | 349 ----- .../solver/generic_velocity_ground_constraint.rs | 240 ---- .../generic_velocity_ground_constraint_element.rs | 143 -- src/dynamics/solver/interaction_groups.rs | 18 +- src/dynamics/solver/island_solver.rs | 93 +- .../joint_constraint/any_joint_constraint.rs | 97 ++ .../solver/joint_constraint/joint_constraint.rs | 540 -------- .../joint_constraint/joint_constraint_builder.rs | 1401 ++++++++++++++++++++ .../joint_constraint/joint_constraints_set.rs | 446 +++++++ .../joint_constraint/joint_generic_constraint.rs | 552 ++++++++ .../joint_generic_constraint_builder.rs | 1286 ++++++++++++++++++ .../joint_generic_velocity_constraint.rs | 562 -------- .../joint_generic_velocity_constraint_builder.rs | 886 ------------- .../joint_constraint/joint_velocity_constraint.rs | 387 +++--- .../joint_velocity_constraint_builder.rs | 1127 ---------------- src/dynamics/solver/joint_constraint/mod.rs | 23 +- src/dynamics/solver/mod.rs | 81 +- src/dynamics/solver/parallel_island_solver.rs | 34 +- src/dynamics/solver/parallel_solver_constraints.rs | 169 ++- src/dynamics/solver/parallel_velocity_solver.rs | 88 +- src/dynamics/solver/solver_body.rs | 59 + src/dynamics/solver/solver_constraints.rs | 564 -------- src/dynamics/solver/solver_constraints_set.rs | 241 ++++ src/dynamics/solver/solver_vel.rs | 59 + src/dynamics/solver/velocity_constraint.rs | 441 ------ src/dynamics/solver/velocity_constraint_element.rs | 211 --- src/dynamics/solver/velocity_constraint_wide.rs | 294 ---- src/dynamics/solver/velocity_ground_constraint.rs | 281 ---- .../solver/velocity_ground_constraint_element.rs | 181 --- .../solver/velocity_ground_constraint_wide.rs | 277 ---- src/dynamics/solver/velocity_solver.rs | 406 +++--- src/geometry/contact_pair.rs | 2 +- src/geometry/narrow_phase.rs | 8 +- .../debug_render_pipeline/debug_render_pipeline.rs | 4 +- src/pipeline/physics_pipeline.rs | 31 +- src/utils.rs | 108 +- 68 files changed, 9275 insertions(+), 7419 deletions(-) create mode 100644 src/dynamics/joint/spring_joint.rs create mode 100644 src/dynamics/solver/contact_constraint/contact_constraints_set.rs create mode 100644 src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs create mode 100644 src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs create mode 100644 src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs create mode 100644 src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs create mode 100644 src/dynamics/solver/contact_constraint/mod.rs create mode 100644 src/dynamics/solver/contact_constraint/one_body_constraint.rs create mode 100644 src/dynamics/solver/contact_constraint/one_body_constraint_element.rs create mode 100644 src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs create mode 100644 src/dynamics/solver/contact_constraint/two_body_constraint.rs create mode 100644 src/dynamics/solver/contact_constraint/two_body_constraint_element.rs create mode 100644 src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs delete mode 100644 src/dynamics/solver/delta_vel.rs delete mode 100644 src/dynamics/solver/generic_velocity_constraint.rs delete mode 100644 src/dynamics/solver/generic_velocity_constraint_element.rs delete mode 100644 src/dynamics/solver/generic_velocity_ground_constraint.rs delete mode 100644 src/dynamics/solver/generic_velocity_ground_constraint_element.rs create mode 100644 src/dynamics/solver/joint_constraint/any_joint_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/joint_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_constraint_builder.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_constraints_set.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_generic_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_generic_constraint_builder.rs delete mode 100644 src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs delete mode 100644 src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs create mode 100644 src/dynamics/solver/solver_body.rs delete mode 100644 src/dynamics/solver/solver_constraints.rs create mode 100644 src/dynamics/solver/solver_constraints_set.rs create mode 100644 src/dynamics/solver/solver_vel.rs delete mode 100644 src/dynamics/solver/velocity_constraint.rs delete mode 100644 src/dynamics/solver/velocity_constraint_element.rs delete mode 100644 src/dynamics/solver/velocity_constraint_wide.rs delete mode 100644 src/dynamics/solver/velocity_ground_constraint.rs delete mode 100644 src/dynamics/solver/velocity_ground_constraint_element.rs delete mode 100644 src/dynamics/solver/velocity_ground_constraint_wide.rs (limited to 'src') diff --git a/src/control/ray_cast_vehicle_controller.rs b/src/control/ray_cast_vehicle_controller.rs index 1e2da51..30979e1 100644 --- a/src/control/ray_cast_vehicle_controller.rs +++ b/src/control/ray_cast_vehicle_controller.rs @@ -4,7 +4,7 @@ use crate::dynamics::{RigidBody, RigidBodyHandle, RigidBodySet}; use crate::geometry::{ColliderHandle, ColliderSet, Ray}; use crate::math::{Point, Real, Rotation, Vector}; use crate::pipeline::{QueryFilter, QueryPipeline}; -use crate::utils::{WCross, WDot}; +use crate::utils::{SimdCross, SimdDot}; /// A character controller to simulate vehicles using ray-casting for the wheels. pub struct DynamicRayCastVehicleController { 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, pub(crate) active_kinematic_set: Vec, pub(crate) active_islands: Vec, + pub(crate) active_islands_additional_solver_iterations: Vec, active_set_timestamp: u32, #[cfg_attr(feature = "serde-serialize", serde(skip))] can_sleep: Vec, // 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 + '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 { pub impulse: N, } -impl Default for JointLimits { +impl Default for JointLimits { fn default() -> Self { Self { min: -N::splat(Real::MAX), @@ -131,6 +131,16 @@ impl Default for JointLimits { } } +impl From<[N; 2]> for JointLimits { + 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; 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 { + pub fn joint_velocity(&self, link: &MultibodyLink) -> DVectorView { 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, - out: &mut Vec, - mut insert_at: Option, - ) { - 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, - constraints: &mut Vec, - insert_at: &mut Option, - ) { + 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: MultibodyLinkId +/// let multibody = &multibody_joint_set[multibody_link_id.multibody]; +/// let link = multibody.link(multibody_link_id.id).expect("Link not found."); +pub struct MultibodyLinkId { + pub(crate) graph_id: RigidBodyGraphIndex, + /// The multibody index to be used as `&multibody_joint_set[multibody]` to + /// retrieve the multibody reference. pub multibody: MultibodyIndex, + /// The multibody link index to be given to [`Multibody::link`]. pub id: usize, } -impl Default for MultibodyJointLink { +impl Default for MultibodyLinkId { fn default() -> Self { Self { graph_id: RigidBodyGraphIndex::new(crate::INVALID_U32), @@ -78,7 +89,7 @@ impl Default for MultibodyJointLink { #[derive(Clone)] pub struct MultibodyJointSet { pub(crate) multibodies: Arena, // NOTE: a Slab would be sufficient. - pub(crate) rb2mb: Coarena, + pub(crate) rb2mb: Coarena, // NOTE: this is mostly for the island extraction. So perhaps we won’t need // that any more in the future when we improve our island builder. pub(crate) connectivity_graph: InteractionGraph, @@ -97,13 +108,22 @@ impl MultibodyJointSet { } /// Iterates through all the multibody joints from this set. - pub fn iter(&self) -> impl Iterator { + pub fn iter( + &self, + ) -> impl Iterator< + Item = ( + MultibodyJointHandle, + &MultibodyLinkId, + &Multibody, + &MultibodyLink, + ), + > { self.rb2mb .iter() .filter(|(_, link)| link.id > 0) // The first link of a rigid-body hasn’t been added by the user. .map(|(h, link)| { let mb = &self.multibodies[link.multibody.0]; - (MultibodyJointHandle(h), mb, mb.link(link.id).unwrap()) + (MultibodyJointHandle(h), link, mb, mb.link(link.id).unwrap()) }) } @@ -118,7 +138,7 @@ impl MultibodyJointSet { let data = data.into(); let link1 = self.rb2mb.get(body1.0).copied().unwrap_or_else(|| { let mb_handle = self.multibodies.insert(Multibody::with_root(body1)); - MultibodyJointLink { + MultibodyLinkId { graph_id: self.connectivity_graph.graph.add_node(body1), multibody: MultibodyIndex(mb_handle), id: 0, @@ -127,7 +147,7 @@ impl MultibodyJointSet { let link2 = self.rb2mb.get(body2.0).copied().unwrap_or_else(|| { let mb_handle = self.multibodies.insert(Multibody::with_root(body2)); - MultibodyJointLink { + MultibodyLinkId { graph_id: self.connectivity_graph.graph.add_node(body2), multibody: MultibodyIndex(mb_handle), id: 0, @@ -257,7 +277,7 @@ impl MultibodyJointSet { /// Returns the link of this multibody attached to the given rigid-body. /// /// Returns `None` if `rb` isn’t part of any rigid-body. - pub fn rigid_body_link(&self, rb: RigidBodyHandle) -> Option<&MultibodyJointLink> { + pub fn rigid_body_link(&self, rb: RigidBodyHandle) -> Option<&MultibodyLinkId> { self.rb2mb.get(rb.0) } @@ -340,15 +360,15 @@ impl MultibodyJointSet { // NOTE: if there is a joint between these two bodies, then // one of the bodies must be the parent of the other. let link1 = mb.link(id1.id)?; - let parent1 = link1.parent_id()?; + let parent1 = link1.parent_id(); - if parent1 == id2.id { + if parent1 == Some(id2.id) { Some((MultibodyJointHandle(rb1.0), mb, &link1)) } else { let link2 = mb.link(id2.id)?; - let parent2 = link2.parent_id()?; + let parent2 = link2.parent_id(); - if parent2 == id1.id { + if parent2 == Some(id1.id) { Some((MultibodyJointHandle(rb2.0), mb, &link2)) } else { None diff --git a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs index a1ec483..d6efd12 100644 --- a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs @@ -1,9 +1,7 @@ #![allow(missing_docs)] // For downcast. use crate::dynamics::joint::MultibodyLink; -use crate::dynamics::solver::{ - AnyJointVelocityConstraint, JointGenericVelocityGroundConstraint, WritebackId, -}; +use crate::dynamics::solver::{JointGenericOneBodyConstraint, WritebackId}; use crate::dynamics::{IntegrationParameters, JointMotor, Multibody}; use crate::math::Real; use na::DVector; @@ -19,18 +17,16 @@ pub fn unit_joint_limit_constraint( dof_id: usize, j_id: &mut usize, jacobians: &mut DVector, - constraints: &mut Vec, - insert_at: &mut Option, + constraints: &mut [JointGenericOneBodyConstraint], + insert_at: &mut usize, ) { let ndofs = multibody.ndofs(); - let joint_velocity = multibody.joint_velocity(link); - let min_enabled = curr_pos < limits[0]; let max_enabled = limits[1] < curr_pos; let erp_inv_dt = params.joint_erp_inv_dt(); let cfm_coeff = params.joint_cfm_coeff(); let rhs_bias = ((curr_pos - limits[1]).max(0.0) - (limits[0] - curr_pos).max(0.0)) * erp_inv_dt; - let rhs_wo_bias = joint_velocity[dof_id]; + let rhs_wo_bias = 0.0; let dof_j_id = *j_id + dof_id + link.assembly_id; jacobians.rows_mut(*j_id, ndofs * 2).fill(0.0); @@ -46,8 +42,8 @@ pub fn unit_joint_limit_constraint( max_enabled as u32 as Real * Real::MAX, ]; - let constraint = JointGenericVelocityGroundConstraint { - mj_lambda2: multibody.solver_id, + let constraint = JointGenericOneBodyConstraint { + solver_vel2: multibody.solver_id, ndofs2: ndofs, j_id2: *j_id, joint_id: usize::MAX, @@ -61,14 +57,9 @@ pub fn unit_joint_limit_constraint( writeback_id: WritebackId::Limit(dof_id), }; - if let Some(at) = insert_at { - constraints[*at] = AnyJointVelocityConstraint::JointGenericGroundConstraint(constraint); - *at += 1; - } else { - constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint( - constraint, - )); - } + constraints[*insert_at] = constraint; + *insert_at += 1; + *j_id += 2 * ndofs; } @@ -84,13 +75,11 @@ pub fn unit_joint_motor_constraint( dof_id: usize, j_id: &mut usize, jacobians: &mut DVector, - constraints: &mut Vec, - insert_at: &mut Option, + constraints: &mut [JointGenericOneBodyConstraint], + insert_at: &mut usize, ) { let inv_dt = params.inv_dt(); let ndofs = multibody.ndofs(); - let joint_velocity = multibody.joint_velocity(link); - let motor_params = motor.motor_params(params.dt); let dof_j_id = *j_id + dof_id + link.assembly_id; @@ -117,11 +106,10 @@ pub fn unit_joint_motor_constraint( ); }; - let dvel = joint_velocity[dof_id]; - rhs_wo_bias += dvel - target_vel; + rhs_wo_bias += -target_vel; - let constraint = JointGenericVelocityGroundConstraint { - mj_lambda2: multibody.solver_id, + let constraint = JointGenericOneBodyConstraint { + solver_vel2: multibody.solver_id, ndofs2: ndofs, j_id2: *j_id, joint_id: usize::MAX, @@ -135,13 +123,8 @@ pub fn unit_joint_motor_constraint( writeback_id: WritebackId::Limit(dof_id), }; - if let Some(at) = insert_at { - constraints[*at] = AnyJointVelocityConstraint::JointGenericGroundConstraint(constraint); - *at += 1; - } else { - constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint( - constraint, - )); - } + constraints[*insert_at] = constraint; + *insert_at += 1; + *j_id += 2 * ndofs; } diff --git a/src/dynamics/joint/rope_joint.rs b/src/dynamics/joint/rope_joint.rs index 3b7317c..44d4809 100644 --- a/src/dynamics/joint/rope_joint.rs +++ b/src/dynamics/joint/rope_joint.rs @@ -1,8 +1,8 @@ use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask}; use crate::dynamics::{JointAxis, MotorModel}; -use crate::math::{Point, Real, UnitVector}; +use crate::math::{Point, Real}; -use super::{JointLimits, JointMotor}; +use super::JointMotor; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq)] @@ -14,12 +14,16 @@ pub struct RopeJoint { } impl RopeJoint { - /// Creates a new rope joint limiting the max distance between to bodies - pub fn new() -> Self { - let data = GenericJointBuilder::new(JointAxesMask::FREE_FIXED_AXES) + /// Creates a new rope joint limiting the max distance between two bodies. + /// + /// The `max_dist` must be strictly greater than 0.0. + pub fn new(max_dist: Real) -> Self { + let data = GenericJointBuilder::new(JointAxesMask::empty()) .coupled_axes(JointAxesMask::LIN_AXES) .build(); - Self { data } + let mut result = Self { data }; + result.set_max_distance(max_dist); + result } /// The underlying generic joint. @@ -62,30 +66,6 @@ impl RopeJoint { self } - /// The principal axis of the joint, expressed in the local-space of the first rigid-body. - #[must_use] - pub fn local_axis1(&self) -> UnitVector { - self.data.local_axis1() - } - - /// Sets the principal axis of the joint, expressed in the local-space of the first rigid-body. - pub fn set_local_axis1(&mut self, axis1: UnitVector) -> &mut Self { - self.data.set_local_axis1(axis1); - self - } - - /// The principal axis of the joint, expressed in the local-space of the second rigid-body. - #[must_use] - pub fn local_axis2(&self) -> UnitVector { - self.data.local_axis2() - } - - /// Sets the principal axis of the joint, expressed in the local-space of the second rigid-body. - pub fn set_local_axis2(&mut self, axis2: UnitVector) -> &mut Self { - self.data.set_local_axis2(axis2); - self - } - /// The motor affecting the joint’s translational degree of freedom. #[must_use] pub fn motor(&self, axis: JointAxis) -> Option<&JointMotor> { @@ -95,9 +75,6 @@ impl RopeJoint { /// Set the spring-like model used by the motor to reach the desired target velocity and position. pub fn set_motor_model(&mut self, model: MotorModel) -> &mut Self { self.data.set_motor_model(JointAxis::X, model); - self.data.set_motor_model(JointAxis::Y, model); - #[cfg(feature = "dim3")] - self.data.set_motor_model(JointAxis::Z, model); self } @@ -105,11 +82,6 @@ impl RopeJoint { pub fn set_motor_velocity(&mut self, target_vel: Real, factor: Real) -> &mut Self { self.data .set_motor_velocity(JointAxis::X, target_vel, factor); - self.data - .set_motor_velocity(JointAxis::Y, target_vel, factor); - #[cfg(feature = "dim3")] - self.data - .set_motor_velocity(JointAxis::Z, target_vel, factor); self } @@ -122,11 +94,6 @@ impl RopeJoint { ) -> &mut Self { self.data .set_motor_position(JointAxis::X, target_pos, stiffness, damping); - self.data - .set_motor_position(JointAxis::Y, target_pos, stiffness, damping); - #[cfg(feature = "dim3")] - self.data - .set_motor_position(JointAxis::Z, target_pos, stiffness, damping); self } @@ -140,35 +107,26 @@ impl RopeJoint { ) -> &mut Self { self.data .set_motor(JointAxis::X, target_pos, target_vel, stiffness, damping); - self.data - .set_motor(JointAxis::Y, target_pos, target_vel, stiffness, damping); - #[cfg(feature = "dim3")] - self.data - .set_motor(JointAxis::Y, target_pos, target_vel, stiffness, damping); self } /// Sets the maximum force the motor can deliver. pub fn set_motor_max_force(&mut self, max_force: Real) -> &mut Self { self.data.set_motor_max_force(JointAxis::X, max_force); - self.data.set_motor_max_force(JointAxis::Y, max_force); - #[cfg(feature = "dim3")] - self.data.set_motor_max_force(JointAxis::Z, max_force); self } - /// The limit maximum distance attached bodies can translate. + /// The maximum distance allowed between the attached objects. #[must_use] - pub fn limits(&self, axis: JointAxis) -> Option<&JointLimits> { - self.data.limits(axis) + pub fn max_distance(&self) -> Option { + self.data.limits(JointAxis::X).map(|l| l.max) } - /// Sets the `[min,max]` limit distances attached bodies can translate. - pub fn set_limits(&mut self, limits: [Real; 2]) -> &mut Self { - self.data.set_limits(JointAxis::X, limits); - self.data.set_limits(JointAxis::Y, limits); - #[cfg(feature = "dim3")] - self.data.set_limits(JointAxis::Z, limits); + /// Sets the maximum allowed distance between the attached objects. + /// + /// The `max_dist` must be strictly greater than 0.0. + pub fn set_max_distance(&mut self, max_dist: Real) -> &mut Self { + self.data.set_limits(JointAxis::X, [0.0, max_dist]); self } } @@ -190,8 +148,8 @@ impl RopeJointBuilder { /// Creates a new builder for rope joints. /// /// This axis is expressed in the local-space of both rigid-bodies. - pub fn new() -> Self { - Self(RopeJoint::new()) + pub fn new(max_dist: Real) -> Self { + Self(RopeJoint::new(max_dist)) } /// Sets whether contacts between the attached rigid-bodies are enabled. @@ -215,20 +173,6 @@ impl RopeJointBuilder { self } - /// Sets the principal axis of the joint, expressed in the local-space of the first rigid-body. - #[must_use] - pub fn local_axis1(mut self, axis1: UnitVector) -> Self { - self.0.set_local_axis1(axis1); - self - } - - /// Sets the principal axis of the joint, expressed in the local-space of the second rigid-body. - #[must_use] - pub fn local_axis2(mut self, axis2: UnitVector) -> Self { - self.0.set_local_axis2(axis2); - self - } - /// Set the spring-like model used by the motor to reach the desired target velocity and position. #[must_use] pub fn motor_model(mut self, model: MotorModel) -> Self { @@ -270,10 +214,12 @@ impl RopeJointBuilder { self } - /// Sets the `[min,max]` limit distances attached bodies can translate. + /// Sets the maximum allowed distance between the attached bodies. + /// + /// The `max_dist` must be strictly greater than 0.0. #[must_use] - pub fn limits(mut self, limits: [Real; 2]) -> Self { - self.0.set_limits(limits); + pub fn max_distance(mut self, max_dist: Real) -> Self { + self.0.set_max_distance(max_dist); self } diff --git a/src/dynamics/joint/spring_joint.rs b/src/dynamics/joint/spring_joint.rs new file mode 100644 index 0000000..5427751 --- /dev/null +++ b/src/dynamics/joint/spring_joint.rs @@ -0,0 +1,172 @@ +use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask}; +use crate::dynamics::{JointAxis, MotorModel}; +use crate::math::{Point, Real}; + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug, PartialEq)] +#[repr(transparent)] +/// A spring-damper joint, applies a force proportional to the distance between two objects. +/// +/// The spring is integrated implicitly, implying that an even undamped spring will still be subject to some +/// amount of numerical damping (so it will eventually come to a rest). More solver iterations, or smaller +/// timesteps, will lower the effect of numerical damping, providing a more realistic result. +pub struct SpringJoint { + /// The underlying joint data. + pub data: GenericJoint, +} + +impl SpringJoint { + /// Creates a new spring joint limiting the max distance between two bodies. + /// + /// The `max_dist` must be strictly greater than 0.0. + pub fn new(rest_length: Real, stiffness: Real, damping: Real) -> Self { + let data = GenericJointBuilder::new(JointAxesMask::empty()) + .coupled_axes(JointAxesMask::LIN_AXES) + .motor_position(JointAxis::X, rest_length, stiffness, damping) + .motor_model(JointAxis::X, MotorModel::ForceBased) + .build(); + Self { data } + } + + /// The underlying generic joint. + pub fn data(&self) -> &GenericJoint { + &self.data + } + + /// Are contacts between the attached rigid-bodies enabled? + pub fn contacts_enabled(&self) -> bool { + self.data.contacts_enabled + } + + /// Sets whether contacts between the attached rigid-bodies are enabled. + pub fn set_contacts_enabled(&mut self, enabled: bool) -> &mut Self { + self.data.set_contacts_enabled(enabled); + self + } + + /// The joint’s anchor, expressed in the local-space of the first rigid-body. + #[must_use] + pub fn local_anchor1(&self) -> Point { + self.data.local_anchor1() + } + + /// Sets the joint’s anchor, expressed in the local-space of the first rigid-body. + pub fn set_local_anchor1(&mut self, anchor1: Point) -> &mut Self { + self.data.set_local_anchor1(anchor1); + self + } + + /// The joint’s anchor, expressed in the local-space of the second rigid-body. + #[must_use] + pub fn local_anchor2(&self) -> Point { + self.data.local_anchor2() + } + + /// Sets the joint’s anchor, expressed in the local-space of the second rigid-body. + pub fn set_local_anchor2(&mut self, anchor2: Point) -> &mut Self { + self.data.set_local_anchor2(anchor2); + self + } + + /// Set the spring model used by this joint to reach the desired target velocity and position. + /// + /// Setting this to `MotorModel::ForceBased` (which is the default value for this joint) makes the spring constants + /// (stiffness and damping) parameter understood as in the regular spring-mass-damper system. With + /// `MotorModel::AccelerationBased`, the spring constants will be automatically scaled by the attached masses, + /// making the spring more mass-independent. + pub fn set_spring_model(&mut self, model: MotorModel) -> &mut Self { + self.data.set_motor_model(JointAxis::X, model); + self + } + + // /// The maximum distance allowed between the attached objects. + // #[must_use] + // pub fn rest_length(&self) -> Option { + // self.data.limits(JointAxis::X).map(|l| l.max) + // } + // + // /// Sets the maximum allowed distance between the attached objects. + // /// + // /// The `max_dist` must be strictly greater than 0.0. + // pub fn set_rest_length(&mut self, max_dist: Real) -> &mut Self { + // self.data.set_limits(JointAxis::X, [0.0, max_dist]); + // self + // } +} + +impl Into for SpringJoint { + fn into(self) -> GenericJoint { + self.data + } +} + +/// A [SpringJoint] joint using the builder pattern. +/// +/// This builds a spring-damper joint which applies a force proportional to the distance between two objects. +/// See the documentation of [SpringJoint] for more information on its behavior. +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug, PartialEq)] +pub struct SpringJointBuilder(pub SpringJoint); + +impl SpringJointBuilder { + /// Creates a new builder for spring joints. + /// + /// This axis is expressed in the local-space of both rigid-bodies. + pub fn new(rest_length: Real, stiffness: Real, damping: Real) -> Self { + Self(SpringJoint::new(rest_length, stiffness, damping)) + } + + /// Sets whether contacts between the attached rigid-bodies are enabled. + #[must_use] + pub fn contacts_enabled(mut self, enabled: bool) -> Self { + self.0.set_contacts_enabled(enabled); + self + } + + /// Sets the joint’s anchor, expressed in the local-space of the first rigid-body. + #[must_use] + pub fn local_anchor1(mut self, anchor1: Point) -> Self { + self.0.set_local_anchor1(anchor1); + self + } + + /// Sets the joint’s anchor, expressed in the local-space of the second rigid-body. + #[must_use] + pub fn local_anchor2(mut self, anchor2: Point) -> Self { + self.0.set_local_anchor2(anchor2); + self + } + + /// Set the spring used by this joint to reach the desired target velocity and position. + /// + /// Setting this to `MotorModel::ForceBased` (which is the default value for this joint) makes the spring constants + /// (stiffness and damping) parameter understood as in the regular spring-mass-damper system. With + /// `MotorModel::AccelerationBased`, the spring constants will be automatically scaled by the attached masses, + /// making the spring more mass-independent. + #[must_use] + pub fn spring_model(mut self, model: MotorModel) -> Self { + self.0.set_spring_model(model); + self + } + + // /// Sets the maximum allowed distance between the attached bodies. + // /// + // /// The `max_dist` must be strictly greater than 0.0. + // #[must_use] + // pub fn max_distance(mut self, max_dist: Real) -> Self { + // self.0.set_max_distance(max_dist); + // self + // } + + /// Builds the spring joint. + #[must_use] + pub fn build(self) -> SpringJoint { + self.0 + } +} + +impl Into for SpringJointBuilder { + fn into(self) -> GenericJoint { + self.0.into() + } +} diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs index 04e5b03..0f9a019 100644 --- a/src/dynamics/mod.rs +++ b/src/dynamics/mod.rs @@ -8,10 +8,10 @@ pub(crate) use self::joint::JointGraphEdge; pub(crate) use self::joint::JointIndex; pub use self::joint::*; pub use self::rigid_body_components::*; -#[cfg(not(feature = "parallel"))] +// #[cfg(not(feature = "parallel"))] pub(crate) use self::solver::IslandSolver; -#[cfg(feature = "parallel")] -pub(crate) use self::solver::ParallelIslandSolver; +// #[cfg(feature = "parallel")] +// pub(crate) use self::solver::ParallelIslandSolver; pub use parry::mass_properties::MassProperties; pub use self::rigid_body::{RigidBody, RigidBodyBuilder}; diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 064087c..9469dc7 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -7,7 +7,7 @@ use crate::geometry::{ ColliderHandle, ColliderMassProps, ColliderParent, ColliderPosition, ColliderSet, ColliderShape, }; use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector}; -use crate::utils::WCross; +use crate::utils::SimdCross; use num::Zero; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -36,6 +36,7 @@ pub struct RigidBody { /// The dominance group this rigid-body is part of. pub(crate) dominance: RigidBodyDominance, pub(crate) enabled: bool, + pub(crate) additional_solver_iterations: usize, /// User-defined data associated to this rigid-body. pub user_data: u128, } @@ -64,6 +65,7 @@ impl RigidBody { dominance: RigidBodyDominance::default(), enabled: true, user_data: 0, + additional_solver_iterations: 0, } } @@ -72,6 +74,27 @@ impl RigidBody { self.ids = Default::default(); } + /// Set the additional number of solver iterations run for this rigid-body and + /// everything interacting with it. + /// + /// See [`Self::set_additional_solver_iterations`] for additional information. + pub fn additional_solver_iterations(&self) -> usize { + self.additional_solver_iterations + } + + /// Set the additional number of solver iterations run for this rigid-body and + /// everything interacting with it. + /// + /// Increasing this number will help improve simulation accuracy on this rigid-body + /// and every rigid-body interacting directly or indirectly with it (through joints + /// or contacts). This implies a performance hit. + /// + /// The default value is 0, meaning [`IntegrationParameters::num_solver_iterations`] will + /// be used as number of solver iterations for this body. + pub fn set_additional_solver_iterations(&mut self, additional_iterations: usize) { + self.additional_solver_iterations = additional_iterations; + } + /// The activation status of this rigid-body. pub fn activation(&self) -> &RigidBodyActivation { &self.activation @@ -1030,6 +1053,11 @@ pub struct RigidBodyBuilder { pub enabled: bool, /// An arbitrary user-defined 128-bit integer associated to the rigid-bodies built by this builder. pub user_data: u128, + /// The additional number of solver iterations run for this rigid-body and + /// everything interacting with it. + /// + /// See [`RigidBody::set_additional_solver_iterations`] for additional information. + pub additional_solver_iterations: usize, } impl RigidBodyBuilder { @@ -1051,6 +1079,7 @@ impl RigidBodyBuilder { dominance_group: 0, enabled: true, user_data: 0, + additional_solver_iterations: 0, } } @@ -1090,6 +1119,15 @@ impl RigidBodyBuilder { Self::new(RigidBodyType::Dynamic) } + /// Sets the additional number of solver iterations run for this rigid-body and + /// everything interacting with it. + /// + /// See [`RigidBody::set_additional_solver_iterations`] for additional information. + pub fn additional_solver_iterations(mut self, additional_iterations: usize) -> Self { + self.additional_solver_iterations = additional_iterations; + self + } + /// Sets the scale applied to the gravity force affecting the rigid-body to be created. pub fn gravity_scale(mut self, scale_factor: Real) -> Self { self.gravity_scale = scale_factor; @@ -1311,6 +1349,7 @@ impl RigidBodyBuilder { rb.vels.angvel = self.angvel; rb.body_type = self.body_type; rb.user_data = self.user_data; + rb.additional_solver_iterations = self.additional_solver_iterations; if self.additional_mass_properties != RigidBodyAdditionalMassProps::MassProps(MassProperties::zero()) diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs index 1a5ca1d..caff27e 100644 --- a/src/dynamics/rigid_body_components.rs +++ b/src/dynamics/rigid_body_components.rs @@ -7,7 +7,7 @@ use crate::math::{ AngVector, AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector, }; use crate::parry::partitioning::IndexedData; -use crate::utils::{WAngularInertia, WCross, WDot}; +use crate::utils::{SimdAngularInertia, SimdCross, SimdDot}; use num::Zero; /// The unique handle of a rigid body added to a `RigidBodySet`. @@ -307,11 +307,52 @@ impl RigidBodyMassProps { self.effective_inv_mass.map(crate::utils::inv) } + /// The square root of the effective world-space angular inertia (that takes the potential rotation locking into account) of + /// this rigid-body. + #[must_use] + pub fn effective_angular_inertia_sqrt(&self) -> AngularInertia { + #[allow(unused_mut)] // mut needed in 3D. + let mut ang_inertia = self.effective_world_inv_inertia_sqrt; + + // Make the matrix invertible. + #[cfg(feature = "dim3")] + { + if self.flags.contains(LockedAxes::ROTATION_LOCKED_X) { + ang_inertia.m11 = 1.0; + } + if self.flags.contains(LockedAxes::ROTATION_LOCKED_Y) { + ang_inertia.m22 = 1.0; + } + if self.flags.contains(LockedAxes::ROTATION_LOCKED_Z) { + ang_inertia.m33 = 1.0; + } + } + + #[allow(unused_mut)] // mut needed in 3D. + let mut result = ang_inertia.inverse(); + + // Remove the locked axes again. + #[cfg(feature = "dim3")] + { + if self.flags.contains(LockedAxes::ROTATION_LOCKED_X) { + result.m11 = 0.0; + } + if self.flags.contains(LockedAxes::ROTATION_LOCKED_Y) { + result.m22 = 0.0; + } + if self.flags.contains(LockedAxes::ROTATION_LOCKED_Z) { + result.m33 = 0.0; + } + } + + result + } + /// The effective world-space angular inertia (that takes the potential rotation locking into account) of /// this rigid-body. #[must_use] pub fn effective_angular_inertia(&self) -> AngularInertia { - self.effective_world_inv_inertia_sqrt.squared().inverse() + self.effective_angular_inertia_sqrt().squared() } /// Recompute the mass-properties of this rigid-bodies based on its currently attached colliders. diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs index d110971..cef9c9e 100644 --- a/src/dynamics/solver/categorization.rs +++ b/src/dynamics/solver/categorization.rs @@ -6,10 +6,10 @@ pub(crate) fn categorize_contacts( multibody_joints: &MultibodyJointSet, manifolds: &[&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], - out_ground: &mut Vec, - out_not_ground: &mut Vec, - out_generic_ground: &mut Vec, - out_generic_not_ground: &mut Vec, + out_one_body: &mut Vec, + out_two_body: &mut Vec, + out_generic_one_body: &mut Vec, + out_generic_two_body: &mut Vec, ) { for manifold_i in manifold_indices { let manifold = &manifolds[*manifold_i]; @@ -26,14 +26,14 @@ pub(crate) fn categorize_contacts( .is_some() { if manifold.data.relative_dominance != 0 { - out_generic_ground.push(*manifold_i); + out_generic_one_body.push(*manifold_i); } else { - out_generic_not_ground.push(*manifold_i); + out_generic_two_body.push(*manifold_i); } } else if manifold.data.relative_dominance != 0 { - out_ground.push(*manifold_i) + out_one_body.push(*manifold_i) } else { - out_not_ground.push(*manifold_i) + out_two_body.push(*manifold_i) } } } @@ -43,10 +43,10 @@ pub(crate) fn categorize_joints( multibody_joints: &MultibodyJointSet, impulse_joints: &[JointGraphEdge], joint_indices: &[JointIndex], - ground_joints: &mut Vec, - nonground_joints: &mut Vec, - generic_ground_joints: &mut Vec, - generic_nonground_joints: &mut Vec, + one_body_joints: &mut Vec, + two_body_joints: &mut Vec, + generic_one_body_joints: &mut Vec, + generic_two_body_joints: &mut Vec, ) { for joint_i in joint_indices { let joint = &impulse_joints[*joint_i].weight; @@ -57,14 +57,14 @@ pub(crate) fn categorize_joints( || multibody_joints.rigid_body_link(joint.body2).is_some() { if !rb1.is_dynamic() || !rb2.is_dynamic() { - generic_ground_joints.push(*joint_i); + generic_one_body_joints.push(*joint_i); } else { - generic_nonground_joints.push(*joint_i); + generic_two_body_joints.push(*joint_i); } } else if !rb1.is_dynamic() || !rb2.is_dynamic() { - ground_joints.push(*joint_i); + one_body_joints.push(*joint_i); } else { - nonground_joints.push(*joint_i); + two_body_joints.push(*joint_i); } } } diff --git a/src/dynamics/solver/contact_constraint/contact_constraints_set.rs b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs new file mode 100644 index 0000000..4d02270 --- /dev/null +++ b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs @@ -0,0 +1,528 @@ +use crate::dynamics::solver::categorization::categorize_contacts; +use crate::dynamics::solver::contact_constraint::{ + GenericOneBodyConstraint, GenericOneBodyConstraintBuilder, GenericTwoBodyConstraint, + GenericTwoBodyConstraintBuilder, OneBodyConstraint, OneBodyConstraintBuilder, + TwoBodyConstraint, TwoBodyConstraintBuilder, +}; +use crate::dynamics::solver::solver_body::SolverBody; +use crate::dynamics::solver::solver_vel::SolverVel; +use crate::dynamics::solver::{reset_buffer, ConstraintTypes, SolverConstraintsSet}; +use crate::dynamics::{ + ImpulseJoint, IntegrationParameters, IslandManager, JointAxesMask, MultibodyJointSet, + RigidBodySet, +}; +use crate::geometry::{ContactManifold, ContactManifoldIndex}; +use crate::math::{Real, MAX_MANIFOLD_POINTS}; +use na::DVector; +use parry::math::DIM; + +#[cfg(feature = "simd-is-enabled")] +use { + crate::dynamics::solver::contact_constraint::{ + OneBodyConstraintSimd, SimdOneBodyConstraintBuilder, TwoBodyConstraintBuilderSimd, + TwoBodyConstraintSimd, + }, + crate::math::SIMD_WIDTH, +}; + +pub struct ConstraintsCounts { + pub num_constraints: usize, + pub num_jacobian_lines: usize, +} + +impl ConstraintsCounts { + pub fn from_contacts(manifold: &ContactManifold) -> Self { + let rest = manifold.data.solver_contacts.len() % MAX_MANIFOLD_POINTS != 0; + Self { + num_constraints: manifold.data.solver_contacts.len() / MAX_MANIFOLD_POINTS + + rest as usize, + num_jacobian_lines: manifold.data.solver_contacts.len() * DIM, + } + } + + pub fn from_joint(joint: &ImpulseJoint) -> Self { + let joint = &joint.data; + let locked_axes = joint.locked_axes.bits(); + let motor_axes = joint.motor_axes.bits() & !locked_axes; + let limit_axes = joint.limit_axes.bits() & !locked_axes; + let coupled_axes = joint.coupled_axes.bits(); + + let num_constraints = (motor_axes & !coupled_axes).count_ones() as usize + + ((motor_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0) as usize + + ((motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0) as usize + + locked_axes.count_ones() as usize + + (limit_axes & !coupled_axes).count_ones() as usize + + ((limit_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0) as usize + + ((lim