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}