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 #ifndef BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_MT_H
17 #define BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_MT_H
18
19 #include "btSequentialImpulseConstraintSolver.h"
20 #include "btBatchedConstraints.h"
21 #include "LinearMath/btThreads.h"
22
23 ///
24 /// btSequentialImpulseConstraintSolverMt
25 ///
26 /// A multithreaded variant of the sequential impulse constraint solver. The constraints to be solved are grouped into
27 /// batches and phases where each batch of constraints within a given phase can be solved in parallel with the rest.
28 /// Ideally we want as few phases as possible, and each phase should have many batches, and all of the batches should
29 /// have about the same number of constraints.
30 /// This method works best on a large island of many constraints.
31 ///
32 /// Supports all of the features of the normal sequential impulse solver such as:
33 /// - split penetration impulse
34 /// - rolling friction
35 /// - interleaving constraints
36 /// - warmstarting
37 /// - 2 friction directions
38 /// - randomized constraint ordering
39 /// - early termination when leastSquaresResidualThreshold is satisfied
40 ///
41 /// When the SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS flag is enabled, unlike the normal SequentialImpulse solver,
42 /// the rolling friction is interleaved as well.
43 /// Interleaving the contact penetration constraints with friction reduces the number of parallel loops that need to be done,
44 /// which reduces threading overhead so it can be a performance win, however, it does seem to produce a less stable simulation,
45 /// at least on stacks of blocks.
46 ///
47 /// When the SOLVER_RANDMIZE_ORDER flag is enabled, the ordering of phases, and the ordering of constraints within each batch
48 /// is randomized, however it does not swap constraints between batches.
49 /// This is to avoid regenerating the batches for each solver iteration which would be quite costly in performance.
50 ///
51 /// Note that a non-zero leastSquaresResidualThreshold could possibly affect the determinism of the simulation
52 /// if the task scheduler's parallelSum operation is non-deterministic. The parallelSum operation can be non-deterministic
53 /// because floating point addition is not associative due to rounding errors.
54 /// The task scheduler can and should ensure that the result of any parallelSum operation is deterministic.
55 ///
ATTRIBUTE_ALIGNED16(class)56 ATTRIBUTE_ALIGNED16(class)
57 btSequentialImpulseConstraintSolverMt : public btSequentialImpulseConstraintSolver
58 {
59 public:
60 virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) BT_OVERRIDE;
61 virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) BT_OVERRIDE;
62 virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) BT_OVERRIDE;
63 virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
64
65 // temp struct used to collect info from persistent manifolds into a cache-friendly struct using multiple threads
66 struct btContactManifoldCachedInfo
67 {
68 static const int MAX_NUM_CONTACT_POINTS = 4;
69
70 int numTouchingContacts;
71 int solverBodyIds[2];
72 int contactIndex;
73 int rollingFrictionIndex;
74 bool contactHasRollingFriction[MAX_NUM_CONTACT_POINTS];
75 btManifoldPoint* contactPoints[MAX_NUM_CONTACT_POINTS];
76 };
77 // temp struct used for setting up joint constraints in parallel
78 struct JointParams
79 {
80 int m_solverConstraint;
81 int m_solverBodyA;
82 int m_solverBodyB;
83 };
84 void internalInitMultipleJoints(btTypedConstraint * *constraints, int iBegin, int iEnd);
85 void internalConvertMultipleJoints(const btAlignedObjectArray<JointParams>& jointParamsArray, btTypedConstraint** constraints, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
86
87 // parameters to control batching
88 static bool s_allowNestedParallelForLoops; // whether to allow nested parallel operations
89 static int s_minimumContactManifoldsForBatching; // don't even try to batch if fewer manifolds than this
90 static btBatchedConstraints::BatchingMethod s_contactBatchingMethod;
91 static btBatchedConstraints::BatchingMethod s_jointBatchingMethod;
92 static int s_minBatchSize; // desired number of constraints per batch
93 static int s_maxBatchSize;
94
95 protected:
96 static const int CACHE_LINE_SIZE = 64;
97
98 btBatchedConstraints m_batchedContactConstraints;
99 btBatchedConstraints m_batchedJointConstraints;
100 int m_numFrictionDirections;
101 bool m_useBatching;
102 bool m_useObsoleteJointConstraints;
103 btAlignedObjectArray<btContactManifoldCachedInfo> m_manifoldCachedInfoArray;
104 btAlignedObjectArray<int> m_rollingFrictionIndexTable; // lookup table mapping contact index to rolling friction index
105 btSpinMutex m_bodySolverArrayMutex;
106 char m_antiFalseSharingPadding[CACHE_LINE_SIZE]; // padding to keep mutexes in separate cachelines
107 btSpinMutex m_kinematicBodyUniqueIdToSolverBodyTableMutex;
108 btAlignedObjectArray<char> m_scratchMemory;
109
110 virtual void randomizeConstraintOrdering(int iteration, int numIterations);
111 virtual btScalar resolveAllJointConstraints(int iteration);
112 virtual btScalar resolveAllContactConstraints();
113 virtual btScalar resolveAllContactFrictionConstraints();
114 virtual btScalar resolveAllContactConstraintsInterleaved();
115 virtual btScalar resolveAllRollingFrictionConstraints();
116
117 virtual void setupBatchedContactConstraints();
118 virtual void setupBatchedJointConstraints();
119 virtual void convertJoints(btTypedConstraint * *constraints, int numConstraints, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
120 virtual void convertContacts(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
121 virtual void convertBodies(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
122
123 int getOrInitSolverBodyThreadsafe(btCollisionObject & body, btScalar timeStep);
124 void allocAllContactConstraints(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
125 void setupAllContactConstraints(const btContactSolverInfo& infoGlobal);
126 void randomizeBatchedConstraintOrdering(btBatchedConstraints * batchedConstraints);
127
128 public:
129 BT_DECLARE_ALIGNED_ALLOCATOR();
130
131 btSequentialImpulseConstraintSolverMt();
132 virtual ~btSequentialImpulseConstraintSolverMt();
133
134 btScalar resolveMultipleJointConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd, int iteration);
135 btScalar resolveMultipleContactConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd);
136 btScalar resolveMultipleContactSplitPenetrationImpulseConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd);
137 btScalar resolveMultipleContactFrictionConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd);
138 btScalar resolveMultipleContactRollingFrictionConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd);
139 btScalar resolveMultipleContactConstraintsInterleaved(const btAlignedObjectArray<int>& contactIndices, int batchBegin, int batchEnd);
140
141 void internalCollectContactManifoldCachedInfo(btContactManifoldCachedInfo * cachedInfoArray, btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
142 void internalAllocContactConstraints(const btContactManifoldCachedInfo* cachedInfoArray, int numManifolds);
143 void internalSetupContactConstraints(int iContactConstraint, const btContactSolverInfo& infoGlobal);
144 void internalConvertBodies(btCollisionObject * *bodies, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
145 void internalWriteBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
146 void internalWriteBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
147 void internalWriteBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
148 };
149
150 #endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_MT_H
151