avian3d/dynamics/solver/joints/
spherical.rs

1//! [`SphericalJoint`] component.
2
3use crate::{dynamics::solver::xpbd::*, prelude::*};
4use bevy::{
5    ecs::{
6        entity::{EntityMapper, MapEntities},
7        reflect::ReflectMapEntities,
8    },
9    prelude::*,
10};
11
12/// A spherical joint prevents relative translation of the attached bodies while allowing rotation around all axes.
13///
14/// Spherical joints can be useful for things like pendula, chains, ragdolls etc.
15#[derive(Component, Clone, Copy, Debug, PartialEq, Reflect)]
16#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))]
17#[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))]
18#[reflect(Debug, Component, MapEntities, PartialEq)]
19pub struct SphericalJoint {
20    /// First entity constrained by the joint.
21    pub entity1: Entity,
22    /// Second entity constrained by the joint.
23    pub entity2: Entity,
24    /// Attachment point on the first body.
25    pub local_anchor1: Vector,
26    /// Attachment point on the second body.
27    pub local_anchor2: Vector,
28    /// An axis that the attached bodies can swing around. This is normally the x-axis.
29    pub swing_axis: Vector3,
30    /// An axis that the attached bodies can twist around. This is normally the y-axis.
31    pub twist_axis: Vector3,
32    /// The extents of the allowed relative rotation of the bodies around the `swing_axis`.
33    pub swing_limit: Option<AngleLimit>,
34    /// The extents of the allowed relative rotation of the bodies around the `twist_axis`.
35    pub twist_limit: Option<AngleLimit>,
36    /// Linear damping applied by the joint.
37    pub damping_linear: Scalar,
38    /// Angular damping applied by the joint.
39    pub damping_angular: Scalar,
40    /// Lagrange multiplier for the positional correction.
41    pub position_lagrange: Scalar,
42    /// Lagrange multiplier for the angular correction caused by the swing limits.
43    pub swing_lagrange: Scalar,
44    /// Lagrange multiplier for the angular correction caused by the twist limits.
45    pub twist_lagrange: Scalar,
46    /// The joint's compliance, the inverse of stiffness, has the unit meters / Newton.
47    pub compliance: Scalar,
48    /// The force exerted by the joint.
49    pub force: Vector,
50    /// The torque exerted by the joint when limiting the relative rotation of the bodies around the `swing_axis`.
51    pub swing_torque: Torque,
52    /// The torque exerted by the joint when limiting the relative rotation of the bodies around the `twist_axis`.
53    pub twist_torque: Torque,
54}
55
56impl XpbdConstraint<2> for SphericalJoint {
57    fn entities(&self) -> [Entity; 2] {
58        [self.entity1, self.entity2]
59    }
60
61    fn clear_lagrange_multipliers(&mut self) {
62        self.position_lagrange = 0.0;
63        self.swing_lagrange = 0.0;
64        self.twist_lagrange = 0.0;
65    }
66
67    fn solve(&mut self, bodies: [&mut RigidBodyQueryItem; 2], dt: Scalar) {
68        let [body1, body2] = bodies;
69        let compliance = self.compliance;
70
71        // Align positions
72        let mut lagrange = self.position_lagrange;
73        self.force = self.align_position(
74            body1,
75            body2,
76            self.local_anchor1,
77            self.local_anchor2,
78            &mut lagrange,
79            compliance,
80            dt,
81        );
82        self.position_lagrange = lagrange;
83
84        // Apply swing limits
85        self.swing_torque = self.apply_swing_limits(body1, body2, dt);
86
87        // Apply twist limits
88        self.twist_torque = self.apply_twist_limits(body1, body2, dt);
89    }
90}
91
92impl Joint for SphericalJoint {
93    fn new(entity1: Entity, entity2: Entity) -> Self {
94        Self {
95            entity1,
96            entity2,
97            local_anchor1: Vector::ZERO,
98            local_anchor2: Vector::ZERO,
99            swing_axis: Vector3::X,
100            twist_axis: Vector3::Y,
101            swing_limit: None,
102            twist_limit: None,
103            damping_linear: 1.0,
104            damping_angular: 1.0,
105            position_lagrange: 0.0,
106            swing_lagrange: 0.0,
107            twist_lagrange: 0.0,
108            compliance: 0.0,
109            force: Vector::ZERO,
110            #[cfg(feature = "2d")]
111            swing_torque: 0.0,
112            #[cfg(feature = "3d")]
113            swing_torque: Vector::ZERO,
114            #[cfg(feature = "2d")]
115            twist_torque: 0.0,
116            #[cfg(feature = "3d")]
117            twist_torque: Vector::ZERO,
118        }
119    }
120
121    fn with_compliance(self, compliance: Scalar) -> Self {
122        Self { compliance, ..self }
123    }
124
125    fn with_local_anchor_1(self, anchor: Vector) -> Self {
126        Self {
127            local_anchor1: anchor,
128            ..self
129        }
130    }
131
132    fn with_local_anchor_2(self, anchor: Vector) -> Self {
133        Self {
134            local_anchor2: anchor,
135            ..self
136        }
137    }
138
139    fn with_linear_velocity_damping(self, damping: Scalar) -> Self {
140        Self {
141            damping_linear: damping,
142            ..self
143        }
144    }
145
146    fn with_angular_velocity_damping(self, damping: Scalar) -> Self {
147        Self {
148            damping_angular: damping,
149            ..self
150        }
151    }
152
153    fn local_anchor_1(&self) -> Vector {
154        self.local_anchor1
155    }
156
157    fn local_anchor_2(&self) -> Vector {
158        self.local_anchor2
159    }
160
161    fn damping_linear(&self) -> Scalar {
162        self.damping_linear
163    }
164
165    fn damping_angular(&self) -> Scalar {
166        self.damping_angular
167    }
168}
169
170impl SphericalJoint {
171    /// Sets the limits of the allowed relative rotation around the `swing_axis`.
172    pub fn with_swing_limits(self, min: Scalar, max: Scalar) -> Self {
173        Self {
174            swing_limit: Some(AngleLimit::new(min, max)),
175            ..self
176        }
177    }
178
179    /// Sets the limits of the allowed relative rotation around the `twist_axis`.
180    #[cfg(feature = "3d")]
181    pub fn with_twist_limits(self, min: Scalar, max: Scalar) -> Self {
182        Self {
183            twist_limit: Some(AngleLimit::new(min, max)),
184            ..self
185        }
186    }
187
188    /// Applies angle limits to limit the relative rotation of the bodies around the `swing_axis`.
189    fn apply_swing_limits(
190        &mut self,
191        body1: &mut RigidBodyQueryItem,
192        body2: &mut RigidBodyQueryItem,
193        dt: Scalar,
194    ) -> Torque {
195        if let Some(joint_limit) = self.swing_limit {
196            let a1 = *body1.rotation * self.swing_axis;
197            let a2 = *body2.rotation * self.swing_axis;
198
199            let n = a1.cross(a2);
200            let n_magnitude = n.length();
201
202            if n_magnitude <= Scalar::EPSILON {
203                return Torque::ZERO;
204            }
205
206            let n = n / n_magnitude;
207
208            if let Some(correction) = joint_limit.compute_correction(n, a1, a2, PI) {
209                let mut lagrange = self.swing_lagrange;
210                let torque = self.align_orientation(
211                    body1,
212                    body2,
213                    correction,
214                    &mut lagrange,
215                    self.compliance,
216                    dt,
217                );
218                self.swing_lagrange = lagrange;
219                return torque;
220            }
221        }
222        Torque::ZERO
223    }
224
225    /// Applies angle limits to limit the relative rotation of the bodies around the `twist_axis`.
226    fn apply_twist_limits(
227        &mut self,
228        body1: &mut RigidBodyQueryItem,
229        body2: &mut RigidBodyQueryItem,
230        dt: Scalar,
231    ) -> Torque {
232        if let Some(joint_limit) = self.twist_limit {
233            let a1 = *body1.rotation * self.swing_axis;
234            let a2 = *body2.rotation * self.swing_axis;
235
236            let b1 = *body1.rotation * self.twist_axis;
237            let b2 = *body2.rotation * self.twist_axis;
238
239            let n = a1 + a2;
240            let n_magnitude = n.length();
241
242            if n_magnitude <= Scalar::EPSILON {
243                return Torque::ZERO;
244            }
245
246            let n = n / n_magnitude;
247
248            let n1 = b1 - n.dot(b1) * n;
249            let n2 = b2 - n.dot(b2) * n;
250            let n1_magnitude = n1.length();
251            let n2_magnitude = n2.length();
252
253            if n1_magnitude <= Scalar::EPSILON || n2_magnitude <= Scalar::EPSILON {
254                return Torque::ZERO;
255            }
256
257            let n1 = n1 / n1_magnitude;
258            let n2 = n2 / n2_magnitude;
259
260            let max_correction = if a1.dot(a2) > -0.5 { 2.0 * PI } else { dt };
261
262            if let Some(correction) = joint_limit.compute_correction(n, n1, n2, max_correction) {
263                let mut lagrange = self.twist_lagrange;
264                let torque = self.align_orientation(
265                    body1,
266                    body2,
267                    correction,
268                    &mut lagrange,
269                    self.compliance,
270                    dt,
271                );
272                self.twist_lagrange = lagrange;
273                return torque;
274            }
275        }
276        Torque::ZERO
277    }
278}
279
280impl PositionConstraint for SphericalJoint {}
281
282impl AngularConstraint for SphericalJoint {}
283
284impl MapEntities for SphericalJoint {
285    fn map_entities<M: EntityMapper>(&mut self, entity_mapper: &mut M) {
286        self.entity1 = entity_mapper.get_mapped(self.entity1);
287        self.entity2 = entity_mapper.get_mapped(self.entity2);
288    }
289}
avian3d/dynamics/solver/joints/spherical.rs

avian3d/dynamics/solver/joints/
spherical.rs
