bevy_rapier3d/dynamics/
multibody.rs

1use crate::math::{Real, Rot};
2use crate::reflect::JointAxisWrapper;
3use bevy::prelude::*;
4use rapier::dynamics::{JointAxesMask, JointAxis, MultibodyJoint as RapierMultibodyJoint};
5use rapier::math::SPATIAL_DIM;
6
7pub use rapier::dynamics::InverseKinematicsOption;
8
9use super::JOINT_DOFS;
10
11#[cfg(doc)]
12use super::MultibodyJoint;
13#[cfg(doc)]
14use crate::plugin::context::RapierContextJoints;
15
16/// Iterates through the free degrees of freedom of a joint with the given locked axes, as
17/// `(dof, axis)` pairs, where `dof` is the index of the DoF in the joint’s slice of the
18/// multibody’s generalized coordinates, and `axis` its [`JointAxis`] index.
19pub(crate) fn free_joint_dofs(locked_axes: JointAxesMask) -> impl Iterator<Item = (usize, usize)> {
20    let locked = locked_axes.bits();
21    (0..SPATIAL_DIM)
22        .filter(move |axis| locked & (1 << axis) == 0)
23        .enumerate()
24}
25
26/// Marker component making the [`MultibodyJoint`] of its entity kinematic.
27///
28/// The velocities of a kinematic multibody joint are never modified by the physics engine: its
29/// degrees of freedom only move according to the generalized velocities set by the user (see
30/// [`RapierContextJoints::multibody_joint_velocity_mut`]), ignoring gravity, contacts and the
31/// other joints of the multibody. This can be added or removed at any time.
32#[derive(Copy, Clone, Default, Debug, PartialEq, Eq, Component, Reflect)]
33#[reflect(Component, Default, PartialEq)]
34pub struct KinematicMultibodyJoint;
35
36/// The per-DoF viscous damping of a [`MultibodyJoint`], indexed by [`JointAxis`].
37///
38/// The damping applies a generalized force opposed to the joint velocity along each free
39/// degree of freedom. Values along locked axes are ignored. Removing this component restores the
40/// default damping (see [`MultibodyJointDamping::default`]).
41#[derive(Copy, Clone, Debug, PartialEq, Component, Reflect)]
42#[reflect(Component, Default, PartialEq)]
43pub struct MultibodyJointDamping(pub [Real; JOINT_DOFS]);
44
45impl Default for MultibodyJointDamping {
46    /// The damping Rapier assigns to new multibody joints: `0.1` on angular axes, zero on
47    /// linear axes.
48    fn default() -> Self {
49        let lin_dofs = JOINT_DOFS - rapier::math::ANG_DIM;
50        Self(std::array::from_fn(
51            |i| if i < lin_dofs { 0.0 } else { 0.1 },
52        ))
53    }
54}
55
56impl MultibodyJointDamping {
57    /// The same damping along every axis.
58    pub fn splat(damping: Real) -> Self {
59        Self([damping; JOINT_DOFS])
60    }
61
62    /// Sets the damping along the given axis.
63    pub fn with(mut self, axis: JointAxis, damping: Real) -> Self {
64        self.0[axis as usize] = damping;
65        self
66    }
67}
68
69/// The per-DoF dry friction of a [`MultibodyJoint`], indexed by [`JointAxis`].
70///
71/// Each value is the largest generalized force (a force for linear axes, a torque for angular
72/// axes) friction may apply to stop the motion of the corresponding degree of freedom. Values
73/// along locked axes are ignored. Removing this component removes the friction.
74#[derive(Copy, Clone, Default, Debug, PartialEq, Component, Reflect)]
75#[reflect(Component, Default, PartialEq)]
76pub struct MultibodyJointFriction(pub [Real; JOINT_DOFS]);
77
78impl MultibodyJointFriction {
79    /// The same friction along every axis.
80    pub fn splat(friction: Real) -> Self {
81        Self([friction; JOINT_DOFS])
82    }
83
84    /// Sets the friction along the given axis.
85    pub fn with(mut self, axis: JointAxis, friction: Real) -> Self {
86        self.0[axis as usize] = friction;
87        self
88    }
89}
90
91/// The per-DoF armature (reflected rotor inertia) of a [`MultibodyJoint`], indexed by
92/// [`JointAxis`].
93///
94/// This inertia is added to the multibody’s mass matrix along each free degree of freedom, which
95/// typically models the inertia of an actuator and improves stability. Values along locked axes
96/// are ignored. Removing this component removes the armature.
97#[derive(Copy, Clone, Default, Debug, PartialEq, Component, Reflect)]
98#[reflect(Component, Default, PartialEq)]
99pub struct MultibodyJointArmature(pub [Real; JOINT_DOFS]);
100
101impl MultibodyJointArmature {
102    /// The same armature along every axis.
103    pub fn splat(armature: Real) -> Self {
104        Self([armature; JOINT_DOFS])
105    }
106
107    /// Sets the armature along the given axis.
108    pub fn with(mut self, axis: JointAxis, armature: Real) -> Self {
109        self.0[axis as usize] = armature;
110        self
111    }
112}
113
114/// Passive springs acting on the degrees of freedom of a [`MultibodyJoint`], indexed by
115/// [`JointAxis`].
116///
117/// Along each free axis, the spring applies the generalized force
118/// `-stiffness * (coord - rest)`, integrated implicitly (so stiff springs remain stable), where
119/// `coord` is the joint coordinate reported by [`MultibodyJointState::coords`]. Axes with a zero
120/// stiffness have no spring. Removing this component removes all the springs.
121#[derive(Copy, Clone, Default, Debug, PartialEq, Component, Reflect)]
122#[reflect(Component, Default, PartialEq)]
123pub struct MultibodyJointSprings {
124    /// The spring stiffness along each axis.
125    pub stiffness: [Real; JOINT_DOFS],
126    /// The rest coordinate of the spring along each axis.
127    pub rest: [Real; JOINT_DOFS],
128}
129
130impl MultibodyJointSprings {
131    /// Sets the spring along the given axis.
132    pub fn with(mut self, axis: JointAxis, stiffness: Real, rest: Real) -> Self {
133        self.stiffness[axis as usize] = stiffness;
134        self.rest[axis as usize] = rest;
135        self
136    }
137}
138
139/// A linear coupling between a degree of freedom of this [`MultibodyJoint`] and a degree of
140/// freedom of another joint of the same multibody.
141///
142/// The coupling enforces `coord(axis) = coeff * source_coord(source_axis) + offset`, similar to
143/// a "mimic" joint. See [`MultibodyJointCouplings`].
144#[derive(Copy, Clone, Debug, PartialEq, Reflect)]
145#[reflect(PartialEq)]
146pub struct MultibodyJointCoupling {
147    /// The coupled axis of the joint containing this coupling.
148    #[reflect(remote = JointAxisWrapper)]
149    pub axis: JointAxis,
150    /// The entity with the [`MultibodyJoint`] (or the root rigid-body) driving this coupling.
151    pub source: Entity,
152    /// The axis of `source` driving this coupling.
153    #[reflect(remote = JointAxisWrapper)]
154    pub source_axis: JointAxis,
155    /// The linear coupling coefficient.
156    pub coeff: Real,
157    /// The constant offset of the coupling.
158    pub offset: Real,
159}
160
161impl MultibodyJointCoupling {
162    /// Couples `axis` of this joint to `source_axis` of `source` with
163    /// `coord(axis) = coeff * source_coord(source_axis) + offset`.
164    pub fn new(
165        axis: JointAxis,
166        source: Entity,
167        source_axis: JointAxis,
168        coeff: Real,
169        offset: Real,
170    ) -> Self {
171        Self {
172            axis,
173            source,
174            source_axis,
175            coeff,
176            offset,
177        }
178    }
179}
180
181/// The couplings between the degrees of freedom of this [`MultibodyJoint`] and other joints of
182/// the same multibody.
183///
184/// A coupling is only active while both joints belong to the same multibody and both coupled
185/// axes are free; it is ignored otherwise. Removing a coupling (or this component) removes it
186/// from the multibody without affecting the joint state.
187#[derive(Clone, Default, Debug, PartialEq, Component, Reflect)]
188#[reflect(Component, Default, PartialEq)]
189pub struct MultibodyJointCouplings(pub Vec<MultibodyJointCoupling>);
190
191/// Marker component disabling the contacts between the links of the multibody containing the
192/// rigid-body of its entity.
193///
194/// This can be added to any rigid-body of the multibody (typically its root), and overrides the
195/// per-joint [`GenericJoint::contacts_enabled`](crate::dynamics::GenericJoint::contacts_enabled)
196/// flags. Removing it enables self-contacts again (unless another link of the multibody still has
197/// this component).
198#[derive(Copy, Clone, Default, Debug, PartialEq, Eq, Component, Reflect)]
199#[reflect(Component, Default, PartialEq)]
200pub struct MultibodySelfContactsDisabled;
201
202/// The state of a [`MultibodyJoint`], written back after each simulation step.
203///
204/// This component is automatically added alongside [`MultibodyJoint`]. Modifying it has no
205/// effect: use the helpers of [`RapierContextJoints`] to control the joint coordinates and
206/// velocities.
207#[derive(Copy, Clone, Debug, PartialEq, Component, Reflect)]
208#[reflect(Component, Default, PartialEq)]
209pub struct MultibodyJointState {
210    /// The generalized coordinates of the joint, indexed by [`JointAxis`], zero along locked
211    /// axes.
212    ///
213    /// These are translations along linear axes and angles along angular axes. With more than
214    /// one free angular axis (3D only), the angular coordinates are integrated angular
215    /// velocities: use [`Self::rotation`] instead.
216    pub coords: [Real; JOINT_DOFS],
217    /// The generalized velocities of the joint, indexed by [`JointAxis`], zero along locked axes.
218    pub velocities: [Real; JOINT_DOFS],
219    /// The rotation of the joint’s second frame relative to its first frame.
220    pub rotation: Rot,
221}
222
223impl Default for MultibodyJointState {
224    fn default() -> Self {
225        Self {
226            coords: [0.0; JOINT_DOFS],
227            velocities: [0.0; JOINT_DOFS],
228            #[cfg(feature = "dim2")]
229            rotation: 0.0,
230            #[cfg(feature = "dim3")]
231            rotation: Rot::IDENTITY,
232        }
233    }
234}
235
236impl MultibodyJointState {
237    /// Reads the state of a Rapier multibody joint, given its generalized velocities (the
238    /// joint’s slice of the multibody’s generalized velocities).
239    pub fn from_rapier(joint: &RapierMultibodyJoint, velocities: &[Real]) -> Self {
240        let coords = joint.coords();
241        let mut result = Self {
242            coords: std::array::from_fn(|i| coords[i]),
243            #[cfg(feature = "dim2")]
244            rotation: joint.joint_rot().angle(),
245            #[cfg(feature = "dim3")]
246            rotation: joint.joint_rot(),
247            ..Default::default()
248        };
249        for (dof, axis) in free_joint_dofs(joint.data.locked_axes) {
250            if let Some(vel) = velocities.get(dof) {
251                result.velocities[axis] = *vel;
252            }
253        }
254        result
255    }
256}
bevy_rapier3d/dynamics/multibody.rs

bevy_rapier3d/dynamics/
multibody.rs
