From db6a8c526d939a125485c89cfb6e540422fe6b4b Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sat, 19 Mar 2022 16:10:49 +0100 Subject: Fix warnings and add comments. --- src/dynamics/integration_parameters.rs | 12 +- src/dynamics/joint/fixed_joint.rs | 17 +++ src/dynamics/joint/generic_joint.rs | 98 +++++++++++++- src/dynamics/joint/impulse_joint/impulse_joint.rs | 5 +- src/dynamics/joint/multibody_joint/multibody.rs | 41 +++--- .../joint/multibody_joint/multibody_joint.rs | 9 +- .../joint/multibody_joint/multibody_joint_set.rs | 4 +- .../joint/multibody_joint/multibody_link.rs | 13 +- src/dynamics/joint/prismatic_joint.rs | 35 +++++ src/dynamics/joint/revolute_joint.rs | 26 ++++ src/dynamics/joint/spherical_joint.rs | 42 +++++- src/dynamics/rigid_body.rs | 2 + src/dynamics/rigid_body_components.rs | 4 +- src/dynamics/rigid_body_set.rs | 9 +- src/dynamics/solver/interaction_groups.rs | 29 ++-- .../solver/joint_constraint/joint_constraint.rs | 10 +- .../joint_velocity_constraint_builder.rs | 148 ++++++++++----------- src/dynamics/solver/solver_constraints.rs | 18 +-- 18 files changed, 388 insertions(+), 134 deletions(-) (limited to 'src/dynamics') diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index e66f72d..84c8117 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -20,10 +20,7 @@ pub struct IntegrationParameters { /// 0-1: multiplier for how much of the constraint violation (e.g. contact penetration) /// will be compensated for during the velocity solve. - /// If zero, you need to enable the positional solver. - /// If non-zero, you do not need the positional solver. - /// A good non-zero value is around `0.2`. - /// (default `0.0`). + /// (default `0.8`). pub erp: Real, /// 0-1: the damping ratio used by the springs for Baumgarte constraints stabilization. /// Lower values make the constraints more compliant (more "springy", allowing more visible penetrations @@ -31,7 +28,13 @@ pub struct IntegrationParameters { /// (default `0.25`). pub damping_ratio: Real, + /// 0-1: multiplier for how much of the joint violation + /// will be compensated for during the velocity solve. + /// (default `1.0`). pub joint_erp: Real, + + /// The fraction of critical damping applied to the joint for constraints regularization. + /// (default `0.25`). pub joint_damping_ratio: Real, /// Amount of penetration the engine wont attempt to correct (default: `0.001m`). @@ -131,6 +134,7 @@ impl IntegrationParameters { 1.0 / (1.0 + cfm_coeff) } + /// The CFM (constranits force mixing) coefficient applied to all joints for constraints regularization pub fn joint_cfm_coeff(&self) -> Real { // Compute CFM assuming a critically damped spring multiplied by the damping ratio. let inv_erp_minus_one = 1.0 / self.joint_erp - 1.0; diff --git a/src/dynamics/joint/fixed_joint.rs b/src/dynamics/joint/fixed_joint.rs index 192b7d9..92f0d7c 100644 --- a/src/dynamics/joint/fixed_joint.rs +++ b/src/dynamics/joint/fixed_joint.rs @@ -4,6 +4,7 @@ use crate::math::{Isometry, Point, Real}; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq)] #[repr(transparent)] +/// A fixed joint, locks all relative motion between two bodies. pub struct FixedJoint { data: GenericJoint, } @@ -15,47 +16,56 @@ impl Default for FixedJoint { } impl FixedJoint { + /// Creates a new fixed joint. #[must_use] pub fn new() -> Self { let data = GenericJointBuilder::new(JointAxesMask::LOCKED_FIXED_AXES).build(); Self { data } } + /// The joint’s frame, expressed in the first rigid-body’s local-space. #[must_use] pub fn local_frame1(&self) -> &Isometry { &self.data.local_frame1 } + /// Sets the joint’s frame, expressed in the first rigid-body’s local-space. pub fn set_local_frame1(&mut self, local_frame: Isometry) -> &mut Self { self.data.set_local_frame1(local_frame); self } + /// The joint’s frame, expressed in the second rigid-body’s local-space. #[must_use] pub fn local_frame2(&self) -> &Isometry { &self.data.local_frame2 } + /// Sets joint’s frame, expressed in the second rigid-body’s local-space. pub fn set_local_frame2(&mut self, local_frame: Isometry) -> &mut Self { self.data.set_local_frame2(local_frame); 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 @@ -68,39 +78,46 @@ impl Into for FixedJoint { } } +/// Create fixed joints using the builder pattern. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq, Default)] pub struct FixedJointBuilder(FixedJoint); impl FixedJointBuilder { + /// Creates a new builder for fixed joints. pub fn new() -> Self { Self(FixedJoint::new()) } + /// Sets the joint’s frame, expressed in the first rigid-body’s local-space. #[must_use] pub fn local_frame1(mut self, local_frame: Isometry) -> Self { self.0.set_local_frame1(local_frame); self } + /// Sets joint’s frame, expressed in the second rigid-body’s local-space. #[must_use] pub fn local_frame2(mut self, local_frame: Isometry) -> Self { self.0.set_local_frame2(local_frame); 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 } + /// Build the fixed joint. #[must_use] pub fn build(self) -> FixedJoint { self.0 diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index 4aea61c..acf6dfb 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -8,55 +8,91 @@ use crate::dynamics::SphericalJoint; #[cfg(feature = "dim3")] bitflags::bitflags! { + /// A bit mask identifying multiple degrees of freedom of a joint. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub struct JointAxesMask: u8 { + /// The translational degree of freedom along the local X axis of a joint. const X = 1 << 0; + /// The translational degree of freedom along the local Y axis of a joint. const Y = 1 << 1; + /// The translational degree of freedom along the local Z axis of a joint. const Z = 1 << 2; + /// The angular degree of freedom along the local X axis of a joint. const ANG_X = 1 << 3; + /// The angular degree of freedom along the local Y axis of a joint. const ANG_Y = 1 << 4; + /// The angular degree of freedom along the local Z axis of a joint. const ANG_Z = 1 << 5; + /// The set of degrees of freedom locked by a revolute joint. const LOCKED_REVOLUTE_AXES = Self::X.bits | Self::Y.bits | Self::Z.bits | Self::ANG_Y.bits | Self::ANG_Z.bits; + /// The set of degrees of freedom locked by a prismatic joint. const LOCKED_PRISMATIC_AXES = Self::Y.bits | Self::Z.bits | Self::ANG_X.bits | Self::ANG_Y.bits | Self::ANG_Z.bits; + /// The set of degrees of freedom locked by a fixed joint. const LOCKED_FIXED_AXES = Self::X.bits | Self::Y.bits | Self::Z.bits | Self::ANG_X.bits | Self::ANG_Y.bits | Self::ANG_Z.bits; + /// The set of degrees of freedom locked by a spherical joint. const LOCKED_SPHERICAL_AXES = Self::X.bits | Self::Y.bits | Self::Z.bits; + /// The set of degrees of freedom left free by a revolute joint. const FREE_REVOLUTE_AXES = Self::ANG_X.bits; + /// The set of degrees of freedom left free by a prismatic joint. const FREE_PRISMATIC_AXES = Self::X.bits; + /// The set of degrees of freedom left free by a fixed joint. const FREE_FIXED_AXES = 0; + /// The set of degrees of freedom left free by a spherical joint. const FREE_SPHERICAL_AXES = Self::ANG_X.bits | Self::ANG_Y.bits | Self::ANG_Z.bits; + /// The set of all translational degrees of freedom. const LIN_AXES = Self::X.bits() | Self::Y.bits() | Self::Z.bits(); + /// The set of all angular degrees of freedom. const ANG_AXES = Self::ANG_X.bits() | Self::ANG_Y.bits() | Self::ANG_Z.bits(); } } #[cfg(feature = "dim2")] bitflags::bitflags! { + /// A bit mask identifying multiple degrees of freedom of a joint. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub struct JointAxesMask: u8 { + /// The translational degree of freedom along the local X axis of a joint. const X = 1 << 0; + /// The translational degree of freedom along the local Y axis of a joint. const Y = 1 << 1; + /// The angular degree of freedom of a joint. const ANG_X = 1 << 2; + /// The set of degrees of freedom locked by a revolute joint. const LOCKED_REVOLUTE_AXES = Self::X.bits | Self::Y.bits; + /// The set of degrees of freedom locked by a prismatic joint. const LOCKED_PRISMATIC_AXES = Self::Y.bits | Self::ANG_X.bits; + /// The set of degrees of freedom locked by a fixed joint. const LOCKED_FIXED_AXES = Self::X.bits | Self::Y.bits | Self::ANG_X.bits; + /// The set of degrees of freedom left free by a revolute joint. const FREE_REVOLUTE_AXES = Self::ANG_X.bits; + /// The set of degrees of freedom left free by a prismatic joint. const FREE_PRISMATIC_AXES = Self::X.bits; + /// The set of degrees of freedom left free by a fixed joint. const FREE_FIXED_AXES = 0; + /// The set of all translational degrees of freedom. const LIN_AXES = Self::X.bits() | Self::Y.bits(); + /// The set of all angular degrees of freedom. const ANG_AXES = Self::ANG_X.bits(); } } +/// Identifiers of degrees of freedoms of a joint. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq)] pub enum JointAxis { + /// The translational degree of freedom along the joint’s local X axis. X = 0, + /// The translational degree of freedom along the joint’s local Y axis. Y, + /// The translational degree of freedom along the joint’s local Z axis. #[cfg(feature = "dim3")] Z, + /// The rotational degree of freedom along the joint’s local X axis. AngX, + /// The rotational degree of freedom along the joint’s local Y axis. #[cfg(feature = "dim3")] AngY, + /// The rotational degree of freedom along the joint’s local Z axis. #[cfg(feature = "dim3")] AngZ, } @@ -67,11 +103,15 @@ impl From for JointAxesMask { } } +/// The limits of a joint along one of its degrees of freedom. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq)] pub struct JointLimits { + /// The minimum bound of the joint limit. pub min: N, + /// The maximum bound of the joint limit. pub max: N, + /// The impulse applied to enforce the joint’s limit. pub impulse: N, } @@ -85,15 +125,23 @@ impl Default for JointLimits { } } +/// A joint’s motor along one of its degrees of freedom. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq)] pub struct JointMotor { + /// The target velocity of the motor. pub target_vel: Real, + /// The target position of the motor. pub target_pos: Real, + /// The stiffness coefficient of the motor’s spring-like equation. pub stiffness: Real, + /// The damping coefficient of the motor’s spring-like equation. pub damping: Real, + /// The maximum force this motor can deliver. pub max_force: Real, + /// The impulse applied by this motor. pub impulse: Real, + /// The spring-like model used for simulating this motor. pub model: MotorModel, } @@ -130,14 +178,27 @@ impl JointMotor { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq)] +/// A generic joint. pub struct GenericJoint { + /// The joint’s frame, expressed in the first rigid-body’s local-space. pub local_frame1: Isometry, + /// The joint’s frame, expressed in the second rigid-body’s local-space. pub local_frame2: Isometry, + /// The degrees-of-freedoms locked by this joint. pub locked_axes: JointAxesMask, + /// The degrees-of-freedoms limited by this joint. pub limit_axes: JointAxesMask, + /// The degrees-of-freedoms motorised by this joint. pub motor_axes: JointAxesMask, + /// The coupled degrees of freedom of this joint. 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. 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. pub motors: [JointMotor; SPATIAL_DIM], } @@ -157,11 +218,13 @@ impl Default for GenericJoint { } impl GenericJoint { + /// Creates a new generic joint that locks the specified degrees of freedom. #[must_use] pub fn new(locked_axes: JointAxesMask) -> Self { *Self::default().lock_axes(locked_axes) } + #[cfg(feature = "simd-is-enabled")] /// Can this joint use SIMD-accelerated constraint formulations? pub(crate) fn supports_simd_constraints(&self) -> bool { self.limit_axes.is_empty() && self.motor_axes.is_empty() @@ -187,61 +250,73 @@ impl GenericJoint { } } + /// Add the specified axes to the set of axes locked by this joint. pub fn lock_axes(&mut self, axes: JointAxesMask) -> &mut Self { self.locked_axes |= axes; self } + /// Sets the joint’s frame, expressed in the first rigid-body’s local-space. pub fn set_local_frame1(&mut self, local_frame: Isometry) -> &mut Self { self.local_frame1 = local_frame; self } + /// Sets the joint’s frame, expressed in the second rigid-body’s local-space. pub fn set_local_frame2(&mut self, local_frame: Isometry) -> &mut Self { self.local_frame2 = local_frame; self } + /// The principal (local X) axis of this joint, expressed in the first rigid-body’s local-space. #[must_use] pub fn local_axis1(&self) -> UnitVector { self.local_frame1 * Vector::x_axis() } + /// Sets the principal (local X) axis of this joint, expressed in the first rigid-body’s local-space. pub fn set_local_axis1(&mut self, local_axis: UnitVector) -> &mut Self { self.local_frame1.rotation = Self::complete_ang_frame(local_axis); self } + /// The principal (local X) axis of this joint, expressed in the second rigid-body’s local-space. #[must_use] pub fn local_axis2(&self) -> UnitVector { self.local_frame2 * Vector::x_axis() } + /// Sets the principal (local X) axis of this joint, expressed in the second rigid-body’s local-space. pub fn set_local_axis2(&mut self, local_axis: UnitVector) -> &mut Self { self.local_frame2.rotation = Self::complete_ang_frame(local_axis); self } + /// The anchor of this joint, expressed in the first rigid-body’s local-space. #[must_use] pub fn local_anchor1(&self) -> Point { self.local_frame1.translation.vector.into() } + /// Sets anchor of this joint, expressed in the first rigid-body’s local-space. pub fn set_local_anchor1(&mut self, anchor1: Point) -> &mut Self { self.local_frame1.translation.vector = anchor1.coords; self } + /// The anchor of this joint, expressed in the second rigid-body’s local-space. #[must_use] pub fn local_anchor2(&self) -> Point { self.local_frame2.translation.vector.into() } + /// Sets anchor of this joint, expressed in the second rigid-body’s local-space. pub fn set_local_anchor2(&mut self, anchor2: Point) -> &mut Self { self.local_frame2.translation.vector = anchor2.coords; self } + /// The joint limits along the specified axis. #[must_use] pub fn limits(&self, axis: JointAxis) -> Option<&JointLimits> { let i = axis as usize; @@ -252,6 +327,7 @@ impl GenericJoint { } } + /// Sets the joint limits along the specified axis. pub fn set_limits(&mut self, axis: JointAxis, limits: [Real; 2]) -> &mut Self { let i = axis as usize; self.limit_axes |= axis.into(); @@ -260,6 +336,7 @@ impl GenericJoint { self } + /// The spring-like motor model along the specified axis of this joint. #[must_use] pub fn motor_model(&self, axis: JointAxis) -> Option { let i = axis as usize; @@ -303,11 +380,13 @@ impl GenericJoint { self.set_motor(axis, target_pos, 0.0, stiffness, damping) } + /// Sets the maximum force the motor can deliver along the specified axis. pub fn set_motor_max_force(&mut self, axis: JointAxis, max_force: Real) -> &mut Self { self.motors[axis as usize].max_force = max_force; self } + /// The motor affecting the joint’s degree of freedom along the specified axis. #[must_use] pub fn motor(&self, axis: JointAxis) -> Option<&JointMotor> { let i = axis as usize; @@ -339,6 +418,7 @@ impl GenericJoint { macro_rules! joint_conversion_methods( ($as_joint: ident, $as_joint_mut: ident, $Joint: ty, $axes: expr) => { + /// Converts the joint to its specific variant, if it is one. #[must_use] pub fn $as_joint(&self) -> Option<&$Joint> { if self.locked_axes == $axes { @@ -350,6 +430,7 @@ macro_rules! joint_conversion_methods( } } + /// Converts the joint to its specific mutable variant, if it is one. #[must_use] pub fn $as_joint_mut(&mut self) -> Option<&mut $Joint> { if self.locked_axes == $axes { @@ -392,63 +473,74 @@ impl GenericJoint { ); } +/// Create generic joints using the builder pattern. #[derive(Copy, Clone, Debug)] pub struct GenericJointBuilder(GenericJoint); impl GenericJointBuilder { + /// Creates a new generic joint builder. #[must_use] pub fn new(locked_axes: JointAxesMask) -> Self { Self(GenericJoint::new(locked_axes)) } + /// Sets the degrees of freedom locked by the joint. #[must_use] - pub fn lock_axes(mut self, axes: JointAxesMask) -> Self { - self.0.lock_axes(axes); + pub fn locked_axes(mut self, axes: JointAxesMask) -> Self { + self.0.locked_axes = axes; self } + /// Sets the joint’s frame, expressed in the first rigid-body’s local-space. #[must_use] pub fn local_frame1(mut self, local_frame: Isometry) -> Self { self.0.set_local_frame1(local_frame); self } + /// Sets the joint’s frame, expressed in the second rigid-body’s local-space. #[must_use] pub fn local_frame2(mut self, local_frame: Isometry) -> Self { self.0.set_local_frame2(local_frame); self } + /// Sets the principal (local X) axis of this joint, expressed in the first rigid-body’s local-space. #[must_use] pub fn local_axis1(mut self, local_axis: UnitVector) -> Self { self.0.set_local_axis1(local_axis); self } + /// Sets the principal (local X) axis of this joint, expressed in the second rigid-body’s local-space. #[must_use] pub fn local_axis2(mut self, local_axis: UnitVector) -> Self { self.0.set_local_axis2(local_axis); self } + /// Sets the anchor of this joint, expressed in the first rigid-body’s local-space. #[must_use] pub fn local_anchor1(mut self, anchor1: Point) -> Self { self.0.set_local_anchor1(anchor1); self } + /// Sets the anchor of this joint, expressed in the second rigid-body’s local-space. #[must_use] pub fn local_anchor2(mut self, anchor2: Point) -> Self { self.0.set_local_anchor2(anchor2); self } + /// Sets the joint limits along the specified axis. #[must_use] pub fn limits(mut self, axis: JointAxis, limits: [Real; 2]) -> Self { self.0.set_limits(axis, limits); self } + /// Sets the coupled degrees of freedom for this joint’s limits and motor. #[must_use] pub fn coupled_axes(mut self, axes: JointAxesMask) -> Self { self.0.coupled_axes = axes; @@ -498,12 +590,14 @@ impl GenericJointBuilder { self } + /// Sets the maximum force the motor can deliver along the specified axis. #[must_use] pub fn motor_max_force(mut self, axis: JointAxis, max_force: Real) -> Self { self.0.set_motor_max_force(axis, max_force); self } + /// Builds the generic joint. #[must_use] pub fn build(self) -> GenericJoint { self.0 diff --git a/src/dynamics/joint/impulse_joint/impulse_joint.rs b/src/dynamics/joint/impulse_joint/impulse_joint.rs index f80380e..8d35c35 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint.rs @@ -3,14 +3,17 @@ use crate::math::{Real, SpacialVector}; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Clone, Debug, PartialEq)] -/// A joint attached to two bodies. +/// An impulse-based joint attached to two bodies. pub struct ImpulseJoint { /// Handle to the first body attached to this joint. pub body1: RigidBodyHandle, /// Handle to the second body attached to this joint. pub body2: RigidBodyHandle, + /// The joint’s description. pub data: GenericJoint, + + /// The impulses applied by this joint. pub impulses: SpacialVector, // A joint needs to know its handle to simplify its removal. diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index 58d11b6..c4cc85f 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -120,7 +120,7 @@ impl Multibody { } } - pub fn with_root(handle: RigidBodyHandle) -> Self { + pub(crate) fn with_root(handle: RigidBodyHandle) -> Self { let mut mb = Multibody::new(); mb.root_is_dynamic = true; let joint = MultibodyJoint::free(Isometry::identity()); @@ -128,7 +128,7 @@ impl Multibody { mb } - pub fn remove_link(self, to_remove: usize, joint_only: bool) -> Vec { + pub(crate) fn remove_link(self, to_remove: usize, joint_only: bool) -> Vec { let mut result = vec![]; let mut link2mb = vec![usize::MAX; self.links.len()]; let mut link_id2new_id = vec![usize::MAX; self.links.len()]; @@ -187,7 +187,7 @@ impl Multibody { result } - pub fn append(&mut self, mut rhs: Multibody, parent: usize, joint: MultibodyJoint) { + pub(crate) fn append(&mut self, mut rhs: Multibody, parent: usize, joint: MultibodyJoint) { let rhs_root_ndofs = rhs.links[0].joint.ndofs(); let rhs_copy_shift = self.ndofs + rhs_root_ndofs; let rhs_copy_ndofs = rhs.ndofs - rhs_root_ndofs; @@ -235,6 +235,7 @@ impl Multibody { self.workspace.resize(self.links.len(), self.ndofs); } + /// The inverse augmented mass matrix of this multibody. pub fn inv_augmented_mass(&self) -> &LU { &self.inv_augmented_mass } @@ -298,7 +299,7 @@ impl Multibody { &mut self.damping } - pub fn add_link( + pub(crate) fn add_link( &mut self, parent: Option, // FIXME: should be a RigidBodyHandle? dof: MultibodyJoint, @@ -368,7 +369,7 @@ impl Multibody { .extend((0..num_jacobians).map(|_| Jacobian::zeros(0))); } - pub fn update_acceleration(&mut self, bodies: &Bodies) + pub(crate) fn update_acceleration(&mut self, bodies: &Bodies) where Bodies: ComponentSet + ComponentSet @@ -451,7 +452,7 @@ impl Multibody { } /// Computes the constant terms of the dynamics. - pub fn update_dynamics(&mut self, dt: Real, bodies: &mut Bodies) + pub(crate) fn update_dynamics(&mut self, dt: Real, bodies: &mut Bodies) where Bodies: ComponentSetMut + ComponentSet, { @@ -756,36 +757,40 @@ impl Multibody { ) } + /// The generalized accelerations of this multibodies. #[inline] pub fn generalized_acceleration(&self) -> DVectorSlice { self.accelerations.rows(0, self.ndofs) } + /// The generalized velocities of this multibodies. #[inline] pub fn generalized_velocity(&self) -> DVectorSlice { self.velocities.rows(0, self.ndofs) } + /// The mutable generalized velocities of this multibodies. #[inline] pub fn generalized_velocity_mut(&mut self) -> DVectorSliceMut { self.velocities.rows_mut(0, self.ndofs) } #[inline] - pub fn integrate(&mut self, dt: Real) { + pub(crate) fn integrate(&mut self, dt: Real) { for rb in self.links.iter_mut() { rb.joint .integrate(dt, &self.velocities.as_slice()[rb.assembly_id..]) } } + /// Apply displacements, in generalized coordinates, to this multibody. pub fn apply_displacements(&mut self, disp: &[Real]) { for link in self.links.iter_mut() { link.joint.apply_displacement(&disp[link.assembly_id..]) } } - pub fn update_root_type(&mut self, bodies: &mut Bodies) + pub(crate) fn update_root_type(&mut self, bodies: &mut Bodies) where Bodies: ComponentSet + ComponentSet, { @@ -851,6 +856,7 @@ impl Multibody { } } + /// Apply forward-kinematics to this multibody and its related rigid-bodies. pub fn forward_kinematics(&mut self, bodies: &mut Bodies, update_mass_props: bool) where Bodies: ComponentSet @@ -917,12 +923,13 @@ impl Multibody { self.update_body_jacobians(); } + /// The total number of freedoms of this multibody. #[inline] pub fn ndofs(&self) -> usize { self.ndofs } - pub fn fill_jacobians( + pub(crate) fn fill_jacobians( &self, link_id: usize, unit_force: Vector, @@ -964,14 +971,16 @@ impl Multibody { (j.dot(&invm_j), j.dot(&self.generalized_velocity())) } - #[inline] - pub fn has_active_internal_constraints(&self) -> bool { - self.links() - .any(|link| link.joint().num_velocity_constraints() != 0) - } + // #[cfg(feature = "parallel")] + // #[inline] + // pub(crate) fn has_active_internal_constraints(&self) -> bool { + // self.links() + // .any(|link| link.joint().num_velocity_constraints() != 0) + // } + #[cfg(feature = "parallel")] #[inline] - pub fn num_active_internal_constraints_and_jacobian_lines(&self) -> (usize, usize) { + pub(crate) fn num_active_internal_constraints_and_jacobian_lines(&self) -> (usize, usize) { let num_constraints: usize = self .links .iter() @@ -981,7 +990,7 @@ impl Multibody { } #[inline] - pub fn generate_internal_constraints( + pub(crate) fn generate_internal_constraints( &self, params: &IntegrationParameters, j_id: &mut usize, diff --git a/src/dynamics/joint/multibody_joint/multibody_joint.rs b/src/dynamics/joint/multibody_joint/multibody_joint.rs index 5a04070..c4d9adb 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint.rs @@ -13,13 +13,16 @@ use na::{UnitQuaternion, Vector3}; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug)] +/// An joint attached to two bodies based on the reduced coordinates formalism. pub struct MultibodyJoint { + /// The joint’s description. pub data: GenericJoint, pub(crate) coords: SpacialVector, pub(crate) joint_rot: Rotation, } impl MultibodyJoint { + /// Creates a new multibody joint from its description. pub fn new(data: GenericJoint) -> Self { Self { data, @@ -45,9 +48,9 @@ impl MultibodyJoint { self.joint_rot = pos.rotation; } - pub fn local_joint_rot(&self) -> &Rotation { - &self.joint_rot - } + // pub(crate) fn local_joint_rot(&self) -> &Rotation { + // &self.joint_rot + // } fn num_free_lin_dofs(&self) -> usize { let locked_bits = self.data.locked_axes.bits(); diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs index 7e512a8..0365062 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs @@ -97,6 +97,7 @@ impl MultibodyJointSet { } } + /// Iterates through all the multibody joints from this set. pub fn iter(&self) -> impl Iterator { self.rb2mb .iter() @@ -246,7 +247,8 @@ impl MultibodyJointSet { } } - pub fn remove_articulations_attached_to_rigid_body( + /// Removes all the multibody joints attached to a rigid-body. + pub fn remove_joints_attached_to_rigid_body( &mut self, rb_to_remove: RigidBodyHandle, islands: &mut IslandManager, diff --git a/src/dynamics/joint/multibody_joint/multibody_link.rs b/src/dynamics/joint/multibody_joint/multibody_link.rs index 3c35446..7336a6c 100644 --- a/src/dynamics/joint/multibody_joint/multibody_link.rs +++ b/src/dynamics/joint/multibody_joint/multibody_link.rs @@ -18,12 +18,13 @@ pub struct MultibodyLink { /* * Change at each time step. */ + /// The multibody joint of this link. pub joint: MultibodyJoint, // TODO: should this be removed in favor of the rigid-body position? - pub local_to_world: Isometry, - pub local_to_parent: Isometry, - pub shift02: Vector, - pub shift23: Vector, + pub(crate) local_to_world: Isometry, + pub(crate) local_to_parent: Isometry, + pub(crate) shift02: Vector, + pub(crate) shift23: Vector, /// The velocity added by the joint, in world-space. pub(crate) joint_velocity: RigidBodyVelocity, @@ -56,10 +57,12 @@ impl MultibodyLink { } } + /// The multibody joint of this link. pub fn joint(&self) -> &MultibodyJoint { &self.joint } + /// The handle of the rigid-body of this link. pub fn rigid_body_handle(&self) -> RigidBodyHandle { self.rigid_body } @@ -86,11 +89,13 @@ impl MultibodyLink { } } + /// The world-space transform of the rigid-body attached to this link. #[inline] pub fn local_to_world(&self) -> &Isometry { &self.local_to_world } + /// The position of the rigid-body attached to this link relative to its parent. #[inline] pub fn local_to_parent(&self) -> &Isometry { &self.local_to_parent diff --git a/src/dynamics/joint/prismatic_joint.rs b/src/dynamics/joint/prismatic_joint.rs index e3f7527..667999f 100644 --- a/src/dynamics/joint/prismatic_joint.rs +++ b/src/dynamics/joint/prismatic_joint.rs @@ -7,11 +7,15 @@ use super::{JointLimits, JointMotor}; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq)] #[repr(transparent)] +/// A prismatic joint, locks all relative motion between two bodies except for translation along the joint’s principal axis. pub struct PrismaticJoint { data: GenericJoint, } impl PrismaticJoint { + /// Creates a new prismatic joint allowing only relative translations along the specified axis. + /// + /// This axis is expressed in the local-space of both rigid-bodies. pub fn new(axis: UnitVector) -> Self { let data = GenericJointBuilder::new(JointAxesMask::LOCKED_PRISMATIC_AXES) .local_axis1(axis) @@ -20,46 +24,60 @@ impl PrismaticJoint { Self { data } } + /// The underlying generic joint. + pub fn data(&self) -> &GenericJoint { + &self.data + } + + /// 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 } + /// 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) -> Option<&JointMotor> { self.data.motor(JointAxis::X) @@ -103,16 +121,19 @@ impl PrismaticJoint { 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 } + /// The limit distance attached bodies can translate along the joint’s principal axis. #[must_use] pub fn limits(&self) -> Option<&JointLimits> { self.data.limits(JointAxis::X) } + /// Sets the `[min,max]` limit distances attached bodies can translate along the joint’s principal axis. pub fn set_limits(&mut self, limits: [Real; 2]) -> &mut Self { self.data.set_limits(JointAxis::X, limits); self @@ -125,31 +146,42 @@ impl Into for PrismaticJoint { } } +/// Create prismatic joints using the builder pattern. +/// +/// A prismatic joint locks all relative motion except for translations along the joint’s principal axis. +#[derive(Copy, Clone, Debug, PartialEq)] pub struct PrismaticJointBuilder(PrismaticJoint); impl PrismaticJointBuilder { + /// Creates a new builder for prismatic joints. + /// + /// This axis is expressed in the local-space of both rigid-bodies. pub fn new(axis: UnitVector) -> Self { Self(PrismaticJoint::new(axis)) } + /// 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 } + /// 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); @@ -190,18 +222,21 @@ impl PrismaticJointBuilder { self } + /// Sets the maximum force the motor can deliver. #[must_use] pub fn motor_max_force(mut self, max_force: Real) -> Self { self.0.set_motor_max_force(max_force); self } + /// Sets the `[min,max]` limit distances attached bodies can translate along the joint’s principal axis. #[must_use] pub fn limits(mut self, limits: [Real; 2]) -> Self { self.0.set_limits(limits); self } + /// Builds the prismatic joint. #[must_use] pub fn build(self) -> PrismaticJoint { self.0 diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs index a9d74f5..28e3968 100644 --- a/src/dynamics/joint/revolute_joint.rs +++ b/src/dynamics/joint/revolute_joint.rs @@ -8,17 +8,22 @@ use crate::math::UnitVector; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq)] #[repr(transparent)] +/// A revolute joint, locks all relative motion except for rotation along the joint’s principal axis. pub struct RevoluteJoint { data: GenericJoint, } impl RevoluteJoint { + /// Creates a new revolute joint allowing only relative rotations. #[cfg(feature = "dim2")] pub fn new() -> Self { let data = GenericJointBuilder::new(JointAxesMask::LOCKED_REVOLUTE_AXES); Self { data: data.build() } } + /// Creates a new revolute joint allowing only relative rotations along the specified axis. + /// + /// This axis is expressed in the local-space of both rigid-bodies. #[cfg(feature = "dim3")] pub fn new(axis: UnitVector) -> Self { let data = GenericJointBuilder::new(JointAxesMask::LOCKED_REVOLUTE_AXES) @@ -28,30 +33,36 @@ impl RevoluteJoint { Self { data } } + /// The underlying generic joint. pub fn data(&self) -> &GenericJoint { &self.data } + /// 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 } + /// The motor affecting the joint’s rotational degree of freedom. #[must_use] pub fn motor(&self) -> Option<&JointMotor> { self.data.motor(JointAxis::AngX) @@ -95,16 +106,19 @@ impl RevoluteJoint { 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::AngX, max_force); self } + /// The limit angle attached bodies can translate along the joint’s principal axis. #[must_use] pub fn limits(&self) -> Option<&JointLimits> { self.data.limits(JointAxis::AngX) } + /// Sets the `[min,max]` limit angle attached bodies can translate along the joint’s principal axis. pub fn set_limits(&mut self, limits: [Real; 2]) -> &mut Self { self.data.set_limits(JointAxis::AngX, limits); self @@ -117,27 +131,36 @@ impl Into for RevoluteJoint { } } +/// Create revolute joints using the builder pattern. +/// +/// A revolute joint locks all relative motion except for rotations along the joint’s principal axis. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq)] pub struct RevoluteJointBuilder(RevoluteJoint); impl RevoluteJointBuilder { + /// Creates a new revolute joint builder. #[cfg(feature = "dim2")] pub fn new() -> Self { Self(RevoluteJoint::new()) } + /// Creates a new revolute joint builder, allowing only relative rotations along the specified axis. + /// + /// This axis is expressed in the local-space of both rigid-bodies. #[cfg(feature = "dim3")] pub fn new(axis: UnitVector) -> Self { Self(RevoluteJoint::new(axis)) } + /// 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); @@ -178,18 +201,21 @@ impl RevoluteJointBuilder { self } + /// Sets the maximum force the motor can deliver. #[must_use] pub fn motor_max_force(mut self, max_force: Real) -> Self { self.0.set_motor_max_force(max_force); self } + /// Sets the `[min,max]` limit angles attached bodies can rotate along the joint’s principal axis. #[must_use] pub fn limits(mut self, limits: [Real; 2]) -> Self { self.0.set_limits(limits); self } + /// Builds the revolute joint. #[must_use] pub fn build(self) -> RevoluteJoint { self.0 diff --git a/src/dynamics/joint/spherical_joint.rs b/src/dynamics/joint/spherical_joint.rs index 3ff029e..89ab7b1 100644 --- a/src/dynamics/joint/spherical_joint.rs +++ b/src/dynamics/joint/spherical_joint.rs @@ -1,10 +1,13 @@ use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask}; -use crate::dynamics::{JointAxis, MotorModel}; +use crate::dynamics::{JointAxis, JointMotor, MotorModel}; use crate::math::{Point, Real}; +use super::JointLimits; + #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq)] #[repr(transparent)] +/// A spherical joint, locks all relative translations between two bodies. pub struct SphericalJoint { data: GenericJoint, } @@ -16,25 +19,47 @@ impl Default for SphericalJoint { } impl SphericalJoint { + /// Creates a new spherical joint locking all relative translations between two bodies. pub fn new() -> Self { let data = GenericJointBuilder::new(JointAxesMask::LOCKED_SPHERICAL_AXES).build(); Self { data } } + /// The underlying generic joint. pub fn data(&self) -> &GenericJoint { &self.data } + /// 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 } + /// The motor affecting the joint’s rotational degree of freedom along the specified axis. + #[must_use] + pub fn motor(&self, axis: JointAxis) -> Option<&JointMotor> { + self.data.motor(axis) + } + /// Set the spring-like model used by the motor to reach the desired target velocity and position. pub fn set_motor_model(&mut self, axis: JointAxis, model: MotorModel) -> &mut Self { self.data.set_motor_model(axis, model); @@ -79,11 +104,19 @@ impl SphericalJoint { self } + /// Sets the maximum force the motor can deliver along the specified axis. pub fn set_motor_max_force(&mut self, axis: JointAxis, max_force: Real) -> &mut Self { self.data.set_motor_max_force(axis, max_force); self } + /// The limit distance attached bodies can translate along the specified axis. + #[must_use] + pub fn limits(&self, axis: JointAxis) -> Option<&JointLimits> { + self.data.limits(axis) + } + + /// Sets the `[min,max]` limit angles attached bodies can translate along the joint’s principal axis. pub fn set_limits(&mut self, axis: JointAxis, limits: [Real; 2]) -> &mut Self { self.data.set_limits(axis, limits); self @@ -96,6 +129,7 @@ impl Into for SphericalJoint { } } +/// Create spherical joints using the builder pattern. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq)] pub struct SphericalJointBuilder(SphericalJoint); @@ -107,16 +141,19 @@ impl Default for SphericalJointBuilder { } impl SphericalJointBuilder { + /// Creates a new builder for spherical joints. pub fn new() -> Self { Self(SphericalJoint::new()) } + /// 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); @@ -166,18 +203,21 @@ impl SphericalJointBuilder { self } + /// Sets the maximum force the motor can deliver along the specified axis. #[must_use] pub fn motor_max_force(mut self, axis: JointAxis, max_force: Real) -> Self { self.0.set_motor_max_force(axis, max_force); self } + /// Sets the `[min,max]` limit distances attached bodies can rotate along the specified axis. #[must_use] pub fn limits(mut self, axis: JointAxis, limits: [Real; 2]) -> Self { self.0.set_limits(axis, limits); self } + /// Builds the spherical joint. #[must_use] pub fn build(self) -> SphericalJoint { self.0 diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index d37994c..1bfdf48 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -133,6 +133,7 @@ impl RigidBody { self.rb_dominance.effective_group(&self.rb_type) } + /// Sets the axes along which this rigid-body cannot translate or rotate. #[inline] pub fn set_locked_axes(&mut self, locked_axes: LockedAxes, wake_up: bool) { if locked_axes != self.rb_mprops.flags { @@ -995,6 +996,7 @@ impl RigidBodyBuilder { self } + /// Sets the axes along which this rigid-body cannot translate or rotate. pub fn locked_axes(mut self, locked_axes: LockedAxes) -> Self { self.mprops_flags = locked_axes; self diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs index 4bfaa28..52072ea 100644 --- a/src/dynamics/rigid_body_components.rs +++ b/src/dynamics/rigid_body_components.rs @@ -639,9 +639,9 @@ pub struct RigidBodyForces { /// Gravity is multiplied by this scaling factor before it's /// applied to this rigid-body. pub gravity_scale: Real, - // Forces applied by the user. + /// Forces applied by the user. pub user_force: Vector, - // Torque applied by the user. + /// Torque applied by the user. pub user_torque: AngVector, } diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index 8f5d8ef..a75d96d 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/src/dynamics/rigid_body_set.rs @@ -168,7 +168,7 @@ impl RigidBodySet { * Remove impulse_joints attached to this rigid-body. */ impulse_joints.remove_joints_attached_to_rigid_body(handle, islands, self); - multibody_joints.remove_articulations_attached_to_rigid_body(handle, islands, self); + multibody_joints.remove_joints_attached_to_rigid_body(handle, islands, self); Some(rb) } @@ -260,6 +260,13 @@ impl RigidBodySet { }) } + /// Update colliders positions after rigid-bodies moved. + /// + /// When a rigid-body moves, the positions of the colliders attached to it need to be updated. + /// This update is generally automatically done at the beggining and the end of each simulation + /// step with `PhysicsPipeline::step`. If the positions need to be updated without running a + /// simulation step (for example when using the `QueryPipeline` alone), this method can be called + /// manually. pub fn propagate_modified_body_positions_to_colliders(&self, colliders: &mut ColliderSet) { for body in self.modified_bodies.iter().filter_map(|h| self.get(*h)) { if body.changes.contains(RigidBodyChanges::POSITION) { diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs index 7f49ec3..451f930 100644 --- a/src/dynamics/solver/interaction_groups.rs +++ b/src/dynamics/solver/interaction_groups.rs @@ -1,14 +1,17 @@ use crate::data::ComponentSet; -#[cfg(feature = "parallel")] -use crate::dynamics::RigidBodyHandle; -use crate::dynamics::{IslandManager, JointGraphEdge, JointIndex, MultibodyJointSet, RigidBodyIds}; +use crate::dynamics::{IslandManager, JointGraphEdge, JointIndex, RigidBodyIds}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; + #[cfg(feature = "simd-is-enabled")] use { crate::data::BundleSet, crate::math::{SIMD_LAST_INDEX, SIMD_WIDTH}, vec_map::VecMap, }; + +#[cfg(feature = "parallel")] +use crate::dynamics::{MultibodyJointSet, RigidBodyHandle}; + #[cfg(feature = "parallel")] pub(crate) trait PairInteraction { fn body_pair(&self) -> (Option, Option); @@ -195,16 +198,16 @@ impl InteractionGroups { } } - #[cfg(not(feature = "parallel"))] - pub fn clear(&mut self) { - #[cfg(feature = "simd-is-enabled")] - { - self.buckets.clear(); - self.body_masks.clear(); - self.grouped_interactions.clear(); - } - self.nongrouped_interactions.clear(); - } + // #[cfg(not(feature = "parallel"))] + // pub fn clear(&mut self) { + // #[cfg(feature = "simd-is-enabled")] + // { + // self.buckets.clear(); + // self.body_masks.clear(); + // self.grouped_interactions.clear(); + // } + // self.nongrouped_interactions.clear(); + // } // TODO: there is a lot of duplicated code with group_manifolds here. // But we don't refactor just now because we may end up with distinct diff --git a/src/dynamics/solver/joint_constraint/joint_constraint.rs b/src/dynamics/solver/joint_constraint/joint_constraint.rs index f571bc6..e1150d5 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint.rs @@ -7,15 +7,19 @@ use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{ }; use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ - ImpulseJoint, IntegrationParameters, JointAxesMask, JointGraphEdge, JointIndex, RigidBodyIds, + ImpulseJoint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity, }; -#[cfg(feature = "simd-is-enabled")] -use crate::math::{Isometry, SimdReal, SIMD_WIDTH}; use crate::math::{Real, SPATIAL_DIM}; use crate::prelude::MultibodyJointSet; use na::DVector; +#[cfg(feature = "simd-is-enabled")] +use crate::math::{Isometry, SimdReal, SIMD_WIDTH}; + +#[cfg(feature = "parallel")] +use crate::dynamics::JointAxesMask; + pub enum AnyJointVelocityConstraint { JointConstraint(JointVelocityConstraint), JointGroundConstraint(JointVelocityGroundConstraint), diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs index 1c65eb4..439030e 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs @@ -5,9 +5,12 @@ use crate::dynamics::solver::joint_constraint::SolverBody; use crate::dynamics::solver::MotorParameters; use crate::dynamics::{IntegrationParameters, JointIndex, JointLimits}; use crate::math::{AngVector, Isometry, Matrix, Point, Real, Rotation, Vector, ANG_DIM, DIM}; -use crate::utils::{IndexMut2, WBasis, WCross, WCrossMatrix, WDot, WQuat, WReal}; +use crate::utils::{IndexMut2, WCrossMatrix, WDot, WQuat, WReal}; use na::SMatrix; +#[cfg(feature = "dim3")] +use crate::utils::WBasis; + #[derive(Debug, Copy, Clone)] pub struct JointVelocityConstraintBuilder { pub basis: Matrix, @@ -660,79 +663,76 @@ impl JointVelocityConstraintBuilder { } } - pub fn motor_linear_coupled_ground( - &self, - _joint_id: [JointIndex; LANES], - _body1: &SolverBody, - _body2: &SolverBody, - _motor_coupled_axes: u8, - _motors: &[MotorParameters], - _limited_coupled_axes: u8, - _limits: &[JointLimits], - _writeback_id: WritebackId, - ) -> JointVelocityGroundConstraint { - todo!() - /* - let zero = N::zero(); - let mut lin_jac = Vector::zeros(); - let mut ang_jac1: AngVector = na::zero(); - let mut ang_jac2: AngVector = na::zero(); - let mut limit = N::zero(); - - for i in 0..DIM { - if limited_coupled_axes & (1 << i) != 0 { - let coeff = self.basis.column(i).dot(&self.lin_err); - lin_jac += self.basis.column(i) * coeff; - #[cfg(feature = "dim2")] - { - ang_jac1 += self.cmat1_basis[i] * coeff; - ang_jac2 += self.cmat2_basis[i] * coeff; - } - #[cfg(feature = "dim3")] - { - ang_jac1 += self.cmat1_basis.column(i) * coeff; - ang_jac2 += self.cmat2_basis.column(i) * coeff; - } - limit += limits[i].max * limits[i].max; - } - } - - limit = limit.simd_sqrt(); - let dist = lin_jac.norm(); - let inv_dist = crate::utils::simd_inv(dist); - lin_jac *= inv_dist; - ang_jac1 *= inv_dist; - ang_jac2 *= inv_dist; - - let dvel = lin_jac.dot(&(body2.linvel - body1.linvel)) - + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel)); - let rhs_wo_bias = dvel + (dist - limit).simd_min(zero) * N::splat(params.inv_dt()); - - ang_jac2 = body2.sqrt_ii * ang_jac2; - - let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); - let cfm_coeff = N::splat(params.joint_cfm_coeff()); - let rhs_bias = (dist - limit).simd_max(zero) * erp_inv_dt; - let rhs = rhs_wo_bias + rhs_bias; - let impulse_bounds = [N::zero(), N::splat(Real::INFINITY)]; - - JointVelocityGroundConstraint { - joint_id, - mj_lambda2: body2.mj_lambda, - im2: body2.im, - impulse: N::zero(), - impulse_bounds, - lin_jac, - ang_jac2, - inv_lhs: N::zero(), // Will be set during ortogonalization. - cfm_coeff, - cfm_gain: N::zero(), - rhs, - rhs_wo_bias, - writeback_id, - } - */ - } + // pub fn motor_linear_coupled_ground( + // &self, + // _joint_id: [JointIndex; LANES], + // _body1: &SolverBody, + // _body2: &SolverBody, + // _motor_coupled_axes: u8, + // _motors: &[MotorParameters], + // _limited_coupled_axes: u8, + // _limits: &[JointLimits], + // _writeback_id: WritebackId, + // ) -> JointVelocityGroundConstraint { + // let zero = N::zero(); + // let mut lin_jac = Vector::zeros(); + // let mut ang_jac1: AngVector = na::zero(); + // let mut ang_jac2: AngVector = na::zero(); + // let mut limit = N::zero(); + + // for i in 0..DIM { + // if limited_coupled_axes & (1 << i) != 0 { + // let coeff = self.basis.column(i).dot(&self.lin_err); + // lin_jac += self.basis.column(i) * coeff; + // #[cfg(feature = "dim2")] + // { + // ang_jac1 += self.cmat1_basis[i] * coeff; + // ang_jac2 += self.cmat2_basis[i] * coeff; + // } + // #[cfg(feature = "dim3")] + // { + // ang_jac1 += self.cmat1_basis.column(i) * coeff; + // ang_jac2 += self.cmat2_basis.column(i) * coeff; + // } + // limit += limits[i].max * limits[i].max; + // } + // } + + // limit = limit.simd_sqrt(); + // let dist = lin_jac.norm(); + // let inv_dist = crate::utils::simd_inv(dist); + // lin_jac *= inv_dist; + // ang_jac1 *= inv_dist; + // ang_jac2 *= inv_dist; + + // let dvel = lin_jac.dot(&(body2.linvel - body1.linvel)) + // + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel)); + // let rhs_wo_bias = dvel + (dist - limit).simd_min(zero) * N::splat(params.inv_dt()); + + // ang_jac2 = body2.sqrt_ii * ang_jac2; + + // let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); + // let cfm_coeff = N::splat(params.joint_cfm_coeff()); + // let rhs_bias = (dist - limit).simd_max(zero) * erp_inv_dt; + // let rhs = rhs_wo_bias + rhs_bias; + // let impulse_bounds = [N::zero(), N::splat(Real::INFINITY)]; + + // JointVelocityGroundConstraint { + // joint_id, + // mj_lambda2: body2.mj_lambda, + // im2: body2.im, + // impulse: N::zero(), + // impulse_bounds, + // lin_jac, + // ang_jac2, + // inv_lhs: N::zero(), // Will be set during ortogonalization. + // cfm_coeff, + // cfm_gain: N::zero(), + // rhs, + // rhs_wo_bias, + // writeback_id, + // } + // } pub fn lock_linear_ground( &self, diff --git a/src/dynamics/solver/solver_constraints.rs b/src/dynamics/solver/solver_constraints.rs index c03789a..081ea9c 100644 --- a/src/dynamics/solver/solver_constraints.rs +++ b/src/dynamics/solver/solver_constraints.rs @@ -43,15 +43,15 @@ impl SolverConstraints { } } - pub fn clear(&mut self) { - self.not_ground_interactions.clear(); - self.ground_interactions.clear(); - self.generic_not_ground_interactions.clear(); - self.generic_ground_interactions.clear(); - self.interaction_groups.clear(); - self.ground_interaction_groups.clear(); - self.velocity_constraints.clear(); - } + // pub fn clear(&mut self) { + // self.not_ground_interactions.clear(); + // self.ground_interactions.clear(); + // self.generic_not_ground_interactions.clear(); + // self.generic_ground_interactions.clear(); + // self.interaction_groups.clear(); + // self.ground_interaction_groups.clear(); + // self.velocity_constraints.clear(); + // } } impl SolverConstraints { -- cgit