aboutsummaryrefslogtreecommitdiff
path: root/src/dynamics
diff options
context:
space:
mode:
authorSébastien Crozet <developer@crozet.re>2022-03-19 16:10:49 +0100
committerSébastien Crozet <sebastien@crozet.re>2022-03-20 21:49:16 +0100
commitdb6a8c526d939a125485c89cfb6e540422fe6b4b (patch)
tree32738172c6bd27e07ed9a4b8f90f5fbbfc07fd5e /src/dynamics
parente2e6fc787112ab35a3d4858aa2cf83fcf41c16a2 (diff)
downloadrapier-db6a8c526d939a125485c89cfb6e540422fe6b4b.tar.gz
rapier-db6a8c526d939a125485c89cfb6e540422fe6b4b.tar.bz2
rapier-db6a8c526d939a125485c89cfb6e540422fe6b4b.zip
Fix warnings and add comments.
Diffstat (limited to 'src/dynamics')
-rw-r--r--src/dynamics/integration_parameters.rs12
-rw-r--r--src/dynamics/joint/fixed_joint.rs17
-rw-r--r--src/dynamics/joint/generic_joint.rs98
-rw-r--r--src/dynamics/joint/impulse_joint/impulse_joint.rs5
-rw-r--r--src/dynamics/joint/multibody_joint/multibody.rs41
-rw-r--r--src/dynamics/joint/multibody_joint/multibody_joint.rs9
-rw-r--r--src/dynamics/joint/multibody_joint/multibody_joint_set.rs4
-rw-r--r--src/dynamics/joint/multibody_joint/multibody_link.rs13
-rw-r--r--src/dynamics/joint/prismatic_joint.rs35
-rw-r--r--src/dynamics/joint/revolute_joint.rs26
-rw-r--r--src/dynamics/joint/spherical_joint.rs42
-rw-r--r--src/dynamics/rigid_body.rs2
-rw-r--r--src/dynamics/rigid_body_components.rs4
-rw-r--r--src/dynamics/rigid_body_set.rs9
-rw-r--r--src/dynamics/solver/interaction_groups.rs29
-rw-r--r--src/dynamics/solver/joint_constraint/joint_constraint.rs10
-rw-r--r--src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs148
-rw-r--r--src/dynamics/solver/solver_constraints.rs18
18 files changed, 388 insertions, 134 deletions
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<Real> {
&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<Real>) -> &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<Real> {
&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<Real>) -> &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<Real> {
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<Real>) -> &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<Real> {
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<Real>) -> &mut Self {
self.data.set_local_anchor2(anchor2);
self
@@ -68,39 +78,46 @@ impl Into<GenericJoint> 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<Real>) -> 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<Real>) -> 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<Real>) -> 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<Real>) -> 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<JointAxis> 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<N> {
+ /// 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<N: WReal> Default for JointLimits<N> {
}
}
+/// 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<Real>,
+ /// The joint’s frame, expressed in the second rigid-body’s local-space.
pub local_frame2: Isometry<Real>,
+ /// 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<Real>; SPATIAL_DIM],
+ /// The motors, along each degrees of freedoms of this joint.
+ ///
+ /// Note that the mostor must also be explicitly enabled by the `motors` bitmask.
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<Real>) -> &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<Real>) -> &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<Real> {
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<Real>) -> &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<Real> {
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<Real>) -> &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<Real> {
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<Real>) -> &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<Real> {
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<Real>) -> &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<Real>> {
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<MotorModel> {
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<Real>) -> 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<Real>) -> 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<Real>) -> 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<Real>) -> 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<Real>) -> 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<Real>) -> 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<Real>,
// 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<Multibody> {
+ pub(crate) fn remove_link(self, to_remove: usize, joint_only: bool) -> Vec<Multibody> {
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<Real, Dynamic, Dynamic> {
&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<usize>, // 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<Bodies>(&mut self, bodies: &Bodies)
+ pub(crate) fn update_acceleration<Bodies>(&mut self, bodies: &Bodies)
where
Bodies: ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyForces>
@@ -451,7 +452,7 @@ impl Multibody {
}
/// Computes the constant terms of the dynamics.
- pub fn update_dynamics<Bodies>(&mut self, dt: Real, bodies: &mut Bodies)
+ pub(crate) fn update_dynamics<Bodies>(&mut self, dt: Real, bodies: &mut Bodies)
where
Bodies: ComponentSetMut<RigidBodyVelocity> + ComponentSet<RigidBodyMassProps>,
{
@@ -756,36 +757,40 @@ impl Multibody {
)
}
+ /// The generalized accelerations of this multibodies.
#[inline]
pub fn generalized_acceleration(&self) -> DVectorSlice<Real> {
self.accelerations.rows(0, self.ndofs)
}
+ /// The generalized velocities of this multibodies.
#[inline]
pub fn generalized_velocity(&self) -> DVectorSlice<Real> {
self.velocities.rows(0, self.ndofs)
}
+ /// The mutable generalized velocities of this multibodies.
#[inline]
pub fn generalized_velocity_mut(&mut self) -> DVectorSliceMut<Real> {
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<Bodies>(&mut self, bodies: &mut Bodies)
+ pub(crate) fn update_root_type<Bodies>(&mut self, bodies: &mut Bodies)
where
Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyPosition>,
{
@@ -851,6 +856,7 @@ impl Multibody {
}
}
+ /// Apply forward-kinematics to this multibody and its related rigid-bodies.
pub fn forward_kinematics<Bodies>(&mut self, bodies: &mut Bodies, update_mass_props: bool)
where
Bodies: ComponentSet<RigidBodyType>
@@ -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<Real>,
@@ -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<Real>,
pub(crate) joint_rot: Rotation<Real>,
}
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<Real> {
- &self.joint_rot
- }
+ // pub(crate) fn local_joint_rot(&self) -> &Rotation<Real> {
+ // &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<Item = (MultibodyJointHandle, &Multibody, &MultibodyLink)> {
self.rb2mb
.iter()
@@ -246,7 +247,8 @@ impl MultibodyJointSet {
}
}
- pub fn remove_articulations_attached_to_rigid_body<Bodies>(
+ /// Removes all the multibody joints attached to a rigid-body.