1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
16 #include "btSequentialImpulseConstraintSolverMt.h"
17
18 #include "LinearMath/btQuickprof.h"
19
20 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
21
22 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
23 #include "BulletDynamics/Dynamics/btRigidBody.h"
24
25 bool btSequentialImpulseConstraintSolverMt::s_allowNestedParallelForLoops = false; // some task schedulers don't like nested loops
26 int btSequentialImpulseConstraintSolverMt::s_minimumContactManifoldsForBatching = 250;
27 int btSequentialImpulseConstraintSolverMt::s_minBatchSize = 50;
28 int btSequentialImpulseConstraintSolverMt::s_maxBatchSize = 100;
29 btBatchedConstraints::BatchingMethod btSequentialImpulseConstraintSolverMt::s_contactBatchingMethod = btBatchedConstraints::BATCHING_METHOD_SPATIAL_GRID_2D;
30 btBatchedConstraints::BatchingMethod btSequentialImpulseConstraintSolverMt::s_jointBatchingMethod = btBatchedConstraints::BATCHING_METHOD_SPATIAL_GRID_2D;
31
btSequentialImpulseConstraintSolverMt()32 btSequentialImpulseConstraintSolverMt::btSequentialImpulseConstraintSolverMt()
33 {
34 m_numFrictionDirections = 1;
35 m_useBatching = false;
36 m_useObsoleteJointConstraints = false;
37 }
38
~btSequentialImpulseConstraintSolverMt()39 btSequentialImpulseConstraintSolverMt::~btSequentialImpulseConstraintSolverMt()
40 {
41 }
42
setupBatchedContactConstraints()43 void btSequentialImpulseConstraintSolverMt::setupBatchedContactConstraints()
44 {
45 BT_PROFILE("setupBatchedContactConstraints");
46 m_batchedContactConstraints.setup(&m_tmpSolverContactConstraintPool,
47 m_tmpSolverBodyPool,
48 s_contactBatchingMethod,
49 s_minBatchSize,
50 s_maxBatchSize,
51 &m_scratchMemory);
52 }
53
setupBatchedJointConstraints()54 void btSequentialImpulseConstraintSolverMt::setupBatchedJointConstraints()
55 {
56 BT_PROFILE("setupBatchedJointConstraints");
57 m_batchedJointConstraints.setup(&m_tmpSolverNonContactConstraintPool,
58 m_tmpSolverBodyPool,
59 s_jointBatchingMethod,
60 s_minBatchSize,
61 s_maxBatchSize,
62 &m_scratchMemory);
63 }
64
internalSetupContactConstraints(int iContactConstraint,const btContactSolverInfo & infoGlobal)65 void btSequentialImpulseConstraintSolverMt::internalSetupContactConstraints(int iContactConstraint, const btContactSolverInfo& infoGlobal)
66 {
67 btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[iContactConstraint];
68
69 btVector3 rel_pos1;
70 btVector3 rel_pos2;
71 btScalar relaxation;
72
73 int solverBodyIdA = contactConstraint.m_solverBodyIdA;
74 int solverBodyIdB = contactConstraint.m_solverBodyIdB;
75
76 btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
77 btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
78
79 btRigidBody* colObj0 = solverBodyA->m_originalBody;
80 btRigidBody* colObj1 = solverBodyB->m_originalBody;
81
82 btManifoldPoint& cp = *static_cast<btManifoldPoint*>(contactConstraint.m_originalContactPoint);
83
84 const btVector3& pos1 = cp.getPositionWorldOnA();
85 const btVector3& pos2 = cp.getPositionWorldOnB();
86
87 rel_pos1 = pos1 - solverBodyA->getWorldTransform().getOrigin();
88 rel_pos2 = pos2 - solverBodyB->getWorldTransform().getOrigin();
89
90 btVector3 vel1;
91 btVector3 vel2;
92
93 solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1, vel1);
94 solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2, vel2);
95
96 btVector3 vel = vel1 - vel2;
97 btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
98
99 setupContactConstraint(contactConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2);
100
101 // setup rolling friction constraints
102 int rollingFrictionIndex = m_rollingFrictionIndexTable[iContactConstraint];
103 if (rollingFrictionIndex >= 0)
104 {
105 btSolverConstraint& spinningFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[rollingFrictionIndex];
106 btAssert(spinningFrictionConstraint.m_frictionIndex == iContactConstraint);
107 setupTorsionalFrictionConstraint(spinningFrictionConstraint,
108 cp.m_normalWorldOnB,
109 solverBodyIdA,
110 solverBodyIdB,
111 cp,
112 cp.m_combinedSpinningFriction,
113 rel_pos1,
114 rel_pos2,
115 colObj0,
116 colObj1,
117 relaxation,
118 0.0f,
119 0.0f);
120 btVector3 axis[2];
121 btPlaneSpace1(cp.m_normalWorldOnB, axis[0], axis[1]);
122 axis[0].normalize();
123 axis[1].normalize();
124
125 applyAnisotropicFriction(colObj0, axis[0], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
126 applyAnisotropicFriction(colObj1, axis[0], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
127 applyAnisotropicFriction(colObj0, axis[1], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
128 applyAnisotropicFriction(colObj1, axis[1], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
129 // put the largest axis first
130 if (axis[1].length2() > axis[0].length2())
131 {
132 btSwap(axis[0], axis[1]);
133 }
134 const btScalar kRollingFrictionThreshold = 0.001f;
135 for (int i = 0; i < 2; ++i)
136 {
137 int iRollingFric = rollingFrictionIndex + 1 + i;
138 btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[iRollingFric];
139 btAssert(rollingFrictionConstraint.m_frictionIndex == iContactConstraint);
140 btVector3 dir = axis[i];
141 if (dir.length() > kRollingFrictionThreshold)
142 {
143 setupTorsionalFrictionConstraint(rollingFrictionConstraint,
144 dir,
145 solverBodyIdA,
146 solverBodyIdB,
147 cp,
148 cp.m_combinedRollingFriction,
149 rel_pos1,
150 rel_pos2,
151 colObj0,
152 colObj1,
153 relaxation,
154 0.0f,
155 0.0f);
156 }
157 else
158 {
159 rollingFrictionConstraint.m_frictionIndex = -1; // disable constraint
160 }
161 }
162 }
163
164 // setup friction constraints
165 // setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip);
166 {
167 ///Bullet has several options to set the friction directions
168 ///By default, each contact has only a single friction direction that is recomputed automatically very frame
169 ///based on the relative linear velocity.
170 ///If the relative velocity it zero, it will automatically compute a friction direction.
171
172 ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS.
173 ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction.
174 ///
175 ///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
176 ///
177 ///The user can manually override the friction directions for certain contacts using a contact callback,
178 ///and set the cp.m_lateralFrictionInitialized to true
179 ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
180 ///this will give a conveyor belt effect
181 ///
182 btSolverConstraint* frictionConstraint1 = &m_tmpSolverContactFrictionConstraintPool[contactConstraint.m_frictionIndex];
183 btAssert(frictionConstraint1->m_frictionIndex == iContactConstraint);
184
185 btSolverConstraint* frictionConstraint2 = NULL;
186 if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
187 {
188 frictionConstraint2 = &m_tmpSolverContactFrictionConstraintPool[contactConstraint.m_frictionIndex + 1];
189 btAssert(frictionConstraint2->m_frictionIndex == iContactConstraint);
190 }
191
192 if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags & BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED))
193 {
194 cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
195 btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
196 if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
197 {
198 cp.m_lateralFrictionDir1 *= 1.f / btSqrt(lat_rel_vel);
199 applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
200 applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
201 setupFrictionConstraint(*frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
202
203 if (frictionConstraint2)
204 {
205 cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
206 cp.m_lateralFrictionDir2.normalize(); //??
207 applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
208 applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
209 setupFrictionConstraint(*frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
210 }
211 }
212 else
213 {
214 btPlaneSpace1(cp.m_normalWorldOnB, cp.m_lateralFrictionDir1, cp.m_lateralFrictionDir2);
215
216 applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
217 applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
218 setupFrictionConstraint(*frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
219
220 if (frictionConstraint2)
221 {
222 applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
223 applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
224 setupFrictionConstraint(*frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
225 }
226
227 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
228 {
229 cp.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED;
230 }
231 }
232 }
233 else
234 {
235 setupFrictionConstraint(*frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
236 if (frictionConstraint2)
237 {
238 setupFrictionConstraint(*frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
239 }
240 }
241 }
242
243 setFrictionConstraintImpulse(contactConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
244 }
245
246 struct SetupContactConstraintsLoop : public btIParallelForBody
247 {
248 btSequentialImpulseConstraintSolverMt* m_solver;
249 const btBatchedConstraints* m_bc;
250 const btContactSolverInfo* m_infoGlobal;
251
SetupContactConstraintsLoopSetupContactConstraintsLoop252 SetupContactConstraintsLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc, const btContactSolverInfo& infoGlobal)
253 {
254 m_solver = solver;
255 m_bc = bc;
256 m_infoGlobal = &infoGlobal;
257 }
forLoopSetupContactConstraintsLoop258 void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
259 {
260 BT_PROFILE("SetupContactConstraintsLoop");
261 for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
262 {
263 const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
264 for (int i = batch.begin; i < batch.end; ++i)
265 {
266 int iContact = m_bc->m_constraintIndices[i];
267 m_solver->internalSetupContactConstraints(iContact, *m_infoGlobal);
268 }
269 }
270 }
271 };
272
setupAllContactConstraints(const btContactSolverInfo & infoGlobal)273 void btSequentialImpulseConstraintSolverMt::setupAllContactConstraints(const btContactSolverInfo& infoGlobal)
274 {
275 BT_PROFILE("setupAllContactConstraints");
276 if (m_useBatching)
277 {
278 const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
279 SetupContactConstraintsLoop loop(this, &batchedCons, infoGlobal);
280 for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
281 {
282 int iPhase = batchedCons.m_phaseOrder[iiPhase];
283 const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
284 int grainSize = 1;
285 btParallelFor(phase.begin, phase.end, grainSize, loop);
286 }
287 }
288 else
289 {
290 for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); ++i)
291 {
292 internalSetupContactConstraints(i, infoGlobal);
293 }
294 }
295 }
296
getOrInitSolverBodyThreadsafe(btCollisionObject & body,btScalar timeStep)297 int btSequentialImpulseConstraintSolverMt::getOrInitSolverBodyThreadsafe(btCollisionObject& body, btScalar timeStep)
298 {
299 //
300 // getOrInitSolverBody is threadsafe only for a single thread per solver (with potentially multiple solvers)
301 //
302 // getOrInitSolverBodyThreadsafe -- attempts to be fully threadsafe (however may affect determinism)
303 //
304 int solverBodyId = -1;
305 bool isRigidBodyType = btRigidBody::upcast(&body) != NULL;
306 if (isRigidBodyType && !body.isStaticOrKinematicObject())
307 {
308 // dynamic body
309 // Dynamic bodies can only be in one island, so it's safe to write to the companionId
310 solverBodyId = body.getCompanionId();
311 if (solverBodyId < 0)
312 {
313 m_bodySolverArrayMutex.lock();
314 // now that we have the lock, check again
315 solverBodyId = body.getCompanionId();
316 if (solverBodyId < 0)
317 {
318 solverBodyId = m_tmpSolverBodyPool.size();
319 btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
320 initSolverBody(&solverBody, &body, timeStep);
321 body.setCompanionId(solverBodyId);
322 }
323 m_bodySolverArrayMutex.unlock();
324 }
325 }
326 else if (isRigidBodyType && body.isKinematicObject())
327 {
328 //
329 // NOTE: must test for kinematic before static because some kinematic objects also
330 // identify as "static"
331 //
332 // Kinematic bodies can be in multiple islands at once, so it is a
333 // race condition to write to them, so we use an alternate method
334 // to record the solverBodyId
335 int uniqueId = body.getWorldArrayIndex();
336 const int INVALID_SOLVER_BODY_ID = -1;
337 if (m_kinematicBodyUniqueIdToSolverBodyTable.size() <= uniqueId)
338 {
339 m_kinematicBodyUniqueIdToSolverBodyTableMutex.lock();
340 // now that we have the lock, check again
341 if (m_kinematicBodyUniqueIdToSolverBodyTable.size() <= uniqueId)
342 {
343 m_kinematicBodyUniqueIdToSolverBodyTable.resize(uniqueId + 1, INVALID_SOLVER_BODY_ID);
344 }
345 m_kinematicBodyUniqueIdToSolverBodyTableMutex.unlock();
346 }
347 solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId];
348 // if no table entry yet,
349 if (INVALID_SOLVER_BODY_ID == solverBodyId)
350 {
351 // need to acquire both locks
352 m_kinematicBodyUniqueIdToSolverBodyTableMutex.lock();
353 m_bodySolverArrayMutex.lock();
354 // now that we have the lock, check again
355 solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId];
356 if (INVALID_SOLVER_BODY_ID == solverBodyId)
357 {
358 // create a table entry for this body
359 solverBodyId = m_tmpSolverBodyPool.size();
360 btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
361 initSolverBody(&solverBody, &body, timeStep);
362 m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId] = solverBodyId;
363 }
364 m_bodySolverArrayMutex.unlock();
365 m_kinematicBodyUniqueIdToSolverBodyTableMutex.unlock();
366 }
367 }
368 else
369 {
370 // all fixed bodies (inf mass) get mapped to a single solver id
371 if (m_fixedBodyId < 0)
372 {
373 m_bodySolverArrayMutex.lock();
374 // now that we have the lock, check again
375 if (m_fixedBodyId < 0)
376 {
377 m_fixedBodyId = m_tmpSolverBodyPool.size();
378 btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
379 initSolverBody(&fixedBody, 0, timeStep);
380 }
381 m_bodySolverArrayMutex.unlock();
382 }
383 solverBodyId = m_fixedBodyId;
384 }
385 btAssert(solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size());
386 return solverBodyId;
387 }
388
internalCollectContactManifoldCachedInfo(btContactManifoldCachedInfo * cachedInfoArray,btPersistentManifold ** manifoldPtr,int numManifolds,const btContactSolverInfo & infoGlobal)389 void btSequentialImpulseConstraintSolverMt::internalCollectContactManifoldCachedInfo(btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
390 {
391 BT_PROFILE("internalCollectContactManifoldCachedInfo");
392 for (int i = 0; i < numManifolds; ++i)
393 {
394 btContactManifoldCachedInfo* cachedInfo = &cachedInfoArray[i];
395 btPersistentManifold* manifold = manifoldPtr[i];
396 btCollisionObject* colObj0 = (btCollisionObject*)manifold->getBody0();
397 btCollisionObject* colObj1 = (btCollisionObject*)manifold->getBody1();
398
399 int solverBodyIdA = getOrInitSolverBodyThreadsafe(*colObj0, infoGlobal.m_timeStep);
400 int solverBodyIdB = getOrInitSolverBodyThreadsafe(*colObj1, infoGlobal.m_timeStep);
401
402 cachedInfo->solverBodyIds[0] = solverBodyIdA;
403 cachedInfo->solverBodyIds[1] = solverBodyIdB;
404 cachedInfo->numTouchingContacts = 0;
405
406 btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
407 btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
408
409 // A contact manifold between 2 static object should not exist!
410 // check the collision flags of your objects if this assert fires.
411 // Incorrectly set collision object flags can degrade performance in various ways.
412 btAssert(!m_tmpSolverBodyPool[solverBodyIdA].m_invMass.isZero() || !m_tmpSolverBodyPool[solverBodyIdB].m_invMass.isZero());
413
414 int iContact = 0;
415 for (int j = 0; j < manifold->getNumContacts(); j++)
416 {
417 btManifoldPoint& cp = manifold->getContactPoint(j);
418
419 if (cp.getDistance() <= manifold->getContactProcessingThreshold())
420 {
421 cachedInfo->contactPoints[iContact] = &cp;
422 cachedInfo->contactHasRollingFriction[iContact] = (cp.m_combinedRollingFriction > 0.f);
423 iContact++;
424 }
425 }
426 cachedInfo->numTouchingContacts = iContact;
427 }
428 }
429
430 struct CollectContactManifoldCachedInfoLoop : public btIParallelForBody
431 {
432 btSequentialImpulseConstraintSolverMt* m_solver;
433 btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* m_cachedInfoArray;
434 btPersistentManifold** m_manifoldPtr;
435 const btContactSolverInfo* m_infoGlobal;
436
CollectContactManifoldCachedInfoLoopCollectContactManifoldCachedInfoLoop437 CollectContactManifoldCachedInfoLoop(btSequentialImpulseConstraintSolverMt* solver, btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifoldPtr, const btContactSolverInfo& infoGlobal)
438 {
439 m_solver = solver;
440 m_cachedInfoArray = cachedInfoArray;
441 m_manifoldPtr = manifoldPtr;
442 m_infoGlobal = &infoGlobal;
443 }
forLoopCollectContactManifoldCachedInfoLoop444 void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
445 {
446 m_solver->internalCollectContactManifoldCachedInfo(m_cachedInfoArray + iBegin, m_manifoldPtr + iBegin, iEnd - iBegin, *m_infoGlobal);
447 }
448 };
449
internalAllocContactConstraints(const btContactManifoldCachedInfo * cachedInfoArray,int numManifolds)450 void btSequentialImpulseConstraintSolverMt::internalAllocContactConstraints(const btContactManifoldCachedInfo* cachedInfoArray, int numManifolds)
451 {
452 BT_PROFILE("internalAllocContactConstraints");
453 // possibly parallel part
454 for (int iManifold = 0; iManifold < numManifolds; ++iManifold)
455 {
456 const btContactManifoldCachedInfo& cachedInfo = cachedInfoArray[iManifold];
457 int contactIndex = cachedInfo.contactIndex;
458 int frictionIndex = contactIndex * m_numFrictionDirections;
459 int rollingFrictionIndex = cachedInfo.rollingFrictionIndex;
460 for (int i = 0; i < cachedInfo.numTouchingContacts; i++)
461 {
462 btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[contactIndex];
463 contactConstraint.m_solverBodyIdA = cachedInfo.solverBodyIds[0];
464 contactConstraint.m_solverBodyIdB = cachedInfo.solverBodyIds[1];
465 contactConstraint.m_originalContactPoint = cachedInfo.contactPoints[i];
466
467 // allocate the friction constraints
468 contactConstraint.m_frictionIndex = frictionIndex;
469 for (int iDir = 0; iDir < m_numFrictionDirections; ++iDir)
470 {
471 btSolverConstraint& frictionConstraint = m_tmpSolverContactFrictionConstraintPool[frictionIndex];
472 frictionConstraint.m_frictionIndex = contactIndex;
473 frictionIndex++;
474 }
475
476 // allocate rolling friction constraints
477 if (cachedInfo.contactHasRollingFriction[i])
478 {
479 m_rollingFrictionIndexTable[contactIndex] = rollingFrictionIndex;
480 // allocate 3 (although we may use only 2 sometimes)
481 for (int i = 0; i < 3; i++)
482 {
483 m_tmpSolverContactRollingFrictionConstraintPool[rollingFrictionIndex].m_frictionIndex = contactIndex;
484 rollingFrictionIndex++;
485 }
486 }
487 else
488 {
489 // indicate there is no rolling friction for this contact point
490 m_rollingFrictionIndexTable[contactIndex] = -1;
491 }
492 contactIndex++;
493 }
494 }
495 }
496
497 struct AllocContactConstraintsLoop : public btIParallelForBody
498 {
499 btSequentialImpulseConstraintSolverMt* m_solver;
500 const btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* m_cachedInfoArray;
501
AllocContactConstraintsLoopAllocContactConstraintsLoop502 AllocContactConstraintsLoop(btSequentialImpulseConstraintSolverMt* solver, btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* cachedInfoArray)
503 {
504 m_solver = solver;
505 m_cachedInfoArray = cachedInfoArray;
506 }
forLoopAllocContactConstraintsLoop507 void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
508 {
509 m_solver->internalAllocContactConstraints(m_cachedInfoArray + iBegin, iEnd - iBegin);
510 }
511 };
512
allocAllContactConstraints(btPersistentManifold ** manifoldPtr,int numManifolds,const btContactSolverInfo & infoGlobal)513 void btSequentialImpulseConstraintSolverMt::allocAllContactConstraints(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
514 {
515 BT_PROFILE("allocAllContactConstraints");
516 btAlignedObjectArray<btContactManifoldCachedInfo> cachedInfoArray; // = m_manifoldCachedInfoArray;
517 cachedInfoArray.resizeNoInitialize(numManifolds);
518 if (/* DISABLES CODE */ (false))
519 {
520 // sequential
521 internalCollectContactManifoldCachedInfo(&cachedInfoArray[0], manifoldPtr, numManifolds, infoGlobal);
522 }
523 else
524 {
525 // may alter ordering of bodies which affects determinism
526 CollectContactManifoldCachedInfoLoop loop(this, &cachedInfoArray[0], manifoldPtr, infoGlobal);
527 int grainSize = 200;
528 btParallelFor(0, numManifolds, grainSize, loop);
529 }
530
531 {
532 // serial part
533 int numContacts = 0;
534 int numRollingFrictionConstraints = 0;
535 for (int iManifold = 0; iManifold < numManifolds; ++iManifold)
536 {
537 btContactManifoldCachedInfo& cachedInfo = cachedInfoArray[iManifold];
538 cachedInfo.contactIndex = numContacts;
539 cachedInfo.rollingFrictionIndex = numRollingFrictionConstraints;
540 numContacts += cachedInfo.numTouchingContacts;
541 for (int i = 0; i < cachedInfo.numTouchingContacts; ++i)
542 {
543 if (cachedInfo.contactHasRollingFriction[i])
544 {
545 numRollingFrictionConstraints += 3;
546 }
547 }
548 }
549 {
550 BT_PROFILE("allocPools");
551 if (m_tmpSolverContactConstraintPool.capacity() < numContacts)
552 {
553 // if we need to reallocate, reserve some extra so we don't have to reallocate again next frame
554 int extraReserve = numContacts / 16;
555 m_tmpSolverContactConstraintPool.reserve(numContacts + extraReserve);
556 m_rollingFrictionIndexTable.reserve(numContacts + extraReserve);
557 m_tmpSolverContactFrictionConstraintPool.reserve((numContacts + extraReserve) * m_numFrictionDirections);
558 m_tmpSolverContactRollingFrictionConstraintPool.reserve(numRollingFrictionConstraints + extraReserve);
559 }
560 m_tmpSolverContactConstraintPool.resizeNoInitialize(numContacts);
561 m_rollingFrictionIndexTable.resizeNoInitialize(numContacts);
562 m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(numContacts * m_numFrictionDirections);
563 m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(numRollingFrictionConstraints);
564 }
565 }
566 {
567 AllocContactConstraintsLoop loop(this, &cachedInfoArray[0]);
568 int grainSize = 200;
569 btParallelFor(0, numManifolds, grainSize, loop);
570 }
571 }
572
convertContacts(btPersistentManifold ** manifoldPtr,int numManifolds,const btContactSolverInfo & infoGlobal)573 void btSequentialImpulseConstraintSolverMt::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
574 {
575 if (!m_useBatching)
576 {
577 btSequentialImpulseConstraintSolver::convertContacts(manifoldPtr, numManifolds, infoGlobal);
578 return;
579 }
580 BT_PROFILE("convertContacts");
581 if (numManifolds > 0)
582 {
583 if (m_fixedBodyId < 0)
584 {
585 m_fixedBodyId = m_tmpSolverBodyPool.size();
586 btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
587 initSolverBody(&fixedBody, 0, infoGlobal.m_timeStep);
588 }
589 allocAllContactConstraints(manifoldPtr, numManifolds, infoGlobal);
590 if (m_useBatching)
591 {
592 setupBatchedContactConstraints();
593 }
594 setupAllContactConstraints(infoGlobal);
595 }
596 }
597
internalInitMultipleJoints(btTypedConstraint ** constraints,int iBegin,int iEnd)598 void btSequentialImpulseConstraintSolverMt::internalInitMultipleJoints(btTypedConstraint** constraints, int iBegin, int iEnd)
599 {
600 BT_PROFILE("internalInitMultipleJoints");
601 for (int i = iBegin; i < iEnd; i++)
602 {
603 btTypedConstraint* constraint = constraints[i];
604 btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
605 if (constraint->isEnabled())
606 {
607 constraint->buildJacobian();
608 constraint->internalSetAppliedImpulse(0.0f);
609 btJointFeedback* fb = constraint->getJointFeedback();
610 if (fb)
611 {
612 fb->m_appliedForceBodyA.setZero();
613 fb->m_appliedTorqueBodyA.setZero();
614 fb->m_appliedForceBodyB.setZero();
615 fb->m_appliedTorqueBodyB.setZero();
616 }
617 constraint->getInfo1(&info1);
618 }
619 else
620 {
621 info1.m_numConstraintRows = 0;
622 info1.nub = 0;
623 }
624 }
625 }
626
627 struct InitJointsLoop : public btIParallelForBody
628 {
629 btSequentialImpulseConstraintSolverMt* m_solver;
630 btTypedConstraint** m_constraints;
631
InitJointsLoopInitJointsLoop632 InitJointsLoop(btSequentialImpulseConstraintSolverMt* solver, btTypedConstraint** constraints)
633 {
634 m_solver = solver;
635 m_constraints = constraints;
636 }
forLoopInitJointsLoop637 void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
638 {
639 m_solver->internalInitMultipleJoints(m_constraints, iBegin, iEnd);
640 }
641 };
642
internalConvertMultipleJoints(const btAlignedObjectArray<JointParams> & jointParamsArray,btTypedConstraint ** constraints,int iBegin,int iEnd,const btContactSolverInfo & infoGlobal)643 void btSequentialImpulseConstraintSolverMt::internalConvertMultipleJoints(const btAlignedObjectArray<JointParams>& jointParamsArray, btTypedConstraint** constraints, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
644 {
645 BT_PROFILE("internalConvertMultipleJoints");
646 for (int i = iBegin; i < iEnd; ++i)
647 {
648 const JointParams& jointParams = jointParamsArray[i];
649 int currentRow = jointParams.m_solverConstraint;
650 if (currentRow != -1)
651 {
652 const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
653 btAssert(currentRow < m_tmpSolverNonContactConstraintPool.size());
654 btAssert(info1.m_numConstraintRows > 0);
655
656 btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
657 btTypedConstraint* constraint = constraints[i];
658
659 convertJoint(currentConstraintRow, constraint, info1, jointParams.m_solverBodyA, jointParams.m_solverBodyB, infoGlobal);
660 }
661 }
662 }
663
664 struct ConvertJointsLoop : public btIParallelForBody
665 {
666 btSequentialImpulseConstraintSolverMt* m_solver;
667 const btAlignedObjectArray<btSequentialImpulseConstraintSolverMt::JointParams>& m_jointParamsArray;
668 btTypedConstraint** m_srcConstraints;
669 const btContactSolverInfo& m_infoGlobal;
670
ConvertJointsLoopConvertJointsLoop671 ConvertJointsLoop(btSequentialImpulseConstraintSolverMt* solver,
672 const btAlignedObjectArray<btSequentialImpulseConstraintSolverMt::JointParams>& jointParamsArray,
673 btTypedConstraint** srcConstraints,
674 const btContactSolverInfo& infoGlobal) : m_jointParamsArray(jointParamsArray),
675 m_infoGlobal(infoGlobal)
676 {
677 m_solver = solver;
678 m_srcConstraints = srcConstraints;
679 }
forLoopConvertJointsLoop680 void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
681 {
682 m_solver->internalConvertMultipleJoints(m_jointParamsArray, m_srcConstraints, iBegin, iEnd, m_infoGlobal);
683 }
684 };
685
convertJoints(btTypedConstraint ** constraints,int numConstraints,const btContactSolverInfo & infoGlobal)686 void btSequentialImpulseConstraintSolverMt::convertJoints(btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal)
687 {
688 if (!m_useBatching)
689 {
690 btSequentialImpulseConstraintSolver::convertJoints(constraints, numConstraints, infoGlobal);
691 return;
692 }
693 BT_PROFILE("convertJoints");
694 bool parallelJointSetup = true;
695 m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
696 if (parallelJointSetup)
697 {
698 InitJointsLoop loop(this, constraints);
699 int grainSize = 40;
700 btParallelFor(0, numConstraints, grainSize, loop);
701 }
702 else
703 {
704 internalInitMultipleJoints(constraints, 0, numConstraints);
705 }
706
707 int totalNumRows = 0;
708 btAlignedObjectArray<JointParams> jointParamsArray;
709 jointParamsArray.resizeNoInitialize(numConstraints);
710
711 //calculate the total number of contraint rows
712 for (int i = 0; i < numConstraints; i++)
713 {
714 btTypedConstraint* constraint = constraints[i];
715
716 JointParams& params = jointParamsArray[i];
717 const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
718
719 if (info1.m_numConstraintRows)
720 {
721 params.m_solverConstraint = totalNumRows;
722 params.m_solverBodyA = getOrInitSolverBody(constraint->getRigidBodyA(), infoGlobal.m_timeStep);
723 params.m_solverBodyB = getOrInitSolverBody(constraint->getRigidBodyB(), infoGlobal.m_timeStep);
724 }
725 else
726 {
727 params.m_solverConstraint = -1;
728 }
729 totalNumRows += info1.m_numConstraintRows;
730 }
731 m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
732
733 ///setup the btSolverConstraints
734 if (parallelJointSetup)
735 {
736 ConvertJointsLoop loop(this, jointParamsArray, constraints, infoGlobal);
737 int grainSize = 20;
738 btParallelFor(0, numConstraints, grainSize, loop);
739 }
740 else
741 {
742 internalConvertMultipleJoints(jointParamsArray, constraints, 0, numConstraints, infoGlobal);
743 }
744 setupBatchedJointConstraints();
745 }
746
internalConvertBodies(btCollisionObject ** bodies,int iBegin,int iEnd,const btContactSolverInfo & infoGlobal)747 void btSequentialImpulseConstraintSolverMt::internalConvertBodies(btCollisionObject** bodies, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
748 {
749 BT_PROFILE("internalConvertBodies");
750 for (int i = iBegin; i < iEnd; i++)
751 {
752 btCollisionObject* obj = bodies[i];
753 obj->setCompanionId(i);
754 btSolverBody& solverBody = m_tmpSolverBodyPool[i];
755 initSolverBody(&solverBody, obj, infoGlobal.m_timeStep);
756
757 btRigidBody* body = btRigidBody::upcast(obj);
758 if (body && body->getInvMass())
759 {
760 btVector3 gyroForce(0, 0, 0);
761 if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT)
762 {
763 gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce);
764 solverBody.m_externalTorqueImpulse -= gyroForce * body->getInvInertiaTensorWorld() * infoGlobal.m_timeStep;
765 }
766 if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD)
767 {
768 gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep);
769 solverBody.m_externalTorqueImpulse += gyroForce;
770 }
771 if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY)
772 {
773 gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep);
774 solverBody.m_externalTorqueImpulse += gyroForce;
775 }
776 }
777 }
778 }
779
780 struct ConvertBodiesLoop : public btIParallelForBody
781 {
782 btSequentialImpulseConstraintSolverMt* m_solver;
783 btCollisionObject** m_bodies;
784 int m_numBodies;
785 const btContactSolverInfo& m_infoGlobal;
786
ConvertBodiesLoopConvertBodiesLoop787 ConvertBodiesLoop(btSequentialImpulseConstraintSolverMt* solver,
788 btCollisionObject** bodies,
789 int numBodies,
790 const btContactSolverInfo& infoGlobal) : m_infoGlobal(infoGlobal)
791 {
792 m_solver = solver;
793 m_bodies = bodies;
794 m_numBodies = numBodies;
795 }
forLoopConvertBodiesLoop796 void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
797 {
798 m_solver->internalConvertBodies(m_bodies, iBegin, iEnd, m_infoGlobal);
799 }
800 };
801
convertBodies(btCollisionObject ** bodies,int numBodies,const btContactSolverInfo & infoGlobal)802 void btSequentialImpulseConstraintSolverMt::convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
803 {
804 BT_PROFILE("convertBodies");
805 m_kinematicBodyUniqueIdToSolverBodyTable.resize(0);
806
807 m_tmpSolverBodyPool.resizeNoInitialize(numBodies + 1);
808
809 m_fixedBodyId = numBodies;
810 {
811 btSolverBody& fixedBody = m_tmpSolverBodyPool[m_fixedBodyId];
812 initSolverBody(&fixedBody, NULL, infoGlobal.m_timeStep);
813 }
814
815 bool parallelBodySetup = true;
816 if (parallelBodySetup)
817 {
818 ConvertBodiesLoop loop(this, bodies, numBodies, infoGlobal);
819 int grainSize = 40;
820 btParallelFor(0, numBodies, grainSize, loop);
821 }
822 else
823 {
824 internalConvertBodies(bodies, 0, numBodies, infoGlobal);
825 }
826 }
827
solveGroupCacheFriendlySetup(btCollisionObject ** bodies,int numBodies,btPersistentManifold ** manifoldPtr,int numManifolds,btTypedConstraint ** constraints,int numConstraints,const btContactSolverInfo & infoGlobal,btIDebugDraw * debugDrawer)828 btScalar btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlySetup(
829 btCollisionObject** bodies,
830 int numBodies,
831 btPersistentManifold** manifoldPtr,
832 int numManifolds,
833 btTypedConstraint** constraints,
834 int numConstraints,
835 const btContactSolverInfo& infoGlobal,
836 btIDebugDraw* debugDrawer)
837 {
838 m_numFrictionDirections = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1;
839 m_useBatching = false;
840 if (numManifolds >= s_minimumContactManifoldsForBatching &&
841 (s_allowNestedParallelForLoops || !btThreadsAreRunning()))
842 {
843 m_useBatching = true;
844 m_batchedContactConstraints.m_debugDrawer = debugDrawer;
845 m_batchedJointConstraints.m_debugDrawer = debugDrawer;
846 }
847 btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies,
848 numBodies,
849 manifoldPtr,
850 numManifolds,
851 constraints,
852 numConstraints,
853 infoGlobal,
854 debugDrawer);
855 return 0.0f;
856 }
857
resolveMultipleContactSplitPenetrationImpulseConstraints(const btAlignedObjectArray<int> & consIndices,int batchBegin,int batchEnd)858 btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactSplitPenetrationImpulseConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd)
859 {
860 btScalar leastSquaresResidual = 0.f;
861 for (int iiCons = batchBegin; iiCons < batchEnd; ++iiCons)
862 {
863 int iCons = consIndices[iiCons];
864 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[iCons];
865 btSolverBody& bodyA = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA];
866 btSolverBody& bodyB = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB];
867 btScalar residual = resolveSplitPenetrationImpulse(bodyA, bodyB, solveManifold);
868 leastSquaresResidual += residual * residual;
869 }
870 return leastSquaresResidual;
871 }
872
873 struct ContactSplitPenetrationImpulseSolverLoop : public btIParallelSumBody
874 {
875 btSequentialImpulseConstraintSolverMt* m_solver;
876 const btBatchedConstraints* m_bc;
877
ContactSplitPenetrationImpulseSolverLoopContactSplitPenetrationImpulseSolverLoop878 ContactSplitPenetrationImpulseSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc)
879 {
880 m_solver = solver;
881 m_bc = bc;
882 }
sumLoopContactSplitPenetrationImpulseSolverLoop883 btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
884 {
885 BT_PROFILE("ContactSplitPenetrationImpulseSolverLoop");
886 btScalar sum = 0;
887 for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
888 {
889 const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
890 sum += m_solver->resolveMultipleContactSplitPenetrationImpulseConstraints(m_bc->m_constraintIndices, batch.begin, batch.end);
891 }
892 return sum;
893 }
894 };
895
solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject ** bodies,int numBodies,btPersistentManifold ** manifoldPtr,int numManifolds,btTypedConstraint ** constraints,int numConstraints,const btContactSolverInfo & infoGlobal,btIDebugDraw * debugDrawer)896 void btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
897 {
898 BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");
899 if (infoGlobal.m_splitImpulse)
900 {
901 for (int iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
902 {
903 btScalar leastSquaresResidual = 0.f;
904 if (m_useBatching)
905 {
906 const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
907 ContactSplitPenetrationImpulseSolverLoop loop(this, &batchedCons);
908 btScalar leastSquaresResidual = 0.f;
909 for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
910 {
911 int iPhase = batchedCons.m_phaseOrder[iiPhase];
912 const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
913 int grainSize = batchedCons.m_phaseGrainSize[iPhase];
914 leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop);
915 }
916 }
917 else
918 {
919 // non-batched
920 leastSquaresResidual = resolveMultipleContactSplitPenetrationImpulseConstraints(m_orderTmpConstraintPool, 0, m_tmpSolverContactConstraintPool.size());
921 }
922 if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
923 {
924 #ifdef VERBOSE_RESIDUAL_PRINTF
925 printf("residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
926 #endif
927 break;
928 }
929 }
930 }
931 }
932
solveSingleIteration(int iteration,btCollisionObject ** bodies,int numBodies,btPersistentManifold ** manifoldPtr,int numManifolds,btTypedConstraint ** constraints,int numConstraints,const btContactSolverInfo & infoGlobal,btIDebugDraw * debugDrawer)933 btScalar btSequentialImpulseConstraintSolverMt::solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
934 {
935 if (!m_useBatching)
936 {
937 return btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
938 }
939 BT_PROFILE("solveSingleIterationMt");
940 btScalar leastSquaresResidual = 0.f;
941
942 if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
943 {
944 if (1) // uncomment this for a bit less random ((iteration & 7) == 0)
945 {
946 randomizeConstraintOrdering(iteration, infoGlobal.m_numIterations);
947 }
948 }
949
950 {
951 ///solve all joint constraints
952 leastSquaresResidual += resolveAllJointConstraints(iteration);
953
954 if (iteration < infoGlobal.m_numIterations)
955 {
956 // this loop is only used for cone-twist constraints,
957 // it would be nice to skip this loop if none of the constraints need it
958 if (m_useObsoleteJointConstraints)
959 {
960 for (int j = 0; j < numConstraints; j++)
961 {
962 if (constraints[j]->isEnabled())
963 {
964 int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(), infoGlobal.m_timeStep);
965 int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(), infoGlobal.m_timeStep);
966 btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
967 btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
968 constraints[j]->solveConstraintObsolete(bodyA, bodyB, infoGlobal.m_timeStep);
969 }
970 }
971 }
972
973 if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
974 {
975 // solve all contact, contact-friction, and rolling friction constraints interleaved
976 leastSquaresResidual += resolveAllContactConstraintsInterleaved();
977 }
978 else //SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
979 {
980 // don't interleave them
981 // solve all contact constraints
982 leastSquaresResidual += resolveAllContactConstraints();
983
984 // solve all contact friction constraints
985 leastSquaresResidual += resolveAllContactFrictionConstraints();
986
987 // solve all rolling friction constraints
988 leastSquaresResidual += resolveAllRollingFrictionConstraints();
989 }
990 }
991 }
992 return leastSquaresResidual;
993 }
994
resolveMultipleJointConstraints(const btAlignedObjectArray<int> & consIndices,int batchBegin,int batchEnd,int iteration)995 btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleJointConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd, int iteration)
996 {
997 btScalar leastSquaresResidual = 0.f;
998 for (int iiCons = batchBegin; iiCons < batchEnd; ++iiCons)
999 {
1000 int iCons = consIndices[iiCons];
1001 const btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[iCons];
1002 if (iteration < constraint.m_overrideNumSolverIterations)
1003 {
1004 btSolverBody& bodyA = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
1005 btSolverBody& bodyB = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
1006 btScalar residual = resolveSingleConstraintRowGeneric(bodyA, bodyB, constraint);
1007 leastSquaresResidual += residual * residual;
1008 }
1009 }
1010 return leastSquaresResidual;
1011 }
1012
resolveMultipleContactConstraints(const btAlignedObjectArray<int> & consIndices,int batchBegin,int batchEnd)1013 btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd)
1014 {
1015 btScalar leastSquaresResidual = 0.f;
1016 for (int iiCons = batchBegin; iiCons < batchEnd; ++iiCons)
1017 {
1018 int iCons = consIndices[iiCons];
1019 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[iCons];
1020 btSolverBody& bodyA = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA];
1021 btSolverBody& bodyB = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB];
1022 btScalar residual = resolveSingleConstraintRowLowerLimit(bodyA, bodyB, solveManifold);
1023 leastSquaresResidual += residual * residual;
1024 }
1025 return leastSquaresResidual;
1026 }
1027
resolveMultipleContactFrictionConstraints(const btAlignedObjectArray<int> & consIndices,int batchBegin,int batchEnd)1028 btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactFrictionConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd)
1029 {
1030 btScalar leastSquaresResidual = 0.f;
1031 for (int iiCons = batchBegin; iiCons < batchEnd; ++iiCons)
1032 {
1033 int iContact = consIndices[iiCons];
1034 btScalar totalImpulse = m_tmpSolverContactConstraintPool[iContact].m_appliedImpulse;
1035
1036 // apply sliding friction
1037 if (totalImpulse > 0.0f)
1038 {
1039 int iBegin = iContact * m_numFrictionDirections;
1040 int iEnd = iBegin + m_numFrictionDirections;
1041 for (int iFriction = iBegin; iFriction < iEnd; ++iFriction)
1042 {
1043 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[iFriction++];
1044 btAssert(solveManifold.m_frictionIndex == iContact);
1045
1046 solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
1047 solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
1048
1049 btSolverBody& bodyA = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA];
1050 btSolverBody& bodyB = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB];
1051 btScalar residual = resolveSingleConstraintRowGeneric(bodyA, bodyB, solveManifold);
1052 leastSquaresResidual += residual * residual;
1053 }
1054 }
1055 }
1056 return leastSquaresResidual;
1057 }
1058
resolveMultipleContactRollingFrictionConstraints(const btAlignedObjectArray<int> & consIndices,int batchBegin,int batchEnd)1059 btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactRollingFrictionConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd)
1060 {
1061 btScalar leastSquaresResidual = 0.f;
1062 for (int iiCons = batchBegin; iiCons < batchEnd; ++iiCons)
1063 {
1064 int iContact = consIndices[iiCons];
1065 int iFirstRollingFriction = m_rollingFrictionIndexTable[iContact];
1066 if (iFirstRollingFriction >= 0)
1067 {
1068 btScalar totalImpulse = m_tmpSolverContactConstraintPool[iContact].m_appliedImpulse;
1069 // apply rolling friction
1070 if (totalImpulse > 0.0f)
1071 {
1072 int iBegin = iFirstRollingFriction;
1073 int iEnd = iBegin + 3;
1074 for (int iRollingFric = iBegin; iRollingFric < iEnd; ++iRollingFric)
1075 {
1076 btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[iRollingFric];
1077 if (rollingFrictionConstraint.m_frictionIndex != iContact)
1078 {
1079 break;
1080 }
1081 btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
1082 if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
1083 {
1084 rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
1085 }
1086
1087 rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
1088 rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
1089
1090 btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
1091 leastSquaresResidual += residual * residual;
1092 }
1093 }
1094 }
1095 }
1096 return leastSquaresResidual;
1097 }
1098
resolveMultipleContactConstraintsInterleaved(const btAlignedObjectArray<int> & contactIndices,int batchBegin,int batchEnd)1099 btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactConstraintsInterleaved(const btAlignedObjectArray<int>& contactIndices,
1100 int batchBegin,
1101 int batchEnd)
1102 {
1103 btScalar leastSquaresResidual = 0.f;
1104 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1105
1106 for (int iiCons = batchBegin; iiCons < batchEnd; iiCons++)
1107 {
1108 btScalar totalImpulse = 0;
1109 int iContact = contactIndices[iiCons];
1110 // apply penetration constraint
1111 {
1112 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[iContact];
1113 btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
1114 leastSquaresResidual += residual * residual;
1115 totalImpulse = solveManifold.m_appliedImpulse;
1116 }
1117
1118 // apply sliding friction
1119 if (totalImpulse > 0.0f)
1120 {
1121 int iBegin = iContact * m_numFrictionDirections;
1122 int iEnd = iBegin + m_numFrictionDirections;
1123 for (int iFriction = iBegin; iFriction < iEnd; ++iFriction)
1124 {
1125 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[iFriction];
1126 btAssert(solveManifold.m_frictionIndex == iContact);
1127
1128 solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
1129 solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
1130
1131 btSolverBody& bodyA = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA];
1132 btSolverBody& bodyB = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB];
1133 btScalar residual = resolveSingleConstraintRowGeneric(bodyA, bodyB, solveManifold);
1134 leastSquaresResidual += residual * residual;
1135 }
1136 }
1137
1138 // apply rolling friction
1139 int iFirstRollingFriction = m_rollingFrictionIndexTable[iContact];
1140 if (totalImpulse > 0.0f && iFirstRollingFriction >= 0)
1141 {
1142 int iBegin = iFirstRollingFriction;
1143 int iEnd = iBegin + 3;
1144 for (int iRollingFric = iBegin; iRollingFric < iEnd; ++iRollingFric)
1145 {
1146 btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[iRollingFric];
1147 if (rollingFrictionConstraint.m_frictionIndex != iContact)
1148 {
1149 break;
1150 }
1151 btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
1152 if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
1153 {
1154 rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
1155 }
1156
1157 rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
1158 rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
1159
1160 btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
1161 leastSquaresResidual += residual * residual;
1162 }
1163 }
1164 }
1165 return leastSquaresResidual;
1166 }
1167
randomizeBatchedConstraintOrdering(btBatchedConstraints * batchedConstraints)1168 void btSequentialImpulseConstraintSolverMt::randomizeBatchedConstraintOrdering(btBatchedConstraints* batchedConstraints)
1169 {
1170 btBatchedConstraints& bc = *batchedConstraints;
1171 // randomize ordering of phases
1172 for (int ii = 1; ii < bc.m_phaseOrder.size(); ++ii)
1173 {
1174 int iSwap = btRandInt2(ii + 1);
1175 bc.m_phaseOrder.swap(ii, iSwap);
1176 }
1177
1178 // for each batch,
1179 for (int iBatch = 0; iBatch < bc.m_batches.size(); ++iBatch)
1180 {
1181 // randomize ordering of constraints within the batch
1182 const btBatchedConstraints::Range& batch = bc.m_batches[iBatch];
1183 for (int iiCons = batch.begin; iiCons < batch.end; ++iiCons)
1184 {
1185 int iSwap = batch.begin + btRandInt2(iiCons - batch.begin + 1);
1186 btAssert(iSwap >= batch.begin && iSwap < batch.end);
1187 bc.m_constraintIndices.swap(iiCons, iSwap);
1188 }
1189 }
1190 }
1191
randomizeConstraintOrdering(int iteration,int numIterations)1192 void btSequentialImpulseConstraintSolverMt::randomizeConstraintOrdering(int iteration, int numIterations)
1193 {
1194 // randomize ordering of joint constraints
1195 randomizeBatchedConstraintOrdering(&m_batchedJointConstraints);
1196
1197 //contact/friction constraints are not solved more than numIterations
1198 if (iteration < numIterations)
1199 {
1200 randomizeBatchedConstraintOrdering(&m_batchedContactConstraints);
1201 }
1202 }
1203
1204 struct JointSolverLoop : public btIParallelSumBody
1205 {
1206 btSequentialImpulseConstraintSolverMt* m_solver;
1207 const btBatchedConstraints* m_bc;
1208 int m_iteration;
1209
JointSolverLoopJointSolverLoop1210 JointSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc, int iteration)
1211 {
1212 m_solver = solver;
1213 m_bc = bc;
1214 m_iteration = iteration;
1215 }
sumLoopJointSolverLoop1216 btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
1217 {
1218 BT_PROFILE("JointSolverLoop");
1219 btScalar sum = 0;
1220 for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
1221 {
1222 const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
1223 sum += m_solver->resolveMultipleJointConstraints(m_bc->m_constraintIndices, batch.begin, batch.end, m_iteration);
1224 }
1225 return sum;
1226 }
1227 };
1228
resolveAllJointConstraints(int iteration)1229 btScalar btSequentialImpulseConstraintSolverMt::resolveAllJointConstraints(int iteration)
1230 {
1231 BT_PROFILE("resolveAllJointConstraints");
1232 const btBatchedConstraints& batchedCons = m_batchedJointConstraints;
1233 JointSolverLoop loop(this, &batchedCons, iteration);
1234 btScalar leastSquaresResidual = 0.f;
1235 for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
1236 {
1237 int iPhase = batchedCons.m_phaseOrder[iiPhase];
1238 const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
1239 int grainSize = 1;
1240 leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop);
1241 }
1242 return leastSquaresResidual;
1243 }
1244
1245 struct ContactSolverLoop : public btIParallelSumBody
1246 {
1247 btSequentialImpulseConstraintSolverMt* m_solver;
1248 const btBatchedConstraints* m_bc;
1249
ContactSolverLoopContactSolverLoop1250 ContactSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc)
1251 {
1252 m_solver = solver;
1253 m_bc = bc;
1254 }
sumLoopContactSolverLoop1255 btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
1256 {
1257 BT_PROFILE("ContactSolverLoop");
1258 btScalar sum = 0;
1259 for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
1260 {
1261 const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
1262 sum += m_solver->resolveMultipleContactConstraints(m_bc->m_constraintIndices, batch.begin, batch.end);
1263 }
1264 return sum;
1265 }
1266 };
1267
resolveAllContactConstraints()1268 btScalar btSequentialImpulseConstraintSolverMt::resolveAllContactConstraints()
1269 {
1270 BT_PROFILE("resolveAllContactConstraints");
1271 const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
1272 ContactSolverLoop loop(this, &batchedCons);
1273 btScalar leastSquaresResidual = 0.f;
1274 for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
1275 {
1276 int iPhase = batchedCons.m_phaseOrder[iiPhase];
1277 const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
1278 int grainSize = batchedCons.m_phaseGrainSize[iPhase];
1279 leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop);
1280 }
1281 return leastSquaresResidual;
1282 }
1283
1284 struct ContactFrictionSolverLoop : public btIParallelSumBody
1285 {
1286 btSequentialImpulseConstraintSolverMt* m_solver;
1287 const btBatchedConstraints* m_bc;
1288
ContactFrictionSolverLoopContactFrictionSolverLoop1289 ContactFrictionSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc)
1290 {
1291 m_solver = solver;
1292 m_bc = bc;
1293 }
sumLoopContactFrictionSolverLoop1294 btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
1295 {
1296 BT_PROFILE("ContactFrictionSolverLoop");
1297 btScalar sum = 0;
1298 for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
1299 {
1300 const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
1301 sum += m_solver->resolveMultipleContactFrictionConstraints(m_bc->m_constraintIndices, batch.begin, batch.end);
1302 }
1303 return sum;
1304 }
1305 };
1306
resolveAllContactFrictionConstraints()1307 btScalar btSequentialImpulseConstraintSolverMt::resolveAllContactFrictionConstraints()
1308 {
1309 BT_PROFILE("resolveAllContactFrictionConstraints");
1310 const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
1311 ContactFrictionSolverLoop loop(this, &batchedCons);
1312 btScalar leastSquaresResidual = 0.f;
1313 for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
1314 {
1315 int iPhase = batchedCons.m_phaseOrder[iiPhase];
1316 const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
1317 int grainSize = batchedCons.m_phaseGrainSize[iPhase];
1318 leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop);
1319 }
1320 return leastSquaresResidual;
1321 }
1322
1323 struct InterleavedContactSolverLoop : public btIParallelSumBody
1324 {
1325 btSequentialImpulseConstraintSolverMt* m_solver;
1326 const btBatchedConstraints* m_bc;
1327
InterleavedContactSolverLoopInterleavedContactSolverLoop1328 InterleavedContactSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc)
1329 {
1330 m_solver = solver;
1331 m_bc = bc;
1332 }
sumLoopInterleavedContactSolverLoop1333 btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
1334 {
1335 BT_PROFILE("InterleavedContactSolverLoop");
1336 btScalar sum = 0;
1337 for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
1338 {
1339 const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
1340 sum += m_solver->resolveMultipleContactConstraintsInterleaved(m_bc->m_constraintIndices, batch.begin, batch.end);
1341 }
1342 return sum;
1343 }
1344 };
1345
resolveAllContactConstraintsInterleaved()1346 btScalar btSequentialImpulseConstraintSolverMt::resolveAllContactConstraintsInterleaved()
1347 {
1348 BT_PROFILE("resolveAllContactConstraintsInterleaved");
1349 const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
1350 InterleavedContactSolverLoop loop(this, &batchedCons);
1351 btScalar leastSquaresResidual = 0.f;
1352 for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
1353 {
1354 int iPhase = batchedCons.m_phaseOrder[iiPhase];
1355 const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
1356 int grainSize = 1;
1357 leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop);
1358 }
1359 return leastSquaresResidual;
1360 }
1361
1362 struct ContactRollingFrictionSolverLoop : public btIParallelSumBody
1363 {
1364 btSequentialImpulseConstraintSolverMt* m_solver;
1365 const btBatchedConstraints* m_bc;
1366
ContactRollingFrictionSolverLoopContactRollingFrictionSolverLoop1367 ContactRollingFrictionSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc)
1368 {
1369 m_solver = solver;
1370 m_bc = bc;
1371 }
sumLoopContactRollingFrictionSolverLoop1372 btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
1373 {
1374 BT_PROFILE("ContactFrictionSolverLoop");
1375 btScalar sum = 0;
1376 for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
1377 {
1378 const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
1379 sum += m_solver->resolveMultipleContactRollingFrictionConstraints(m_bc->m_constraintIndices, batch.begin, batch.end);
1380 }
1381 return sum;
1382 }
1383 };
1384
resolveAllRollingFrictionConstraints()1385 btScalar btSequentialImpulseConstraintSolverMt::resolveAllRollingFrictionConstraints()
1386 {
1387 BT_PROFILE("resolveAllRollingFrictionConstraints");
1388 btScalar leastSquaresResidual = 0.f;
1389 //
1390 // We do not generate batches for rolling friction constraints. We assume that
1391 // one of two cases is true:
1392 //
1393 // 1. either most bodies in the simulation have rolling friction, in which case we can use the
1394 // batches for contacts and use a lookup table to translate contact indices to rolling friction
1395 // (ignoring any contact indices that don't map to a rolling friction constraint). As long as
1396 // most contacts have a corresponding rolling friction constraint, this should parallelize well.
1397 //
1398 // -OR-
1399 //
1400 // 2. few bodies in the simulation have rolling friction, so it is not worth trying to use the
1401 // batches from contacts as most of the contacts won't have corresponding rolling friction
1402 // constraints and most threads would end up doing very little work. Most of the time would
1403 // go to threading overhead, so we don't bother with threading.
1404 //
1405 int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
1406 if (numRollingFrictionPoolConstraints >= m_tmpSolverContactConstraintPool.size())
1407 {
1408 // use batching if there are many rolling friction constraints
1409 const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
1410 ContactRollingFrictionSolverLoop loop(this, &batchedCons);
1411 btScalar leastSquaresResidual = 0.f;
1412 for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
1413 {
1414 int iPhase = batchedCons.m_phaseOrder[iiPhase];
1415 const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
1416 int grainSize = 1;
1417 leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop);
1418 }
1419 }
1420 else
1421 {
1422 // no batching, also ignores SOLVER_RANDMIZE_ORDER
1423 for (int j = 0; j < numRollingFrictionPoolConstraints; j++)
1424 {
1425 btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
1426 if (rollingFrictionConstraint.m_frictionIndex >= 0)
1427 {
1428 btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
1429 if (totalImpulse > 0.0f)
1430 {
1431 btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
1432 if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
1433 rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
1434
1435 rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
1436 rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
1437
1438 btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
1439 leastSquaresResidual += residual * residual;
1440 }
1441 }
1442 }
1443 }
1444 return leastSquaresResidual;
1445 }
1446
internalWriteBackContacts(int iBegin,int iEnd,const btContactSolverInfo & infoGlobal)1447 void btSequentialImpulseConstraintSolverMt::internalWriteBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
1448 {
1449 BT_PROFILE("internalWriteBackContacts");
1450 writeBackContacts(iBegin, iEnd, infoGlobal);
1451 //for ( int iContact = iBegin; iContact < iEnd; ++iContact)
1452 //{
1453 // const btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[ iContact ];
1454 // btManifoldPoint* pt = (btManifoldPoint*) contactConstraint.m_originalContactPoint;
1455 // btAssert( pt );
1456 // pt->m_appliedImpulse = contactConstraint.m_appliedImpulse;
1457 // pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[ contactConstraint.m_frictionIndex ].m_appliedImpulse;
1458 // if ( m_numFrictionDirections == 2 )
1459 // {
1460 // pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[ contactConstraint.m_frictionIndex + 1 ].m_appliedImpulse;
1461 // }
1462 //}
1463 }
1464
internalWriteBackJoints(int iBegin,int iEnd,const btContactSolverInfo & infoGlobal)1465 void btSequentialImpulseConstraintSolverMt::internalWriteBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
1466 {
1467 BT_PROFILE("internalWriteBackJoints");
1468 writeBackJoints(iBegin, iEnd, infoGlobal);
1469 }
1470
internalWriteBackBodies(int iBegin,int iEnd,const btContactSolverInfo & infoGlobal)1471 void btSequentialImpulseConstraintSolverMt::internalWriteBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
1472 {
1473 BT_PROFILE("internalWriteBackBodies");
1474 writeBackBodies(iBegin, iEnd, infoGlobal);
1475 }
1476
1477 struct WriteContactPointsLoop : public btIParallelForBody
1478 {
1479 btSequentialImpulseConstraintSolverMt* m_solver;
1480 const btContactSolverInfo* m_infoGlobal;
1481
WriteContactPointsLoopWriteContactPointsLoop1482 WriteContactPointsLoop(btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal)
1483 {
1484 m_solver = solver;
1485 m_infoGlobal = &infoGlobal;
1486 }
forLoopWriteContactPointsLoop1487 void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
1488 {
1489 m_solver->internalWriteBackContacts(iBegin, iEnd, *m_infoGlobal);
1490 }
1491 };
1492
1493 struct WriteJointsLoop : public btIParallelForBody
1494 {
1495 btSequentialImpulseConstraintSolverMt* m_solver;
1496 const btContactSolverInfo* m_infoGlobal;
1497
WriteJointsLoopWriteJointsLoop1498 WriteJointsLoop(btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal)
1499 {
1500 m_solver = solver;
1501 m_infoGlobal = &infoGlobal;
1502 }
forLoopWriteJointsLoop1503 void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
1504 {
1505 m_solver->internalWriteBackJoints(iBegin, iEnd, *m_infoGlobal);
1506 }
1507 };
1508
1509 struct WriteBodiesLoop : public btIParallelForBody
1510 {
1511 btSequentialImpulseConstraintSolverMt* m_solver;
1512 const btContactSolverInfo* m_infoGlobal;
1513
WriteBodiesLoopWriteBodiesLoop1514 WriteBodiesLoop(btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal)
1515 {
1516 m_solver = solver;
1517 m_infoGlobal = &infoGlobal;
1518 }
forLoopWriteBodiesLoop1519 void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
1520 {
1521 m_solver->internalWriteBackBodies(iBegin, iEnd, *m_infoGlobal);
1522 }
1523 };
1524
solveGroupCacheFriendlyFinish(btCollisionObject ** bodies,int numBodies,const btContactSolverInfo & infoGlobal)1525 btScalar btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
1526 {
1527 BT_PROFILE("solveGroupCacheFriendlyFinish");
1528
1529 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
1530 {
1531 WriteContactPointsLoop loop(this, infoGlobal);
1532 int grainSize = 500;
1533 btParallelFor(0, m_tmpSolverContactConstraintPool.size(), grainSize, loop);
1534 }
1535
1536 {
1537 WriteJointsLoop loop(this, infoGlobal);
1538 int grainSize = 400;
1539 btParallelFor(0, m_tmpSolverNonContactConstraintPool.size(), grainSize, loop);
1540 }
1541 {
1542 WriteBodiesLoop loop(this, infoGlobal);
1543 int grainSize = 100;
1544 btParallelFor(0, m_tmpSolverBodyPool.size(), grainSize, loop);
1545 }
1546
1547 m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
1548 m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
1549 m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0);
1550 m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0);
1551
1552 m_tmpSolverBodyPool.resizeNoInitialize(0);
1553 return 0.f;
1554 }
1555