Bullet Collision Detection & Physics Library: btBatchedConstraints.cpp Source File

Bullet Collision Detection & Physics Library

btBatchedConstraints.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
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 "btBatchedConstraints.h"
17
18 #include "LinearMath/btIDebugDraw.h"
19 #include "LinearMath/btMinMax.h"
20 #include "LinearMath/btStackAlloc.h"
21 #include "LinearMath/btQuickprof.h"
22
23 #include <string.h> //for memset
24
25 const int kNoMerge = -1;
26
27 bool btBatchedConstraints::s_debugDrawBatches = false;
28
29 struct btBatchedConstraintInfo
30 {
31 int constraintIndex;
32 int numConstraintRows;
33 int bodyIds[2];
34 };
35
36 struct btBatchInfo
37 {
38 int numConstraints;
39 int mergeIndex;
40
41 btBatchInfo() : numConstraints(0), mergeIndex(kNoMerge) {}
42 };
43
44 bool btBatchedConstraints::validate(btConstraintArray* constraints, const btAlignedObjectArray<btSolverBody>& bodies) const
45 {
46 //
47 // validate: for debugging only. Verify coloring of bodies, that no body is touched by more than one batch in any given phase
48 //
49 int errors = 0;
50 const int kUnassignedBatch = -1;
51
52 btAlignedObjectArray<int> bodyBatchId;
53 for (int iPhase = 0; iPhase < m_phases.size(); ++iPhase)
54 {
55 bodyBatchId.resizeNoInitialize(0);
56 bodyBatchId.resize(bodies.size(), kUnassignedBatch);
57 const Range& phase = m_phases[iPhase];
58 for (int iBatch = phase.begin; iBatch < phase.end; ++iBatch)
59 {
60 const Range& batch = m_batches[iBatch];
61 for (int iiCons = batch.begin; iiCons < batch.end; ++iiCons)
62 {
63 int iCons = m_constraintIndices[iiCons];
64 const btSolverConstraint& cons = constraints->at(iCons);
65 const btSolverBody& bodyA = bodies[cons.m_solverBodyIdA];
66 const btSolverBody& bodyB = bodies[cons.m_solverBodyIdB];
67 if (!bodyA.internalGetInvMass().isZero())
68 {
69 int thisBodyBatchId = bodyBatchId[cons.m_solverBodyIdA];
70 if (thisBodyBatchId == kUnassignedBatch)
71 {
72 bodyBatchId[cons.m_solverBodyIdA] = iBatch;
73 }
74 else if (thisBodyBatchId != iBatch)
75 {
76 btAssert(!"dynamic body is used in 2 different batches in the same phase");
77 errors++;
78 }
79 }
80 if (!bodyB.internalGetInvMass().isZero())
81 {
82 int thisBodyBatchId = bodyBatchId[cons.m_solverBodyIdB];
83 if (thisBodyBatchId == kUnassignedBatch)
84 {
85 bodyBatchId[cons.m_solverBodyIdB] = iBatch;
86 }
87 else if (thisBodyBatchId != iBatch)
88 {
89 btAssert(!"dynamic body is used in 2 different batches in the same phase");
90 errors++;
91 }
92 }
93 }
94 }
95 }
96 return errors == 0;
97 }
98
99 static void debugDrawSingleBatch(const btBatchedConstraints* bc,
100 btConstraintArray* constraints,
101 const btAlignedObjectArray<btSolverBody>& bodies,
102 int iBatch,
103 const btVector3& color,
104 const btVector3& offset)
105 {
106 if (bc && bc->m_debugDrawer && iBatch < bc->m_batches.size())
107 {
108 const btBatchedConstraints::Range& b = bc->m_batches[iBatch];
109 for (int iiCon = b.begin; iiCon < b.end; ++iiCon)
110 {
111 int iCon = bc->m_constraintIndices[iiCon];
112 const btSolverConstraint& con = constraints->at(iCon);
113 int iBody0 = con.m_solverBodyIdA;
114 int iBody1 = con.m_solverBodyIdB;
115 btVector3 pos0 = bodies[iBody0].getWorldTransform().getOrigin() + offset;
116 btVector3 pos1 = bodies[iBody1].getWorldTransform().getOrigin() + offset;
117 bc->m_debugDrawer->drawLine(pos0, pos1, color);
118 }
119 }
120 }
121
122 static void debugDrawPhase(const btBatchedConstraints* bc,
123 btConstraintArray* constraints,
124 const btAlignedObjectArray<btSolverBody>& bodies,
125 int iPhase,
126 const btVector3& color0,
127 const btVector3& color1,
128 const btVector3& offset)
129 {
130 BT_PROFILE("debugDrawPhase");
131 if (bc && bc->m_debugDrawer && iPhase < bc->m_phases.size())
132 {
133 const btBatchedConstraints::Range& phase = bc->m_phases[iPhase];
134 for (int iBatch = phase.begin; iBatch < phase.end; ++iBatch)
135 {
136 float tt = float(iBatch - phase.begin) / float(btMax(1, phase.end - phase.begin - 1));
137 btVector3 col = lerp(color0, color1, tt);
138 debugDrawSingleBatch(bc, constraints, bodies, iBatch, col, offset);
139 }
140 }
141 }
142
143 static void debugDrawAllBatches(const btBatchedConstraints* bc,
144 btConstraintArray* constraints,
145 const btAlignedObjectArray<btSolverBody>& bodies)
146 {
147 BT_PROFILE("debugDrawAllBatches");
148 if (bc && bc->m_debugDrawer && bc->m_phases.size() > 0)
149 {
150 btVector3 bboxMin(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT);
151 btVector3 bboxMax = -bboxMin;
152 for (int iBody = 0; iBody < bodies.size(); ++iBody)
153 {
154 const btVector3& pos = bodies[iBody].getWorldTransform().getOrigin();
155 bboxMin.setMin(pos);
156 bboxMax.setMax(pos);
157 }
158 btVector3 bboxExtent = bboxMax - bboxMin;
159 btVector3 offsetBase = btVector3(0, bboxExtent.y() * 1.1f, 0);
160 btVector3 offsetStep = btVector3(0, 0, bboxExtent.z() * 1.1f);
161 int numPhases = bc->m_phases.size();
162 for (int iPhase = 0; iPhase < numPhases; ++iPhase)
163 {
164 float b = float(iPhase) / float(numPhases - 1);
165 btVector3 color0 = btVector3(1, 0, b);
166 btVector3 color1 = btVector3(0, 1, b);
167 btVector3 offset = offsetBase + offsetStep * (float(iPhase) - float(numPhases - 1) * 0.5);
168 debugDrawPhase(bc, constraints, bodies, iPhase, color0, color1, offset);
169 }
170 }
171 }
172
173 static void initBatchedBodyDynamicFlags(btAlignedObjectArray<bool>* outBodyDynamicFlags, const btAlignedObjectArray<btSolverBody>& bodies)
174 {
175 BT_PROFILE("initBatchedBodyDynamicFlags");
176 btAlignedObjectArray<bool>& bodyDynamicFlags = *outBodyDynamicFlags;
177 bodyDynamicFlags.resizeNoInitialize(bodies.size());
178 for (int i = 0; i < bodies.size(); ++i)
179 {
180 const btSolverBody& body = bodies[i];
181 bodyDynamicFlags[i] = (body.internalGetInvMass().x() > btScalar(0));
182 }
183 }
184
185 static int runLengthEncodeConstraintInfo(btBatchedConstraintInfo* outConInfos, int numConstraints)
186 {
187 BT_PROFILE("runLengthEncodeConstraintInfo");
188 // detect and run-length encode constraint rows that repeat the same bodies
189 int iDest = 0;
190 int iSrc = 0;
191 while (iSrc < numConstraints)
192 {
193 const btBatchedConstraintInfo& srcConInfo = outConInfos[iSrc];
194 btBatchedConstraintInfo& conInfo = outConInfos[iDest];
195 conInfo.constraintIndex = iSrc;
196 conInfo.bodyIds[0] = srcConInfo.bodyIds[0];
197 conInfo.bodyIds[1] = srcConInfo.bodyIds[1];
198 while (iSrc < numConstraints && outConInfos[iSrc].bodyIds[0] == srcConInfo.bodyIds[0] && outConInfos[iSrc].bodyIds[1] == srcConInfo.bodyIds[1])
199 {
200 ++iSrc;
201 }
202 conInfo.numConstraintRows = iSrc - conInfo.constraintIndex;
203 ++iDest;
204 }
205 return iDest;
206 }
207
208 struct ReadSolverConstraintsLoop : public btIParallelForBody
209 {
210 btBatchedConstraintInfo* m_outConInfos;
211 btConstraintArray* m_constraints;
212
213 ReadSolverConstraintsLoop(btBatchedConstraintInfo* outConInfos, btConstraintArray* constraints)
214 {
215 m_outConInfos = outConInfos;
216 m_constraints = constraints;
217 }
218 void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
219 {
220 for (int i = iBegin; i < iEnd; ++i)
221 {
222 btBatchedConstraintInfo& conInfo = m_outConInfos[i];
223 const btSolverConstraint& con = m_constraints->at(i);
224 conInfo.bodyIds[0] = con.m_solverBodyIdA;
225 conInfo.bodyIds[1] = con.m_solverBodyIdB;
226 conInfo.constraintIndex = i;
227 conInfo.numConstraintRows = 1;
228 }
229 }
230 };
231
232 static int initBatchedConstraintInfo(btBatchedConstraintInfo* outConInfos, btConstraintArray* constraints)
233 {
234 BT_PROFILE("initBatchedConstraintInfo");
235 int numConstraints = constraints->size();
236 bool inParallel = true;
237 if (inParallel)
238 {
239 ReadSolverConstraintsLoop loop(outConInfos, constraints);
240 int grainSize = 1200;
241 btParallelFor(0, numConstraints, grainSize, loop);
242 }
243 else
244 {
245 for (int i = 0; i < numConstraints; ++i)
246 {
247 btBatchedConstraintInfo& conInfo = outConInfos[i];
248 const btSolverConstraint& con = constraints->at(i);
249 conInfo.bodyIds[0] = con.m_solverBodyIdA;
250 conInfo.bodyIds[1] = con.m_solverBodyIdB;
251 conInfo.constraintIndex = i;
252 conInfo.numConstraintRows = 1;
253 }
254 }
255 bool useRunLengthEncoding = true;
256 if (useRunLengthEncoding)
257 {
258 numConstraints = runLengthEncodeConstraintInfo(outConInfos, numConstraints);
259 }
260 return numConstraints;
261 }
262
263 static void expandConstraintRowsInPlace(int* constraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraints, int numConstraintRows)
264 {
265 BT_PROFILE("expandConstraintRowsInPlace");
266 if (numConstraintRows > numConstraints)
267 {
268 // we walk the array in reverse to avoid overwriteing
269 for (int iCon = numConstraints - 1; iCon >= 0; --iCon)
270 {
271 const btBatchedConstraintInfo& conInfo = conInfos[iCon];
272 int iBatch = constraintBatchIds[iCon];
273 for (int i = conInfo.numConstraintRows - 1; i >= 0; --i)
274 {
275 int iDest = conInfo.constraintIndex + i;
276 btAssert(iDest >= iCon);
277 btAssert(iDest >= 0 && iDest < numConstraintRows);
278 constraintBatchIds[iDest] = iBatch;
279 }
280 }
281 }
282 }
283
284 static void expandConstraintRows(int* destConstraintBatchIds, const int* srcConstraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraints, int numConstraintRows)
285 {
286 BT_PROFILE("expandConstraintRows");
287 for (int iCon = 0; iCon < numConstraints; ++iCon)
288 {
289 const btBatchedConstraintInfo& conInfo = conInfos[iCon];
290 int iBatch = srcConstraintBatchIds[iCon];
291 for (int i = 0; i < conInfo.numConstraintRows; ++i)
292 {
293 int iDest = conInfo.constraintIndex + i;
294 btAssert(iDest >= iCon);
295 btAssert(iDest >= 0 && iDest < numConstraintRows);
296 destConstraintBatchIds[iDest] = iBatch;
297 }
298 }
299 }
300
301 struct ExpandConstraintRowsLoop : public btIParallelForBody
302 {
303 int* m_destConstraintBatchIds;
304 const int* m_srcConstraintBatchIds;
305 const btBatchedConstraintInfo* m_conInfos;
306 int m_numConstraintRows;
307
308 ExpandConstraintRowsLoop(int* destConstraintBatchIds, const int* srcConstraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraintRows)
309 {
310 m_destConstraintBatchIds = destConstraintBatchIds;
311 m_srcConstraintBatchIds = srcConstraintBatchIds;
312 m_conInfos = conInfos;
313 m_numConstraintRows = numConstraintRows;
314 }
315 void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
316 {
317 expandConstraintRows(m_destConstraintBatchIds, m_srcConstraintBatchIds + iBegin, m_conInfos + iBegin, iEnd - iBegin, m_numConstraintRows);
318 }
319 };
320
321 static void expandConstraintRowsMt(int* destConstraintBatchIds, const int* srcConstraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraints, int numConstraintRows)
322 {
323 BT_PROFILE("expandConstraintRowsMt");
324 ExpandConstraintRowsLoop loop(destConstraintBatchIds, srcConstraintBatchIds, conInfos, numConstraintRows);
325 int grainSize = 600;
326 btParallelFor(0, numConstraints, grainSize, loop);
327 }
328
329 static void initBatchedConstraintInfoArray(btAlignedObjectArray<btBatchedConstraintInfo>* outConInfos, btConstraintArray* constraints)
330 {
331 BT_PROFILE("initBatchedConstraintInfoArray");
332 btAlignedObjectArray<btBatchedConstraintInfo>& conInfos = *outConInfos;
333 int numConstraints = constraints->size();
334 conInfos.resizeNoInitialize(numConstraints);
335
336 int newSize = initBatchedConstraintInfo(&outConInfos->at(0), constraints);
337 conInfos.resizeNoInitialize(newSize);
338 }
339
340 static void mergeSmallBatches(btBatchInfo* batches, int iBeginBatch, int iEndBatch, int minBatchSize, int maxBatchSize)
341 {
342 BT_PROFILE("mergeSmallBatches");
343 for (int iBatch = iEndBatch - 1; iBatch >= iBeginBatch; --iBatch)
344 {
345 btBatchInfo& batch = batches[iBatch];
346 if (batch.mergeIndex == kNoMerge && batch.numConstraints > 0 && batch.numConstraints < minBatchSize)
347 {
348 for (int iDestBatch = iBatch - 1; iDestBatch >= iBeginBatch; --iDestBatch)
349 {
350 btBatchInfo& destBatch = batches[iDestBatch];
351 if (destBatch.mergeIndex == kNoMerge && (destBatch.numConstraints + batch.numConstraints) < maxBatchSize)
352 {
353 destBatch.numConstraints += batch.numConstraints;
354 batch.numConstraints = 0;
355 batch.mergeIndex = iDestBatch;
356 break;
357 }
358 }
359 }
360 }
361 // flatten mergeIndexes
362 // e.g. in case where A was merged into B and then B was merged into C, we need A to point to C instead of B
363 // Note: loop goes forward through batches because batches always merge from higher indexes to lower,
364 // so by going from low to high it reduces the amount of trail-following
365 for (int iBatch = iBeginBatch; iBatch < iEndBatch; ++iBatch)
366 {
367 btBatchInfo& batch = batches[iBatch];
368 if (batch.mergeIndex != kNoMerge)
369 {
370 int iMergeDest = batches[batch.mergeIndex].mergeIndex;
371 // follow trail of merges to the end
372 while (iMergeDest != kNoMerge)
373 {
374 int iNext = batches[iMergeDest].mergeIndex;
375 if (iNext == kNoMerge)
376 {
377 batch.mergeIndex = iMergeDest;
378 break;
379 }
380 iMergeDest = iNext;
381 }
382 }
383 }
384 }
385
386 static void updateConstraintBatchIdsForMerges(int* constraintBatchIds, int numConstraints, const btBatchInfo* batches, int numBatches)
387 {
388 BT_PROFILE("updateConstraintBatchIdsForMerges");
389 // update batchIds to account for merges
390 for (int i = 0; i < numConstraints; ++i)
391 {
392 int iBatch = constraintBatchIds[i];
393 btAssert(iBatch < numBatches);
394 // if this constraint references a batch that was merged into another batch
395 if (batches[iBatch].mergeIndex != kNoMerge)
396 {
397 // update batchId
398 constraintBatchIds[i] = batches[iBatch].mergeIndex;
399 }
400 }
401 }
402
403 struct UpdateConstraintBatchIdsForMergesLoop : public btIParallelForBody
404 {
405 int* m_constraintBatchIds;
406 const btBatchInfo* m_batches;
407 int m_numBatches;
408
409 UpdateConstraintBatchIdsForMergesLoop(int* constraintBatchIds, const btBatchInfo* batches, int numBatches)
410 {
411 m_constraintBatchIds = constraintBatchIds;
412 m_batches = batches;
413 m_numBatches = numBatches;
414 }
415 void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
416 {
417 BT_PROFILE("UpdateConstraintBatchIdsForMergesLoop");
418 updateConstraintBatchIdsForMerges(m_constraintBatchIds + iBegin, iEnd - iBegin, m_batches, m_numBatches);
419 }
420 };
421
422 static void updateConstraintBatchIdsForMergesMt(int* constraintBatchIds, int numConstraints, const btBatchInfo* batches, int numBatches)
423 {
424 BT_PROFILE("updateConstraintBatchIdsForMergesMt");
425 UpdateConstraintBatchIdsForMergesLoop loop(constraintBatchIds, batches, numBatches);
426 int grainSize = 800;
427 btParallelFor(0, numConstraints, grainSize, loop);
428 }
429
430 inline bool BatchCompare(const btBatchedConstraints::Range& a, const btBatchedConstraints::Range& b)
431 {
432 int lenA = a.end - a.begin;
433 int lenB = b.end - b.begin;
434 return lenA > lenB;
435 }
436
437 static void writeOutConstraintIndicesForRangeOfBatches(btBatchedConstraints* bc,
438 const int* constraintBatchIds,
439 int numConstraints,
440 int* constraintIdPerBatch,
441 int batchBegin,
442 int batchEnd)
443 {
444 BT_PROFILE("writeOutConstraintIndicesForRangeOfBatches");
445 for (int iCon = 0; iCon < numConstraints; ++iCon)
446 {
447 int iBatch = constraintBatchIds[iCon];
448 if (iBatch >= batchBegin && iBatch < batchEnd)
449 {
450 int iDestCon = constraintIdPerBatch[iBatch];
451 constraintIdPerBatch[iBatch] = iDestCon + 1;
452 bc->m_constraintIndices[iDestCon] = iCon;
453 }
454 }
455 }
456
457 struct WriteOutConstraintIndicesLoop : public btIParallelForBody
458 {
459 btBatchedConstraints* m_batchedConstraints;
460 const int* m_constraintBatchIds;
461 int m_numConstraints;
462 int* m_constraintIdPerBatch;
463 int m_maxNumBatchesPerPhase;
464
465 WriteOutConstraintIndicesLoop(btBatchedConstraints* bc, const int* constraintBatchIds, int numConstraints, int* constraintIdPerBatch, int maxNumBatchesPerPhase)
466 {
467 m_batchedConstraints = bc;
468 m_constraintBatchIds = constraintBatchIds;
469 m_numConstraints = numConstraints;
470 m_constraintIdPerBatch = constraintIdPerBatch;
471 m_maxNumBatchesPerPhase = maxNumBatchesPerPhase;
472 }
473 void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
474 {
475 BT_PROFILE("WriteOutConstraintIndicesLoop");
476 int batchBegin = iBegin * m_maxNumBatchesPerPhase;
477 int batchEnd = iEnd * m_maxNumBatchesPerPhase;
478 writeOutConstraintIndicesForRangeOfBatches(m_batchedConstraints,
479 m_constraintBatchIds,
480 m_numConstraints,
481 m_constraintIdPerBatch,
482 batchBegin,
483 batchEnd);
484 }
485 };
486
487 static void writeOutConstraintIndicesMt(btBatchedConstraints* bc,
488 const int* constraintBatchIds,
489 int numConstraints,
490 int* constraintIdPerBatch,
491 int maxNumBatchesPerPhase,
492 int numPhases)
493 {
494 BT_PROFILE("writeOutConstraintIndicesMt");
495 bool inParallel = true;
496 if (inParallel)
497 {
498 WriteOutConstraintIndicesLoop loop(bc, constraintBatchIds, numConstraints, constraintIdPerBatch, maxNumBatchesPerPhase);
499 btParallelFor(0, numPhases, 1, loop);
500 }
501 else
502 {
503 for (int iCon = 0; iCon < numConstraints; ++iCon)
504 {
505 int iBatch = constraintBatchIds[iCon];
506 int iDestCon = constraintIdPerBatch[iBatch];
507 constraintIdPerBatch[iBatch] = iDestCon + 1;
508 bc->m_constraintIndices[iDestCon] = iCon;
509 }
510 }
511 }
512
513 static void writeGrainSizes(btBatchedConstraints* bc)
514 {
515 typedef btBatchedConstraints::Range Range;
516 int numPhases = bc->m_phases.size();
517 bc->m_phaseGrainSize.resizeNoInitialize(numPhases);
518 int numThreads = btGetTaskScheduler()->getNumThreads();
519 for (int iPhase = 0; iPhase < numPhases; ++iPhase)
520 {
521 const Range& phase = bc->m_phases[iPhase];
522 int numBatches = phase.end - phase.begin;
523 float grainSize = floor((0.25f * numBatches / float(numThreads)) + 0.0f);
524 bc->m_phaseGrainSize[iPhase] = btMax(1, int(grainSize));
525 }
526 }
527
528 static void writeOutBatches(btBatchedConstraints* bc,
529 const int* constraintBatchIds,
530 int numConstraints,
531 const btBatchInfo* batches,
532 int* batchWork,
533 int maxNumBatchesPerPhase,
534 int numPhases)
535 {
536 BT_PROFILE("writeOutBatches");
537 typedef btBatchedConstraints::Range Range;
538 bc->m_constraintIndices.reserve(numConstraints);
539 bc->m_batches.resizeNoInitialize(0);
540 bc->m_phases.resizeNoInitialize(0);
541
542 //int maxNumBatches = numPhases * maxNumBatchesPerPhase;
543 {
544 int* constraintIdPerBatch = batchWork; // for each batch, keep an index into the next available slot in the m_constraintIndices array
545 int iConstraint = 0;
546 for (int iPhase = 0; iPhase < numPhases; ++iPhase)
547 {
548 int curPhaseBegin = bc->m_batches.size();
549 int iBegin = iPhase * maxNumBatchesPerPhase;
550 int iEnd = iBegin + maxNumBatchesPerPhase;
551 for (int i = iBegin; i < iEnd; ++i)
552 {
553 const btBatchInfo& batch = batches[i];
554 int curBatchBegin = iConstraint;
555 constraintIdPerBatch[i] = curBatchBegin; // record the start of each batch in m_constraintIndices array
556 int numConstraints = batch.numConstraints;
557 iConstraint += numConstraints;
558 if (numConstraints > 0)
559 {
560 bc->m_batches.push_back(Range(curBatchBegin, iConstraint));
561 }
562 }
563 // if any batches were emitted this phase,
564 if (bc->m_batches.size() > curPhaseBegin)
565 {
566 // output phase
567 bc->m_phases.push_back(Range(curPhaseBegin, bc->m_batches.size()));
568 }
569 }
570
571 btAssert(iConstraint == numConstraints);
572 bc->m_constraintIndices.resizeNoInitialize(numConstraints);
573 writeOutConstraintIndicesMt(bc, constraintBatchIds, numConstraints, constraintIdPerBatch, maxNumBatchesPerPhase, numPhases);
574 }
575 // for each phase
576 for (int iPhase = 0; iPhase < bc->m_phases.size(); ++iPhase)
577 {
578 // sort the batches from largest to smallest (can be helpful to some task schedulers)
579 const Range& curBatches = bc->m_phases[iPhase];
580 bc->m_batches.quickSortInternal(BatchCompare, curBatches.begin, curBatches.end - 1);
581 }
582 bc->m_phaseOrder.resize(bc->m_phases.size());
583 for (int i = 0; i < bc->m_phases.size(); ++i)
584 {
585 bc->m_phaseOrder[i] = i;
586 }
587 writeGrainSizes(bc);
588 }
589
590 //
591 // PreallocatedMemoryHelper -- helper object for allocating a number of chunks of memory in a single contiguous block.
592 // It is generally more efficient to do a single larger allocation than many smaller allocations.
593 //
594 // Example Usage:
595 //
596 // btVector3* bodyPositions = NULL;
597 // btBatchedConstraintInfo* conInfos = NULL;
598 // {
599 // PreallocatedMemoryHelper<8> memHelper;
600 // memHelper.addChunk( (void**) &bodyPositions, sizeof( btVector3 ) * bodies.size() );
601 // memHelper.addChunk( (void**) &conInfos, sizeof( btBatchedConstraintInfo ) * numConstraints );
602 // void* memPtr = malloc( memHelper.getSizeToAllocate() ); // allocate the memory
603 // memHelper.setChunkPointers( memPtr ); // update pointers to chunks
604 // }
605 template <int N>
606 class PreallocatedMemoryHelper
607 {
608 struct Chunk
609 {
610 void** ptr;
611 size_t size;
612 };
613 Chunk m_chunks[N];
614 int m_numChunks;
615
616 public:
617 PreallocatedMemoryHelper() { m_numChunks = 0; }
618 void addChunk(void** ptr, size_t sz)
619 {
620 btAssert(m_numChunks < N);
621 if (m_numChunks < N)
622 {
623 Chunk& chunk = m_chunks[m_numChunks];
624 chunk.ptr = ptr;
625 chunk.size = sz;
626 m_numChunks++;
627 }
628 }
629 size_t getSizeToAllocate() const
630 {
631 size_t totalSize = 0;
632 for (int i = 0; i < m_numChunks; ++i)
633 {
634 totalSize += m_chunks[i].size;
635 }
636 return totalSize;
637 }
638 void setChunkPointers(void* mem) const
639 {
640 size_t totalSize = 0;
641 for (int i = 0; i < m_numChunks; ++i)
642 {
643 const Chunk& chunk = m_chunks[i];
644 char* chunkPtr = static_cast<char*>(mem) + totalSize;
645 *chunk.ptr = chunkPtr;
646 totalSize += chunk.size;
647 }
648 }
649 };
650
651 static btVector3 findMaxDynamicConstraintExtent(
652 btVector3* bodyPositions,
653 bool* bodyDynamicFlags,
654 btBatchedConstraintInfo* conInfos,
655 int numConstraints,
656 int numBodies)
657 {
658 BT_PROFILE("findMaxDynamicConstraintExtent");
659 btVector3 consExtent = btVector3(1, 1, 1) * 0.001;
660 for (int iCon = 0; iCon < numConstraints; ++iCon)
661 {
662 const btBatchedConstraintInfo& con = conInfos[iCon];
663 int iBody0 = con.bodyIds[0];
664 int iBody1 = con.bodyIds[1];
665 btAssert(iBody0 >= 0 && iBody0 < numBodies);
666 btAssert(iBody1 >= 0 && iBody1 < numBodies);
667 // is it a dynamic constraint?
668 if (bodyDynamicFlags[iBody0] && bodyDynamicFlags[iBody1])
669 {
670 btVector3 delta = bodyPositions[iBody1] - bodyPositions[iBody0];
671 consExtent.setMax(delta.absolute());
672 }
673 }
674 return consExtent;
675 }
676
677 struct btIntVec3
678 {
679 int m_ints[3];
680
681 SIMD_FORCE_INLINE const int& operator[](int i) const { return m_ints[i]; }
682 SIMD_FORCE_INLINE int& operator[](int i) { return m_ints[i]; }
683 };
684
685 struct AssignConstraintsToGridBatchesParams
686 {
687 bool* bodyDynamicFlags;
688 btIntVec3* bodyGridCoords;
689 int numBodies;
690 btBatchedConstraintInfo* conInfos;
691 int* constraintBatchIds;
692 btIntVec3 gridChunkDim;
693 int maxNumBatchesPerPhase;
694 int numPhases;
695 int phaseMask;
696
697 AssignConstraintsToGridBatchesParams()
698 {
699 memset(this, 0, sizeof(*this));
700 }
701 };
702
703 static void assignConstraintsToGridBatches(const AssignConstraintsToGridBatchesParams& params, int iConBegin, int iConEnd)
704 {
705 BT_PROFILE("assignConstraintsToGridBatches");
706 // (can be done in parallel)
707 for (int iCon = iConBegin; iCon < iConEnd; ++iCon)
708 {
709 const btBatchedConstraintInfo& con = params.conInfos[iCon];
710 int iBody0 = con.bodyIds[0];
711 int iBody1 = con.bodyIds[1];
712 int iPhase = iCon; //iBody0; // pseudorandom choice to distribute evenly amongst phases
713 iPhase &= params.phaseMask;
714 int gridCoord[3];
715 // is it a dynamic constraint?
716 if (params.bodyDynamicFlags[iBody0] && params.bodyDynamicFlags[iBody1])
717 {
718 const btIntVec3& body0Coords = params.bodyGridCoords[iBody0];
719 const btIntVec3& body1Coords = params.bodyGridCoords[iBody1];
720 // for each dimension x,y,z,
721 for (int i = 0; i < 3; ++i)
722 {
723 int coordMin = btMin(body0Coords.m_ints[i], body1Coords.m_ints[i]);
724 int coordMax = btMax(body0Coords.m_ints[i], body1Coords.m_ints[i]);
725 if (coordMin != coordMax)
726 {
727 btAssert(coordMax == coordMin + 1);
728 if ((coordMin & 1) == 0)
729 {
730 iPhase &= ~(1 << i); // force bit off
731 }
732 else
733 {
734 iPhase |= (1 << i); // force bit on
735 iPhase &= params.phaseMask;
736 }
737 }
738 gridCoord[i] = coordMin;
739 }
740 }
741 else
742 {
743 if (!params.bodyDynamicFlags[iBody0])
744 {
745 iBody0 = con.bodyIds[1];
746 }
747 btAssert(params.bodyDynamicFlags[iBody0]);
748 const btIntVec3& body0Coords = params.bodyGridCoords[iBody0];
749 // for each dimension x,y,z,
750 for (int i = 0; i < 3; ++i)
751 {
752 gridCoord[i] = body0Coords.m_ints[i];
753 }
754 }
755 // calculate chunk coordinates
756 int chunkCoord[3];
757 btIntVec3 gridChunkDim = params.gridChunkDim;
758 // for each dimension x,y,z,
759 for (int i = 0; i < 3; ++i)
760 {
761 int coordOffset = (iPhase >> i) & 1;
762 chunkCoord[i] = (gridCoord[i] - coordOffset) / 2;
763 btClamp(chunkCoord[i], 0, gridChunkDim[i] - 1);
764 btAssert(chunkCoord[i] < gridChunkDim[i]);
765 }
766 int iBatch = iPhase * params.maxNumBatchesPerPhase + chunkCoord[0] + chunkCoord[1] * gridChunkDim[0] + chunkCoord[2] * gridChunkDim[0] * gridChunkDim[1];
767 btAssert(iBatch >= 0 && iBatch < params.maxNumBatchesPerPhase * params.numPhases);
768 params.constraintBatchIds[iCon] = iBatch;
769 }
770 }
771
772 struct AssignConstraintsToGridBatchesLoop : public btIParallelForBody
773 {
774 const AssignConstraintsToGridBatchesParams* m_params;
775
776 AssignConstraintsToGridBatchesLoop(const AssignConstraintsToGridBatchesParams& params)
777 {
778 m_params = &params;
779 }
780 void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
781 {
782 assignConstraintsToGridBatches(*m_params, iBegin, iEnd);
783 }
784 };
785
786 //
787 // setupSpatialGridBatchesMt -- generate batches using a uniform 3D grid
788 //
789 /*
790
791 Bodies are treated as 3D points at their center of mass. We only consider dynamic bodies at this stage,
792 because only dynamic bodies are mutated when a constraint is solved, thus subject to race conditions.
793
794 1. Compute a bounding box around all dynamic bodies
795 2. Compute the maximum extent of all dynamic constraints. Each dynamic constraint is treated as a line segment, and we need the size of
796 box that will fully enclose any single dynamic constraint
797
798 3. Establish the cell size of our grid, the cell size in each dimension must be at least as large as the dynamic constraints max-extent,
799 so that no dynamic constraint can span more than 2 cells of our grid on any axis of the grid. The cell size should be adjusted
800 larger in order to keep the total number of cells from being excessively high
801
802 Key idea: Given that each constraint spans 1 or 2 grid cells in each dimension, we can handle all constraints by processing
803 in chunks of 2x2x2 cells with 8 different 1-cell offsets ((0,0,0),(0,0,1),(0,1,0),(0,1,1),(1,0,0)...).
804 For each of the 8 offsets, we create a phase, and for each 2x2x2 chunk with dynamic constraints becomes a batch in that phase.
805
806 4. Once the grid is established, we can calculate for each constraint which phase and batch it belongs in.
807
808 5. Do a merge small batches on the batches of each phase separately, to try to even out the sizes of batches
809
810 Optionally, we can "collapse" one dimension of our 3D grid to turn it into a 2D grid, which reduces the number of phases
811 to 4. With fewer phases, there are more constraints per phase and this makes it easier to create batches of a useful size.
812 */
813 //
814 static void setupSpatialGridBatchesMt(
815 btBatchedConstraints* batchedConstraints,
816 btAlignedObjectArray<char>* scratchMemory,
817 btConstraintArray* constraints,
818 const btAlignedObjectArray<btSolverBody>& bodies,
819 int minBatchSize,
820 int maxBatchSize,
821 bool use2DGrid)
822 {
823 BT_PROFILE("setupSpatialGridBatchesMt");
824 const int numPhases = 8;
825 int numConstraints = constraints->size();
826 int numConstraintRows = constraints->size();
827
828 const int maxGridChunkCount = 128;
829 int allocNumBatchesPerPhase = maxGridChunkCount;
830 int minNumBatchesPerPhase = 16;
831 int allocNumBatches = allocNumBatchesPerPhase * numPhases;
832
833 btVector3* bodyPositions = NULL;
834 bool* bodyDynamicFlags = NULL;
835 btIntVec3* bodyGridCoords = NULL;
836 btBatchInfo* batches = NULL;
837 int* batchWork = NULL;
838 btBatchedConstraintInfo* conInfos = NULL;
839 int* constraintBatchIds = NULL;
840 int* constraintRowBatchIds = NULL;
841 {
842 PreallocatedMemoryHelper<10> memHelper;
843 memHelper.addChunk((void**)&bodyPositions, sizeof(btVector3) * bodies.size());
844 memHelper.addChunk((void**)&bodyDynamicFlags, sizeof(bool) * bodies.size());
845 memHelper.addChunk((void**)&bodyGridCoords, sizeof(btIntVec3) * bodies.size());
846 memHelper.addChunk((void**)&batches, sizeof(btBatchInfo) * allocNumBatches);
847 memHelper.addChunk((void**)&batchWork, sizeof(int) * allocNumBatches);
848 memHelper.addChunk((void**)&conInfos, sizeof(btBatchedConstraintInfo) * numConstraints);
849 memHelper.addChunk((void**)&constraintBatchIds, sizeof(int) * numConstraints);
850 memHelper.addChunk((void**)&constraintRowBatchIds, sizeof(int) * numConstraintRows);
851 size_t scratchSize = memHelper.getSizeToAllocate();
852 // if we need to reallocate
853 if (scratchMemory->capacity() < scratchSize)
854 {
855 // allocate 6.25% extra to avoid repeated reallocs
856 scratchMemory->reserve(scratchSize + scratchSize / 16);
857 }
858 scratchMemory->resizeNoInitialize(scratchSize);
859 char* memPtr = &scratchMemory->at(0);
860 memHelper.setChunkPointers(memPtr);
861 }
862
863 numConstraints = initBatchedConstraintInfo(conInfos, constraints);
864
865 // compute bounding box around all dynamic bodies
866 // (could be done in parallel)
867 btVector3 bboxMin(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT);
868 btVector3 bboxMax = -bboxMin;
869 //int dynamicBodyCount = 0;
870 for (int i = 0; i < bodies.size(); ++i)
871 {
872 const btSolverBody& body = bodies[i];
873 btVector3 bodyPos = body.getWorldTransform().getOrigin();
874 bool isDynamic = (body.internalGetInvMass().x() > btScalar(0));
875 bodyPositions[i] = bodyPos;
876 bodyDynamicFlags[i] = isDynamic;
877 if (isDynamic)
878 {
879 //dynamicBodyCount++;
880 bboxMin.setMin(bodyPos);
881 bboxMax.setMax(bodyPos);
882 }
883 }
884
885 // find max extent of all dynamic constraints
886 // (could be done in parallel)
887 btVector3 consExtent = findMaxDynamicConstraintExtent(bodyPositions, bodyDynamicFlags, conInfos, numConstraints, bodies.size());
888
889 btVector3 gridExtent = bboxMax - bboxMin;
890
891 btVector3 gridCellSize = consExtent;
892 int gridDim[3];
893 gridDim[0] = int(1.0 + gridExtent.x() / gridCellSize.x());
894 gridDim[1] = int(1.0 + gridExtent.y() / gridCellSize.y());
895 gridDim[2] = int(1.0 + gridExtent.z() / gridCellSize.z());
896
897 // if we can collapse an axis, it will cut our number of phases in half which could be more efficient
898 int phaseMask = 7;
899 bool collapseAxis = use2DGrid;
900 if (collapseAxis)
901 {
902 // pick the smallest axis to collapse, leaving us with the greatest number of cells in our grid
903 int iAxisToCollapse = 0;
904 int axisDim = gridDim[iAxisToCollapse];
905 //for each dimension
906 for (int i = 0; i < 3; ++i)
907 {
908 if (gridDim[i] < axisDim)
909 {
910 iAxisToCollapse = i;
911 axisDim = gridDim[i];
912 }
913 }
914 // collapse it
915 gridCellSize[iAxisToCollapse] = gridExtent[iAxisToCollapse] * 2.0f;
916 phaseMask &= ~(1 << iAxisToCollapse);
917 }
918
919 int numGridChunks = 0;
920 btIntVec3 gridChunkDim; // each chunk is 2x2x2 group of cells
921 while (true)
922 {
923 gridDim[0] = int(1.0 + gridExtent.x() / gridCellSize.x());
924 gridDim[1] = int(1.0 + gridExtent.y() / gridCellSize.y());
925 gridDim[2] = int(1.0 + gridExtent.z() / gridCellSize.z());
926 gridChunkDim[0] = btMax(1, (gridDim[0] + 0) / 2);
927 gridChunkDim[1] = btMax(1, (gridDim[1] + 0) / 2);
928 gridChunkDim[2] = btMax(1, (gridDim[2] + 0) / 2);
929 numGridChunks = gridChunkDim[0] * gridChunkDim[1] * gridChunkDim[2];
930 float nChunks = float(gridChunkDim[0]) * float(gridChunkDim[1]) * float(gridChunkDim[2]); // suceptible to integer overflow
931 if (numGridChunks <= maxGridChunkCount && nChunks <= maxGridChunkCount)
932 {
933 break;
934 }
935 gridCellSize *= 1.25; // should roughly cut numCells in half
936 }
937 btAssert(numGridChunks <= maxGridChunkCount);
938 int maxNumBatchesPerPhase = numGridChunks;
939
940 // for each dynamic body, compute grid coords
941 btVector3 invGridCellSize = btVector3(1, 1, 1) / gridCellSize;
942 // (can be done in parallel)
943 for (int iBody = 0; iBody < bodies.size(); ++iBody)
944 {
945 btIntVec3& coords = bodyGridCoords[iBody];
946 if (bodyDynamicFlags[iBody])
947 {
948 btVector3 v = (bodyPositions[iBody] - bboxMin) * invGridCellSize;
949 coords.m_ints[0] = int(v.x());
950 coords.m_ints[1] = int(v.y());
951 coords.m_ints[2] = int(v.z());
952 btAssert(coords.m_ints[0] >= 0 && coords.m_ints[0] < gridDim[0]);
953 btAssert(coords.m_ints[1] >= 0 && coords.m_ints[1] < gridDim[1]);
954 btAssert(coords.m_ints[2] >= 0 && coords.m_ints[2] < gridDim[2]);
955 }
956 else
957 {
958 coords.m_ints[0] = -1;
959 coords.m_ints[1] = -1;
960 coords.m_ints[2] = -1;
961 }
962 }
963
964 for (int iPhase = 0; iPhase < numPhases; ++iPhase)
965 {
966 int batchBegin = iPhase * maxNumBatchesPerPhase;
967 int batchEnd = batchBegin + maxNumBatchesPerPhase;
968 for (int iBatch = batchBegin; iBatch < batchEnd; ++iBatch)
969 {
970 btBatchInfo& batch = batches[iBatch];
971 batch = btBatchInfo();
972 }
973 }
974
975 {
976 AssignConstraintsToGridBatchesParams params;
977 params.bodyDynamicFlags = bodyDynamicFlags;
978 params.bodyGridCoords = bodyGridCoords;
979 params.numBodies = bodies.size();
980 params.conInfos = conInfos;
981 params.constraintBatchIds = constraintBatchIds;
982 params.gridChunkDim = gridChunkDim;
983 params.maxNumBatchesPerPhase = maxNumBatchesPerPhase;
984 params.numPhases = numPhases;
985 params.phaseMask = phaseMask;
986 bool inParallel = true;
987 if (inParallel)
988 {
989 AssignConstraintsToGridBatchesLoop loop(params);
990 int grainSize = 250;
991 btParallelFor(0, numConstraints, grainSize, loop);
992 }
993 else
994 {
995 assignConstraintsToGridBatches(params, 0, numConstraints);
996 }
997 }
998 for (int iCon = 0; iCon < numConstraints; ++iCon)
999 {
1000 const btBatchedConstraintInfo& con = conInfos[iCon];
1001 int iBatch = constraintBatchIds[iCon];
1002 btBatchInfo& batch = batches[iBatch];
1003 batch.numConstraints += con.numConstraintRows;
1004 }
1005
1006 for (int iPhase = 0; iPhase < numPhases; ++iPhase)
1007 {
1008 // if phase is legit,
1009 if (iPhase == (iPhase & phaseMask))
1010 {
1011 int iBeginBatch = iPhase * maxNumBatchesPerPhase;
1012 int iEndBatch = iBeginBatch + maxNumBatchesPerPhase;
1013 mergeSmallBatches(batches, iBeginBatch, iEndBatch, minBatchSize, maxBatchSize);
1014 }
1015 }
1016 // all constraints have been assigned a batchId
1017 updateConstraintBatchIdsForMergesMt(constraintBatchIds, numConstraints, batches, maxNumBatchesPerPhase * numPhases);
1018
1019 if (numConstraintRows > numConstraints)
1020 {
1021 expandConstraintRowsMt(&constraintRowBatchIds[0], &constraintBatchIds[0], &conInfos[0], numConstraints, numConstraintRows);
1022 }
1023 else
1024 {
1025 constraintRowBatchIds = constraintBatchIds;
1026 }
1027
1028 writeOutBatches(batchedConstraints, constraintRowBatchIds, numConstraintRows, batches, batchWork, maxNumBatchesPerPhase, numPhases);
1029 btAssert(batchedConstraints->validate(constraints, bodies));
1030 }
1031
1032 static void setupSingleBatch(
1033 btBatchedConstraints* bc,
1034 int numConstraints)
1035 {
1036 BT_PROFILE("setupSingleBatch");
1037 typedef btBatchedConstraints::Range Range;
1038
1039 bc->m_constraintIndices.resize(numConstraints);
1040 for (int i = 0; i < numConstraints; ++i)
1041 {
1042 bc->m_constraintIndices[i] = i;
1043 }
1044
1045 bc->m_batches.resizeNoInitialize(0);
1046 bc->m_phases.resizeNoInitialize(0);
1047 bc->m_phaseOrder.resizeNoInitialize(0);
1048 bc->m_phaseGrainSize.resizeNoInitialize(0);
1049
1050 if (numConstraints > 0)
1051 {
1052 bc->m_batches.push_back(Range(0, numConstraints));
1053 bc->m_phases.push_back(Range(0, 1));
1054 bc->m_phaseOrder.push_back(0);
1055 bc->m_phaseGrainSize.push_back(1);
1056 }
1057 }
1058
1059 void btBatchedConstraints::setup(
1060 btConstraintArray* constraints,
1061 const btAlignedObjectArray<btSolverBody>& bodies,
1062 BatchingMethod batchingMethod,
1063 int minBatchSize,
1064 int maxBatchSize,
1065 btAlignedObjectArray<char>* scratchMemory)
1066 {
1067 if (constraints->size() >= minBatchSize * 4)
1068 {
1069 bool use2DGrid = batchingMethod == BATCHING_METHOD_SPATIAL_GRID_2D;
1070 setupSpatialGridBatchesMt(this, scratchMemory, constraints, bodies, minBatchSize, maxBatchSize, use2DGrid);
1071 if (s_debugDrawBatches)
1072 {
1073 debugDrawAllBatches(this, constraints, bodies);
1074 }
1075 }
1076 else
1077 {
1078 setupSingleBatch(this, constraints->size());
1079 }
1080 }
AssignConstraintsToGridBatchesLoop::m_params
const AssignConstraintsToGridBatchesParams * m_params
Definition: btBatchedConstraints.cpp:774
btAlignedObjectArray::reserve
void reserve(int _Count)
Definition: btAlignedObjectArray.h:287
UpdateConstraintBatchIdsForMergesLoop
Definition: btBatchedConstraints.cpp:403
btBatchedConstraints::validate
bool validate(btConstraintArray *constraints, const btAlignedObjectArray< btSolverBody > &bodies) const
Definition: btBatchedConstraints.cpp:44
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition: btAlignedObjectArray.h:264
btBatchedConstraintInfo::bodyIds
int bodyIds[2]
Definition: btBatchedConstraints.cpp:33
PreallocatedMemoryHelper::m_numChunks
int m_numChunks
Definition: btBatchedConstraints.cpp:614
BT_LARGE_FLOAT
#define BT_LARGE_FLOAT
Definition: btScalar.h:296
AssignConstraintsToGridBatchesParams::AssignConstraintsToGridBatchesParams
AssignConstraintsToGridBatchesParams()
Definition: btBatchedConstraints.cpp:697
UpdateConstraintBatchIdsForMergesLoop::m_batches
const btBatchInfo * m_batches
Definition: btBatchedConstraints.cpp:406
AssignConstraintsToGridBatchesParams::bodyDynamicFlags
bool * bodyDynamicFlags
Definition: btBatchedConstraints.cpp:687
btBatchedConstraintInfo
Definition: btBatchedConstraints.cpp:29
PreallocatedMemoryHelper::Chunk::size
size_t size
Definition: btBatchedConstraints.cpp:611
writeGrainSizes
static void writeGrainSizes(btBatchedConstraints *bc)
Definition: btBatchedConstraints.cpp:513
assignConstraintsToGridBatches
static void assignConstraintsToGridBatches(const AssignConstraintsToGridBatchesParams &params, int iConBegin, int iConEnd)
Definition: btBatchedConstraints.cpp:703
btAlignedObjectArray< btSolverConstraint >
AssignConstraintsToGridBatchesParams::constraintBatchIds
int * constraintBatchIds
Definition: btBatchedConstraints.cpp:691
AssignConstraintsToGridBatchesLoop::forLoop
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
Definition: btBatchedConstraints.cpp:780
btIntVec3::operator[]
const int & operator[](int i) const
Definition: btBatchedConstraints.cpp:681
WriteOutConstraintIndicesLoop::m_numConstraints
int m_numConstraints
Definition: btBatchedConstraints.cpp:461
PreallocatedMemoryHelper
Definition: btBatchedConstraints.cpp:606
expandConstraintRows
static void expandConstraintRows(int *destConstraintBatchIds, const int *srcConstraintBatchIds, const btBatchedConstraintInfo *conInfos, int numConstraints, int numConstraintRows)
Definition: btBatchedConstraints.cpp:284
ReadSolverConstraintsLoop::ReadSolverConstraintsLoop
ReadSolverConstraintsLoop(btBatchedConstraintInfo *outConInfos, btConstraintArray *constraints)
Definition: btBatchedConstraints.cpp:213
AssignConstraintsToGridBatchesParams::maxNumBatchesPerPhase
int maxNumBatchesPerPhase
Definition: btBatchedConstraints.cpp:693
btGetTaskScheduler
btITaskScheduler * btGetTaskScheduler()
Definition: btThreads.cpp:407
btIDebugDraw::drawLine
virtual void drawLine(const btVector3 &from, const btVector3 &to, const btVector3 &color)=0
ExpandConstraintRowsLoop::m_numConstraintRows
int m_numConstraintRows
Definition: btBatchedConstraints.cpp:306
btBatchedConstraints::Range
Definition: btBatchedConstraints.h:34
btSolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
Definition: btSolverConstraint.h:29
btAlignedObjectArray::resizeNoInitialize
void resizeNoInitialize(int newsize)
resize changes the number of elements in the array.
Definition: btAlignedObjectArray.h:201
btAssert
#define btAssert(x)
Definition: btScalar.h:133
btAlignedObjectArray::at
const T & at(int n) const
Definition: btAlignedObjectArray.h:154
btITaskScheduler::getNumThreads
virtual int getNumThreads() const =0
writeOutBatches
static void writeOutBatches(btBatchedConstraints *bc, const int *constraintBatchIds, int numConstraints, const btBatchInfo *batches, int *batchWork, int maxNumBatchesPerPhase, int numPhases)
Definition: btBatchedConstraints.cpp:528
mergeSmallBatches
static void mergeSmallBatches(btBatchInfo *batches, int iBeginBatch, int iEndBatch, int minBatchSize, int maxBatchSize)
Definition: btBatchedConstraints.cpp:340
ExpandConstraintRowsLoop::ExpandConstraintRowsLoop
ExpandConstraintRowsLoop(int *destConstraintBatchIds, const int *srcConstraintBatchIds, const btBatchedConstraintInfo *conInfos, int numConstraintRows)
Definition: btBatchedConstraints.cpp:308
SIMD_FORCE_INLINE
#define SIMD_FORCE_INLINE
Definition: btScalar.h:83
btSolverBody::internalGetInvMass
const btVector3 & internalGetInvMass() const
Definition: btSolverBody.h:212
btBatchedConstraints::m_phaseOrder
btAlignedObjectArray< int > m_phaseOrder
Definition: btBatchedConstraints.h:47
btClamp
void btClamp(T &a, const T &lb, const T &ub)
Definition: btMinMax.h:57
PreallocatedMemoryHelper::setChunkPointers
void setChunkPointers(void *mem) const
Definition: btBatchedConstraints.cpp:638
PreallocatedMemoryHelper::Chunk::ptr
void ** ptr
Definition: btBatchedConstraints.cpp:610
AssignConstraintsToGridBatchesParams::gridChunkDim
btIntVec3 gridChunkDim
Definition: btBatchedConstraints.cpp:692
ExpandConstraintRowsLoop::m_srcConstraintBatchIds
const int * m_srcConstraintBatchIds
Definition: btBatchedConstraints.cpp:304
ExpandConstraintRowsLoop
Definition: btBatchedConstraints.cpp:301
setupSpatialGridBatchesMt
static void setupSpatialGridBatchesMt(btBatchedConstraints *batchedConstraints, btAlignedObjectArray< char > *scratchMemory, btConstraintArray *constraints, const btAlignedObjectArray< btSolverBody > &bodies, int minBatchSize, int maxBatchSize, bool use2DGrid)
Definition: btBatchedConstraints.cpp:814
updateConstraintBatchIdsForMerges
static void updateConstraintBatchIdsForMerges(int *constraintBatchIds, int numConstraints, const btBatchInfo *batches, int numBatches)
Definition: btBatchedConstraints.cpp:386
AssignConstraintsToGridBatchesParams::phaseMask
int phaseMask
Definition: btBatchedConstraints.cpp:695
initBatchedConstraintInfoArray
static void initBatchedConstraintInfoArray(btAlignedObjectArray< btBatchedConstraintInfo > *outConInfos, btConstraintArray *constraints)
Definition: btBatchedConstraints.cpp:329
btIntVec3::operator[]
int & operator[](int i)
Definition: btBatchedConstraints.cpp:682
debugDrawSingleBatch
static void debugDrawSingleBatch(const btBatchedConstraints *bc, btConstraintArray *constraints, const btAlignedObjectArray< btSolverBody > &bodies, int iBatch, const btVector3 &color, const btVector3 &offset)
Definition: btBatchedConstraints.cpp:99
runLengthEncodeConstraintInfo
static int runLengthEncodeConstraintInfo(btBatchedConstraintInfo *outConInfos, int numConstraints)
Definition: btBatchedConstraints.cpp:185
UpdateConstraintBatchIdsForMergesLoop::UpdateConstraintBatchIdsForMergesLoop
UpdateConstraintBatchIdsForMergesLoop(int *constraintBatchIds, const btBatchInfo *batches, int numBatches)
Definition: btBatchedConstraints.cpp:409
btBatchedConstraints::Range::end
int end
Definition: btBatchedConstraints.h:37
PreallocatedMemoryHelper::Chunk
Definition: btBatchedConstraints.cpp:608
initBatchedConstraintInfo
static int initBatchedConstraintInfo(btBatchedConstraintInfo *outConInfos, btConstraintArray *constraints)
Definition: btBatchedConstraints.cpp:232
AssignConstraintsToGridBatchesLoop
Definition: btBatchedConstraints.cpp:772
ReadSolverConstraintsLoop::m_constraints
btConstraintArray * m_constraints
Definition: btBatchedConstraints.cpp:211
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
BT_OVERRIDE
#define BT_OVERRIDE
Definition: btThreads.h:26
WriteOutConstraintIndicesLoop::m_constraintIdPerBatch
int * m_constraintIdPerBatch
Definition: btBatchedConstraints.cpp:462
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
btBatchedConstraints
Definition: btBatchedConstraints.h:26
btBatchInfo::btBatchInfo
btBatchInfo()
Definition: btBatchedConstraints.cpp:41
AssignConstraintsToGridBatchesParams::bodyGridCoords
btIntVec3 * bodyGridCoords
Definition: btBatchedConstraints.cpp:688
ExpandConstraintRowsLoop::forLoop
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
Definition: btBatchedConstraints.cpp:315
ReadSolverConstraintsLoop::m_outConInfos
btBatchedConstraintInfo * m_outConInfos
Definition: btBatchedConstraints.cpp:210
btStackAlloc.h
AssignConstraintsToGridBatchesParams::numBodies
int numBodies
Definition: btBatchedConstraints.cpp:689
PreallocatedMemoryHelper::m_chunks
Chunk m_chunks[N]
Definition: btBatchedConstraints.cpp:613
ReadSolverConstraintsLoop
Definition: btBatchedConstraints.cpp:208
btBatchedConstraints::BATCHING_METHOD_SPATIAL_GRID_2D
Definition: btBatchedConstraints.h:30
btBatchInfo::mergeIndex
int mergeIndex
Definition: btBatchedConstraints.cpp:39
btBatchedConstraints.h
expandConstraintRowsMt
static void expandConstraintRowsMt(int *destConstraintBatchIds, const int *srcConstraintBatchIds, const btBatchedConstraintInfo *conInfos, int numConstraints, int numConstraintRows)
Definition: btBatchedConstraints.cpp:321
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
expandConstraintRowsInPlace
static void expandConstraintRowsInPlace(int *constraintBatchIds, const btBatchedConstraintInfo *conInfos, int numConstraints, int numConstraintRows)
Definition: btBatchedConstraints.cpp:263
initBatchedBodyDynamicFlags
static void initBatchedBodyDynamicFlags(btAlignedObjectArray< bool > *outBodyDynamicFlags, const btAlignedObjectArray< btSolverBody > &bodies)
Definition: btBatchedConstraints.cpp:173
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
btAlignedObjectArray::capacity
int capacity() const
return the pre-allocated (reserved) elements, this is at least as large as the total number of elemen...
Definition: btAlignedObjectArray.h:282
debugDrawPhase
static void debugDrawPhase(const btBatchedConstraints *bc, btConstraintArray *constraints, const btAlignedObjectArray< btSolverBody > &bodies, int iPhase, const btVector3 &color0, const btVector3 &color1, const btVector3 &offset)
Definition: btBatchedConstraints.cpp:122
btBatchedConstraints::m_phases
btAlignedObjectArray< Range > m_phases
Definition: btBatchedConstraints.h:45
btSolverConstraint::m_solverBodyIdB
int m_solverBodyIdB
Definition: btSolverConstraint.h:63
btBatchedConstraints::s_debugDrawBatches
static bool s_debugDrawBatches
Definition: btBatchedConstraints.h:50
btBatchedConstraints::m_constraintIndices
btAlignedObjectArray< int > m_constraintIndices
Definition: btBatchedConstraints.h:43
btIntVec3
Definition: btBatchedConstraints.cpp:677
btMinMax.h
findMaxDynamicConstraintExtent
static btVector3 findMaxDynamicConstraintExtent(btVector3 *bodyPositions, bool *bodyDynamicFlags, btBatchedConstraintInfo *conInfos, int numConstraints, int numBodies)
Definition: btBatchedConstraints.cpp:651
WriteOutConstraintIndicesLoop::WriteOutConstraintIndicesLoop
WriteOutConstraintIndicesLoop(btBatchedConstraints *bc, const int *constraintBatchIds, int numConstraints, int *constraintIdPerBatch, int maxNumBatchesPerPhase)
Definition: btBatchedConstraints.cpp:465
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:149
UpdateConstraintBatchIdsForMergesLoop::forLoop
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
Definition: btBatchedConstraints.cpp:415
btVector3::absolute
btVector3 absolute() const
Return a vector with the absolute values of each element.
Definition: btVector3.h:364
BT_PROFILE
#define BT_PROFILE(name)
Definition: btQuickprof.h:198
ExpandConstraintRowsLoop::m_destConstraintBatchIds
int * m_destConstraintBatchIds
Definition: btBatchedConstraints.cpp:303
btBatchedConstraints::m_phaseGrainSize
btAlignedObjectArray< char > m_phaseGrainSize
Definition: btBatchedConstraints.h:46
btBatchedConstraints::Range::begin
int begin
Definition: btBatchedConstraints.h:36
BatchCompare
bool BatchCompare(const btBatchedConstraints::Range &a, const btBatchedConstraints::Range &b)
Definition: btBatchedConstraints.cpp:430
btBatchedConstraints::BatchingMethod
BatchingMethod
Definition: btBatchedConstraints.h:28
AssignConstraintsToGridBatchesParams
Definition: btBatchedConstraints.cpp:685
btBatchedConstraints::setup
void setup(btConstraintArray *constraints, const btAlignedObjectArray< btSolverBody > &bodies, BatchingMethod batchingMethod, int minBatchSize, int maxBatchSize, btAlignedObjectArray< char > *scratchMemory)
Definition: btBatchedConstraints.cpp:1059
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:103
btIDebugDraw.h
btBatchInfo
Definition: btBatchedConstraints.cpp:36
btVector3::isZero
bool isZero() const
Definition: btVector3.h:683
btSolverBody::getWorldTransform
const btTransform & getWorldTransform() const
Definition: btSolverBody.h:126
AssignConstraintsToGridBatchesLoop::AssignConstraintsToGridBatchesLoop
AssignConstraintsToGridBatchesLoop(const AssignConstraintsToGridBatchesParams &params)
Definition: btBatchedConstraints.cpp:776
writeOutConstraintIndicesForRangeOfBatches
static void writeOutConstraintIndicesForRangeOfBatches(btBatchedConstraints *bc, const int *constraintBatchIds, int numConstraints, int *constraintIdPerBatch, int batchBegin, int batchEnd)
Definition: btBatchedConstraints.cpp:437
WriteOutConstraintIndicesLoop::m_constraintBatchIds
const int * m_constraintBatchIds
Definition: btBatchedConstraints.cpp:460
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:210
WriteOutConstraintIndicesLoop
Definition: btBatchedConstraints.cpp:457
btQuickprof.h
PreallocatedMemoryHelper::getSizeToAllocate
size_t getSizeToAllocate() const
Definition: btBatchedConstraints.cpp:629
setupSingleBatch
static void setupSingleBatch(btBatchedConstraints *bc, int numConstraints)
Definition: btBatchedConstraints.cpp:1032
btSolverConstraint::m_solverBodyIdA
int m_solverBodyIdA
Definition: btSolverConstraint.h:62
ExpandConstraintRowsLoop::m_conInfos
const btBatchedConstraintInfo * m_conInfos
Definition: btBatchedConstraints.cpp:305
WriteOutConstraintIndicesLoop::forLoop
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
Definition: btBatchedConstraints.cpp:473
btBatchedConstraints::m_debugDrawer
btIDebugDraw * m_debugDrawer
Definition: btBatchedConstraints.h:48
btMax
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
AssignConstraintsToGridBatchesParams::numPhases
int numPhases
Definition: btBatchedConstraints.cpp:694
btParallelFor
void btParallelFor(int iBegin, int iEnd, int grainSize, const btIParallelForBody &body)
Definition: btThreads.cpp:412
writeOutConstraintIndicesMt
static void writeOutConstraintIndicesMt(btBatchedConstraints *bc, const int *constraintBatchIds, int numConstraints, int *constraintIdPerBatch, int maxNumBatchesPerPhase, int numPhases)
Definition: btBatchedConstraints.cpp:487
btIParallelForBody
Definition: btThreads.h:101
btVector3::setMax
void setMax(const btVector3 &other)
Set each element to the max of the current values and the values of another btVector3.
Definition: btVector3.h:609
AssignConstraintsToGridBatchesParams::conInfos
btBatchedConstraintInfo * conInfos
Definition: btBatchedConstraints.cpp:690
btBatchedConstraintInfo::constraintIndex
int constraintIndex
Definition: btBatchedConstraints.cpp:31
PreallocatedMemoryHelper::PreallocatedMemoryHelper
PreallocatedMemoryHelper()
Definition: btBatchedConstraints.cpp:617
ReadSolverConstraintsLoop::forLoop
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
Definition: btBatchedConstraints.cpp:218
btBatchedConstraintInfo::numConstraintRows
int numConstraintRows
Definition: btBatchedConstraints.cpp:32
btMin
const T & btMin(const T &a, const T &b)
Definition: btMinMax.h:21
UpdateConstraintBatchIdsForMergesLoop::m_numBatches
int m_numBatches
Definition: btBatchedConstraints.cpp:407
UpdateConstraintBatchIdsForMergesLoop::m_constraintBatchIds
int * m_constraintBatchIds
Definition: btBatchedConstraints.cpp:405
btIntVec3::m_ints
int m_ints[3]
Definition: btBatchedConstraints.cpp:679
debugDrawAllBatches
static void debugDrawAllBatches(const btBatchedConstraints *bc, btConstraintArray *constraints, const btAlignedObjectArray< btSolverBody > &bodies)
Definition: btBatchedConstraints.cpp:143
lerp
btVector3 lerp(const btVector3 &v1, const btVector3 &v2, const btScalar &t)
Return the linear interpolation between two vectors.
Definition: btVector3.h:934
btBatchedConstraints::m_batches
btAlignedObjectArray< Range > m_batches
Definition: btBatchedConstraints.h:44
PreallocatedMemoryHelper::addChunk
void addChunk(void **ptr, size_t sz)
Definition: btBatchedConstraints.cpp:618
btVector3::setMin
void setMin(const btVector3 &other)
Set each element to the min of the current values and the values of another btVector3.
Definition: btVector3.h:626
btBatchInfo::numConstraints
int numConstraints
Definition: btBatchedConstraints.cpp:38
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:294
WriteOutConstraintIndicesLoop::m_batchedConstraints
btBatchedConstraints * m_batchedConstraints
Definition: btBatchedConstraints.cpp:459
updateConstraintBatchIdsForMergesMt
static void updateConstraintBatchIdsForMergesMt(int *constraintBatchIds, int numConstraints, const btBatchInfo *batches, int numBatches)
Definition: btBatchedConstraints.cpp:422
WriteOutConstraintIndicesLoop::m_maxNumBatchesPerPhase
int m_maxNumBatchesPerPhase
Definition: btBatchedConstraints.cpp:463
kNoMerge
const int kNoMerge
Definition: btBatchedConstraints.cpp:25