avian3d/dynamics/solver/contact/tangent_part.rs
1use crate::prelude::*;
2use bevy::reflect::Reflect;
3
4#[cfg(feature = "2d")]
5pub type TangentImpulse = Scalar;
6#[cfg(feature = "3d")]
7pub type TangentImpulse = Vector2;
8
9// TODO: One-body constraint version
10/// The tangential friction part of a [`ContactConstraintPoint`](super::ContactConstraintPoint).
11#[derive(Clone, Debug, Default, PartialEq, Reflect)]
12pub struct ContactTangentPart {
13 /// The contact impulse magnitude along the contact tangent.
14 ///
15 /// This corresponds to the magnitude of the friction impulse.
16 pub impulse: TangentImpulse,
17
18 /// The inertial properties of the bodies projected onto the contact tangent,
19 /// or in other words, the mass "seen" by the constraint along the tangent.
20 #[cfg(feature = "2d")]
21 pub effective_mass: Scalar,
22 /// The inverse of the inertial properties of the bodies projected onto the contact tangents,
23 /// or in other words, the inverse mass "seen" by the constraint along the tangents.
24 #[cfg(feature = "3d")]
25 pub effective_inverse_mass: [Scalar; 3],
26}
27
28impl ContactTangentPart {
29 /// Generates a new [`ContactTangentPart`].
30 #[allow(clippy::too_many_arguments)]
31 pub fn generate(
32 inverse_mass_sum: Scalar,
33 angular_inertia1: impl Into<ComputedAngularInertia>,
34 angular_inertia2: impl Into<ComputedAngularInertia>,
35 r1: Vector,
36 r2: Vector,
37 tangents: [Vector; DIM - 1],
38 warm_start_impulse: Option<TangentImpulse>,
39 ) -> Self {
40 let angular_inertia1: ComputedAngularInertia = angular_inertia1.into();
41 let angular_inertia2: ComputedAngularInertia = angular_inertia2.into();
42 let i1 = angular_inertia1.inverse();
43 let i2 = angular_inertia2.inverse();
44
45 let mut part = Self {
46 impulse: warm_start_impulse.unwrap_or_default(),
47 #[cfg(feature = "2d")]
48 effective_mass: 0.0,
49 #[cfg(feature = "3d")]
50 effective_inverse_mass: [0.0; 3],
51 };
52
53 // Derivation for the projected tangent masses. This is for 3D, but the 2D version is largely the same.
54 //
55 // Friction constraints aim to prevent relative tangential motion at contact points.
56 // The velocity constraint is satisfied when the relative velocity along the tangent
57 // is equal to zero.
58 //
59 // In 3D, there are two tangent directions and therefore two constraints:
60 //
61 // dot(lin_vel1_p, tangent_x) = dot(lin_vel2_p, tangent_x)
62 // dot(lin_vel1_p, tangent_y) = dot(lin_vel2_p, tangent_y)
63 //
64 // where lin_vel1_p and lin_vel2_p are the velocities of the bodies at the contact point p:
65 //
66 // lin_vel1_p = lin_vel1 + ang_vel1 x r1
67 // lin_vel2_p = lin_vel2 + ang_vel2 x r2
68 //
69 // Based on this, we get:
70 //
71 // dot(lin_vel1_p, tangent_x) = dot(lin_vel1, tangent_x) + dot(ang_vel1 x r1, tangent_x)
72 // = dot(lin_vel1, tangent_x) + dot(r1 x tangent_x, ang_vel1)
73 //
74 // Restating the original constraints with the derived formula:
75 //
76 // dot(lin_vel1, tangent_x) + dot(r1 x tangent_x, ang_vel1) = dot(lin_vel2, tangent_x) + dot(r2 x tangent_x, ang_vel2)
77 // dot(lin_vel1, tangent_y) + dot(r1 x tangent_y, ang_vel1) = dot(lin_vel2, tangent_y) + dot(r2 x tangent_y, ang_vel2)
78 //
79 // Finally, moving the right-hand side to the left:
80 //
81 // dot(lin_vel1, tangent_x) + dot(r1 x tangent_x, ang_vel1) - dot(lin_vel2, tangent_x) - dot(r2 x tangent_x, ang_vel2) = 0
82 // dot(lin_vel1, tangent_y) + dot(r1 x tangent_y, ang_vel1) - dot(lin_vel2, tangent_y) - dot(r2 x tangent_y, ang_vel2) = 0
83 //
84 // By inspection, we can see that the Jacobian is the following:
85 //
86 // linear1 angular1 linear2 angular2
87 // J_x = [ -tangent_x, -(r1 x tangent_x), tangent_x, r2 x tangent_x ]
88 // J_y = [ -tangent_y, -(r1 x tangent_y), tangent_y, r2 x tangent_y ]
89 //
90 // From this, we can derive the effective inverse mass for both tangent directions:
91 //
92 // K_x = J_x * M^-1 * J_x^T
93 // = m1 + m2 + (r1 x tangent_x)^T * I1 * (r1 x tangent_x) + (r2 x tangent_x)^T * I2 * (r2 x tangent_x)
94 // K_y = J_y * M^-1 * J_y^T
95 // = m1 + m2 + (r1 x tangent_y)^T * I1 * (r1 x tangent_y) + (r2 x tangent_y)^T * I2 * (r2 x tangent_y)
96 //
97 // See "Constraints Derivation for Rigid Body Simulation in 3D" section 2.1.3
98 // by Daniel Chappuis for the full derivation of the effective inverse mass.
99 //
100 // Finally, the transposes can be simplified with dot products, because a^T * b = dot(a, b),
101 // where a and b are two column vectors.
102 //
103 // K_x = m1 + m2 + dot(r1 x tangent_x, I1 * (r1 x tangent_x)) + dot(r2 x tangent_x, I2 * (r2 x tangent_x))
104 // K_y = m1 + m2 + dot(r1 x tangent_y, I1 * (r1 x tangent_y)) + dot(r2 x tangent_y, I2 * (r2 x tangent_y))
105
106 #[cfg(feature = "2d")]
107 {
108 let rt1 = cross(r1, tangents[0]);
109 let rt2 = cross(r2, tangents[0]);
110
111 let k = inverse_mass_sum + i1 * rt1 * rt1 + i2 * rt2 * rt2;
112
113 part.effective_mass = k.recip_or_zero();
114 }
115
116 #[cfg(feature = "3d")]
117 {
118 // Based on Rapier's two-body constraint.
119 // https://github.com/dimforge/rapier/blob/af1ac9baa26b1199ae2728e91adf5345bcd1c693/src/dynamics/solver/contact_constraint/two_body_constraint.rs#L257-L289
120
121 let rt11 = cross(r1, tangents[0]);
122 let rt12 = cross(r2, tangents[0]);
123 let rt21 = cross(r1, tangents[1]);
124 let rt22 = cross(r2, tangents[1]);
125
126 // Multiply by the inverse inertia early to reuse the values.
127 let i1_rt11 = i1 * rt11;
128 let i2_rt12 = i2 * rt12;
129 let i1_rt21 = i1 * rt21;
130 let i2_rt22 = i2 * rt22;
131
132 let k1 = inverse_mass_sum + rt11.dot(i1_rt11) + rt12.dot(i2_rt12);
133 let k2 = inverse_mass_sum + rt21.dot(i1_rt21) + rt22.dot(i2_rt22);
134
135 // Note: The invertion is done in `solve_impulse`, unlike in 2D.
136 part.effective_inverse_mass[0] = k1;
137 part.effective_inverse_mass[1] = k2;
138
139 // This is needed for solving the two tangent directions simultaneously.
140 // TODO. Derive and explain the math for this, or consider an alternative approach,
141 // like using the Jacobians to compute the actual effective mass matrix.
142 part.effective_inverse_mass[2] = 2.0 * (rt11.dot(i1_rt21) + rt12.dot(i2_rt22));
143 }
144
145 part
146 }
147
148 /// Solves the friction constraint, updating the total impulse in `self` and returning
149 /// the incremental impulse to apply to each body.
150 pub fn solve_impulse(
151 &mut self,
152 tangent_directions: [Vector; DIM - 1],
153 relative_velocity: Vector,
154 // The desired relative velocity along the contact surface, used to simulate things like conveyor belts.
155 #[cfg(feature = "2d")] surface_speed: Scalar,
156 #[cfg(feature = "3d")] surface_velocity: Vector,
157 friction: Scalar,
158 normal_impulse: Scalar,
159 ) -> Vector {
160 // Compute the maximum bound for the friction impulse.
161 //
162 // According to the Coulomb friction law:
163 //
164 // length(friction_force) <= coefficient * length(normal_force)
165 //
166 // Now, we need to find the Lagrange multiplier, which corresponds
167 // to the constraint force magnitude.
168 //
169 // F_c = J^T * lagrange, where J is the Jacobian, which in this case is of unit length.
170 //
171 // We get the following:
172 //
173 // length(J^T * force_magnitude) <= coefficient * length(normal_force)
174 // <=> abs(force_magnitude) <= coefficient * length(normal_force)
175 // <=> -coefficient * length(normal_force) <= force_magnitude <= coefficient * length(normal_force)
176 //
177 // We are dealing with impulses instead of forces. Multiplying by delta time,
178 // we get the minimum and maximum bound for the friction impulse:
179 //
180 // -coefficient * length(normal_impulse) <= impulse_magnitude <= coefficient * length(normal_impulse)
181
182 let impulse_limit = friction * normal_impulse;
183
184 #[cfg(feature = "2d")]
185 {
186 // Compute the relative velocity along the tangent.
187 // Add the relative speed along the surface to the total tangent speed.
188 let tangent = tangent_directions[0];
189 let tangent_speed = relative_velocity.dot(tangent) + surface_speed;
190
191 // Compute the incremental tangent impoulse magnitude.
192 let mut impulse = self.effective_mass * (-tangent_speed);
193 // Clamp the accumulated impulse.
194 let new_impulse = (self.impulse + impulse).clamp(-impulse_limit, impulse_limit);
195 impulse = new_impulse - self.impulse;
196 self.impulse = new_impulse;
197 // Return the incremental friction impulse.
198 impulse * tangent
199 }
200 #[cfg(feature = "3d")]
201 {
202 // Compute the relative velocity along the tangents.
203 // Add the relative velocity along the surface to the total tangent speed.
204 let relative_velocity = relative_velocity + surface_velocity;
205 let tangent_speed1 = relative_velocity.dot(tangent_directions[0]);
206 let tangent_speed2 = relative_velocity.dot(tangent_directions[1]);
207
208 // Solve the two tangent directions simultaneously.
209 // Based on Rapier's two-body constraint.
210 // https://github.com/dimforge/rapier/blob/af1ac9baa26b1199ae2728e91adf5345bcd1c693/src/dynamics/solver/contact_constraint/two_body_constraint_element.rs#L127-L133
211 let t11 = tangent_speed1.powi(2);
212 let t22 = tangent_speed2.powi(2);
213 let t12 = tangent_speed1 * tangent_speed2;
214 let inv = t11 * self.effective_inverse_mass[0]
215 + t22 * self.effective_inverse_mass[1]
216 + t12 * self.effective_inverse_mass[2];
217
218 // Compute the effective mass "seen" by the constraint along the tangent.
219 // Note the guard against division by zero.
220 let effective_mass = (t11 + t22) * inv.max(1e-16).recip();
221
222 // Compute the incremental tangent impoulse.
223 let delta_impulse = effective_mass * Vector2::new(tangent_speed1, tangent_speed2);
224
225 // Clamp the accumulated impulse.
226 let new_impulse = (self.impulse - delta_impulse).clamp_length_max(impulse_limit);
227 let impulse = new_impulse - self.impulse;
228
229 if !impulse.is_finite() {
230 return Vector::ZERO;
231 }
232
233 self.impulse = new_impulse;
234
235 // Return the clamped incremental friction impulse.
236 impulse.x * tangent_directions[0] + impulse.y * tangent_directions[1]
237 }
238 }
239}