avian3d/dynamics/solver/contact/
mod.rs

1//! Constraints and other types used for solving contacts.
2
3mod normal_part;
4mod tangent_part;
5
6pub use normal_part::ContactNormalPart;
7pub use tangent_part::ContactTangentPart;
8
9use crate::prelude::*;
10use bevy::{
11    ecs::entity::{Entity, EntityMapper, MapEntities},
12    reflect::Reflect,
13    utils::default,
14};
15
16// TODO: One-body constraint version
17/// Data and logic for solving a single contact point for a [`ContactConstraint`].
18#[derive(Clone, Debug, PartialEq, Reflect)]
19pub struct ContactConstraintPoint {
20    /// The normal part of the contact constraint.
21    pub normal_part: ContactNormalPart,
22
23    /// The tangential friction part of the contact constraint.
24    ///
25    /// `None` if the coefficient of friction is zero.
26    pub tangent_part: Option<ContactTangentPart>,
27
28    // TODO: This could probably just be a boolean?
29    /// The largest incremental contact impulse magnitude along the contact normal during this frame.
30    ///
31    /// This is used for determining whether restitution should be applied.
32    pub max_normal_impulse: Scalar,
33
34    // TODO: If a rotation delta was used for bodies, these local anchors could be removed.
35    /// The local contact point relative to the center of mass of the first body.
36    pub local_anchor1: Vector,
37
38    /// The local contact point relative to the center of mass of the second body.
39    pub local_anchor2: Vector,
40
41    /// The world-space contact point relative to the center of mass of the first body.
42    pub anchor1: Vector,
43
44    /// The world-space contact point relative to the center of mass of the second body.
45    pub anchor2: Vector,
46
47    /// The relative velocity of the bodies along the normal at the contact point.
48    pub normal_speed: Scalar,
49
50    /// The pre-solve separation distance between the bodies.
51    ///
52    /// A negative separation indicates penetration.
53    pub initial_separation: Scalar,
54}
55
56/// A contact constraint used for resolving inter-penetration between two bodies.
57///
58/// Each constraint corresponds to a [`ContactManifold`] indicated by the `manifold_index`.
59/// The contact points are stored in `points`, and they all share the same `normal`.
60#[derive(Clone, Debug, PartialEq, Reflect)]
61pub struct ContactConstraint {
62    /// The first rigid body entity in the contact.
63    pub body1: Entity,
64    /// The second rigid body entity in the contact.
65    pub body2: Entity,
66    /// The first collider entity in the contact.
67    pub collider1: Entity,
68    /// The second collider entity in the contact.
69    pub collider2: Entity,
70    /// The combined coefficient of dynamic [friction](Friction) of the bodies.
71    pub friction: Scalar,
72    /// The combined coefficient of [restitution](Restitution) of the bodies.
73    pub restitution: Scalar,
74    /// The desired relative linear speed of the bodies along the surface,
75    /// expressed in world space as `tangent_speed2 - tangent_speed1`.
76    ///
77    /// Defaults to zero. If set to a non-zero value, this can be used to simulate effects
78    /// such as conveyor belts.
79    #[cfg(feature = "2d")]
80    pub tangent_speed: Scalar,
81    /// The desired relative linear velocity of the bodies along the surface,
82    /// expressed in world space as `tangent_velocity2 - tangent_velocity1`.
83    ///
84    /// Defaults to zero. If set to a non-zero value, this can be used to simulate effects
85    /// such as conveyor belts.
86    #[cfg(feature = "3d")]
87    pub tangent_velocity: Vector,
88    /// The world-space contact normal shared by all points in the contact manifold.
89    pub normal: Vector,
90    /// The contact points in the manifold. Each point shares the same `normal`.
91    pub points: Vec<ContactConstraintPoint>,
92    /// The index of the [`ContactPair`] in the [`ContactGraph`].
93    ///
94    /// This is primarily used for ordering contact constraints deterministically
95    /// when parallelism is enabled. The index may be invalidated by contact removal.
96    // TODO: We should figure out a better way to handle deterministic constraint generation.
97    #[cfg(feature = "parallel")]
98    pub pair_index: usize,
99    /// The index of the [`ContactManifold`] in the [`ContactPair`] stored for the two bodies.
100    pub manifold_index: usize,
101}
102
103impl ContactConstraint {
104    /// Warm starts the contact constraint by applying the impulses from the previous frame or substep.
105    pub fn warm_start(
106        &self,
107        body1: &mut RigidBodyQueryItem,
108        body2: &mut RigidBodyQueryItem,
109        normal: Vector,
110        tangent_directions: [Vector; DIM - 1],
111        warm_start_coefficient: Scalar,
112    ) {
113        let inv_mass1 = body1.effective_inverse_mass();
114        let inv_mass2 = body2.effective_inverse_mass();
115        let inv_inertia1 = body1.effective_global_angular_inertia().inverse();
116        let inv_inertia2 = body2.effective_global_angular_inertia().inverse();
117
118        for point in self.points.iter() {
119            // Fixed anchors
120            let r1 = point.anchor1;
121            let r2 = point.anchor2;
122
123            let tangent_impulse = point
124                .tangent_part
125                .as_ref()
126                .map_or(default(), |part| part.impulse);
127
128            #[cfg(feature = "2d")]
129            let p = warm_start_coefficient
130                * (point.normal_part.impulse * normal + tangent_impulse * tangent_directions[0]);
131            #[cfg(feature = "3d")]
132            let p = warm_start_coefficient
133                * (point.normal_part.impulse * normal
134                    + tangent_impulse.x * tangent_directions[0]
135                    + tangent_impulse.y * tangent_directions[1]);
136
137            if body1.rb.is_dynamic() && body1.dominance() <= body2.dominance() {
138                body1.linear_velocity.0 -= p * inv_mass1;
139                body1.angular_velocity.0 -= inv_inertia1 * cross(r1, p);
140            }
141            if body2.rb.is_dynamic() && body2.dominance() <= body1.dominance() {
142                body2.linear_velocity.0 += p * inv_mass2;
143                body2.angular_velocity.0 += inv_inertia2 * cross(r2, p);
144            }
145        }
146    }
147
148    /// Solves the [`ContactConstraint`], applying an impulse to the given bodies.
149    pub fn solve(
150        &mut self,
151        body1: &mut RigidBodyQueryItem,
152        body2: &mut RigidBodyQueryItem,
153        delta_secs: Scalar,
154        use_bias: bool,
155        max_overlap_solve_speed: Scalar,
156    ) {
157        let inv_mass1 = body1.effective_inverse_mass();
158        let inv_mass2 = body2.effective_inverse_mass();
159        let inv_inertia1 = body1.effective_global_angular_inertia().inverse();
160        let inv_inertia2 = body2.effective_global_angular_inertia().inverse();
161
162        let delta_translation = body2.accumulated_translation.0 - body1.accumulated_translation.0;
163
164        // Normal impulses
165        for point in self.points.iter_mut() {
166            let r1 = *body1.rotation * point.local_anchor1;
167            let r2 = *body2.rotation * point.local_anchor2;
168
169            // TODO: Consider rotation delta for anchors
170            let delta_separation = delta_translation + (r2 - r1);
171            let separation = delta_separation.dot(self.normal) + point.initial_separation;
172
173            // Fixed anchors
174            let r1 = point.anchor1;
175            let r2 = point.anchor2;
176
177            // Relative velocity at contact point
178            let relative_velocity = body2.velocity_at_point(r2) - body1.velocity_at_point(r1);
179
180            // Compute the incremental impulse. The clamping and impulse accumulation is handled by the method.
181            let impulse_magnitude = point.normal_part.solve_impulse(
182                separation,
183                relative_velocity,
184                self.normal,
185                use_bias,
186                max_overlap_solve_speed,
187                delta_secs,
188            );
189
190            // Store the maximum impulse for restitution.
191            point.max_normal_impulse = impulse_magnitude.max(point.max_normal_impulse);
192
193            if impulse_magnitude == 0.0 {
194                continue;
195            }
196
197            let impulse = impulse_magnitude * self.normal;
198
199            // Apply the impulse.
200            if body1.rb.is_dynamic() && body1.dominance() <= body2.dominance() {
201                body1.linear_velocity.0 -= impulse * inv_mass1;
202                body1.angular_velocity.0 -= inv_inertia1 * cross(r1, impulse);
203            }
204            if body2.rb.is_dynamic() && body2.dominance() <= body1.dominance() {
205                body2.linear_velocity.0 += impulse * inv_mass2;
206                body2.angular_velocity.0 += inv_inertia2 * cross(r2, impulse);
207            }
208        }
209
210        let tangent_directions =
211            self.tangent_directions(body1.linear_velocity.0, body2.linear_velocity.0);
212
213        // Friction
214        for point in self.points.iter_mut() {
215            let Some(ref mut friction_part) = point.tangent_part else {
216                continue;
217            };
218
219            // Fixed anchors
220            let r1 = point.anchor1;
221            let r2 = point.anchor2;
222
223            // Relative velocity at contact point
224            let relative_velocity = body2.velocity_at_point(r2) - body1.velocity_at_point(r1);
225
226            // Compute the incremental impulse. The clamping and impulse accumulation is handled by the method.
227            let impulse = friction_part.solve_impulse(
228                tangent_directions,
229                relative_velocity,
230                #[cfg(feature = "2d")]
231                self.tangent_speed,
232                #[cfg(feature = "3d")]
233                self.tangent_velocity,
234                self.friction,
235                point.normal_part.impulse,
236            );
237
238            // Apply the impulse.
239            if body1.rb.is_dynamic() && body1.dominance() <= body2.dominance() {
240                body1.linear_velocity.0 -= impulse * inv_mass1;
241                body1.angular_velocity.0 -= inv_inertia1 * cross(r1, impulse);
242            }
243            if body2.rb.is_dynamic() && body2.dominance() <= body1.dominance() {
244                body2.linear_velocity.0 += impulse * inv_mass2;
245                body2.angular_velocity.0 += inv_inertia2 * cross(r2, impulse);
246            }
247        }
248    }
249
250    /// Applies [restitution](`Restitution`) for the given bodies if the relative speed
251    /// along the contact normal exceeds the given `threshold`.
252    pub fn apply_restitution(
253        &mut self,
254        body1: &mut RigidBodyQueryItem,
255        body2: &mut RigidBodyQueryItem,
256        threshold: Scalar,
257    ) {
258        for point in self.points.iter_mut() {
259            // Skip restitution for speeds below the threshold.
260            // We also skip contacts that don't apply an impulse to account for speculative contacts.
261            if point.normal_speed > -threshold || point.max_normal_impulse == 0.0 {
262                continue;
263            }
264
265            // Fixed anchors
266            let r1 = point.anchor1;
267            let r2 = point.anchor2;
268
269            let inv_mass1 = body1.effective_inverse_mass();
270            let inv_mass2 = body2.effective_inverse_mass();
271            let inv_inertia1 = body1.effective_global_angular_inertia().inverse();
272            let inv_inertia2 = body2.effective_global_angular_inertia().inverse();
273
274            // Relative velocity at contact point
275            let relative_velocity = body2.velocity_at_point(r2) - body1.velocity_at_point(r1);
276            let normal_speed = relative_velocity.dot(self.normal);
277
278            // Compute the incremental normal impulse to account for restitution.
279            let mut impulse = -point.normal_part.effective_mass
280                * (normal_speed + self.restitution * point.normal_speed);
281
282            // Clamp the accumulated impulse.
283            let new_impulse = (point.normal_part.impulse + impulse).max(0.0);
284            impulse = new_impulse - point.normal_part.impulse;
285            point.normal_part.impulse = new_impulse;
286            point.max_normal_impulse = impulse.max(point.max_normal_impulse);
287
288            // Apply the impulse.
289            let impulse = impulse * self.normal;
290
291            if body1.rb.is_dynamic() && body1.dominance() <= body2.dominance() {
292                body1.linear_velocity.0 -= impulse * inv_mass1;
293                body1.angular_velocity.0 -= inv_inertia1 * cross(r1, impulse);
294            }
295            if body2.rb.is_dynamic() && body2.dominance() <= body1.dominance() {
296                body2.linear_velocity.0 += impulse * inv_mass2;
297                body2.angular_velocity.0 += inv_inertia2 * cross(r2, impulse);
298            }
299        }
300    }
301
302    /// Computes `DIM - 1` tangent directions.
303    #[allow(unused_variables)]
304    pub fn tangent_directions(&self, velocity1: Vector, velocity2: Vector) -> [Vector; DIM - 1] {
305        #[cfg(feature = "2d")]
306        {
307            [Vector::new(self.normal.y, -self.normal.x)]
308        }
309        #[cfg(feature = "3d")]
310        {
311            let force_direction = -self.normal;
312            let relative_velocity = velocity1 - velocity2;
313            let tangent_velocity =
314                relative_velocity - force_direction * force_direction.dot(relative_velocity);
315
316            let tangent = tangent_velocity
317                .try_normalize()
318                .unwrap_or(force_direction.any_orthonormal_vector());
319            let bitangent = force_direction.cross(tangent);
320            [tangent, bitangent]
321        }
322    }
323}
324
325impl MapEntities for ContactConstraint {
326    fn map_entities<M: EntityMapper>(&mut self, entity_mapper: &mut M) {
327        self.body1 = entity_mapper.get_mapped(self.body1);
328        self.body2 = entity_mapper.get_mapped(self.body2);
329        self.collider1 = entity_mapper.get_mapped(self.collider1);
330        self.collider2 = entity_mapper.get_mapped(self.collider2);
331    }
332}
avian3d/dynamics/solver/contact/mod.rs

avian3d/dynamics/solver/contact/
mod.rs
