avian3d/dynamics/solver/joints/
prismatic.rs

1//! [`PrismaticJoint`] 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 prismatic joint prevents relative movement of the attached bodies, except for translation along one `free_axis`.
13///
14/// Prismatic joints can be useful for things like elevators, pistons, sliding doors and moving platforms.
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 PrismaticJoint {
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    /// A free axis that the attached bodies can translate along relative to each other.
29    pub free_axis: Vector,
30    /// The extents of the allowed relative translation along the free axis.
31    pub free_axis_limits: Option<DistanceLimit>,
32    /// Linear damping applied by the joint.
33    pub damping_linear: Scalar,
34    /// Angular damping applied by the joint.
35    pub damping_angular: Scalar,
36    /// Lagrange multiplier for the positional correction.
37    pub position_lagrange: Scalar,
38    /// Lagrange multiplier for the angular correction caused by the alignment of the bodies.
39    pub align_lagrange: Scalar,
40    /// The joint's compliance, the inverse of stiffness, has the unit meters / Newton.
41    pub compliance: Scalar,
42    /// The force exerted by the joint.
43    pub force: Vector,
44    /// The torque exerted by the joint when aligning the bodies.
45    pub align_torque: Torque,
46}
47
48impl XpbdConstraint<2> for PrismaticJoint {
49    fn entities(&self) -> [Entity; 2] {
50        [self.entity1, self.entity2]
51    }
52
53    fn clear_lagrange_multipliers(&mut self) {
54        self.position_lagrange = 0.0;
55        self.align_lagrange = 0.0;
56    }
57
58    fn solve(&mut self, bodies: [&mut RigidBodyQueryItem; 2], dt: Scalar) {
59        let [body1, body2] = bodies;
60        let compliance = self.compliance;
61
62        // Align orientations
63        let difference = self.get_rotation_difference(&body1.rotation, &body2.rotation);
64        let mut lagrange = self.align_lagrange;
65        self.align_torque =
66            self.align_orientation(body1, body2, difference, &mut lagrange, compliance, dt);
67        self.align_lagrange = lagrange;
68
69        // Constrain the relative positions of the bodies, only allowing translation along one free axis
70        self.force = self.constrain_positions(body1, body2, dt);
71    }
72}
73
74impl Joint for PrismaticJoint {
75    fn new(entity1: Entity, entity2: Entity) -> Self {
76        Self {
77            entity1,
78            entity2,
79            local_anchor1: Vector::ZERO,
80            local_anchor2: Vector::ZERO,
81            free_axis: Vector::X,
82            free_axis_limits: None,
83            damping_linear: 1.0,
84            damping_angular: 1.0,
85            position_lagrange: 0.0,
86            align_lagrange: 0.0,
87            compliance: 0.0,
88            force: Vector::ZERO,
89            #[cfg(feature = "2d")]
90            align_torque: 0.0,
91            #[cfg(feature = "3d")]
92            align_torque: Vector::ZERO,
93        }
94    }
95
96    fn with_compliance(self, compliance: Scalar) -> Self {
97        Self { compliance, ..self }
98    }
99
100    fn with_local_anchor_1(self, anchor: Vector) -> Self {
101        Self {
102            local_anchor1: anchor,
103            ..self
104        }
105    }
106
107    fn with_local_anchor_2(self, anchor: Vector) -> Self {
108        Self {
109            local_anchor2: anchor,
110            ..self
111        }
112    }
113
114    fn with_linear_velocity_damping(self, damping: Scalar) -> Self {
115        Self {
116            damping_linear: damping,
117            ..self
118        }
119    }
120
121    fn with_angular_velocity_damping(self, damping: Scalar) -> Self {
122        Self {
123            damping_angular: damping,
124            ..self
125        }
126    }
127
128    fn local_anchor_1(&self) -> Vector {
129        self.local_anchor1
130    }
131
132    fn local_anchor_2(&self) -> Vector {
133        self.local_anchor2
134    }
135
136    fn damping_linear(&self) -> Scalar {
137        self.damping_linear
138    }
139
140    fn damping_angular(&self) -> Scalar {
141        self.damping_angular
142    }
143}
144
145impl PrismaticJoint {
146    /// Constrains the relative positions of the bodies, only allowing translation along one free axis.
147    ///
148    /// Returns the force exerted by this constraint.
149    fn constrain_positions(
150        &mut self,
151        body1: &mut RigidBodyQueryItem,
152        body2: &mut RigidBodyQueryItem,
153        dt: Scalar,
154    ) -> Vector {
155        let world_r1 = *body1.rotation * self.local_anchor1;
156        let world_r2 = *body2.rotation * self.local_anchor2;
157
158        let mut delta_x = Vector::ZERO;
159
160        let axis1 = *body1.rotation * self.free_axis;
161        if let Some(limits) = self.free_axis_limits {
162            delta_x += limits.compute_correction_along_axis(
163                body1.current_position() + world_r1,
164                body2.current_position() + world_r2,
165                axis1,
166            );
167        }
168
169        let zero_distance_limit = DistanceLimit::ZERO;
170
171        #[cfg(feature = "2d")]
172        {
173            let axis2 = Vector::new(axis1.y, -axis1.x);
174            delta_x += zero_distance_limit.compute_correction_along_axis(
175                body1.current_position() + world_r1,
176                body2.current_position() + world_r2,
177                axis2,
178            );
179        }
180        #[cfg(feature = "3d")]
181        {
182            let axis2 = axis1.any_orthogonal_vector();
183            let axis3 = axis1.cross(axis2);
184
185            delta_x += zero_distance_limit.compute_correction_along_axis(
186                body1.current_position() + world_r1,
187                body2.current_position() + world_r2,
188                axis2,
189            );
190            delta_x += zero_distance_limit.compute_correction_along_axis(
191                body1.current_position() + world_r1,
192                body2.current_position() + world_r2,
193                axis3,
194            );
195        }
196
197        let magnitude = delta_x.length();
198
199        if magnitude <= Scalar::EPSILON {
200            return Vector::ZERO;
201        }
202
203        let dir = delta_x / magnitude;
204
205        // Compute generalized inverse masses
206        let w1 = PositionConstraint::compute_generalized_inverse_mass(self, body1, world_r1, dir);
207        let w2 = PositionConstraint::compute_generalized_inverse_mass(self, body2, world_r2, dir);
208
209        // Compute Lagrange multiplier update
210        let delta_lagrange = self.compute_lagrange_update(
211            self.position_lagrange,
212            magnitude,
213            &[w1, w2],
214            self.compliance,
215            dt,
216        );
217        self.position_lagrange += delta_lagrange;
218
219        // Apply positional correction to align the positions of the bodies
220        self.apply_positional_lagrange_update(
221            body1,
222            body2,
223            delta_lagrange,
224            dir,
225            world_r1,
226            world_r2,
227        );
228
229        // Return constraint force
230        self.compute_force(self.position_lagrange, dir, dt)
231    }
232
233    /// Sets the joint's free axis. Relative translations are allowed along this free axis.
234    pub fn with_free_axis(self, axis: Vector) -> Self {
235        Self {
236            free_axis: axis,
237            ..self
238        }
239    }
240
241    /// Sets the translational limits along the joint's free axis.
242    pub fn with_limits(self, min: Scalar, max: Scalar) -> Self {
243        Self {
244            free_axis_limits: Some(DistanceLimit::new(min, max)),
245            ..self
246        }
247    }
248
249    #[cfg(feature = "2d")]
250    fn get_rotation_difference(&self, rot1: &Rotation, rot2: &Rotation) -> Scalar {
251        rot1.angle_between(*rot2)
252    }
253
254    #[cfg(feature = "3d")]
255    fn get_rotation_difference(&self, rot1: &Rotation, rot2: &Rotation) -> Vector {
256        // TODO: The XPBD paper doesn't have this minus sign, but it seems to be needed for stability.
257        //       The angular correction code might have a wrong sign elsewhere.
258        -2.0 * (rot1.0 * rot2.inverse().0).xyz()
259    }
260}
261
262impl PositionConstraint for PrismaticJoint {}
263
264impl AngularConstraint for PrismaticJoint {}
265
266impl MapEntities for PrismaticJoint {
267    fn map_entities<M: EntityMapper>(&mut self, entity_mapper: &mut M) {
268        self.entity1 = entity_mapper.get_mapped(self.entity1);
269        self.entity2 = entity_mapper.get_mapped(self.entity2);
270    }
271}
avian3d/dynamics/solver/joints/prismatic.rs

avian3d/dynamics/solver/joints/
prismatic.rs
