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}