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 // Libraries
27 #include <reactphysics3d/systems/ContactSolverSystem.h>
28 #include <reactphysics3d/engine/PhysicsWorld.h>
29 #include <reactphysics3d/body/RigidBody.h>
30 #include <reactphysics3d/constraint/ContactPoint.h>
31 #include <reactphysics3d/utils/Profiler.h>
32 #include <reactphysics3d/engine/Island.h>
33 #include <reactphysics3d/collision/Collider.h>
34 #include <reactphysics3d/components/CollisionBodyComponents.h>
35 #include <reactphysics3d/components/ColliderComponents.h>
36 #include <reactphysics3d/collision/ContactManifold.h>
37
38 using namespace reactphysics3d;
39 using namespace std;
40
41 // Constants initialization
42 const decimal ContactSolverSystem::BETA = decimal(0.2);
43 const decimal ContactSolverSystem::BETA_SPLIT_IMPULSE = decimal(0.2);
44 const decimal ContactSolverSystem::SLOP = decimal(0.01);
45
46 // Constructor
ContactSolverSystem(MemoryManager & memoryManager,PhysicsWorld & world,Islands & islands,CollisionBodyComponents & bodyComponents,RigidBodyComponents & rigidBodyComponents,ColliderComponents & colliderComponents,decimal & restitutionVelocityThreshold)47 ContactSolverSystem::ContactSolverSystem(MemoryManager& memoryManager, PhysicsWorld& world, Islands& islands,
48 CollisionBodyComponents& bodyComponents, RigidBodyComponents& rigidBodyComponents,
49 ColliderComponents& colliderComponents, decimal& restitutionVelocityThreshold)
50 :mMemoryManager(memoryManager), mWorld(world), mRestitutionVelocityThreshold(restitutionVelocityThreshold),
51 mContactConstraints(nullptr), mContactPoints(nullptr),
52 mIslands(islands), mAllContactManifolds(nullptr), mAllContactPoints(nullptr),
53 mBodyComponents(bodyComponents), mRigidBodyComponents(rigidBodyComponents),
54 mColliderComponents(colliderComponents), mIsSplitImpulseActive(true) {
55
56 #ifdef IS_RP3D_PROFILING_ENABLED
57
58 mProfiler = nullptr;
59 #endif
60
61 }
62
63 // Initialize the contact constraints
init(List<ContactManifold> * contactManifolds,List<ContactPoint> * contactPoints,decimal timeStep)64 void ContactSolverSystem::init(List<ContactManifold>* contactManifolds, List<ContactPoint>* contactPoints, decimal timeStep) {
65
66 mAllContactManifolds = contactManifolds;
67 mAllContactPoints = contactPoints;
68
69 RP3D_PROFILE("ContactSolver::init()", mProfiler);
70
71 mTimeStep = timeStep;
72
73 uint nbContactManifolds = mAllContactManifolds->size();
74 uint nbContactPoints = mAllContactPoints->size();
75
76 mNbContactManifolds = 0;
77 mNbContactPoints = 0;
78
79 mContactConstraints = nullptr;
80 mContactPoints = nullptr;
81
82 if (nbContactManifolds == 0 || nbContactPoints == 0) return;
83
84 mContactPoints = static_cast<ContactPointSolver*>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
85 sizeof(ContactPointSolver) * nbContactPoints));
86 assert(mContactPoints != nullptr);
87
88 mContactConstraints = static_cast<ContactManifoldSolver*>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
89 sizeof(ContactManifoldSolver) * nbContactManifolds));
90 assert(mContactConstraints != nullptr);
91
92 // For each island of the world
93 for (uint i = 0; i < mIslands.getNbIslands(); i++) {
94
95 if (mIslands.nbContactManifolds[i] > 0) {
96 initializeForIsland(i);
97 }
98 }
99
100 // Warmstarting
101 warmStart();
102 }
103
104 // Release allocated memory
reset()105 void ContactSolverSystem::reset() {
106
107 if (mAllContactPoints->size() > 0) mMemoryManager.release(MemoryManager::AllocationType::Frame, mContactPoints, sizeof(ContactPointSolver) * mAllContactPoints->size());
108 if (mAllContactManifolds->size() > 0) mMemoryManager.release(MemoryManager::AllocationType::Frame, mContactConstraints, sizeof(ContactManifoldSolver) * mAllContactManifolds->size());
109 }
110
111 // Initialize the constraint solver for a given island
initializeForIsland(uint islandIndex)112 void ContactSolverSystem::initializeForIsland(uint islandIndex) {
113
114 RP3D_PROFILE("ContactSolver::initializeForIsland()", mProfiler);
115
116 assert(mIslands.bodyEntities[islandIndex].size() > 0);
117 assert(mIslands.nbContactManifolds[islandIndex] > 0);
118
119 // For each contact manifold of the island
120 uint contactManifoldsIndex = mIslands.contactManifoldsIndices[islandIndex];
121 uint nbContactManifolds = mIslands.nbContactManifolds[islandIndex];
122 for (uint m=contactManifoldsIndex; m < contactManifoldsIndex + nbContactManifolds; m++) {
123
124 ContactManifold& externalManifold = (*mAllContactManifolds)[m];
125
126 assert(externalManifold.nbContactPoints > 0);
127
128 // Get the two bodies of the contact
129 RigidBody* body1 = static_cast<RigidBody*>(mBodyComponents.getBody(externalManifold.bodyEntity1));
130 RigidBody* body2 = static_cast<RigidBody*>(mBodyComponents.getBody(externalManifold.bodyEntity2));
131 assert(body1 != nullptr);
132 assert(body2 != nullptr);
133 assert(!mBodyComponents.getIsEntityDisabled(externalManifold.bodyEntity1));
134 assert(!mBodyComponents.getIsEntityDisabled(externalManifold.bodyEntity2));
135
136 const uint rigidBodyIndex1 = mRigidBodyComponents.getEntityIndex(externalManifold.bodyEntity1);
137 const uint rigidBodyIndex2 = mRigidBodyComponents.getEntityIndex(externalManifold.bodyEntity2);
138
139 Collider* collider1 = mColliderComponents.getCollider(externalManifold.colliderEntity1);
140 Collider* collider2 = mColliderComponents.getCollider(externalManifold.colliderEntity2);
141
142 // Get the position of the two bodies
143 const Vector3& x1 = mRigidBodyComponents.mCentersOfMassWorld[rigidBodyIndex1];
144 const Vector3& x2 = mRigidBodyComponents.mCentersOfMassWorld[rigidBodyIndex2];
145
146 // Initialize the internal contact manifold structure using the external contact manifold
147 new (mContactConstraints + mNbContactManifolds) ContactManifoldSolver();
148 mContactConstraints[mNbContactManifolds].rigidBodyComponentIndexBody1 = rigidBodyIndex1;
149 mContactConstraints[mNbContactManifolds].rigidBodyComponentIndexBody2 = rigidBodyIndex2;
150 mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = RigidBody::getWorldInertiaTensorInverse(mWorld, externalManifold.bodyEntity1);
151 mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = RigidBody::getWorldInertiaTensorInverse(mWorld, externalManifold.bodyEntity2);
152 mContactConstraints[mNbContactManifolds].massInverseBody1 = mRigidBodyComponents.mInverseMasses[rigidBodyIndex1];
153 mContactConstraints[mNbContactManifolds].massInverseBody2 = mRigidBodyComponents.mInverseMasses[rigidBodyIndex2];
154 mContactConstraints[mNbContactManifolds].nbContacts = externalManifold.nbContactPoints;
155 mContactConstraints[mNbContactManifolds].frictionCoefficient = computeMixedFrictionCoefficient(collider1, collider2);
156 mContactConstraints[mNbContactManifolds].rollingResistanceFactor = computeMixedRollingResistance(collider1, collider2);
157 mContactConstraints[mNbContactManifolds].externalContactManifold = &externalManifold;
158 mContactConstraints[mNbContactManifolds].normal.setToZero();
159 mContactConstraints[mNbContactManifolds].frictionPointBody1.setToZero();
160 mContactConstraints[mNbContactManifolds].frictionPointBody2.setToZero();
161
162 // Get the velocities of the bodies
163 const Vector3& v1 = mRigidBodyComponents.mLinearVelocities[rigidBodyIndex1];
164 const Vector3& w1 = mRigidBodyComponents.mAngularVelocities[rigidBodyIndex1];
165 const Vector3& v2 = mRigidBodyComponents.mLinearVelocities[rigidBodyIndex2];
166 const Vector3& w2 = mRigidBodyComponents.mAngularVelocities[rigidBodyIndex2];
167
168 // For each contact point of the contact manifold
169 assert(externalManifold.nbContactPoints > 0);
170 uint contactPointsStartIndex = externalManifold.contactPointsIndex;
171 uint nbContactPoints = static_cast<uint>(externalManifold.nbContactPoints);
172 for (uint c=contactPointsStartIndex; c < contactPointsStartIndex + nbContactPoints; c++) {
173
174 ContactPoint& externalContact = (*mAllContactPoints)[c];
175
176 // Get the contact point on the two bodies
177 Vector3 p1 = mColliderComponents.getLocalToWorldTransform(externalManifold.colliderEntity1) * externalContact.getLocalPointOnShape1();
178 Vector3 p2 = mColliderComponents.getLocalToWorldTransform(externalManifold.colliderEntity2) * externalContact.getLocalPointOnShape2();
179
180 new (mContactPoints + mNbContactPoints) ContactPointSolver();
181 mContactPoints[mNbContactPoints].externalContact = &externalContact;
182 mContactPoints[mNbContactPoints].normal = externalContact.getNormal();
183 mContactPoints[mNbContactPoints].r1.x = p1.x - x1.x;
184 mContactPoints[mNbContactPoints].r1.y = p1.y - x1.y;
185 mContactPoints[mNbContactPoints].r1.z = p1.z - x1.z;
186 mContactPoints[mNbContactPoints].r2.x = p2.x - x2.x;
187 mContactPoints[mNbContactPoints].r2.y = p2.y - x2.y;
188 mContactPoints[mNbContactPoints].r2.z = p2.z - x2.z;
189 mContactPoints[mNbContactPoints].penetrationDepth = externalContact.getPenetrationDepth();
190 mContactPoints[mNbContactPoints].isRestingContact = externalContact.getIsRestingContact();
191 externalContact.setIsRestingContact(true);
192 mContactPoints[mNbContactPoints].penetrationImpulse = externalContact.getPenetrationImpulse();
193 mContactPoints[mNbContactPoints].penetrationSplitImpulse = 0.0;
194
195 mContactConstraints[mNbContactManifolds].frictionPointBody1.x += p1.x;
196 mContactConstraints[mNbContactManifolds].frictionPointBody1.y += p1.y;
197 mContactConstraints[mNbContactManifolds].frictionPointBody1.z += p1.z;
198 mContactConstraints[mNbContactManifolds].frictionPointBody2.x += p2.x;
199 mContactConstraints[mNbContactManifolds].frictionPointBody2.y += p2.y;
200 mContactConstraints[mNbContactManifolds].frictionPointBody2.z += p2.z;
201
202 // Compute the velocity difference
203 //deltaV = v2 + w2.cross(mContactPoints[mNbContactPoints].r2) - v1 - w1.cross(mContactPoints[mNbContactPoints].r1);
204 Vector3 deltaV(v2.x + w2.y * mContactPoints[mNbContactPoints].r2.z - w2.z * mContactPoints[mNbContactPoints].r2.y
205 - v1.x - w1.y * mContactPoints[mNbContactPoints].r1.z - w1.z * mContactPoints[mNbContactPoints].r1.y,
206 v2.y + w2.z * mContactPoints[mNbContactPoints].r2.x - w2.x * mContactPoints[mNbContactPoints].r2.z
207 - v1.y - w1.z * mContactPoints[mNbContactPoints].r1.x - w1.x * mContactPoints[mNbContactPoints].r1.z,
208 v2.z + w2.x * mContactPoints[mNbContactPoints].r2.y - w2.y * mContactPoints[mNbContactPoints].r2.x
209 - v1.z - w1.x * mContactPoints[mNbContactPoints].r1.y - w1.y * mContactPoints[mNbContactPoints].r1.x);
210
211 // r1CrossN = mContactPoints[mNbContactPoints].r1.cross(mContactPoints[mNbContactPoints].normal);
212 Vector3 r1CrossN(mContactPoints[mNbContactPoints].r1.y * mContactPoints[mNbContactPoints].normal.z -
213 mContactPoints[mNbContactPoints].r1.z * mContactPoints[mNbContactPoints].normal.y,
214 mContactPoints[mNbContactPoints].r1.z * mContactPoints[mNbContactPoints].normal.x -
215 mContactPoints[mNbContactPoints].r1.x * mContactPoints[mNbContactPoints].normal.z,
216 mContactPoints[mNbContactPoints].r1.x * mContactPoints[mNbContactPoints].normal.y -
217 mContactPoints[mNbContactPoints].r1.y * mContactPoints[mNbContactPoints].normal.x);
218 // r2CrossN = mContactPoints[mNbContactPoints].r2.cross(mContactPoints[mNbContactPoints].normal);
219 Vector3 r2CrossN(mContactPoints[mNbContactPoints].r2.y * mContactPoints[mNbContactPoints].normal.z -
220 mContactPoints[mNbContactPoints].r2.z * mContactPoints[mNbContactPoints].normal.y,
221 mContactPoints[mNbContactPoints].r2.z * mContactPoints[mNbContactPoints].normal.x -
222 mContactPoints[mNbContactPoints].r2.x * mContactPoints[mNbContactPoints].normal.z,
223 mContactPoints[mNbContactPoints].r2.x * mContactPoints[mNbContactPoints].normal.y -
224 mContactPoints[mNbContactPoints].r2.y * mContactPoints[mNbContactPoints].normal.x);
225
226 mContactPoints[mNbContactPoints].i1TimesR1CrossN = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * r1CrossN;
227 mContactPoints[mNbContactPoints].i2TimesR2CrossN = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * r2CrossN;
228
229 // Compute the inverse mass matrix K for the penetration constraint
230 decimal massPenetration = mContactConstraints[mNbContactManifolds].massInverseBody1 + mContactConstraints[mNbContactManifolds].massInverseBody2 +
231 ((mContactPoints[mNbContactPoints].i1TimesR1CrossN).cross(mContactPoints[mNbContactPoints].r1)).dot(mContactPoints[mNbContactPoints].normal) +
232 ((mContactPoints[mNbContactPoints].i2TimesR2CrossN).cross(mContactPoints[mNbContactPoints].r2)).dot(mContactPoints[mNbContactPoints].normal);
233 mContactPoints[mNbContactPoints].inversePenetrationMass = massPenetration > decimal(0.0) ? decimal(1.0) / massPenetration : decimal(0.0);
234
235 // Compute the restitution velocity bias "b". We compute this here instead
236 // of inside the solve() method because we need to use the velocity difference
237 // at the beginning of the contact. Note that if it is a resting contact (normal
238 // velocity bellow a given threshold), we do not add a restitution velocity bias
239 mContactPoints[mNbContactPoints].restitutionBias = 0.0;
240 // deltaVDotN = deltaV.dot(mContactPoints[mNbContactPoints].normal);
241 decimal deltaVDotN = deltaV.x * mContactPoints[mNbContactPoints].normal.x +
242 deltaV.y * mContactPoints[mNbContactPoints].normal.y +
243 deltaV.z * mContactPoints[mNbContactPoints].normal.z;
244 const decimal restitutionFactor = computeMixedRestitutionFactor(collider1, collider2);
245 if (deltaVDotN < -mRestitutionVelocityThreshold) {
246 mContactPoints[mNbContactPoints].restitutionBias = restitutionFactor * deltaVDotN;
247 }
248
249 mContactConstraints[mNbContactManifolds].normal.x += mContactPoints[mNbContactPoints].normal.x;
250 mContactConstraints[mNbContactManifolds].normal.y += mContactPoints[mNbContactPoints].normal.y;
251 mContactConstraints[mNbContactManifolds].normal.z += mContactPoints[mNbContactPoints].normal.z;
252
253 mNbContactPoints++;
254 }
255
256 mContactConstraints[mNbContactManifolds].frictionPointBody1 /=static_cast<decimal>(mContactConstraints[mNbContactManifolds].nbContacts);
257 mContactConstraints[mNbContactManifolds].frictionPointBody2 /=static_cast<decimal>(mContactConstraints[mNbContactManifolds].nbContacts);
258 mContactConstraints[mNbContactManifolds].r1Friction.x = mContactConstraints[mNbContactManifolds].frictionPointBody1.x - x1.x;
259 mContactConstraints[mNbContactManifolds].r1Friction.y = mContactConstraints[mNbContactManifolds].frictionPointBody1.y - x1.y;
260 mContactConstraints[mNbContactManifolds].r1Friction.z = mContactConstraints[mNbContactManifolds].frictionPointBody1.z - x1.z;
261 mContactConstraints[mNbContactManifolds].r2Friction.x = mContactConstraints[mNbContactManifolds].frictionPointBody2.x - x2.x;
262 mContactConstraints[mNbContactManifolds].r2Friction.y = mContactConstraints[mNbContactManifolds].frictionPointBody2.y - x2.y;
263 mContactConstraints[mNbContactManifolds].r2Friction.z = mContactConstraints[mNbContactManifolds].frictionPointBody2.z - x2.z;
264 mContactConstraints[mNbContactManifolds].oldFrictionVector1 = externalManifold.frictionVector1;
265 mContactConstraints[mNbContactManifolds].oldFrictionVector2 = externalManifold.frictionVector2;
266
267 // Initialize the accumulated impulses with the previous step accumulated impulses
268 mContactConstraints[mNbContactManifolds].friction1Impulse = externalManifold.frictionImpulse1;
269 mContactConstraints[mNbContactManifolds].friction2Impulse = externalManifold.frictionImpulse2;
270 mContactConstraints[mNbContactManifolds].frictionTwistImpulse = externalManifold.frictionTwistImpulse;
271
272 // Compute the inverse K matrix for the rolling resistance constraint
273 bool isBody1DynamicType = body1->getType() == BodyType::DYNAMIC;
274 bool isBody2DynamicType = body2->getType() == BodyType::DYNAMIC;
275 mContactConstraints[mNbContactManifolds].inverseRollingResistance.setToZero();
276 if (mContactConstraints[mNbContactManifolds].rollingResistanceFactor > 0 && (isBody1DynamicType || isBody2DynamicType)) {
277
278 mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 + mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2;
279 decimal det = mContactConstraints[mNbContactManifolds].inverseRollingResistance.getDeterminant();
280
281 // If the matrix is not inversible
282 if (approxEqual(det, decimal(0.0))) {
283 mContactConstraints[mNbContactManifolds].inverseRollingResistance.setToZero();
284 }
285 else {
286 mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseRollingResistance.getInverse();
287 }
288 }
289
290 mContactConstraints[mNbContactManifolds].normal.normalize();
291
292 // deltaVFrictionPoint = v2 + w2.cross(mContactConstraints[mNbContactManifolds].r2Friction) -
293 // v1 - w1.cross(mContactConstraints[mNbContactManifolds].r1Friction);
294 Vector3 deltaVFrictionPoint(v2.x + w2.y * mContactConstraints[mNbContactManifolds].r2Friction.z -
295 w2.z * mContactConstraints[mNbContactManifolds].r2Friction.y -
296 v1.x - w1.y * mContactConstraints[mNbContactManifolds].r1Friction.z -
297 w1.z * mContactConstraints[mNbContactManifolds].r1Friction.y,
298 v2.y + w2.z * mContactConstraints[mNbContactManifolds].r2Friction.x -
299 w2.x * mContactConstraints[mNbContactManifolds].r2Friction.z -
300 v1.y - w1.z * mContactConstraints[mNbContactManifolds].r1Friction.x -
301 w1.x * mContactConstraints[mNbContactManifolds].r1Friction.z,
302 v2.z + w2.x * mContactConstraints[mNbContactManifolds].r2Friction.y -
303 w2.y * mContactConstraints[mNbContactManifolds].r2Friction.x -
304 v1.z - w1.x * mContactConstraints[mNbContactManifolds].r1Friction.y -
305 w1.y * mContactConstraints[mNbContactManifolds].r1Friction.x);
306
307 // Compute the friction vectors
308 computeFrictionVectors(deltaVFrictionPoint, mContactConstraints[mNbContactManifolds]);
309
310 // Compute the inverse mass matrix K for the friction constraints at the center of
311 // the contact manifold
312 mContactConstraints[mNbContactManifolds].r1CrossT1 = mContactConstraints[mNbContactManifolds].r1Friction.cross(mContactConstraints[mNbContactManifolds].frictionVector1);
313 mContactConstraints[mNbContactManifolds].r1CrossT2 = mContactConstraints[mNbContactManifolds].r1Friction.cross(mContactConstraints[mNbContactManifolds].frictionVector2);
314 mContactConstraints[mNbContactManifolds].r2CrossT1 = mContactConstraints[mNbContactManifolds].r2Friction.cross(mContactConstraints[mNbContactManifolds].frictionVector1);
315 mContactConstraints[mNbContactManifolds].r2CrossT2 = mContactConstraints[mNbContactManifolds].r2Friction.cross(mContactConstraints[mNbContactManifolds].frictionVector2);
316 decimal friction1Mass = mContactConstraints[mNbContactManifolds].massInverseBody1 + mContactConstraints[mNbContactManifolds].massInverseBody2 +
317 ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * mContactConstraints[mNbContactManifolds].r1CrossT1).cross(mContactConstraints[mNbContactManifolds].r1Friction)).dot(
318 mContactConstraints[mNbContactManifolds].frictionVector1) +
319 ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * mContactConstraints[mNbContactManifolds].r2CrossT1).cross(mContactConstraints[mNbContactManifolds].r2Friction)).dot(
320 mContactConstraints[mNbContactManifolds].frictionVector1);
321 decimal friction2Mass = mContactConstraints[mNbContactManifolds].massInverseBody1 + mContactConstraints[mNbContactManifolds].massInverseBody2 +
322 ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * mContactConstraints[mNbContactManifolds].r1CrossT2).cross(mContactConstraints[mNbContactManifolds].r1Friction)).dot(
323 mContactConstraints[mNbContactManifolds].frictionVector2) +
324 ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * mContactConstraints[mNbContactManifolds].r2CrossT2).cross(mContactConstraints[mNbContactManifolds].r2Friction)).dot(
325 mContactConstraints[mNbContactManifolds].frictionVector2);
326 decimal frictionTwistMass = mContactConstraints[mNbContactManifolds].normal.dot(mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 *
327 mContactConstraints[mNbContactManifolds].normal) +
328 mContactConstraints[mNbContactManifolds].normal.dot(mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 *
329 mContactConstraints[mNbContactManifolds].normal);
330 mContactConstraints[mNbContactManifolds].inverseFriction1Mass = friction1Mass > decimal(0.0) ? decimal(1.0) / friction1Mass : decimal(0.0);
331 mContactConstraints[mNbContactManifolds].inverseFriction2Mass = friction2Mass > decimal(0.0) ? decimal(1.0) / friction2Mass : decimal(0.0);
332 mContactConstraints[mNbContactManifolds].inverseTwistFrictionMass = frictionTwistMass > decimal(0.0) ? decimal(1.0) / frictionTwistMass : decimal(0.0);
333
334 mNbContactManifolds++;
335 }
336 }
337
338 // Warm start the solver.
339 /// For each constraint, we apply the previous impulse (from the previous step)
340 /// at the beginning. With this technique, we will converge faster towards
341 /// the solution of the linear system
warmStart()342 void ContactSolverSystem::warmStart() {
343
344 RP3D_PROFILE("ContactSolver::warmStart()", mProfiler);
345
346 uint contactPointIndex = 0;
347
348 // For each constraint
349 for (uint c=0; c<mNbContactManifolds; c++) {
350
351 bool atLeastOneRestingContactPoint = false;
352
353 for (short int i=0; i<mContactConstraints[c].nbContacts; i++) {
354
355
356 // If it is not a new contact (this contact was already existing at last time step)
357 if (mContactPoints[contactPointIndex].isRestingContact) {
358
359 atLeastOneRestingContactPoint = true;
360
361 // --------- Penetration --------- //
362
363 // Update the velocities of the body 1 by applying the impulse P
364 Vector3 impulsePenetration(mContactPoints[contactPointIndex].normal.x * mContactPoints[contactPointIndex].penetrationImpulse,
365 mContactPoints[contactPointIndex].normal.y * mContactPoints[contactPointIndex].penetrationImpulse,
366 mContactPoints[contactPointIndex].normal.z * mContactPoints[contactPointIndex].penetrationImpulse);
367 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * impulsePenetration.x;
368 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * impulsePenetration.y;
369 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * impulsePenetration.z;
370
371 mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse;
372 mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse;
373 mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse;
374
375 // Update the velocities of the body 2 by applying the impulse P
376 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * impulsePenetration.x;
377 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * impulsePenetration.y;
378 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * impulsePenetration.z;
379
380 mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse;
381 mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse;
382 mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse;
383 }
384 else { // If it is a new contact point
385
386 // Initialize the accumulated impulses to zero
387 mContactPoints[contactPointIndex].penetrationImpulse = 0.0;
388 }
389
390 contactPointIndex++;
391 }
392
393 // If we solve the friction constraints at the center of the contact manifold and there is
394 // at least one resting contact point in the contact manifold
395 if (atLeastOneRestingContactPoint) {
396
397 // Project the old friction impulses (with old friction vectors) into the new friction
398 // vectors to get the new friction impulses
399 Vector3 oldFrictionImpulse(mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1.x +
400 mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2.x,
401 mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1.y +
402 mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2.y,
403 mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1.z +
404 mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2.z);
405 mContactConstraints[c].friction1Impulse = oldFrictionImpulse.dot(mContactConstraints[c].frictionVector1);
406 mContactConstraints[c].friction2Impulse = oldFrictionImpulse.dot(mContactConstraints[c].frictionVector2);
407
408 // ------ First friction constraint at the center of the contact manifold ------ //
409
410 // Compute the impulse P = J^T * lambda
411 Vector3 angularImpulseBody1(-mContactConstraints[c].r1CrossT1.x * mContactConstraints[c].friction1Impulse,
412 -mContactConstraints[c].r1CrossT1.y * mContactConstraints[c].friction1Impulse,
413 -mContactConstraints[c].r1CrossT1.z * mContactConstraints[c].friction1Impulse);
414 Vector3 linearImpulseBody2(mContactConstraints[c].frictionVector1.x * mContactConstraints[c].friction1Impulse,
415 mContactConstraints[c].frictionVector1.y * mContactConstraints[c].friction1Impulse,
416 mContactConstraints[c].frictionVector1.z * mContactConstraints[c].friction1Impulse);
417 Vector3 angularImpulseBody2(mContactConstraints[c].r2CrossT1.x * mContactConstraints[c].friction1Impulse,
418 mContactConstraints[c].r2CrossT1.y * mContactConstraints[c].friction1Impulse,
419 mContactConstraints[c].r2CrossT1.z * mContactConstraints[c].friction1Impulse);
420
421 // Update the velocities of the body 1 by applying the impulse P
422 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2;
423 mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
424
425 // Update the velocities of the body 1 by applying the impulse P
426 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
427 mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
428
429 // ------ Second friction constraint at the center of the contact manifold ----- //
430
431 // Compute the impulse P = J^T * lambda
432 angularImpulseBody1.x = -mContactConstraints[c].r1CrossT2.x * mContactConstraints[c].friction2Impulse;
433 angularImpulseBody1.y = -mContactConstraints[c].r1CrossT2.y * mContactConstraints[c].friction2Impulse;
434 angularImpulseBody1.z = -mContactConstraints[c].r1CrossT2.z * mContactConstraints[c].friction2Impulse;
435 linearImpulseBody2.x = mContactConstraints[c].frictionVector2.x * mContactConstraints[c].friction2Impulse;
436 linearImpulseBody2.y = mContactConstraints[c].frictionVector2.y * mContactConstraints[c].friction2Impulse;
437 linearImpulseBody2.z = mContactConstraints[c].frictionVector2.z * mContactConstraints[c].friction2Impulse;
438 angularImpulseBody2.x = mContactConstraints[c].r2CrossT2.x * mContactConstraints[c].friction2Impulse;
439 angularImpulseBody2.y = mContactConstraints[c].r2CrossT2.y * mContactConstraints[c].friction2Impulse;
440 angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * mContactConstraints[c].friction2Impulse;
441
442 // Update the velocities of the body 1 by applying the impulse P
443 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
444 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
445 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
446
447 mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
448
449 // Update the velocities of the body 2 by applying the impulse P
450 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
451 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
452 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
453
454 mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
455
456 // ------ Twist friction constraint at the center of the contact manifold ------ //
457
458 // Compute the impulse P = J^T * lambda
459 angularImpulseBody1.x = -mContactConstraints[c].normal.x * mContactConstraints[c].frictionTwistImpulse;
460 angularImpulseBody1.y = -mContactConstraints[c].normal.y * mContactConstraints[c].frictionTwistImpulse;
461 angularImpulseBody1.z = -mContactConstraints[c].normal.z * mContactConstraints[c].frictionTwistImpulse;
462
463 angularImpulseBody2.x = mContactConstraints[c].normal.x * mContactConstraints[c].frictionTwistImpulse;
464 angularImpulseBody2.y = mContactConstraints[c].normal.y * mContactConstraints[c].frictionTwistImpulse;
465 angularImpulseBody2.z = mContactConstraints[c].normal.z * mContactConstraints[c].frictionTwistImpulse;
466
467 // Update the velocities of the body 1 by applying the impulse P
468 mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
469
470 // Update the velocities of the body 2 by applying the impulse P
471 mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
472
473 // ------ Rolling resistance at the center of the contact manifold ------ //
474
475 // Compute the impulse P = J^T * lambda
476 angularImpulseBody2 = mContactConstraints[c].rollingResistanceImpulse;
477
478 // Update the velocities of the body 1 by applying the impulse P
479 mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2;
480
481 // Update the velocities of the body 1 by applying the impulse P
482 mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
483 }
484 else { // If it is a new contact manifold
485
486 // Initialize the accumulated impulses to zero
487 mContactConstraints[c].friction1Impulse = 0.0;
488 mContactConstraints[c].friction2Impulse = 0.0;
489 mContactConstraints[c].frictionTwistImpulse = 0.0;
490 mContactConstraints[c].rollingResistanceImpulse.setToZero();
491 }
492 }
493 }
494
495 // Solve the contacts
solve()496 void ContactSolverSystem::solve() {
497
498 RP3D_PROFILE("ContactSolverSystem::solve()", mProfiler);
499
500 decimal deltaLambda;
501 decimal lambdaTemp;
502 uint contactPointIndex = 0;
503
504 const decimal beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA;
505
506 // For each contact manifold
507 for (uint c=0; c<mNbContactManifolds; c++) {
508
509 decimal sumPenetrationImpulse = 0.0;
510
511 // Get the constrained velocities
512 const Vector3& v1 = mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1];
513 const Vector3& w1 = mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1];
514 const Vector3& v2 = mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2];
515 const Vector3& w2 = mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2];
516
517 for (short int i=0; i<mContactConstraints[c].nbContacts; i++) {
518
519 // --------- Penetration --------- //
520
521 // Compute J*v
522 //Vector3 deltaV = v2 + w2.cross(mContactPoints[contactPointIndex].r2) - v1 - w1.cross(mContactPoints[contactPointIndex].r1);
523 Vector3 deltaV(v2.x + w2.y * mContactPoints[contactPointIndex].r2.z - w2.z * mContactPoints[contactPointIndex].r2.y - v1.x -
524 w1.y * mContactPoints[contactPointIndex].r1.z + w1.z * mContactPoints[contactPointIndex].r1.y,
525 v2.y + w2.z * mContactPoints[contactPointIndex].r2.x - w2.x * mContactPoints[contactPointIndex].r2.z - v1.y -
526 w1.z * mContactPoints[contactPointIndex].r1.x + w1.x * mContactPoints[contactPointIndex].r1.z,
527 v2.z + w2.x * mContactPoints[contactPointIndex].r2.y - w2.y * mContactPoints[contactPointIndex].r2.x - v1.z -
528 w1.x * mContactPoints[contactPointIndex].r1.y + w1.y * mContactPoints[contactPointIndex].r1.x);
529 decimal deltaVDotN = deltaV.x * mContactPoints[contactPointIndex].normal.x + deltaV.y * mContactPoints[contactPointIndex].normal.y +
530 deltaV.z * mContactPoints[contactPointIndex].normal.z;
531 decimal Jv = deltaVDotN;
532
533 // Compute the bias "b" of the constraint
534 decimal biasPenetrationDepth = 0.0;
535 if (mContactPoints[contactPointIndex].penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) *
536 max(0.0f, float(mContactPoints[contactPointIndex].penetrationDepth - SLOP));
537 decimal b = biasPenetrationDepth + mContactPoints[contactPointIndex].restitutionBias;
538
539 // Compute the Lagrange multiplier lambda
540 if (mIsSplitImpulseActive) {
541 deltaLambda = - (Jv + mContactPoints[contactPointIndex].restitutionBias) *
542 mContactPoints[contactPointIndex].inversePenetrationMass;
543 }
544 else {
545 deltaLambda = - (Jv + b) * mContactPoints[contactPointIndex].inversePenetrationMass;
546 }
547 lambdaTemp = mContactPoints[contactPointIndex].penetrationImpulse;
548 mContactPoints[contactPointIndex].penetrationImpulse = std::max(mContactPoints[contactPointIndex].penetrationImpulse +
549 deltaLambda, decimal(0.0));
550 deltaLambda = mContactPoints[contactPointIndex].penetrationImpulse - lambdaTemp;
551
552 Vector3 linearImpulse(mContactPoints[contactPointIndex].normal.x * deltaLambda,
553 mContactPoints[contactPointIndex].normal.y * deltaLambda,
554 mContactPoints[contactPointIndex].normal.z * deltaLambda);
555
556 // Update the velocities of the body 1 by applying the impulse P
557 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x;
558 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y;
559 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z;
560
561 mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambda;
562 mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambda;
563 mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambda;
564
565 // Update the velocities of the body 2 by applying the impulse P
566 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x;
567 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y;
568 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z;
569
570 mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambda;
571 mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambda;
572 mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambda;
573
574 sumPenetrationImpulse += mContactPoints[contactPointIndex].penetrationImpulse;
575
576 // If the split impulse position correction is active
577 if (mIsSplitImpulseActive) {
578
579 // Split impulse (position correction)
580 const Vector3& v1Split = mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1];
581 const Vector3& w1Split = mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1];
582 const Vector3& v2Split = mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2];
583 const Vector3& w2Split = mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2];
584
585 //Vector3 deltaVSplit = v2Split + w2Split.cross(mContactPoints[contactPointIndex].r2) - v1Split - w1Split.cross(mContactPoints[contactPointIndex].r1);
586 Vector3 deltaVSplit(v2Split.x + w2Split.y * mContactPoints[contactPointIndex].r2.z - w2Split.z * mContactPoints[contactPointIndex].r2.y - v1Split.x -
587 w1Split.y * mContactPoints[contactPointIndex].r1.z + w1Split.z * mContactPoints[contactPointIndex].r1.y,
588 v2Split.y + w2Split.z * mContactPoints[contactPointIndex].r2.x - w2Split.x * mContactPoints[contactPointIndex].r2.z - v1Split.y -
589 w1Split.z * mContactPoints[contactPointIndex].r1.x + w1Split.x * mContactPoints[contactPointIndex].r1.z,
590 v2Split.z + w2Split.x * mContactPoints[contactPointIndex].r2.y - w2Split.y * mContactPoints[contactPointIndex].r2.x - v1Split.z -
591 w1Split.x * mContactPoints[contactPointIndex].r1.y + w1Split.y * mContactPoints[contactPointIndex].r1.x);
592 decimal JvSplit = deltaVSplit.x * mContactPoints[contactPointIndex].normal.x +
593 deltaVSplit.y * mContactPoints[contactPointIndex].normal.y +
594 deltaVSplit.z * mContactPoints[contactPointIndex].normal.z;
595 decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) *
596 mContactPoints[contactPointIndex].inversePenetrationMass;
597 decimal lambdaTempSplit = mContactPoints[contactPointIndex].penetrationSplitImpulse;
598 mContactPoints[contactPointIndex].penetrationSplitImpulse = std::max(
599 mContactPoints[contactPointIndex].penetrationSplitImpulse +
600 deltaLambdaSplit, decimal(0.0));
601 deltaLambdaSplit = mContactPoints[contactPointIndex].penetrationSplitImpulse - lambdaTempSplit;
602
603 Vector3 linearImpulse(mContactPoints[contactPointIndex].normal.x * deltaLambdaSplit,
604 mContactPoints[contactPointIndex].normal.y * deltaLambdaSplit,
605 mContactPoints[contactPointIndex].normal.z * deltaLambdaSplit);
606
607 // Update the velocities of the body 1 by applying the impulse P
608 mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x;
609 mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y;
610 mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z;
611
612 mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambdaSplit;
613 mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambdaSplit;
614 mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambdaSplit;
615
616 // Update the velocities of the body 1 by applying the impulse P
617 mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x;
618 mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y;
619 mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z;
620
621 mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambdaSplit;
622 mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambdaSplit;
623 mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambdaSplit;
624 }
625
626 contactPointIndex++;
627 }
628
629 // ------ First friction constraint at the center of the contact manifold ------ //
630
631 // Compute J*v
632 // deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction) - v1 - w1.cross(mContactConstraints[c].r1Friction);
633 Vector3 deltaV(v2.x + w2.y * mContactConstraints[c].r2Friction.z - w2.z * mContactConstraints[c].r2Friction.y - v1.x -
634 w1.y * mContactConstraints[c].r1Friction.z + w1.z * mContactConstraints[c].r1Friction.y,
635
636 v2.y + w2.z * mContactConstraints[c].r2Friction.x - w2.x * mContactConstraints[c].r2Friction.z - v1.y -
637 w1.z * mContactConstraints[c].r1Friction.x + w1.x * mContactConstraints[c].r1Friction.z,
638
639 v2.z + w2.x * mContactConstraints[c].r2Friction.y - w2.y * mContactConstraints[c].r2Friction.x - v1.z -
640 w1.x * mContactConstraints[c].r1Friction.y + w1.y * mContactConstraints[c].r1Friction.x);
641 decimal Jv = deltaV.x * mContactConstraints[c].frictionVector1.x +
642 deltaV.y * mContactConstraints[c].frictionVector1.y +
643 deltaV.z * mContactConstraints[c].frictionVector1.z;
644
645 // Compute the Lagrange multiplier lambda
646 decimal deltaLambda = -Jv * mContactConstraints[c].inverseFriction1Mass;
647 decimal frictionLimit = mContactConstraints[c].frictionCoefficient * sumPenetrationImpulse;
648 lambdaTemp = mContactConstraints[c].friction1Impulse;
649 mContactConstraints[c].friction1Impulse = std::max(-frictionLimit,
650 std::min(mContactConstraints[c].friction1Impulse +
651 deltaLambda, frictionLimit));
652 deltaLambda = mContactConstraints[c].friction1Impulse - lambdaTemp;
653
654 // Compute the impulse P=J^T * lambda
655 Vector3 angularImpulseBody1(-mContactConstraints[c].r1CrossT1.x * deltaLambda,
656 -mContactConstraints[c].r1CrossT1.y * deltaLambda,
657 -mContactConstraints[c].r1CrossT1.z * deltaLambda);
658 Vector3 linearImpulseBody2(mContactConstraints[c].frictionVector1.x * deltaLambda,
659 mContactConstraints[c].frictionVector1.y * deltaLambda,
660 mContactConstraints[c].frictionVector1.z * deltaLambda);
661 Vector3 angularImpulseBody2(mContactConstraints[c].r2CrossT1.x * deltaLambda,
662 mContactConstraints[c].r2CrossT1.y * deltaLambda,
663 mContactConstraints[c].r2CrossT1.z * deltaLambda);
664
665
666 // Update the velocities of the body 1 by applying the impulse P
667 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
668 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
669 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
670
671 mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
672
673 // Update the velocities of the body 2 by applying the impulse P
674 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
675 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
676 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
677
678 mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
679
680 // ------ Second friction constraint at the center of the contact manifold ----- //
681
682 // Compute J*v
683 //deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction) - v1 - w1.cross(mContactConstraints[c].r1Friction);
684 deltaV.x = v2.x + w2.y * mContactConstraints[c].r2Friction.z - w2.z * mContactConstraints[c].r2Friction.y - v1.x -
685 w1.y * mContactConstraints[c].r1Friction.z + w1.z * mContactConstraints[c].r1Friction.y;
686 deltaV.y = v2.y + w2.z * mContactConstraints[c].r2Friction.x - w2.x * mContactConstraints[c].r2Friction.z - v1.y -
687 w1.z * mContactConstraints[c].r1Friction.x + w1.x * mContactConstraints[c].r1Friction.z;
688 deltaV.z = v2.z + w2.x * mContactConstraints[c].r2Friction.y - w2.y * mContactConstraints[c].r2Friction.x - v1.z -
689 w1.x * mContactConstraints[c].r1Friction.y + w1.y * mContactConstraints[c].r1Friction.x;
690 Jv = deltaV.x * mContactConstraints[c].frictionVector2.x + deltaV.y * mContactConstraints[c].frictionVector2.y +
691 deltaV.z * mContactConstraints[c].frictionVector2.z;
692
693 // Compute the Lagrange multiplier lambda
694 deltaLambda = -Jv * mContactConstraints[c].inverseFriction2Mass;
695 frictionLimit = mContactConstraints[c].frictionCoefficient * sumPenetrationImpulse;
696 lambdaTemp = mContactConstraints[c].friction2Impulse;
697 mContactConstraints[c].friction2Impulse = std::max(-frictionLimit,
698 std::min(mContactConstraints[c].friction2Impulse +
699 deltaLambda, frictionLimit));
700 deltaLambda = mContactConstraints[c].friction2Impulse - lambdaTemp;
701
702 // Compute the impulse P=J^T * lambda
703 angularImpulseBody1.x = -mContactConstraints[c].r1CrossT2.x * deltaLambda;
704 angularImpulseBody1.y = -mContactConstraints[c].r1CrossT2.y * deltaLambda;
705 angularImpulseBody1.z = -mContactConstraints[c].r1CrossT2.z * deltaLambda;
706
707 linearImpulseBody2.x = mContactConstraints[c].frictionVector2.x * deltaLambda;
708 linearImpulseBody2.y = mContactConstraints[c].frictionVector2.y * deltaLambda;
709 linearImpulseBody2.z = mContactConstraints[c].frictionVector2.z * deltaLambda;
710
711 angularImpulseBody2.x = mContactConstraints[c].r2CrossT2.x * deltaLambda;
712 angularImpulseBody2.y = mContactConstraints[c].r2CrossT2.y * deltaLambda;
713 angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * deltaLambda;
714
715 // Update the velocities of the body 1 by applying the impulse P
716 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
717 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
718 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
719 mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
720
721 // Update the velocities of the body 2 by applying the impulse P
722 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
723 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
724 mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
725 mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
726
727 // ------ Twist friction constraint at the center of the contact manifol ------ //
728
729 // Compute J*v
730 deltaV = w2 - w1;
731 Jv = deltaV.x * mContactConstraints[c].normal.x + deltaV.y * mContactConstraints[c].normal.y +
732 deltaV.z * mContactConstraints[c].normal.z;
733
734 deltaLambda = -Jv * (mContactConstraints[c].inverseTwistFrictionMass);
735 frictionLimit = mContactConstraints[c].frictionCoefficient * sumPenetrationImpulse;
736 lambdaTemp = mContactConstraints[c].frictionTwistImpulse;
737 mContactConstraints[c].frictionTwistImpulse = std::max(-frictionLimit,
738 std::min(mContactConstraints[c].frictionTwistImpulse
739 + deltaLambda, frictionLimit));
740 deltaLambda = mContactConstraints[c].frictionTwistImpulse - lambdaTemp;
741
742 // Compute the impulse P=J^T * lambda
743 angularImpulseBody2.x = mContactConstraints[c].normal.x * deltaLambda;
744 angularImpulseBody2.y = mContactConstraints[c].normal.y * deltaLambda;
745 angularImpulseBody2.z = mContactConstraints[c].normal.z * deltaLambda;
746
747 // Update the velocities of the body 1 by applying the impulse P
748 mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2;
749
750 // Update the velocities of the body 1 by applying the impulse P
751 mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
752
753 // --------- Rolling resistance constraint at the center of the contact manifold --------- //
754
755 if (mContactConstraints[c].rollingResistanceFactor > 0) {
756
757 // Compute J*v
758 const Vector3 JvRolling = w2 - w1;
759
760 // Compute the Lagrange multiplier lambda
761 Vector3 deltaLambdaRolling = mContactConstraints[c].inverseRollingResistance * (-JvRolling);
762 decimal rollingLimit = mContactConstraints[c].rollingResistanceFactor * sumPenetrationImpulse;
763 Vector3 lambdaTempRolling = mContactConstraints[c].rollingResistanceImpulse;
764 mContactConstraints[c].rollingResistanceImpulse = clamp(mContactConstraints[c].rollingResistanceImpulse +
765 deltaLambdaRolling, rollingLimit);
766 deltaLambdaRolling = mContactConstraints[c].rollingResistanceImpulse - lambdaTempRolling;
767
768 // Update the velocities of the body 1 by applying the impulse P
769 mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * deltaLambdaRolling;
770
771 // Update the velocities of the body 2 by applying the impulse P
772 mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * deltaLambdaRolling;
773 }
774 }
775 }
776
777 // Compute the collision restitution factor from the restitution factor of each collider
computeMixedRestitutionFactor(Collider * collider1,Collider * collider2) const778 decimal ContactSolverSystem::computeMixedRestitutionFactor(Collider* collider1, Collider* collider2) const {
779 decimal restitution1 = collider1->getMaterial().getBounciness();
780 decimal restitution2 = collider2->getMaterial().getBounciness();
781
782 // Return the largest restitution factor
783 return (restitution1 > restitution2) ? restitution1 : restitution2;
784 }
785
786 // Compute the mixed friction coefficient from the friction coefficient of each collider
computeMixedFrictionCoefficient(Collider * collider1,Collider * collider2) const787 decimal ContactSolverSystem::computeMixedFrictionCoefficient(Collider* collider1, Collider* collider2) const {
788
789 // Use the geometric mean to compute the mixed friction coefficient
790 return std::sqrt(collider1->getMaterial().getFrictionCoefficient() *
791 collider2->getMaterial().getFrictionCoefficient());
792 }
793
794 // Compute th mixed rolling resistance factor between two colliders
computeMixedRollingResistance(Collider * collider1,Collider * collider2) const795 inline decimal ContactSolverSystem::computeMixedRollingResistance(Collider* collider1, Collider* collider2) const {
796 return decimal(0.5f) * (collider1->getMaterial().getRollingResistance() + collider2->getMaterial().getRollingResistance());
797 }
798
799 // Store the computed impulses to use them to
800 // warm start the solver at the next iteration
storeImpulses()801 void ContactSolverSystem::storeImpulses() {
802
803 RP3D_PROFILE("ContactSolver::storeImpulses()", mProfiler);
804
805 uint contactPointIndex = 0;
806
807 // For each contact manifold
808 for (uint c=0; c<mNbContactManifolds; c++) {
809
810 for (short int i=0; i<mContactConstraints[c].nbContacts; i++) {
811
812 mContactPoints[contactPointIndex].externalContact->setPenetrationImpulse(mContactPoints[contactPointIndex].penetrationImpulse);
813
814 contactPointIndex++;
815 }
816
817 mContactConstraints[c].externalContactManifold->frictionImpulse1 = mContactConstraints[c].friction1Impulse;
818 mContactConstraints[c].externalContactManifold->frictionImpulse2 = mContactConstraints[c].friction2Impulse;
819 mContactConstraints[c].externalContactManifold->frictionTwistImpulse = mContactConstraints[c].frictionTwistImpulse;
820 mContactConstraints[c].externalContactManifold->rollingResistanceImpulse = mContactConstraints[c].rollingResistanceImpulse;
821 mContactConstraints[c].externalContactManifold->frictionVector1 = mContactConstraints[c].frictionVector1;
822 mContactConstraints[c].externalContactManifold->frictionVector2 = mContactConstraints[c].frictionVector2;
823 }
824 }
825
826 // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane
827 // for a contact manifold. The two vectors have to be such that : t1 x t2 = contactNormal.
computeFrictionVectors(const Vector3 & deltaVelocity,ContactManifoldSolver & contact) const828 void ContactSolverSystem::computeFrictionVectors(const Vector3& deltaVelocity,
829 ContactManifoldSolver& contact) const {
830
831 RP3D_PROFILE("ContactSolver::computeFrictionVectors()", mProfiler);
832
833 assert(contact.normal.length() > decimal(0.0));
834
835 // Compute the velocity difference vector in the tangential plane
836 Vector3 normalVelocity(deltaVelocity.x * contact.normal.x * contact.normal.x,
837 deltaVelocity.y * contact.normal.y * contact.normal.y,
838 deltaVelocity.z * contact.normal.z * contact.normal.z);
839 Vector3 tangentVelocity(deltaVelocity.x - normalVelocity.x, deltaVelocity.y - normalVelocity.y,
840 deltaVelocity.z - normalVelocity.z);
841
842 // If the velocty difference in the tangential plane is not zero
843 decimal lengthTangentVelocity = tangentVelocity.length();
844 if (lengthTangentVelocity > MACHINE_EPSILON) {
845
846 // Compute the first friction vector in the direction of the tangent
847 // velocity difference
848 contact.frictionVector1 = tangentVelocity / lengthTangentVelocity;
849 }
850 else {
851
852 // Get any orthogonal vector to the normal as the first friction vector
853 contact.frictionVector1 = contact.normal.getOneUnitOrthogonalVector();
854 }
855
856 // The second friction vector is computed by the cross product of the first
857 // friction vector and the contact normal
858 contact.frictionVector2 = contact.normal.cross(contact.frictionVector1).getUnit();
859 }
860