54 const int kUnassignedBatch = -1;
57 for (
int iPhase = 0; iPhase < m_phases.size(); ++iPhase)
60 bodyBatchId.
resize( bodies.
size(), kUnassignedBatch );
61 const Range& phase = m_phases[iPhase];
62 for (
int iBatch = phase.
begin; iBatch < phase.
end; ++iBatch)
64 const Range& batch = m_batches[iBatch];
65 for (
int iiCons = batch.
begin; iiCons < batch.
end; ++iiCons)
67 int iCons = m_constraintIndices[iiCons];
74 if (thisBodyBatchId == kUnassignedBatch)
78 else if (thisBodyBatchId != iBatch)
80 btAssert( !
"dynamic body is used in 2 different batches in the same phase" );
87 if (thisBodyBatchId == kUnassignedBatch)
91 else if (thisBodyBatchId != iBatch)
93 btAssert( !
"dynamic body is used in 2 different batches in the same phase" );
112 if (bc && bc->
m_debugDrawer && iBatch < bc->m_batches.size())
115 for (
int iiCon = b.
begin; iiCon < b.
end; ++iiCon)
121 btVector3 pos0 = bodies[iBody0].getWorldTransform().getOrigin() + offset;
122 btVector3 pos1 = bodies[iBody1].getWorldTransform().getOrigin() + offset;
139 if ( bc && bc->
m_debugDrawer && iPhase < bc->m_phases.size() )
142 for (
int iBatch = phase.
begin; iBatch < phase.
end; ++iBatch)
144 float tt = float(iBatch - phase.
begin) / float(
btMax(1, phase.
end - phase.
begin - 1));
162 for (
int iBody = 0; iBody < bodies.
size(); ++iBody)
164 const btVector3& pos = bodies[iBody].getWorldTransform().getOrigin();
168 btVector3 bboxExtent = bboxMax - bboxMin;
171 int numPhases = bc->
m_phases.size();
172 for (
int iPhase = 0; iPhase < numPhases; ++iPhase)
174 float b = float(iPhase)/float(numPhases-1);
177 btVector3 offset = offsetBase + offsetStep*(float(iPhase) - float(numPhases-1)*0.5);
178 debugDrawPhase(bc, constraints, bodies, iPhase, color0, color1, offset);
189 for (
int i = 0; i < bodies.
size(); ++i)
203 while (iSrc < numConstraints)
210 while (iSrc < numConstraints && outConInfos[iSrc].
bodyIds[0] == srcConInfo.
bodyIds[0] && outConInfos[iSrc].
bodyIds[1] == srcConInfo.
bodyIds[1])
228 m_outConInfos = outConInfos;
229 m_constraints = constraints;
233 for (
int i = iBegin; i < iEnd; ++i)
249 int numConstraints = constraints->
size();
250 bool inParallel =
true;
254 int grainSize = 1200;
259 for (
int i = 0; i < numConstraints; ++i)
269 bool useRunLengthEncoding =
true;
270 if (useRunLengthEncoding)
274 return numConstraints;
281 if (numConstraintRows > numConstraints)
284 for (
int iCon = numConstraints - 1; iCon >= 0; --iCon)
287 int iBatch = constraintBatchIds[iCon];
292 btAssert(iDest >= 0 && iDest < numConstraintRows);
293 constraintBatchIds[iDest] = iBatch;
303 for (
int iCon = 0; iCon < numConstraints; ++iCon )
306 int iBatch = srcConstraintBatchIds[ iCon ];
311 btAssert( iDest >= 0 && iDest < numConstraintRows );
312 destConstraintBatchIds[ iDest ] = iBatch;
327 m_destConstraintBatchIds = destConstraintBatchIds;
328 m_srcConstraintBatchIds = srcConstraintBatchIds;
329 m_conInfos = conInfos;
334 expandConstraintRows(m_destConstraintBatchIds, m_srcConstraintBatchIds + iBegin, m_conInfos + iBegin, iEnd - iBegin, m_numConstraintRows);
352 int numConstraints = constraints->
size();
363 for (
int iBatch = iEndBatch - 1; iBatch >= iBeginBatch; --iBatch )
368 for (
int iDestBatch = iBatch - 1; iDestBatch >= iBeginBatch; --iDestBatch )
385 for (
int iBatch = iBeginBatch; iBatch < iEndBatch; ++iBatch )
409 BT_PROFILE(
"updateConstraintBatchIdsForMerges");
411 for (
int i = 0; i < numConstraints; ++i)
413 int iBatch = constraintBatchIds[i];
416 if (batches[iBatch].mergeIndex !=
kNoMerge)
419 constraintBatchIds[i] = batches[iBatch].
mergeIndex;
433 m_constraintBatchIds = constraintBatchIds;
435 m_numBatches = numBatches;
439 BT_PROFILE(
"UpdateConstraintBatchIdsForMergesLoop" );
447 BT_PROFILE(
"updateConstraintBatchIdsForMergesMt" );
463 const int* constraintBatchIds,
465 int* constraintIdPerBatch,
470 BT_PROFILE(
"writeOutConstraintIndicesForRangeOfBatches");
471 for (
int iCon = 0; iCon < numConstraints; ++iCon )
473 int iBatch = constraintBatchIds[ iCon ];
474 if (iBatch >= batchBegin && iBatch < batchEnd)
476 int iDestCon = constraintIdPerBatch[ iBatch ];
477 constraintIdPerBatch[ iBatch ] = iDestCon + 1;
494 m_batchedConstraints = bc;
495 m_constraintBatchIds = constraintBatchIds;
496 m_numConstraints = numConstraints;
497 m_constraintIdPerBatch = constraintIdPerBatch;
498 m_maxNumBatchesPerPhase = maxNumBatchesPerPhase;
502 BT_PROFILE(
"WriteOutConstraintIndicesLoop" );
503 int batchBegin = iBegin * m_maxNumBatchesPerPhase;
504 int batchEnd = iEnd * m_maxNumBatchesPerPhase;
506 m_constraintBatchIds,
508 m_constraintIdPerBatch,
517 const int* constraintBatchIds,
519 int* constraintIdPerBatch,
520 int maxNumBatchesPerPhase,
525 bool inParallel =
true;
533 for (
int iCon = 0; iCon < numConstraints; ++iCon )
535 int iBatch = constraintBatchIds[ iCon ];
536 int iDestCon = constraintIdPerBatch[ iBatch ];
537 constraintIdPerBatch[ iBatch ] = iDestCon + 1;
547 int numPhases = bc->
m_phases.size();
550 for (
int iPhase = 0; iPhase < numPhases; ++iPhase)
552 const Range& phase = bc->
m_phases[ iPhase ];
553 int numBatches = phase.end - phase.begin;
554 float grainSize = floor((0.25f*numBatches /
float(numThreads)) + 0.0f);
561 const int* constraintBatchIds,
565 int maxNumBatchesPerPhase,
573 bc->
m_phases.resizeNoInitialize( 0 );
577 int* constraintIdPerBatch = batchWork;
579 for (
int iPhase = 0; iPhase < numPhases; ++iPhase)
581 int curPhaseBegin = bc->
m_batches.size();
582 int iBegin = iPhase * maxNumBatchesPerPhase;
583 int iEnd = iBegin + maxNumBatchesPerPhase;
584 for (
int i = iBegin; i < iEnd; ++i )
587 int curBatchBegin = iConstraint;
588 constraintIdPerBatch[ i ] = curBatchBegin;
590 iConstraint += numConstraints;
591 if ( numConstraints > 0 )
593 bc->
m_batches.push_back( Range( curBatchBegin, iConstraint ) );
597 if ( bc->
m_batches.size() > curPhaseBegin )
604 btAssert(iConstraint == numConstraints);
609 for (
int iPhase = 0; iPhase < bc->
m_phases.size(); ++iPhase)
612 const Range& curBatches = bc->
m_phases[iPhase];
616 for (
int i = 0; i < bc->
m_phases.size(); ++i)
654 if ( m_numChunks < N )
656 Chunk& chunk = m_chunks[ m_numChunks ];
664 size_t totalSize = 0;
665 for (
int i = 0; i < m_numChunks; ++i)
667 totalSize += m_chunks[i].
size;
673 size_t totalSize = 0;
674 for (
int i = 0; i < m_numChunks; ++i)
676 const Chunk& chunk = m_chunks[ i ];
677 char* chunkPtr =
static_cast<char*
>(mem) + totalSize;
678 *chunk.
ptr = chunkPtr;
679 totalSize += chunk.
size;
688 bool* bodyDynamicFlags,
696 for (
int iCon = 0; iCon < numConstraints; ++iCon)
701 btAssert(iBody0 >= 0 && iBody0 < numBodies);
702 btAssert(iBody1 >= 0 && iBody1 < numBodies);
704 if (bodyDynamicFlags[iBody0] && bodyDynamicFlags[iBody1])
706 btVector3 delta = bodyPositions[iBody1] - bodyPositions[iBody0];
737 memset(
this, 0,
sizeof(*
this));
746 for (
int iCon = iConBegin; iCon < iConEnd; ++iCon )
760 for (
int i = 0; i < 3; ++i)
764 if (coordMin != coordMax)
766 btAssert( coordMax == coordMin + 1 );
767 if ((coordMin&1) == 0)
777 gridCoord[ i ] = coordMin;
789 for (
int i = 0; i < 3; ++i )
791 gridCoord[ i ] = body0Coords.
m_ints[ i ];
798 for (
int i = 0; i < 3; ++i )
800 int coordOffset = ( iPhase >> i ) & 1;
801 chunkCoord[ i ] = (gridCoord[ i ] - coordOffset)/2;
802 btClamp( chunkCoord[ i ], 0, gridChunkDim[ i ] - 1);
803 btAssert( chunkCoord[ i ] < gridChunkDim[ i ] );
805 int iBatch = iPhase * params.
maxNumBatchesPerPhase + chunkCoord[ 0 ] + chunkCoord[ 1 ] * gridChunkDim[ 0 ] + chunkCoord[ 2 ] * gridChunkDim[ 0 ] * gridChunkDim[ 1 ];
866 const int numPhases = 8;
867 int numConstraints = constraints->
size();
870 const int maxGridChunkCount = 128;
871 int allocNumBatchesPerPhase = maxGridChunkCount;
872 int minNumBatchesPerPhase = 16;
873 int allocNumBatches = allocNumBatchesPerPhase * numPhases;
876 bool* bodyDynamicFlags = NULL;
879 int* batchWork = NULL;
881 int* constraintBatchIds = NULL;
882 int* constraintRowBatchIds = NULL;
886 memHelper.
addChunk( (
void**) &bodyDynamicFlags,
sizeof(
bool ) * bodies.
size() );
889 memHelper.
addChunk( (
void**) &batchWork,
sizeof(
int )* allocNumBatches );
891 memHelper.
addChunk( (
void**) &constraintBatchIds,
sizeof(
int ) * numConstraints );
892 memHelper.
addChunk( (
void**) &constraintRowBatchIds,
sizeof(
int ) * numConstraintRows );
895 if (scratchMemory->
capacity() < scratchSize)
898 scratchMemory->
reserve( scratchSize + scratchSize/16 );
901 char* memPtr = &scratchMemory->
at(0);
912 for (
int i = 0; i < bodies.
size(); ++i)
917 bodyPositions[i] = bodyPos;
918 bodyDynamicFlags[i] = isDynamic;
931 btVector3 gridExtent = bboxMax - bboxMin;
935 gridDim[ 0 ] = int( 1.0 + gridExtent.x() / gridCellSize.
x() );
936 gridDim[ 1 ] = int( 1.0 + gridExtent.y() / gridCellSize.
y() );
937 gridDim[ 2 ] = int( 1.0 + gridExtent.z() / gridCellSize.
z() );
941 bool collapseAxis = use2DGrid;
945 int iAxisToCollapse = 0;
946 int axisDim = gridDim[iAxisToCollapse];
948 for (
int i = 0; i < 3; ++i )
950 if (gridDim[i] < axisDim)
953 axisDim = gridDim[i];
957 gridCellSize[iAxisToCollapse] = gridExtent[iAxisToCollapse] * 2.0f;
958 phaseMask &= ~(1 << iAxisToCollapse);
961 int numGridChunks = 0;
965 gridDim[0] = int( 1.0 + gridExtent.x() / gridCellSize.
x() );
966 gridDim[1] = int( 1.0 + gridExtent.y() / gridCellSize.
y() );
967 gridDim[2] = int( 1.0 + gridExtent.z() / gridCellSize.
z() );
968 gridChunkDim[ 0 ] =
btMax( 1, ( gridDim[ 0 ] + 0 ) / 2 );
969 gridChunkDim[ 1 ] =
btMax( 1, ( gridDim[ 1 ] + 0 ) / 2 );
970 gridChunkDim[ 2 ] =
btMax( 1, ( gridDim[ 2 ] + 0 ) / 2 );
971 numGridChunks = gridChunkDim[ 0 ] * gridChunkDim[ 1 ] * gridChunkDim[ 2 ];
972 float nChunks = float(gridChunkDim[0]) * float(gridChunkDim[1]) * float(gridChunkDim[2]);
973 if ( numGridChunks <= maxGridChunkCount && nChunks <= maxGridChunkCount )
977 gridCellSize *= 1.25;
979 btAssert(numGridChunks <= maxGridChunkCount );
980 int maxNumBatchesPerPhase = numGridChunks;
985 for (
int iBody = 0; iBody < bodies.
size(); ++iBody)
987 btIntVec3& coords = bodyGridCoords[iBody];
988 if (bodyDynamicFlags[iBody])
990 btVector3 v = ( bodyPositions[ iBody ] - bboxMin )*invGridCellSize;
1006 for (
int iPhase = 0; iPhase < numPhases; ++iPhase)
1008 int batchBegin = iPhase * maxNumBatchesPerPhase;
1009 int batchEnd = batchBegin + maxNumBatchesPerPhase;
1010 for (
int iBatch = batchBegin; iBatch < batchEnd; ++iBatch )
1028 bool inParallel =
true;
1032 int grainSize = 250;
1040 for (
int iCon = 0; iCon < numConstraints; ++iCon )
1043 int iBatch = constraintBatchIds[ iCon ];
1048 for (
int iPhase = 0; iPhase < numPhases; ++iPhase)
1051 if (iPhase == (iPhase&phaseMask))
1053 int iBeginBatch = iPhase * maxNumBatchesPerPhase;
1054 int iEndBatch = iBeginBatch + maxNumBatchesPerPhase;
1055 mergeSmallBatches( batches, iBeginBatch, iEndBatch, minBatchSize, maxBatchSize );
1061 if (numConstraintRows > numConstraints)
1063 expandConstraintRowsMt(&constraintRowBatchIds[0], &constraintBatchIds[0], &conInfos[0], numConstraints, numConstraintRows);
1067 constraintRowBatchIds = constraintBatchIds;
1070 writeOutBatches(batchedConstraints, constraintRowBatchIds, numConstraintRows, batches, batchWork, maxNumBatchesPerPhase, numPhases);
1084 for (
int i = 0; i < numConstraints; ++i )
1090 bc->
m_phases.resizeNoInitialize( 0 );
1094 if (numConstraints > 0)
1096 bc->
m_batches.push_back( Range( 0, numConstraints ) );
1097 bc->
m_phases.push_back( Range( 0, 1 ) );
1113 if (constraints->
size() >= minBatchSize*4)
1115 bool use2DGrid = batchingMethod == BATCHING_METHOD_SPATIAL_GRID_2D;
1117 if (s_debugDrawBatches)
const AssignConstraintsToGridBatchesParams * m_params
void push_back(const T &_Val)
AssignConstraintsToGridBatchesParams()
static void writeGrainSizes(btBatchedConstraints *bc)
static void assignConstraintsToGridBatches(const AssignConstraintsToGridBatchesParams ¶ms, int iConBegin, int iConEnd)
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
const T & at(int n) const
static void expandConstraintRows(int *destConstraintBatchIds, const int *srcConstraintBatchIds, const btBatchedConstraintInfo *conInfos, int numConstraints, int numConstraintRows)
ReadSolverConstraintsLoop(btBatchedConstraintInfo *outConInfos, btConstraintArray *constraints)
int maxNumBatchesPerPhase
btITaskScheduler * btGetTaskScheduler()
virtual void drawLine(const btVector3 &from, const btVector3 &to, const btVector3 &color)=0
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
void resizeNoInitialize(int newsize)
resize changes the number of elements in the array.
virtual int getNumThreads() const =0
static void writeOutBatches(btBatchedConstraints *bc, const int *constraintBatchIds, int numConstraints, const btBatchInfo *batches, int *batchWork, int maxNumBatchesPerPhase, int numPhases)
btVector3 absolute() const
Return a vector with the absolute values of each element.
static void mergeSmallBatches(btBatchInfo *batches, int iBeginBatch, int iEndBatch, int minBatchSize, int maxBatchSize)
ExpandConstraintRowsLoop(int *destConstraintBatchIds, const int *srcConstraintBatchIds, const btBatchedConstraintInfo *conInfos, int numConstraintRows)
#define SIMD_FORCE_INLINE
btAlignedObjectArray< int > m_phaseOrder
void btClamp(T &a, const T &lb, const T &ub)
const int * m_srcConstraintBatchIds
static void setupSpatialGridBatchesMt(btBatchedConstraints *batchedConstraints, btAlignedObjectArray< char > *scratchMemory, btConstraintArray *constraints, const btAlignedObjectArray< btSolverBody > &bodies, int minBatchSize, int maxBatchSize, bool use2DGrid)
static void updateConstraintBatchIdsForMerges(int *constraintBatchIds, int numConstraints, const btBatchInfo *batches, int numBatches)
const btScalar & x() const
Return the x value.
const int & operator[](int i) const
static void initBatchedConstraintInfoArray(btAlignedObjectArray< 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 int runLengthEncodeConstraintInfo(btBatchedConstraintInfo *outConInfos, int numConstraints)
static int initBatchedConstraintInfo(btBatchedConstraintInfo *outConInfos, btConstraintArray *constraints)
btConstraintArray * m_constraints
int size() const
return the number of elements in the array
size_t getSizeToAllocate() const
int * m_constraintIdPerBatch
btIntVec3 * bodyGridCoords
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
btBatchedConstraintInfo * m_outConInfos
void setChunkPointers(void *mem) const
static void expandConstraintRowsMt(int *destConstraintBatchIds, const int *srcConstraintBatchIds, const btBatchedConstraintInfo *conInfos, int numConstraints, int numConstraintRows)
static void expandConstraintRowsInPlace(int *constraintBatchIds, const btBatchedConstraintInfo *conInfos, int numConstraints, int numConstraintRows)
static void initBatchedBodyDynamicFlags(btAlignedObjectArray< bool > *outBodyDynamicFlags, const btAlignedObjectArray< btSolverBody > &bodies)
static void debugDrawPhase(const btBatchedConstraints *bc, btConstraintArray *constraints, const btAlignedObjectArray< btSolverBody > &bodies, int iPhase, const btVector3 &color0, const btVector3 &color1, const btVector3 &offset)
btAlignedObjectArray< Range > m_phases
static bool s_debugDrawBatches
btAlignedObjectArray< int > m_constraintIndices
const btScalar & y() const
Return the y value.
const btVector3 & internalGetInvMass() const
int capacity() const
return the pre-allocated (reserved) elements, this is at least as large as the total number of elemen...
static btVector3 findMaxDynamicConstraintExtent(btVector3 *bodyPositions, bool *bodyDynamicFlags, btBatchedConstraintInfo *conInfos, int numConstraints, int numBodies)
WriteOutConstraintIndicesLoop(btBatchedConstraints *bc, const int *constraintBatchIds, int numConstraints, int *constraintIdPerBatch, int maxNumBatchesPerPhase)
btVector3 can be used to represent 3D points and vectors.
int * m_destConstraintBatchIds
btAlignedObjectArray< char > m_phaseGrainSize
bool BatchCompare(const btBatchedConstraints::Range &a, const btBatchedConstraints::Range &b)
void setup(btConstraintArray *constraints, const btAlignedObjectArray< btSolverBody > &bodies, BatchingMethod batchingMethod, int minBatchSize, int maxBatchSize, btAlignedObjectArray< char > *scratchMemory)
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
AssignConstraintsToGridBatchesLoop(const AssignConstraintsToGridBatchesParams ¶ms)
static void writeOutConstraintIndicesForRangeOfBatches(btBatchedConstraints *bc, const int *constraintBatchIds, int numConstraints, int *constraintIdPerBatch, int batchBegin, int batchEnd)
const int * m_constraintBatchIds
void resize(int newsize, const T &fillData=T())
static void setupSingleBatch(btBatchedConstraints *bc, int numConstraints)
const btBatchedConstraintInfo * m_conInfos
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
btIDebugDraw * m_debugDrawer
const T & btMax(const T &a, const T &b)
const btTransform & getWorldTransform() const
void btParallelFor(int iBegin, int iEnd, int grainSize, const btIParallelForBody &body)
static void writeOutConstraintIndicesMt(btBatchedConstraints *bc, const int *constraintBatchIds, int numConstraints, int *constraintIdPerBatch, int maxNumBatchesPerPhase, int numPhases)
void setMax(const btVector3 &other)
Set each element to the max of the current values and the values of another btVector3.
btBatchedConstraintInfo * conInfos
PreallocatedMemoryHelper()
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
const T & btMin(const T &a, const T &b)
bool validate(btConstraintArray *constraints, const btAlignedObjectArray< btSolverBody > &bodies) const
static void debugDrawAllBatches(const btBatchedConstraints *bc, btConstraintArray *constraints, const btAlignedObjectArray< btSolverBody > &bodies)
btVector3 lerp(const btVector3 &v1, const btVector3 &v2, const btScalar &t)
Return the linear interpolation between two vectors.
btAlignedObjectArray< Range > m_batches
void addChunk(void **ptr, size_t sz)
void setMin(const btVector3 &other)
Set each element to the min of the current values and the values of another btVector3.
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btBatchedConstraints * m_batchedConstraints
static void updateConstraintBatchIdsForMergesMt(int *constraintBatchIds, int numConstraints, const btBatchInfo *batches, int numBatches)
int m_maxNumBatchesPerPhase
const btScalar & z() const
Return the z value.