avian3d/dynamics/solver/contact/
normal_part.rs

1use crate::{dynamics::solver::softness_parameters::SoftnessCoefficients, prelude::*};
2use bevy::reflect::Reflect;
3
4pub type NormalImpulse = Scalar;
5
6// TODO: Block solver for solving two contact points simultaneously
7// TODO: One-body constraint version
8/// The normal part of a [`ContactConstraintPoint`](super::ContactConstraintPoint).
9/// Aims to resolve overlap.
10#[derive(Clone, Debug, PartialEq, Reflect)]
11pub struct ContactNormalPart {
12    /// The magnitude of the contact impulse along the contact normal.
13    pub impulse: NormalImpulse,
14
15    /// The inertial properties of the bodies projected onto the contact normal,
16    /// or in other words, the mass "seen" by the constraint along the normal.
17    pub effective_mass: Scalar,
18
19    /// The softness parameters used for tuning contact response.
20    pub softness: SoftnessCoefficients,
21}
22
23impl ContactNormalPart {
24    /// Generates a new [`ContactNormalPart`].
25    #[allow(clippy::too_many_arguments)]
26    pub fn generate(
27        inv_mass_sum: Scalar,
28        angular_inertia1: impl Into<ComputedAngularInertia>,
29        angular_inertia2: impl Into<ComputedAngularInertia>,
30        r1: Vector,
31        r2: Vector,
32        normal: Vector,
33        warm_start_impulse: Option<NormalImpulse>,
34        softness: SoftnessCoefficients,
35    ) -> Self {
36        let angular_inertia1: ComputedAngularInertia = angular_inertia1.into();
37        let angular_inertia2: ComputedAngularInertia = angular_inertia2.into();
38        let i1 = angular_inertia1.inverse();
39        let i2 = angular_inertia2.inverse();
40
41        // Derivation for the projected normal mass. This is for 3D, but the 2D version is basically equivalent.
42        //
43        // The penetration constraint function is the following:
44        //
45        // C(s) = dot(p2 - p1, n) = dot(x2 + r2 - x1 - r1, n)
46        //
47        // where
48        // - p1 and p2 are world-space contact points for each body
49        // - n is the surface normal pointing from the first body towards the second (the order matters)
50        // - x1 and x2 are the centers of mass
51        // - r1 and r2 are vectors from the centers of mass to the corresponding contact points
52        //
53        // The contact constraint is satisfied when the bodies are not penetrating:
54        //
55        // C(s) >= 0
56        //
57        // We can compute the velocity constraint by getting the time derivative:
58        //
59        // C_vel(s) = d/dt(dot(x2 + r2 - x1 - r1, n))
60        //          = dot(d/dt(x2 + r2 - x1 - r1), n) + dot(x2 + r2 - x1 - r1, d/dt(n))
61        //
62        // The penetration is assumed to be small, so we can ignore the second term:
63        //
64        // C_vel(s) = dot(d/dt(x2 + r2 - x1 - r1), n)
65        //          = dot(lin_vel2 + ang_vel2 x r2 - lin_vel1 - ang_vel1 x r1, n)
66        //          = dot(lin_vel2, n) + dot(ang_vel2, r2 x n) - dot(v1, n) - dot(ang_vel1, r1 x n)
67        //
68        // By inspection, we can see that the Jacobian is the following:
69        //
70        //      linear1  angular1 linear2 angular2
71        // J = [ -normal, -(r1 x n), n, r2 x n ]
72        //
73        // From this, we can derive the effective inverse mass:
74        //
75        // K = J_x * M^-1 * J_x^T
76        //   = 1/m1 + 1/m2 + (r1 x n)^T * I1^-1 * (r1 x n) + (r2 x n)^T * I2^-1 * (r2 x n)
77        //
78        // See "Constraints Derivation for Rigid Body Simulation in 3D" section 2.1.3
79        // by Daniel Chappuis for the full derivation of the effective inverse mass.
80        //
81        // Finally, the transposes can be simplified with dot products, because a^T * b = dot(a, b),
82        // where a and b are two column vectors.
83        //
84        // K = 1/m1 + 1/m2 + dot(r1 x n, I1^-1 * (r1 x n)) + dot(r2 x n, I2^-1 * (r2 x n))
85
86        let r1_cross_n = cross(r1, normal);
87        let r2_cross_n = cross(r2, normal);
88
89        #[cfg(feature = "2d")]
90        let k = inv_mass_sum + i1 * r1_cross_n * r1_cross_n + i2 * r2_cross_n * r2_cross_n;
91        #[cfg(feature = "3d")]
92        let k = inv_mass_sum + r1_cross_n.dot(i1 * r1_cross_n) + r2_cross_n.dot(i2 * r2_cross_n);
93
94        Self {
95            impulse: warm_start_impulse.unwrap_or_default(),
96            effective_mass: k.recip_or_zero(),
97            softness,
98        }
99    }
100
101    /// Solves the non-penetration constraint, updating the total impulse in `self` and returning
102    /// the incremental impulse to apply to each body.
103    pub fn solve_impulse(
104        &mut self,
105        separation: Scalar,
106        relative_velocity: Vector,
107        normal: Vector,
108        use_bias: bool,
109        max_overlap_solve_speed: Scalar,
110        delta_secs: Scalar,
111    ) -> Scalar {
112        // Compute the relative velocity along the normal.
113        let normal_speed = relative_velocity.dot(normal);
114
115        // Compute the incremental normal impulse.
116        let mut impulse = if separation > 0.0 {
117            // Speculative contact: Push back the part of the velocity that would cause penetration.
118            -self.effective_mass * (normal_speed + separation / delta_secs)
119        } else if use_bias {
120            // Contact using bias: Incorporate softness parameters.
121            //
122            // 1. Velocity bias: Allows the constraint to solve overlap by boosting
123            //    the constraint response a bit, taking into account the current overlap.
124            //    This is known as Baumgarte stabilization.
125            //
126            // 2. Mass coefficient: Scales the effective mass "seen" by the constraint.
127            //
128            // 3. Impulse coefficient: Scales the accumulated impulse that is subtracted
129            //    from the total impulse to prevent the total impulse from becoming too large.
130            //
131            // See the `softness_parameters` module for more details and references.
132
133            // TODO: We might want optional slop, a small amount of allowed penetration.
134            let bias = (self.softness.bias * separation).max(-max_overlap_solve_speed);
135            let scaled_mass = self.softness.mass_scale * self.effective_mass;
136            let scaled_impulse = self.softness.impulse_scale * self.impulse;
137
138            -scaled_mass * (normal_speed + bias) - scaled_impulse
139        } else {
140            // Contact without bias: Solve normally without softness parameters.
141            // This is useful for a "relaxation" phase which helps get rid of overshoot.
142            -self.effective_mass * normal_speed
143        };
144
145        // Clamp the accumulated impulse.
146        let new_impulse = (self.impulse + impulse).max(0.0);
147        impulse = new_impulse - self.impulse;
148        self.impulse = new_impulse;
149
150        // Return the clamped incremental normal impulse.
151        impulse
152    }
153}