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}