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