avian3d/dynamics/joints/
fixed.rs

1use crate::{
2    dynamics::joints::{EntityConstraint, JointSystems},
3    prelude::*,
4};
5use bevy::{
6    ecs::{
7        entity::{EntityMapper, MapEntities},
8        reflect::ReflectMapEntities,
9    },
10    prelude::*,
11};
12
13/// A fixed [joint](dynamics::joints) prevents any relative movement between two bodies,
14/// effectively locking them together.
15///
16/// This can be useful for cases where attaching multiple colliders to a single body is not enough,
17/// for example when you need to read forces applied by the joint to determine when the connection should break.
18///
19/// <div class="warning">
20///
21/// Due to the nature of iterative solvers, fixed joints can still have some error and allow small relative movement,
22/// even when using infinite stiffness. If you do not need features such as stiffness or reading back forces,
23/// consider attaching multiple colliders to a single rigid body instead. This is more efficient and stable than using joints.
24///
25/// </div>
26///
27/// Each fixed joint is defined by a [`JointFrame`] on each body. The joint aims to keep the anchor point
28/// and basis of each frame aligned, locking them together.
29///
30#[doc = include_str!("./images/point_constraint.svg")]
31#[derive(Component, Clone, Debug, PartialEq, Reflect)]
32#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))]
33#[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))]
34#[reflect(Component, Debug, MapEntities, PartialEq)]
35pub struct FixedJoint {
36    /// The first body constrained by the joint.
37    pub body1: Entity,
38    /// The second body constrained by the joint.
39    pub body2: Entity,
40    /// The reference frame of the first body, defining the joint anchor and basis
41    /// relative to the body transform.
42    pub frame1: JointFrame,
43    /// The reference frame of the second body, defining the joint anchor and basis
44    /// relative to the body transform.
45    pub frame2: JointFrame,
46    /// The compliance of the point-to-point constraint (inverse of stiffness, m / N).
47    pub point_compliance: Scalar,
48    /// The compliance of the angular constraint (inverse of stiffness, N * m / rad).
49    pub angle_compliance: Scalar,
50}
51
52impl EntityConstraint<2> for FixedJoint {
53    fn entities(&self) -> [Entity; 2] {
54        [self.body1, self.body2]
55    }
56}
57
58impl FixedJoint {
59    /// Creates a new [`FixedJoint`] between two entities.
60    #[inline]
61    pub const fn new(body1: Entity, body2: Entity) -> Self {
62        Self {
63            body1,
64            body2,
65            frame1: JointFrame::IDENTITY,
66            frame2: JointFrame::IDENTITY,
67            point_compliance: 0.0,
68            angle_compliance: 0.0,
69        }
70    }
71
72    /// Sets the local [`JointFrame`] of the first body, configuring both the [`JointAnchor`] and [`JointBasis`].
73    #[inline]
74    pub fn with_local_frame1(mut self, frame: impl Into<Isometry>) -> Self {
75        self.frame1 = JointFrame::local(frame);
76        self
77    }
78
79    /// Sets the local [`JointFrame`] of the second body, configuring both the [`JointAnchor`] and [`JointBasis`].
80    #[inline]
81    pub fn with_local_frame2(mut self, frame: impl Into<Isometry>) -> Self {
82        self.frame2 = JointFrame::local(frame);
83        self
84    }
85
86    /// Sets the global anchor point on both bodies.
87    ///
88    /// This configures the [`JointAnchor`] of each [`JointFrame`].
89    #[inline]
90    pub const fn with_anchor(mut self, anchor: Vector) -> Self {
91        self.frame1.anchor = JointAnchor::FromGlobal(anchor);
92        self.frame2.anchor = JointAnchor::FromGlobal(anchor);
93        self
94    }
95
96    /// Sets the local anchor point on the first body.
97    ///
98    /// This configures the [`JointAnchor`] of the first [`JointFrame`].
99    #[inline]
100    pub const fn with_local_anchor1(mut self, anchor: Vector) -> Self {
101        self.frame1.anchor = JointAnchor::Local(anchor);
102        self
103    }
104
105    /// Sets the local anchor point on the second body.
106    ///
107    /// This configures the [`JointAnchor`] of the second [`JointFrame`].
108    #[inline]
109    pub const fn with_local_anchor2(mut self, anchor: Vector) -> Self {
110        self.frame2.anchor = JointAnchor::Local(anchor);
111        self
112    }
113
114    /// Sets the global basis for both bodies.
115    ///
116    /// This configures the [`JointBasis`] of each [`JointFrame`].
117    #[inline]
118    pub fn with_basis(mut self, basis: impl Into<Rot>) -> Self {
119        let basis = basis.into();
120        self.frame1.basis = JointBasis::FromGlobal(basis);
121        self.frame2.basis = JointBasis::FromGlobal(basis);
122        self
123    }
124
125    /// Sets the local basis for the first body.
126    ///
127    /// This configures the [`JointBasis`] of the first [`JointFrame`].
128    #[inline]
129    pub fn with_local_basis1(mut self, basis: impl Into<Rot>) -> Self {
130        self.frame1.basis = JointBasis::Local(basis.into());
131        self
132    }
133
134    /// Sets the local basis for the second body.
135    ///
136    /// This configures the [`JointBasis`] of the second [`JointFrame`].
137    #[inline]
138    pub fn with_local_basis2(mut self, basis: impl Into<Rot>) -> Self {
139        self.frame2.basis = JointBasis::Local(basis.into());
140        self
141    }
142
143    /// Returns the local [`JointFrame`] of the first body.
144    ///
145    /// If the [`JointAnchor`] is set to [`FromGlobal`](JointAnchor::FromGlobal),
146    /// and the local anchor has not yet been computed, or the [`JointBasis`] is set to
147    /// [`FromGlobal`](JointBasis::FromGlobal), and the local basis has not yet
148    /// been computed, this will return `None`.
149    #[inline]
150    pub fn local_frame1(&self) -> Option<Isometry> {
151        self.frame1.get_local_isometry()
152    }
153
154    /// Returns the local [`JointFrame`] of the second body.
155    ///
156    /// If the [`JointAnchor`] is set to [`FromGlobal`](JointAnchor::FromGlobal),
157    /// and the local anchor has not yet been computed, or the [`JointBasis`] is set to
158    /// [`FromGlobal`](JointBasis::FromGlobal), and the local basis has not yet
159    /// been computed, this will return `None`.
160    #[inline]
161    pub fn local_frame2(&self) -> Option<Isometry> {
162        self.frame2.get_local_isometry()
163    }
164
165    /// Returns the local anchor point on the first body.
166    ///
167    /// If the [`JointAnchor`] is set to [`FromGlobal`](JointAnchor::FromGlobal),
168    /// and the local anchor has not yet been computed, this will return `None`.
169    #[inline]
170    pub const fn local_anchor1(&self) -> Option<Vector> {
171        match self.frame1.anchor {
172            JointAnchor::Local(anchor) => Some(anchor),
173            _ => None,
174        }
175    }
176
177    /// Returns the local anchor point on the second body.
178    ///
179    /// If the [`JointAnchor`] is set to [`FromGlobal`](JointAnchor::FromGlobal),
180    /// and the local anchor has not yet been computed, this will return `None`.
181    #[inline]
182    pub const fn local_anchor2(&self) -> Option<Vector> {
183        match self.frame2.anchor {
184            JointAnchor::Local(anchor) => Some(anchor),
185            _ => None,
186        }
187    }
188
189    /// Returns the local basis of the first body.
190    ///
191    /// If the [`JointBasis`] is set to [`FromGlobal`](JointBasis::FromGlobal),
192    /// and the local basis has not yet been computed, this will return `None`.
193    #[inline]
194    pub fn local_basis1(&self) -> Option<Rot> {
195        match self.frame1.basis {
196            JointBasis::Local(basis) => Some(basis),
197            _ => None,
198        }
199    }
200
201    /// Returns the local basis of the second body.
202    ///
203    /// If the [`JointBasis`] is set to [`FromGlobal`](JointBasis::FromGlobal),
204    /// and the local basis has not yet been computed, this will return `None`.
205    #[inline]
206    pub fn local_basis2(&self) -> Option<Rot> {
207        match self.frame2.basis {
208            JointBasis::Local(basis) => Some(basis),
209            _ => None,
210        }
211    }
212
213    /// Sets the joint's compliance (inverse of stiffness).
214    #[inline]
215    #[deprecated(
216        since = "0.4.0",
217        note = "Use `with_point_compliance` and `with_angle_compliance` instead."
218    )]
219    pub const fn with_compliance(mut self, compliance: Scalar) -> Self {
220        self.point_compliance = compliance;
221        self.angle_compliance = compliance;
222        self
223    }
224
225    /// Sets the compliance of the point-to-point compliance (inverse of stiffness, m / N).
226    #[inline]
227    pub const fn with_point_compliance(mut self, compliance: Scalar) -> Self {
228        self.point_compliance = compliance;
229        self
230    }
231
232    /// Sets the compliance of the angular constraint (inverse of stiffness, (N * m / rad).
233    #[inline]
234    pub const fn with_angle_compliance(mut self, compliance: Scalar) -> Self {
235        self.angle_compliance = compliance;
236        self
237    }
238}
239
240impl MapEntities for FixedJoint {
241    fn map_entities<M: EntityMapper>(&mut self, entity_mapper: &mut M) {
242        self.body1 = entity_mapper.get_mapped(self.body1);
243        self.body2 = entity_mapper.get_mapped(self.body2);
244    }
245}
246
247pub(super) fn plugin(app: &mut App) {
248    app.add_systems(
249        PhysicsSchedule,
250        update_local_frames.in_set(JointSystems::PrepareLocalFrames),
251    );
252}
253
254fn update_local_frames(
255    mut joints: Query<&mut FixedJoint, Changed<FixedJoint>>,
256    bodies: Query<(&Position, &Rotation)>,
257) {
258    for mut joint in &mut joints {
259        if matches!(joint.frame1.anchor, JointAnchor::Local(_))
260            && matches!(joint.frame2.anchor, JointAnchor::Local(_))
261            && matches!(joint.frame1.basis, JointBasis::Local(_))
262            && matches!(joint.frame2.basis, JointBasis::Local(_))
263        {
264            continue;
265        }
266
267        let Ok([(pos1, rot1), (pos2, rot2)]) = bodies.get_many(joint.entities()) else {
268            continue;
269        };
270
271        // TODO: Use weighted COM average for the anchors of dynamic Auto-Auto pairs.
272        let [frame1, frame2] =
273            JointFrame::compute_local(joint.frame1, joint.frame2, pos1.0, pos2.0, rot1, rot2);
274        joint.frame1 = frame1;
275        joint.frame2 = frame2;
276    }
277}
278
279#[cfg(feature = "debug-plugin")]
280impl DebugRenderConstraint<2> for FixedJoint {
281    type Context = ();
282
283    fn debug_render(
284        &self,
285        positions: [Vector; 2],
286        rotations: [Rotation; 2],
287        _context: &mut Self::Context,
288        gizmos: &mut Gizmos<PhysicsGizmos>,
289        config: &PhysicsGizmos,
290    ) {
291        let [pos1, pos2] = positions;
292        let [rot1, rot2] = rotations;
293
294        let Some(local_anchor1) = self.local_anchor1() else {
295            return;
296        };
297        let Some(local_anchor2) = self.local_anchor2() else {
298            return;
299        };
300
301        let anchor1 = pos1 + rot1 * local_anchor1;
302        let anchor2 = pos2 + rot2 * local_anchor2;
303
304        if let Some(anchor_color) = config.joint_anchor_color {
305            gizmos.draw_line(pos1, anchor1, anchor_color);
306            gizmos.draw_line(pos2, anchor2, anchor_color);
307        }
308
309        if let Some(color) = config.joint_separation_color {
310            gizmos.draw_line(anchor1, anchor2, color);
311        }
312    }
313}
avian3d/dynamics/joints/fixed.rs

avian3d/dynamics/joints/
fixed.rs
