52 const int kUnassignedBatch = -1;
55 for (
int iPhase = 0; iPhase <
m_phases.size(); ++iPhase)
58 bodyBatchId.
resize(bodies.
size(), kUnassignedBatch);
60 for (
int iBatch = phase.
begin; iBatch < phase.
end; ++iBatch)
63 for (
int iiCons = batch.
begin; iiCons < batch.
end; ++iiCons)
72 if (thisBodyBatchId == kUnassignedBatch)
76 else if (thisBodyBatchId != iBatch)
78 btAssert(!
"dynamic body is used in 2 different batches in the same phase");
85 if (thisBodyBatchId == kUnassignedBatch)
89 else if (thisBodyBatchId != iBatch)
91 btAssert(!
"dynamic body is used in 2 different batches in the same phase");
108 if (bc && bc->
m_debugDrawer && iBatch < bc->m_batches.size())
111 for (
int iiCon = b.
begin; iiCon < b.
end; ++iiCon)
117 btVector3 pos0 = bodies[iBody0].getWorldTransform().getOrigin() + offset;
118 btVector3 pos1 = bodies[iBody1].getWorldTransform().getOrigin() + offset;
136 for (
int iBatch = phase.
begin; iBatch < phase.
end; ++iBatch)
138 float tt = float(iBatch - phase.
begin) / float(
btMax(1, phase.
end - phase.
begin - 1));
154 for (
int iBody = 0; iBody < bodies.
size(); ++iBody)
156 const btVector3& pos = bodies[iBody].getWorldTransform().getOrigin();
160 btVector3 bboxExtent = bboxMax - bboxMin;
163 int numPhases = bc->
m_phases.size();
164 for (
int iPhase = 0; iPhase < numPhases; ++iPhase)
166 float b = float(iPhase) / float(numPhases - 1);
169 btVector3 offset = offsetBase + offsetStep * (float(iPhase) - float(numPhases - 1) * 0.5);
170 debugDrawPhase(bc, constraints, bodies, iPhase, color0, color1, offset);
180 for (
int i = 0; i < bodies.
size(); ++i)
193 while (iSrc < numConstraints)
200 while (iSrc < numConstraints && outConInfos[iSrc].bodyIds[0] == srcConInfo.
bodyIds[0] && outConInfos[iSrc].
bodyIds[1] == srcConInfo.
bodyIds[1])
222 for (
int i = iBegin; i < iEnd; ++i)
237 int numConstraints = constraints->
size();
238 bool inParallel =
true;
242 int grainSize = 1200;
247 for (
int i = 0; i < numConstraints; ++i)
257 bool useRunLengthEncoding =
true;
258 if (useRunLengthEncoding)
262 return numConstraints;
268 if (numConstraintRows > numConstraints)
271 for (
int iCon = numConstraints - 1; iCon >= 0; --iCon)
274 int iBatch = constraintBatchIds[iCon];
279 btAssert(iDest >= 0 && iDest < numConstraintRows);
280 constraintBatchIds[iDest] = iBatch;
289 for (
int iCon = 0; iCon < numConstraints; ++iCon)
292 int iBatch = srcConstraintBatchIds[iCon];
297 btAssert(iDest >= 0 && iDest < numConstraintRows);
298 destConstraintBatchIds[iDest] = iBatch;
335 int numConstraints = constraints->
size();
345 for (
int iBatch = iEndBatch - 1; iBatch >= iBeginBatch; --iBatch)
350 for (
int iDestBatch = iBatch - 1; iDestBatch >= iBeginBatch; --iDestBatch)
367 for (
int iBatch = iBeginBatch; iBatch < iEndBatch; ++iBatch)
390 BT_PROFILE(
"updateConstraintBatchIdsForMerges");
392 for (
int i = 0; i < numConstraints; ++i)
394 int iBatch = constraintBatchIds[i];
397 if (batches[iBatch].mergeIndex !=
kNoMerge)
400 constraintBatchIds[i] = batches[iBatch].
mergeIndex;
419 BT_PROFILE(
"UpdateConstraintBatchIdsForMergesLoop");
426 BT_PROFILE(
"updateConstraintBatchIdsForMergesMt");
440 const int* constraintBatchIds,
442 int* constraintIdPerBatch,
446 BT_PROFILE(
"writeOutConstraintIndicesForRangeOfBatches");
447 for (
int iCon = 0; iCon < numConstraints; ++iCon)
449 int iBatch = constraintBatchIds[iCon];
450 if (iBatch >= batchBegin && iBatch < batchEnd)
452 int iDestCon = constraintIdPerBatch[iBatch];
453 constraintIdPerBatch[iBatch] = iDestCon + 1;
490 const int* constraintBatchIds,
492 int* constraintIdPerBatch,
493 int maxNumBatchesPerPhase,
497 bool inParallel =
true;
505 for (
int iCon = 0; iCon < numConstraints; ++iCon)
507 int iBatch = constraintBatchIds[iCon];
508 int iDestCon = constraintIdPerBatch[iBatch];
509 constraintIdPerBatch[iBatch] = iDestCon + 1;
518 int numPhases = bc->
m_phases.size();
521 for (
int iPhase = 0; iPhase < numPhases; ++iPhase)
523 const Range& phase = bc->
m_phases[iPhase];
524 int numBatches = phase.end - phase.begin;
525 float grainSize = std::floor((0.25f * numBatches /
float(numThreads)) + 0.0f);
531 const int* constraintBatchIds,
535 int maxNumBatchesPerPhase,
546 int* constraintIdPerBatch = batchWork;
548 for (
int iPhase = 0; iPhase < numPhases; ++iPhase)
550 int curPhaseBegin = bc->
m_batches.size();
551 int iBegin = iPhase * maxNumBatchesPerPhase;
552 int iEnd = iBegin + maxNumBatchesPerPhase;
553 for (
int i = iBegin; i < iEnd; ++i)
556 int curBatchBegin = iConstraint;
557 constraintIdPerBatch[i] = curBatchBegin;
559 iConstraint += numConstraints;
560 if (numConstraints > 0)
562 bc->
m_batches.push_back(Range(curBatchBegin, iConstraint));
566 if (bc->
m_batches.size() > curPhaseBegin)
573 btAssert(iConstraint == numConstraints);
578 for (
int iPhase = 0; iPhase < bc->
m_phases.size(); ++iPhase)
581 const Range& curBatches = bc->
m_phases[iPhase];
585 for (
int i = 0; i < bc->
m_phases.size(); ++i)
633 size_t totalSize = 0;
642 size_t totalSize = 0;
646 char* chunkPtr =
static_cast<char*
>(mem) + totalSize;
647 *chunk.
ptr = chunkPtr;
648 totalSize += chunk.
size;
655 bool* bodyDynamicFlags,
662 for (
int iCon = 0; iCon < numConstraints; ++iCon)
667 btAssert(iBody0 >= 0 && iBody0 < numBodies);
668 btAssert(iBody1 >= 0 && iBody1 < numBodies);
670 if (bodyDynamicFlags[iBody0] && bodyDynamicFlags[iBody1])
672 btVector3 delta = bodyPositions[iBody1] - bodyPositions[iBody0];
701 memset(
this, 0,
sizeof(*
this));
709 for (
int iCon = iConBegin; iCon < iConEnd; ++iCon)
723 for (
int i = 0; i < 3; ++i)
727 if (coordMin != coordMax)
730 if ((coordMin & 1) == 0)
740 gridCoord[i] = coordMin;
752 for (
int i = 0; i < 3; ++i)
754 gridCoord[i] = body0Coords.
m_ints[i];
761 for (
int i = 0; i < 3; ++i)
763 int coordOffset = (iPhase >> i) & 1;
764 chunkCoord[i] = (gridCoord[i] - coordOffset) / 2;
765 btClamp(chunkCoord[i], 0, gridChunkDim[i] - 1);
766 btAssert(chunkCoord[i] < gridChunkDim[i]);
768 int iBatch = iPhase * params.
maxNumBatchesPerPhase + chunkCoord[0] + chunkCoord[1] * gridChunkDim[0] + chunkCoord[2] * gridChunkDim[0] * gridChunkDim[1];
826 const int numPhases = 8;
827 int numConstraints = constraints->
size();
828 int numConstraintRows = constraints->
size();
830 const int maxGridChunkCount = 128;
831 int allocNumBatchesPerPhase = maxGridChunkCount;
832 int minNumBatchesPerPhase = 16;
833 int allocNumBatches = allocNumBatchesPerPhase * numPhases;
836 bool* bodyDynamicFlags = NULL;
839 int* batchWork = NULL;
841 int* constraintBatchIds = NULL;
842 int* constraintRowBatchIds = NULL;
846 memHelper.
addChunk((
void**)&bodyDynamicFlags,
sizeof(
bool) * bodies.
size());
849 memHelper.
addChunk((
void**)&batchWork,
sizeof(
int) * allocNumBatches);
851 memHelper.
addChunk((
void**)&constraintBatchIds,
sizeof(
int) * numConstraints);
852 memHelper.
addChunk((
void**)&constraintRowBatchIds,
sizeof(
int) * numConstraintRows);
855 if (
static_cast<size_t>(scratchMemory->
capacity()) < scratchSize)
858 scratchMemory->
reserve(scratchSize + scratchSize / 16);
861 char* memPtr = &scratchMemory->
at(0);
872 for (
int i = 0; i < bodies.
size(); ++i)
877 bodyPositions[i] = bodyPos;
878 bodyDynamicFlags[i] = isDynamic;
891 btVector3 gridExtent = bboxMax - bboxMin;
897 gridDim[0] = int(1.0 + gridExtent.
x() / gridCellSize.
x());
898 gridDim[1] = int(1.0 + gridExtent.
y() / gridCellSize.
y());
899 gridDim[2] = int(1.0 + gridExtent.
z() / gridCellSize.
z());
903 bool collapseAxis = use2DGrid;
907 int iAxisToCollapse = 0;
908 int axisDim = gridDim[iAxisToCollapse];
910 for (
int i = 0; i < 3; ++i)
912 if (gridDim[i] < axisDim)
915 axisDim = gridDim[i];
919 gridCellSize[iAxisToCollapse] = gridExtent[iAxisToCollapse] * 2.0f;
920 phaseMask &= ~(1 << iAxisToCollapse);
923 int numGridChunks = 0;
927 gridDim[0] = int(1.0 + gridExtent.
x() / gridCellSize.
x());
928 gridDim[1] = int(1.0 + gridExtent.
y() / gridCellSize.
y());
929 gridDim[2] = int(1.0 + gridExtent.
z() / gridCellSize.
z());
930 gridChunkDim[0] =
btMax(1, (gridDim[0] + 0) / 2);
931 gridChunkDim[1] =
btMax(1, (gridDim[1] + 0) / 2);
932 gridChunkDim[2] =
btMax(1, (gridDim[2] + 0) / 2);
933 numGridChunks = gridChunkDim[0] * gridChunkDim[1] * gridChunkDim[2];
934 float nChunks = float(gridChunkDim[0]) * float(gridChunkDim[1]) * float(gridChunkDim[2]);
935 if (numGridChunks <= maxGridChunkCount && nChunks <= maxGridChunkCount)
939 gridCellSize *= 1.25;
941 btAssert(numGridChunks <= maxGridChunkCount);
942 int maxNumBatchesPerPhase = numGridChunks;
947 for (
int iBody = 0; iBody < bodies.
size(); ++iBody)
949 btIntVec3& coords = bodyGridCoords[iBody];
950 if (bodyDynamicFlags[iBody])
952 btVector3 v = (bodyPositions[iBody] - bboxMin) * invGridCellSize;
968 for (
int iPhase = 0; iPhase < numPhases; ++iPhase)
970 int batchBegin = iPhase * maxNumBatchesPerPhase;
971 int batchEnd = batchBegin + maxNumBatchesPerPhase;
972 for (
int iBatch = batchBegin; iBatch < batchEnd; ++iBatch)
990 bool inParallel =
true;
1002 for (
int iCon = 0; iCon < numConstraints; ++iCon)
1005 int iBatch = constraintBatchIds[iCon];
1010 for (
int iPhase = 0; iPhase < numPhases; ++iPhase)
1013 if (iPhase == (iPhase & phaseMask))
1015 int iBeginBatch = iPhase * maxNumBatchesPerPhase;
1016 int iEndBatch = iBeginBatch + maxNumBatchesPerPhase;
1023 if (numConstraintRows > numConstraints)
1025 expandConstraintRowsMt(&constraintRowBatchIds[0], &constraintBatchIds[0], &conInfos[0], numConstraints, numConstraintRows);
1029 constraintRowBatchIds = constraintBatchIds;
1032 writeOutBatches(batchedConstraints, constraintRowBatchIds, numConstraintRows, batches, batchWork, maxNumBatchesPerPhase, numPhases);
1044 for (
int i = 0; i < numConstraints; ++i)
1050 bc->
m_phases.resizeNoInitialize(0);
1054 if (numConstraints > 0)
1056 bc->
m_batches.push_back(Range(0, numConstraints));
1057 bc->
m_phases.push_back(Range(0, 1));
1071 if (constraints->
size() >= minBatchSize * 4)
static void writeGrainSizes(btBatchedConstraints *bc)
static void updateConstraintBatchIdsForMergesMt(int *constraintBatchIds, int numConstraints, const btBatchInfo *batches, int numBatches)
static void initBatchedConstraintInfoArray(btAlignedObjectArray< btBatchedConstraintInfo > *outConInfos, btConstraintArray *constraints)
static void debugDrawAllBatches(const btBatchedConstraints *bc, btConstraintArray *constraints, const btAlignedObjectArray< btSolverBody > &bodies)
static void updateConstraintBatchIdsForMerges(int *constraintBatchIds, int numConstraints, const btBatchInfo *batches, int numBatches)
static void expandConstraintRowsInPlace(int *constraintBatchIds, const btBatchedConstraintInfo *conInfos, int numConstraints, int numConstraintRows)
static void setupSingleBatch(btBatchedConstraints *bc, int numConstraints)
static void writeOutConstraintIndicesMt(btBatchedConstraints *bc, const int *constraintBatchIds, int numConstraints, int *constraintIdPerBatch, int maxNumBatchesPerPhase, int numPhases)
static int runLengthEncodeConstraintInfo(btBatchedConstraintInfo *outConInfos, int numConstraints)
static void debugDrawPhase(const btBatchedConstraints *bc, btConstraintArray *constraints, const btAlignedObjectArray< btSolverBody > &bodies, int iPhase, const btVector3 &color0, const btVector3 &color1, const btVector3 &offset)
bool BatchCompare(const btBatchedConstraints::Range &a, const btBatchedConstraints::Range &b)
static void mergeSmallBatches(btBatchInfo *batches, int iBeginBatch, int iEndBatch, int minBatchSize, int maxBatchSize)
static void writeOutConstraintIndicesForRangeOfBatches(btBatchedConstraints *bc, const int *constraintBatchIds, int numConstraints, int *constraintIdPerBatch, int batchBegin, int batchEnd)
static void expandConstraintRowsMt(int *destConstraintBatchIds, const int *srcConstraintBatchIds, const btBatchedConstraintInfo *conInfos, int numConstraints, int numConstraintRows)
static void setupSpatialGridBatchesMt(btBatchedConstraints *batchedConstraints, btAlignedObjectArray< char > *scratchMemory, btConstraintArray *constraints, const btAlignedObjectArray< btSolverBody > &bodies, int minBatchSize, int maxBatchSize, bool use2DGrid)
static btVector3 findMaxDynamicConstraintExtent(btVector3 *bodyPositions, bool *bodyDynamicFlags, btBatchedConstraintInfo *conInfos, int numConstraints, int numBodies)
static void initBatchedBodyDynamicFlags(btAlignedObjectArray< bool > *outBodyDynamicFlags, const btAlignedObjectArray< btSolverBody > &bodies)
static void expandConstraintRows(int *destConstraintBatchIds, const int *srcConstraintBatchIds, const btBatchedConstraintInfo *conInfos, int numConstraints, int numConstraintRows)
static int initBatchedConstraintInfo(btBatchedConstraintInfo *outConInfos, btConstraintArray *constraints)
static void debugDrawSingleBatch(const btBatchedConstraints *bc, btConstraintArray *constraints, const btAlignedObjectArray< btSolverBody > &bodies, int iBatch, const btVector3 &color, const btVector3 &offset)
static void writeOutBatches(btBatchedConstraints *bc, const int *constraintBatchIds, int numConstraints, const btBatchInfo *batches, int *batchWork, int maxNumBatchesPerPhase, int numPhases)
static void assignConstraintsToGridBatches(const AssignConstraintsToGridBatchesParams ¶ms, int iConBegin, int iConEnd)
void btClamp(T &a, const T &lb, const T &ub)
const T & btMax(const T &a, const T &b)
const T & btMin(const T &a, const T &b)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
#define SIMD_FORCE_INLINE
btAlignedObjectArray< btSolverConstraint > btConstraintArray
btITaskScheduler * btGetTaskScheduler()
void btParallelFor(int iBegin, int iEnd, int grainSize, const btIParallelForBody &body)
btVector3 lerp(const btVector3 &v1, const btVector3 &v2, const btScalar &t)
Return the linear interpolation between two vectors.
void addChunk(void **ptr, size_t sz)
size_t getSizeToAllocate() const
void setChunkPointers(void *mem) const
PreallocatedMemoryHelper()
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
void resizeNoInitialize(int newsize)
resize changes the number of elements in the array.
int size() const
return the number of elements in the array
void resize(int newsize, const T &fillData=T())
void push_back(const T &_Val)
int capacity() const
return the pre-allocated (reserved) elements, this is at least as large as the total number of elemen...
const T & at(int n) const
virtual void drawLine(const btVector3 &from, const btVector3 &to, const btVector3 &color)=0
virtual int getNumThreads() const =0
btVector3 can be used to represent 3D points and vectors.
void setMax(const btVector3 &other)
Set each element to the max of the current values and the values of another btVector3.
const btScalar & z() const
Return the z value.
btVector3 absolute() const
Return a vector with the absolute values of each element.
const btScalar & x() const
Return the x value.
void setMin(const btVector3 &other)
Set each element to the min of the current values and the values of another btVector3.
const btScalar & y() const
Return the y value.
AssignConstraintsToGridBatchesLoop(const AssignConstraintsToGridBatchesParams ¶ms)
const AssignConstraintsToGridBatchesParams * m_params
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
btBatchedConstraintInfo * conInfos
btIntVec3 * bodyGridCoords
AssignConstraintsToGridBatchesParams()
int maxNumBatchesPerPhase
const btBatchedConstraintInfo * m_conInfos
int * m_destConstraintBatchIds
const int * m_srcConstraintBatchIds
ExpandConstraintRowsLoop(int *destConstraintBatchIds, const int *srcConstraintBatchIds, const btBatchedConstraintInfo *conInfos, int numConstraintRows)
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
btBatchedConstraintInfo * m_outConInfos
ReadSolverConstraintsLoop(btBatchedConstraintInfo *outConInfos, btConstraintArray *constraints)
btConstraintArray * m_constraints
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
int m_maxNumBatchesPerPhase
WriteOutConstraintIndicesLoop(btBatchedConstraints *bc, const int *constraintBatchIds, int numConstraints, int *constraintIdPerBatch, int maxNumBatchesPerPhase)
btBatchedConstraints * m_batchedConstraints
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
int * m_constraintIdPerBatch
const int * m_constraintBatchIds
bool validate(btConstraintArray *constraints, const btAlignedObjectArray< btSolverBody > &bodies) const
static bool s_debugDrawBatches
btAlignedObjectArray< Range > m_batches
btAlignedObjectArray< int > m_constraintIndices
void setup(btConstraintArray *constraints, const btAlignedObjectArray< btSolverBody > &bodies, BatchingMethod batchingMethod, int minBatchSize, int maxBatchSize, btAlignedObjectArray< char > *scratchMemory)
btAlignedObjectArray< char > m_phaseGrainSize
btAlignedObjectArray< int > m_phaseOrder
btAlignedObjectArray< Range > m_phases
btIDebugDraw * m_debugDrawer
@ BATCHING_METHOD_SPATIAL_GRID_2D
const int & operator[](int i) const
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
const btTransform & getWorldTransform() const
const btVector3 & internalGetInvMass() const
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...