1 /********************************************************************************
2 * ReactPhysics3D physics library, http://www.reactphysics3d.com *
3 * Copyright (c) 2010-2020 Daniel Chappuis *
4 *********************************************************************************
5 * *
6 * This software is provided 'as-is', without any express or implied warranty. *
7 * In no event will the authors be held liable for any damages arising from the *
8 * use of this software. *
9 * *
10 * Permission is granted to anyone to use this software for any purpose, *
11 * including commercial applications, and to alter it and redistribute it *
12 * freely, subject to the following restrictions: *
13 * *
14 * 1. The origin of this software must not be misrepresented; you must not claim *
15 * that you wrote the original software. If you use this software in a *
16 * product, an acknowledgment in the product documentation would be *
17 * appreciated but is not required. *
18 * *
19 * 2. Altered source versions must be plainly marked as such, and must not be *
20 * misrepresented as being the original software. *
21 * *
22 * 3. This notice may not be removed or altered from any source distribution. *
23 * *
24 ********************************************************************************/
25
26 #ifndef REACTPHYSICS3D_CONTACT_SOLVER_SYSTEM_H
27 #define REACTPHYSICS3D_CONTACT_SOLVER_SYSTEM_H
28
29 // Libraries
30 #include <reactphysics3d/configuration.h>
31 #include <reactphysics3d/mathematics/Vector3.h>
32 #include <reactphysics3d/mathematics/Matrix3x3.h>
33
34 /// ReactPhysics3D namespace
35 namespace reactphysics3d {
36
37 // Declarations
38 class ContactPoint;
39 class Joint;
40 class ContactManifold;
41 class MemoryManager;
42 class Profiler;
43 class Island;
44 struct Islands;
45 class RigidBody;
46 class Collider;
47 class PhysicsWorld;
48 class CollisionBodyComponents;
49 class DynamicsComponents;
50 class RigidBodyComponents;
51 class ColliderComponents;
52
53 // Class ContactSolverSystem
54 /**
55 * This class represents the contact solver system that is used to solve rigid bodies contacts.
56 * The constraint solver is based on the "Sequential Impulse" technique described by
57 * Erin Catto in his GDC slides (http://code.google.com/p/box2d/downloads/list).
58 *
59 * A constraint between two bodies is represented by a function C(x) which is equal to zero
60 * when the constraint is satisfied. The condition C(x)=0 describes a valid position and the
61 * condition dC(x)/dt=0 describes a valid velocity. We have dC(x)/dt = Jv + b = 0 where J is
62 * the Jacobian matrix of the constraint, v is a vector that contains the velocity of both
63 * bodies and b is the constraint bias. We are looking for a force F_c that will act on the
64 * bodies to keep the constraint satisfied. Note that from the virtual work principle, we have
65 * F_c = J^t * lambda where J^t is the transpose of the Jacobian matrix and lambda is a
66 * Lagrange multiplier. Therefore, finding the force F_c is equivalent to finding the Lagrange
67 * multiplier lambda.
68 *
69 * An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a
70 * body to change its velocity. The idea of the Sequential Impulse technique is to apply
71 * impulses to bodies of each constraints in order to keep the constraint satisfied.
72 *
73 * --- Step 1 ---
74 *
75 * First, we integrate the applied force F_a acting of each rigid body (like gravity, ...) and
76 * we obtain some new velocities v2' that tends to violate the constraints.
77 *
78 * v2' = v1 + dt * M^-1 * F_a
79 *
80 * where M is a matrix that contains mass and inertia tensor information.
81 *
82 * --- Step 2 ---
83 *
84 * During the second step, we iterate over all the constraints for a certain number of
85 * iterations and for each constraint we compute the impulse to apply to the bodies needed
86 * so that the new velocity of the bodies satisfy Jv + b = 0. From the Newton law, we know that
87 * M * deltaV = P_c where M is the mass of the body, deltaV is the difference of velocity and
88 * P_c is the constraint impulse to apply to the body. Therefore, we have
89 * v2 = v2' + M^-1 * P_c. For each constraint, we can compute the Lagrange multiplier lambda
90 * using : lambda = -m_c (Jv2' + b) where m_c = 1 / (J * M^-1 * J^t). Now that we have the
91 * Lagrange multiplier lambda, we can compute the impulse P_c = J^t * lambda * dt to apply to
92 * the bodies to satisfy the constraint.
93 *
94 * --- Step 3 ---
95 *
96 * In the third step, we integrate the new position x2 of the bodies using the new velocities
97 * v2 computed in the second step with : x2 = x1 + dt * v2.
98 *
99 * Note that in the following code (as it is also explained in the slides from Erin Catto),
100 * the value lambda is not only the lagrange multiplier but is the multiplication of the
101 * Lagrange multiplier with the timestep dt. Therefore, in the following code, when we use
102 * lambda, we mean (lambda * dt).
103 *
104 * We are using the accumulated impulse technique that is also described in the slides from
105 * Erin Catto.
106 *
107 * We are also using warm starting. The idea is to warm start the solver at the beginning of
108 * each step by applying the last impulstes for the constraints that we already existing at the
109 * previous step. This allows the iterative solver to converge faster towards the solution.
110 *
111 * For contact constraints, we are also using split impulses so that the position correction
112 * that uses Baumgarte stabilization does not change the momentum of the bodies.
113 *
114 * There are two ways to apply the friction constraints. Either the friction constraints are
115 * applied at each contact point or they are applied only at the center of the contact manifold
116 * between two bodies. If we solve the friction constraints at each contact point, we need
117 * two constraints (two tangential friction directions) and if we solve the friction
118 * constraints at the center of the contact manifold, we need two constraints for tangential
119 * friction but also another twist friction constraint to prevent spin of the body around the
120 * contact manifold center.
121 */
122 class ContactSolverSystem {
123
124 private:
125
126 // Structure ContactPointSolver
127 /**
128 * Contact solver internal data structure that to store all the
129 * information relative to a contact point
130 */
131 struct ContactPointSolver {
132
133 /// Pointer to the external contact
134 ContactPoint* externalContact;
135
136 /// Normal vector of the contact
137 Vector3 normal;
138
139 /// Vector from the body 1 center to the contact point
140 Vector3 r1;
141
142 /// Vector from the body 2 center to the contact point
143 Vector3 r2;
144
145 /// Penetration depth
146 decimal penetrationDepth;
147
148 /// Velocity restitution bias
149 decimal restitutionBias;
150
151 /// Accumulated normal impulse
152 decimal penetrationImpulse;
153
154 /// Accumulated split impulse for penetration correction
155 decimal penetrationSplitImpulse;
156
157 /// Inverse of the matrix K for the penenetration
158 decimal inversePenetrationMass;
159
160 /// Cross product of r1 with the contact normal
161 Vector3 i1TimesR1CrossN;
162
163 /// Cross product of r2 with the contact normal
164 Vector3 i2TimesR2CrossN;
165
166 /// True if the contact was existing last time step
167 bool isRestingContact;
168 };
169
170 // Structure ContactManifoldSolver
171 /**
172 * Contact solver internal data structure to store all the
173 * information relative to a contact manifold.
174 */
175 struct ContactManifoldSolver {
176
177 /// Pointer to the external contact manifold
178 ContactManifold* externalContactManifold;
179
180 /// Index of body 1 in the dynamics components arrays
181 uint32 rigidBodyComponentIndexBody1;
182
183 /// Index of body 2 in the dynamics components arrays
184 uint32 rigidBodyComponentIndexBody2;
185
186 /// Inverse of the mass of body 1
187 decimal massInverseBody1;
188
189 // Inverse of the mass of body 2
190 decimal massInverseBody2;
191
192 /// Inverse inertia tensor of body 1
193 Matrix3x3 inverseInertiaTensorBody1;
194
195 /// Inverse inertia tensor of body 2
196 Matrix3x3 inverseInertiaTensorBody2;
197
198 /// Mix friction coefficient for the two bodies
199 decimal frictionCoefficient;
200
201 /// Rolling resistance factor between the two bodies
202 decimal rollingResistanceFactor;
203
204 // - Variables used when friction constraints are apply at the center of the manifold-//
205
206 /// Average normal vector of the contact manifold
207 Vector3 normal;
208
209 /// Point on body 1 where to apply the friction constraints
210 Vector3 frictionPointBody1;
211
212 /// Point on body 2 where to apply the friction constraints
213 Vector3 frictionPointBody2;
214
215 /// R1 vector for the friction constraints
216 Vector3 r1Friction;
217
218 /// R2 vector for the friction constraints
219 Vector3 r2Friction;
220
221 /// Cross product of r1 with 1st friction vector
222 Vector3 r1CrossT1;
223
224 /// Cross product of r1 with 2nd friction vector
225 Vector3 r1CrossT2;
226
227 /// Cross product of r2 with 1st friction vector
228 Vector3 r2CrossT1;
229
230 /// Cross product of r2 with 2nd friction vector
231 Vector3 r2CrossT2;
232
233 /// Matrix K for the first friction constraint
234 decimal inverseFriction1Mass;
235
236 /// Matrix K for the second friction constraint
237 decimal inverseFriction2Mass;
238
239 /// Matrix K for the twist friction constraint
240 decimal inverseTwistFrictionMass;
241
242 /// Matrix K for the rolling resistance constraint
243 Matrix3x3 inverseRollingResistance;
244
245 /// First friction direction at contact manifold center
246 Vector3 frictionVector1;
247
248 /// Second friction direction at contact manifold center
249 Vector3 frictionVector2;
250
251 /// Old 1st friction direction at contact manifold center
252 Vector3 oldFrictionVector1;
253
254 /// Old 2nd friction direction at contact manifold center
255 Vector3 oldFrictionVector2;
256
257 /// First friction direction impulse at manifold center
258 decimal friction1Impulse;
259
260 /// Second friction direction impulse at manifold center
261 decimal friction2Impulse;
262
263 /// Twist friction impulse at contact manifold center
264 decimal frictionTwistImpulse;
265
266 /// Rolling resistance impulse
267 Vector3 rollingResistanceImpulse;
268
269 /// Number of contact points
270 int8 nbContacts;
271 };
272
273 // -------------------- Constants --------------------- //
274
275 /// Beta value for the penetration depth position correction without split impulses
276 static const decimal BETA;
277
278 /// Beta value for the penetration depth position correction with split impulses
279 static const decimal BETA_SPLIT_IMPULSE;
280
281 /// Slop distance (allowed penetration distance between bodies)
282 static const decimal SLOP;
283
284 // -------------------- Attributes -------------------- //
285
286 /// Memory manager
287 MemoryManager& mMemoryManager;
288
289 /// Physics world
290 PhysicsWorld& mWorld;
291
292 /// Current time step
293 decimal mTimeStep;
294
295 /// Reference to the velocity threshold for contact velocity restitution
296 decimal& mRestitutionVelocityThreshold;
297
298 /// Contact constraints
299 ContactManifoldSolver* mContactConstraints;
300
301 /// Contact points
302 ContactPointSolver* mContactPoints;
303
304 /// Number of contact point constraints
305 uint mNbContactPoints;
306
307 /// Number of contact constraints
308 uint mNbContactManifolds;
309
310 /// Reference to the islands
311 Islands& mIslands;
312
313 /// Pointer to the list of contact manifolds from narrow-phase
314 List<ContactManifold>* mAllContactManifolds;
315
316 /// Pointer to the list of contact points from narrow-phase
317 List<ContactPoint>* mAllContactPoints;
318
319 /// Reference to the body components
320 CollisionBodyComponents& mBodyComponents;
321
322 /// Reference to the dynamics components
323 RigidBodyComponents& mRigidBodyComponents;
324
325 /// Reference to the colliders components
326 ColliderComponents& mColliderComponents;
327
328 /// True if the split impulse position correction is active
329 bool mIsSplitImpulseActive;
330
331 #ifdef IS_RP3D_PROFILING_ENABLED
332
333 /// Pointer to the profiler
334 Profiler* mProfiler;
335
336 #endif
337
338 // -------------------- Methods -------------------- //
339
340 /// Compute the collision restitution factor from the restitution factor of each collider
341 decimal computeMixedRestitutionFactor(Collider* collider1,
342 Collider* collider2) const;
343
344 /// Compute the mixed friction coefficient from the friction coefficient of each collider
345 decimal computeMixedFrictionCoefficient(Collider* collider1, Collider* collider2) const;
346
347 /// Compute th mixed rolling resistance factor between two colliders
348 decimal computeMixedRollingResistance(Collider* collider1, Collider* collider2) const;
349
350 /// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction
351 /// plane for a contact manifold. The two vectors have to be
352 /// such that : t1 x t2 = contactNormal.
353 void computeFrictionVectors(const Vector3& deltaVelocity,
354 ContactManifoldSolver& contactPoint) const;
355
356 /// Warm start the solver.
357 void warmStart();
358
359 public:
360
361 // -------------------- Methods -------------------- //
362
363 /// Constructor
364 ContactSolverSystem(MemoryManager& memoryManager, PhysicsWorld& world, Islands& islands, CollisionBodyComponents& bodyComponents,
365 RigidBodyComponents& rigidBodyComponents, ColliderComponents& colliderComponents, decimal& restitutionVelocityThreshold);
366
367 /// Destructor
368 ~ContactSolverSystem() = default;
369
370 /// Initialize the contact constraints
371 void init(List<ContactManifold>* contactManifolds, List<ContactPoint>* contactPoints, decimal timeStep);
372
373 /// Initialize the constraint solver for a given island
374 void initializeForIsland(uint islandIndex);
375
376 /// Store the computed impulses to use them to
377 /// warm start the solver at the next iteration
378 void storeImpulses();
379
380 /// Solve the contacts
381 void solve();
382
383 /// Release allocated memory
384 void reset();
385
386 /// Return true if the split impulses position correction technique is used for contacts
387 bool isSplitImpulseActive() const;
388
389 /// Activate or Deactivate the split impulses for contacts
390 void setIsSplitImpulseActive(bool isActive);
391
392 #ifdef IS_RP3D_PROFILING_ENABLED
393
394 /// Set the profiler
395 void setProfiler(Profiler* profiler);
396
397 #endif
398 };
399
400 // Return true if the split impulses position correction technique is used for contacts
isSplitImpulseActive()401 inline bool ContactSolverSystem::isSplitImpulseActive() const {
402 return mIsSplitImpulseActive;
403 }
404
405 // Activate or Deactivate the split impulses for contacts
setIsSplitImpulseActive(bool isActive)406 inline void ContactSolverSystem::setIsSplitImpulseActive(bool isActive) {
407 mIsSplitImpulseActive = isActive;
408 }
409
410 #ifdef IS_RP3D_PROFILING_ENABLED
411
412 // Set the profiler
setProfiler(Profiler * profiler)413 inline void ContactSolverSystem::setProfiler(Profiler* profiler) {
414
415 mProfiler = profiler;
416 }
417
418 #endif
419
420 }
421
422 #endif
423