diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp')
| -rw-r--r-- | thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp | 1794 |
1 files changed, 873 insertions, 921 deletions
diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp index c82ba87f9f..b51dfaad3c 100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btBatchedConstraints.h" #include "LinearMath/btIDebugDraw.h" @@ -21,606 +20,573 @@ subject to the following restrictions: #include "LinearMath/btStackAlloc.h" #include "LinearMath/btQuickprof.h" -#include <string.h> //for memset +#include <string.h> //for memset const int kNoMerge = -1; bool btBatchedConstraints::s_debugDrawBatches = false; - struct btBatchedConstraintInfo { - int constraintIndex; - int numConstraintRows; - int bodyIds[2]; + int constraintIndex; + int numConstraintRows; + int bodyIds[2]; }; - struct btBatchInfo { - int numConstraints; - int mergeIndex; + int numConstraints; + int mergeIndex; - btBatchInfo() : numConstraints(0), mergeIndex(kNoMerge) {} + btBatchInfo() : numConstraints(0), mergeIndex(kNoMerge) {} }; - bool btBatchedConstraints::validate(btConstraintArray* constraints, const btAlignedObjectArray<btSolverBody>& bodies) const { - // - // validate: for debugging only. Verify coloring of bodies, that no body is touched by more than one batch in any given phase - // - int errors = 0; - const int kUnassignedBatch = -1; - - btAlignedObjectArray<int> bodyBatchId; - for (int iPhase = 0; iPhase < m_phases.size(); ++iPhase) - { - bodyBatchId.resizeNoInitialize(0); - bodyBatchId.resize( bodies.size(), kUnassignedBatch ); - const Range& phase = m_phases[iPhase]; - for (int iBatch = phase.begin; iBatch < phase.end; ++iBatch) - { - const Range& batch = m_batches[iBatch]; - for (int iiCons = batch.begin; iiCons < batch.end; ++iiCons) - { - int iCons = m_constraintIndices[iiCons]; - const btSolverConstraint& cons = constraints->at(iCons); - const btSolverBody& bodyA = bodies[cons.m_solverBodyIdA]; - const btSolverBody& bodyB = bodies[cons.m_solverBodyIdB]; - if (! bodyA.internalGetInvMass().isZero()) - { - int thisBodyBatchId = bodyBatchId[cons.m_solverBodyIdA]; - if (thisBodyBatchId == kUnassignedBatch) - { - bodyBatchId[cons.m_solverBodyIdA] = iBatch; - } - else if (thisBodyBatchId != iBatch) - { - btAssert( !"dynamic body is used in 2 different batches in the same phase" ); - errors++; - } - } - if (! bodyB.internalGetInvMass().isZero()) - { - int thisBodyBatchId = bodyBatchId[cons.m_solverBodyIdB]; - if (thisBodyBatchId == kUnassignedBatch) - { - bodyBatchId[cons.m_solverBodyIdB] = iBatch; - } - else if (thisBodyBatchId != iBatch) - { - btAssert( !"dynamic body is used in 2 different batches in the same phase" ); - errors++; - } - } - } - } - } - return errors == 0; + // + // validate: for debugging only. Verify coloring of bodies, that no body is touched by more than one batch in any given phase + // + int errors = 0; + const int kUnassignedBatch = -1; + + btAlignedObjectArray<int> bodyBatchId; + for (int iPhase = 0; iPhase < m_phases.size(); ++iPhase) + { + bodyBatchId.resizeNoInitialize(0); + bodyBatchId.resize(bodies.size(), kUnassignedBatch); + const Range& phase = m_phases[iPhase]; + for (int iBatch = phase.begin; iBatch < phase.end; ++iBatch) + { + const Range& batch = m_batches[iBatch]; + for (int iiCons = batch.begin; iiCons < batch.end; ++iiCons) + { + int iCons = m_constraintIndices[iiCons]; + const btSolverConstraint& cons = constraints->at(iCons); + const btSolverBody& bodyA = bodies[cons.m_solverBodyIdA]; + const btSolverBody& bodyB = bodies[cons.m_solverBodyIdB]; + if (!bodyA.internalGetInvMass().isZero()) + { + int thisBodyBatchId = bodyBatchId[cons.m_solverBodyIdA]; + if (thisBodyBatchId == kUnassignedBatch) + { + bodyBatchId[cons.m_solverBodyIdA] = iBatch; + } + else if (thisBodyBatchId != iBatch) + { + btAssert(!"dynamic body is used in 2 different batches in the same phase"); + errors++; + } + } + if (!bodyB.internalGetInvMass().isZero()) + { + int thisBodyBatchId = bodyBatchId[cons.m_solverBodyIdB]; + if (thisBodyBatchId == kUnassignedBatch) + { + bodyBatchId[cons.m_solverBodyIdB] = iBatch; + } + else if (thisBodyBatchId != iBatch) + { + btAssert(!"dynamic body is used in 2 different batches in the same phase"); + errors++; + } + } + } + } + } + return errors == 0; } - -static void debugDrawSingleBatch( const btBatchedConstraints* bc, - btConstraintArray* constraints, - const btAlignedObjectArray<btSolverBody>& bodies, - int iBatch, - const btVector3& color, - const btVector3& offset - ) +static void debugDrawSingleBatch(const btBatchedConstraints* bc, + btConstraintArray* constraints, + const btAlignedObjectArray<btSolverBody>& bodies, + int iBatch, + const btVector3& color, + const btVector3& offset) { - if (bc && bc->m_debugDrawer && iBatch < bc->m_batches.size()) - { - const btBatchedConstraints::Range& b = bc->m_batches[iBatch]; - for (int iiCon = b.begin; iiCon < b.end; ++iiCon) - { - int iCon = bc->m_constraintIndices[iiCon]; - const btSolverConstraint& con = constraints->at(iCon); - int iBody0 = con.m_solverBodyIdA; - int iBody1 = con.m_solverBodyIdB; - btVector3 pos0 = bodies[iBody0].getWorldTransform().getOrigin() + offset; - btVector3 pos1 = bodies[iBody1].getWorldTransform().getOrigin() + offset; - bc->m_debugDrawer->drawLine(pos0, pos1, color); - } - } + if (bc && bc->m_debugDrawer && iBatch < bc->m_batches.size()) + { + const btBatchedConstraints::Range& b = bc->m_batches[iBatch]; + for (int iiCon = b.begin; iiCon < b.end; ++iiCon) + { + int iCon = bc->m_constraintIndices[iiCon]; + const btSolverConstraint& con = constraints->at(iCon); + int iBody0 = con.m_solverBodyIdA; + int iBody1 = con.m_solverBodyIdB; + btVector3 pos0 = bodies[iBody0].getWorldTransform().getOrigin() + offset; + btVector3 pos1 = bodies[iBody1].getWorldTransform().getOrigin() + offset; + bc->m_debugDrawer->drawLine(pos0, pos1, color); + } + } } - -static void debugDrawPhase( const btBatchedConstraints* bc, - btConstraintArray* constraints, - const btAlignedObjectArray<btSolverBody>& bodies, - int iPhase, - const btVector3& color0, - const btVector3& color1, - const btVector3& offset - ) +static void debugDrawPhase(const btBatchedConstraints* bc, + btConstraintArray* constraints, + const btAlignedObjectArray<btSolverBody>& bodies, + int iPhase, + const btVector3& color0, + const btVector3& color1, + const btVector3& offset) { - BT_PROFILE( "debugDrawPhase" ); - if ( bc && bc->m_debugDrawer && iPhase < bc->m_phases.size() ) - { - const btBatchedConstraints::Range& phase = bc->m_phases[iPhase]; - for (int iBatch = phase.begin; iBatch < phase.end; ++iBatch) - { - float tt = float(iBatch - phase.begin) / float(btMax(1, phase.end - phase.begin - 1)); - btVector3 col = lerp(color0, color1, tt); - debugDrawSingleBatch(bc, constraints, bodies, iBatch, col, offset); - } - } + BT_PROFILE("debugDrawPhase"); + if (bc && bc->m_debugDrawer && iPhase < bc->m_phases.size()) + { + const btBatchedConstraints::Range& phase = bc->m_phases[iPhase]; + for (int iBatch = phase.begin; iBatch < phase.end; ++iBatch) + { + float tt = float(iBatch - phase.begin) / float(btMax(1, phase.end - phase.begin - 1)); + btVector3 col = lerp(color0, color1, tt); + debugDrawSingleBatch(bc, constraints, bodies, iBatch, col, offset); + } + } } - -static void debugDrawAllBatches( const btBatchedConstraints* bc, - btConstraintArray* constraints, - const btAlignedObjectArray<btSolverBody>& bodies - ) +static void debugDrawAllBatches(const btBatchedConstraints* bc, + btConstraintArray* constraints, + const btAlignedObjectArray<btSolverBody>& bodies) { - BT_PROFILE( "debugDrawAllBatches" ); - if ( bc && bc->m_debugDrawer && bc->m_phases.size() > 0 ) - { - btVector3 bboxMin(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT); - btVector3 bboxMax = -bboxMin; - for (int iBody = 0; iBody < bodies.size(); ++iBody) - { - const btVector3& pos = bodies[iBody].getWorldTransform().getOrigin(); - bboxMin.setMin(pos); - bboxMax.setMax(pos); - } - btVector3 bboxExtent = bboxMax - bboxMin; - btVector3 offsetBase = btVector3( 0, bboxExtent.y()*1.1f, 0 ); - btVector3 offsetStep = btVector3( 0, 0, bboxExtent.z()*1.1f ); - int numPhases = bc->m_phases.size(); - for (int iPhase = 0; iPhase < numPhases; ++iPhase) - { - float b = float(iPhase)/float(numPhases-1); - btVector3 color0 = btVector3(1,0,b); - btVector3 color1 = btVector3(0,1,b); - btVector3 offset = offsetBase + offsetStep*(float(iPhase) - float(numPhases-1)*0.5); - debugDrawPhase(bc, constraints, bodies, iPhase, color0, color1, offset); - } - } + BT_PROFILE("debugDrawAllBatches"); + if (bc && bc->m_debugDrawer && bc->m_phases.size() > 0) + { + btVector3 bboxMin(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT); + btVector3 bboxMax = -bboxMin; + for (int iBody = 0; iBody < bodies.size(); ++iBody) + { + const btVector3& pos = bodies[iBody].getWorldTransform().getOrigin(); + bboxMin.setMin(pos); + bboxMax.setMax(pos); + } + btVector3 bboxExtent = bboxMax - bboxMin; + btVector3 offsetBase = btVector3(0, bboxExtent.y() * 1.1f, 0); + btVector3 offsetStep = btVector3(0, 0, bboxExtent.z() * 1.1f); + int numPhases = bc->m_phases.size(); + for (int iPhase = 0; iPhase < numPhases; ++iPhase) + { + float b = float(iPhase) / float(numPhases - 1); + btVector3 color0 = btVector3(1, 0, b); + btVector3 color1 = btVector3(0, 1, b); + btVector3 offset = offsetBase + offsetStep * (float(iPhase) - float(numPhases - 1) * 0.5); + debugDrawPhase(bc, constraints, bodies, iPhase, color0, color1, offset); + } + } } - static void initBatchedBodyDynamicFlags(btAlignedObjectArray<bool>* outBodyDynamicFlags, const btAlignedObjectArray<btSolverBody>& bodies) { - BT_PROFILE("initBatchedBodyDynamicFlags"); - btAlignedObjectArray<bool>& bodyDynamicFlags = *outBodyDynamicFlags; - bodyDynamicFlags.resizeNoInitialize(bodies.size()); - for (int i = 0; i < bodies.size(); ++i) - { - const btSolverBody& body = bodies[ i ]; - bodyDynamicFlags[i] = ( body.internalGetInvMass().x() > btScalar( 0 ) ); - } + BT_PROFILE("initBatchedBodyDynamicFlags"); + btAlignedObjectArray<bool>& bodyDynamicFlags = *outBodyDynamicFlags; + bodyDynamicFlags.resizeNoInitialize(bodies.size()); + for (int i = 0; i < bodies.size(); ++i) + { + const btSolverBody& body = bodies[i]; + bodyDynamicFlags[i] = (body.internalGetInvMass().x() > btScalar(0)); + } } - static int runLengthEncodeConstraintInfo(btBatchedConstraintInfo* outConInfos, int numConstraints) { - BT_PROFILE("runLengthEncodeConstraintInfo"); - // detect and run-length encode constraint rows that repeat the same bodies - int iDest = 0; - int iSrc = 0; - while (iSrc < numConstraints) - { - const btBatchedConstraintInfo& srcConInfo = outConInfos[iSrc]; - btBatchedConstraintInfo& conInfo = outConInfos[iDest]; - conInfo.constraintIndex = iSrc; - conInfo.bodyIds[0] = srcConInfo.bodyIds[0]; - conInfo.bodyIds[1] = srcConInfo.bodyIds[1]; - while (iSrc < numConstraints && outConInfos[iSrc].bodyIds[0] == srcConInfo.bodyIds[0] && outConInfos[iSrc].bodyIds[1] == srcConInfo.bodyIds[1]) - { - ++iSrc; - } - conInfo.numConstraintRows = iSrc - conInfo.constraintIndex; - ++iDest; - } - return iDest; + BT_PROFILE("runLengthEncodeConstraintInfo"); + // detect and run-length encode constraint rows that repeat the same bodies + int iDest = 0; + int iSrc = 0; + while (iSrc < numConstraints) + { + const btBatchedConstraintInfo& srcConInfo = outConInfos[iSrc]; + btBatchedConstraintInfo& conInfo = outConInfos[iDest]; + conInfo.constraintIndex = iSrc; + conInfo.bodyIds[0] = srcConInfo.bodyIds[0]; + conInfo.bodyIds[1] = srcConInfo.bodyIds[1]; + while (iSrc < numConstraints && outConInfos[iSrc].bodyIds[0] == srcConInfo.bodyIds[0] && outConInfos[iSrc].bodyIds[1] == srcConInfo.bodyIds[1]) + { + ++iSrc; + } + conInfo.numConstraintRows = iSrc - conInfo.constraintIndex; + ++iDest; + } + return iDest; } - struct ReadSolverConstraintsLoop : public btIParallelForBody { - btBatchedConstraintInfo* m_outConInfos; - btConstraintArray* m_constraints; - - ReadSolverConstraintsLoop( btBatchedConstraintInfo* outConInfos, btConstraintArray* constraints ) - { - m_outConInfos = outConInfos; - m_constraints = constraints; - } - void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE - { - for (int i = iBegin; i < iEnd; ++i) - { - btBatchedConstraintInfo& conInfo = m_outConInfos[i]; - const btSolverConstraint& con = m_constraints->at( i ); - conInfo.bodyIds[0] = con.m_solverBodyIdA; - conInfo.bodyIds[1] = con.m_solverBodyIdB; - conInfo.constraintIndex = i; - conInfo.numConstraintRows = 1; - } - } + btBatchedConstraintInfo* m_outConInfos; + btConstraintArray* m_constraints; + + ReadSolverConstraintsLoop(btBatchedConstraintInfo* outConInfos, btConstraintArray* constraints) + { + m_outConInfos = outConInfos; + m_constraints = constraints; + } + void forLoop(int iBegin, int iEnd) const BT_OVERRIDE + { + for (int i = iBegin; i < iEnd; ++i) + { + btBatchedConstraintInfo& conInfo = m_outConInfos[i]; + const btSolverConstraint& con = m_constraints->at(i); + conInfo.bodyIds[0] = con.m_solverBodyIdA; + conInfo.bodyIds[1] = con.m_solverBodyIdB; + conInfo.constraintIndex = i; + conInfo.numConstraintRows = 1; + } + } }; - static int initBatchedConstraintInfo(btBatchedConstraintInfo* outConInfos, btConstraintArray* constraints) { - BT_PROFILE("initBatchedConstraintInfo"); - int numConstraints = constraints->size(); - bool inParallel = true; - if (inParallel) - { - ReadSolverConstraintsLoop loop(outConInfos, constraints); - int grainSize = 1200; - btParallelFor(0, numConstraints, grainSize, loop); - } - else - { - for (int i = 0; i < numConstraints; ++i) - { - btBatchedConstraintInfo& conInfo = outConInfos[i]; - const btSolverConstraint& con = constraints->at( i ); - conInfo.bodyIds[0] = con.m_solverBodyIdA; - conInfo.bodyIds[1] = con.m_solverBodyIdB; - conInfo.constraintIndex = i; - conInfo.numConstraintRows = 1; - } - } - bool useRunLengthEncoding = true; - if (useRunLengthEncoding) - { - numConstraints = runLengthEncodeConstraintInfo(outConInfos, numConstraints); - } - return numConstraints; + BT_PROFILE("initBatchedConstraintInfo"); + int numConstraints = constraints->size(); + bool inParallel = true; + if (inParallel) + { + ReadSolverConstraintsLoop loop(outConInfos, constraints); + int grainSize = 1200; + btParallelFor(0, numConstraints, grainSize, loop); + } + else + { + for (int i = 0; i < numConstraints; ++i) + { + btBatchedConstraintInfo& conInfo = outConInfos[i]; + const btSolverConstraint& con = constraints->at(i); + conInfo.bodyIds[0] = con.m_solverBodyIdA; + conInfo.bodyIds[1] = con.m_solverBodyIdB; + conInfo.constraintIndex = i; + conInfo.numConstraintRows = 1; + } + } + bool useRunLengthEncoding = true; + if (useRunLengthEncoding) + { + numConstraints = runLengthEncodeConstraintInfo(outConInfos, numConstraints); + } + return numConstraints; } - static void expandConstraintRowsInPlace(int* constraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraints, int numConstraintRows) { - BT_PROFILE("expandConstraintRowsInPlace"); - if (numConstraintRows > numConstraints) - { - // we walk the array in reverse to avoid overwriteing - for (int iCon = numConstraints - 1; iCon >= 0; --iCon) - { - const btBatchedConstraintInfo& conInfo = conInfos[iCon]; - int iBatch = constraintBatchIds[iCon]; - for (int i = conInfo.numConstraintRows - 1; i >= 0; --i) - { - int iDest = conInfo.constraintIndex + i; - btAssert(iDest >= iCon); - btAssert(iDest >= 0 && iDest < numConstraintRows); - constraintBatchIds[iDest] = iBatch; - } - } - } + BT_PROFILE("expandConstraintRowsInPlace"); + if (numConstraintRows > numConstraints) + { + // we walk the array in reverse to avoid overwriteing + for (int iCon = numConstraints - 1; iCon >= 0; --iCon) + { + const btBatchedConstraintInfo& conInfo = conInfos[iCon]; + int iBatch = constraintBatchIds[iCon]; + for (int i = conInfo.numConstraintRows - 1; i >= 0; --i) + { + int iDest = conInfo.constraintIndex + i; + btAssert(iDest >= iCon); + btAssert(iDest >= 0 && iDest < numConstraintRows); + constraintBatchIds[iDest] = iBatch; + } + } + } } - static void expandConstraintRows(int* destConstraintBatchIds, const int* srcConstraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraints, int numConstraintRows) { - BT_PROFILE("expandConstraintRows"); - for ( int iCon = 0; iCon < numConstraints; ++iCon ) - { - const btBatchedConstraintInfo& conInfo = conInfos[ iCon ]; - int iBatch = srcConstraintBatchIds[ iCon ]; - for ( int i = 0; i < conInfo.numConstraintRows; ++i ) - { - int iDest = conInfo.constraintIndex + i; - btAssert( iDest >= iCon ); - btAssert( iDest >= 0 && iDest < numConstraintRows ); - destConstraintBatchIds[ iDest ] = iBatch; - } - } + BT_PROFILE("expandConstraintRows"); + for (int iCon = 0; iCon < numConstraints; ++iCon) + { + const btBatchedConstraintInfo& conInfo = conInfos[iCon]; + int iBatch = srcConstraintBatchIds[iCon]; + for (int i = 0; i < conInfo.numConstraintRows; ++i) + { + int iDest = conInfo.constraintIndex + i; + btAssert(iDest >= iCon); + btAssert(iDest >= 0 && iDest < numConstraintRows); + destConstraintBatchIds[iDest] = iBatch; + } + } } - struct ExpandConstraintRowsLoop : public btIParallelForBody { - int* m_destConstraintBatchIds; - const int* m_srcConstraintBatchIds; - const btBatchedConstraintInfo* m_conInfos; - int m_numConstraintRows; - - ExpandConstraintRowsLoop( int* destConstraintBatchIds, const int* srcConstraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraintRows) - { - m_destConstraintBatchIds = destConstraintBatchIds; - m_srcConstraintBatchIds = srcConstraintBatchIds; - m_conInfos = conInfos; - m_numConstraintRows = numConstraintRows; - } - void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE - { - expandConstraintRows(m_destConstraintBatchIds, m_srcConstraintBatchIds + iBegin, m_conInfos + iBegin, iEnd - iBegin, m_numConstraintRows); - } + int* m_destConstraintBatchIds; + const int* m_srcConstraintBatchIds; + const btBatchedConstraintInfo* m_conInfos; + int m_numConstraintRows; + + ExpandConstraintRowsLoop(int* destConstraintBatchIds, const int* srcConstraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraintRows) + { + m_destConstraintBatchIds = destConstraintBatchIds; + m_srcConstraintBatchIds = srcConstraintBatchIds; + m_conInfos = conInfos; + m_numConstraintRows = numConstraintRows; + } + void forLoop(int iBegin, int iEnd) const BT_OVERRIDE + { + expandConstraintRows(m_destConstraintBatchIds, m_srcConstraintBatchIds + iBegin, m_conInfos + iBegin, iEnd - iBegin, m_numConstraintRows); + } }; - static void expandConstraintRowsMt(int* destConstraintBatchIds, const int* srcConstraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraints, int numConstraintRows) { - BT_PROFILE("expandConstraintRowsMt"); - ExpandConstraintRowsLoop loop(destConstraintBatchIds, srcConstraintBatchIds, conInfos, numConstraintRows); - int grainSize = 600; - btParallelFor(0, numConstraints, grainSize, loop); + BT_PROFILE("expandConstraintRowsMt"); + ExpandConstraintRowsLoop loop(destConstraintBatchIds, srcConstraintBatchIds, conInfos, numConstraintRows); + int grainSize = 600; + btParallelFor(0, numConstraints, grainSize, loop); } - static void initBatchedConstraintInfoArray(btAlignedObjectArray<btBatchedConstraintInfo>* outConInfos, btConstraintArray* constraints) { - BT_PROFILE("initBatchedConstraintInfoArray"); - btAlignedObjectArray<btBatchedConstraintInfo>& conInfos = *outConInfos; - int numConstraints = constraints->size(); - conInfos.resizeNoInitialize(numConstraints); + BT_PROFILE("initBatchedConstraintInfoArray"); + btAlignedObjectArray<btBatchedConstraintInfo>& conInfos = *outConInfos; + int numConstraints = constraints->size(); + conInfos.resizeNoInitialize(numConstraints); - int newSize = initBatchedConstraintInfo(&outConInfos->at(0), constraints); - conInfos.resizeNoInitialize(newSize); + int newSize = initBatchedConstraintInfo(&outConInfos->at(0), constraints); + conInfos.resizeNoInitialize(newSize); } - static void mergeSmallBatches(btBatchInfo* batches, int iBeginBatch, int iEndBatch, int minBatchSize, int maxBatchSize) { - BT_PROFILE("mergeSmallBatches"); - for ( int iBatch = iEndBatch - 1; iBatch >= iBeginBatch; --iBatch ) - { - btBatchInfo& batch = batches[ iBatch ]; - if ( batch.mergeIndex == kNoMerge && batch.numConstraints > 0 && batch.numConstraints < minBatchSize ) - { - for ( int iDestBatch = iBatch - 1; iDestBatch >= iBeginBatch; --iDestBatch ) - { - btBatchInfo& destBatch = batches[ iDestBatch ]; - if ( destBatch.mergeIndex == kNoMerge && ( destBatch.numConstraints + batch.numConstraints ) < maxBatchSize ) - { - destBatch.numConstraints += batch.numConstraints; - batch.numConstraints = 0; - batch.mergeIndex = iDestBatch; - break; - } - } - } - } - // flatten mergeIndexes - // 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 - // Note: loop goes forward through batches because batches always merge from higher indexes to lower, - // so by going from low to high it reduces the amount of trail-following - for ( int iBatch = iBeginBatch; iBatch < iEndBatch; ++iBatch ) - { - btBatchInfo& batch = batches[ iBatch ]; - if ( batch.mergeIndex != kNoMerge ) - { - int iMergeDest = batches[ batch.mergeIndex ].mergeIndex; - // follow trail of merges to the end - while ( iMergeDest != kNoMerge ) - { - int iNext = batches[ iMergeDest ].mergeIndex; - if ( iNext == kNoMerge ) - { - batch.mergeIndex = iMergeDest; - break; - } - iMergeDest = iNext; - } - } - } + BT_PROFILE("mergeSmallBatches"); + for (int iBatch = iEndBatch - 1; iBatch >= iBeginBatch; --iBatch) + { + btBatchInfo& batch = batches[iBatch]; + if (batch.mergeIndex == kNoMerge && batch.numConstraints > 0 && batch.numConstraints < minBatchSize) + { + for (int iDestBatch = iBatch - 1; iDestBatch >= iBeginBatch; --iDestBatch) + { + btBatchInfo& destBatch = batches[iDestBatch]; + if (destBatch.mergeIndex == kNoMerge && (destBatch.numConstraints + batch.numConstraints) < maxBatchSize) + { + destBatch.numConstraints += batch.numConstraints; + batch.numConstraints = 0; + batch.mergeIndex = iDestBatch; + break; + } + } + } + } + // flatten mergeIndexes + // 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 + // Note: loop goes forward through batches because batches always merge from higher indexes to lower, + // so by going from low to high it reduces the amount of trail-following + for (int iBatch = iBeginBatch; iBatch < iEndBatch; ++iBatch) + { + btBatchInfo& batch = batches[iBatch]; + if (batch.mergeIndex != kNoMerge) + { + int iMergeDest = batches[batch.mergeIndex].mergeIndex; + // follow trail of merges to the end + while (iMergeDest != kNoMerge) + { + int iNext = batches[iMergeDest].mergeIndex; + if (iNext == kNoMerge) + { + batch.mergeIndex = iMergeDest; + break; + } + iMergeDest = iNext; + } + } + } } - static void updateConstraintBatchIdsForMerges(int* constraintBatchIds, int numConstraints, const btBatchInfo* batches, int numBatches) { - BT_PROFILE("updateConstraintBatchIdsForMerges"); - // update batchIds to account for merges - for (int i = 0; i < numConstraints; ++i) - { - int iBatch = constraintBatchIds[i]; - btAssert(iBatch < numBatches); - // if this constraint references a batch that was merged into another batch - if (batches[iBatch].mergeIndex != kNoMerge) - { - // update batchId - constraintBatchIds[i] = batches[iBatch].mergeIndex; - } - } + BT_PROFILE("updateConstraintBatchIdsForMerges"); + // update batchIds to account for merges + for (int i = 0; i < numConstraints; ++i) + { + int iBatch = constraintBatchIds[i]; + btAssert(iBatch < numBatches); + // if this constraint references a batch that was merged into another batch + if (batches[iBatch].mergeIndex != kNoMerge) + { + // update batchId + constraintBatchIds[i] = batches[iBatch].mergeIndex; + } + } } - struct UpdateConstraintBatchIdsForMergesLoop : public btIParallelForBody { - int* m_constraintBatchIds; - const btBatchInfo* m_batches; - int m_numBatches; - - UpdateConstraintBatchIdsForMergesLoop( int* constraintBatchIds, const btBatchInfo* batches, int numBatches ) - { - m_constraintBatchIds = constraintBatchIds; - m_batches = batches; - m_numBatches = numBatches; - } - void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE - { - BT_PROFILE( "UpdateConstraintBatchIdsForMergesLoop" ); - updateConstraintBatchIdsForMerges( m_constraintBatchIds + iBegin, iEnd - iBegin, m_batches, m_numBatches ); - } + int* m_constraintBatchIds; + const btBatchInfo* m_batches; + int m_numBatches; + + UpdateConstraintBatchIdsForMergesLoop(int* constraintBatchIds, const btBatchInfo* batches, int numBatches) + { + m_constraintBatchIds = constraintBatchIds; + m_batches = batches; + m_numBatches = numBatches; + } + void forLoop(int iBegin, int iEnd) const BT_OVERRIDE + { + BT_PROFILE("UpdateConstraintBatchIdsForMergesLoop"); + updateConstraintBatchIdsForMerges(m_constraintBatchIds + iBegin, iEnd - iBegin, m_batches, m_numBatches); + } }; - static void updateConstraintBatchIdsForMergesMt(int* constraintBatchIds, int numConstraints, const btBatchInfo* batches, int numBatches) { - BT_PROFILE( "updateConstraintBatchIdsForMergesMt" ); - UpdateConstraintBatchIdsForMergesLoop loop(constraintBatchIds, batches, numBatches); - int grainSize = 800; - btParallelFor(0, numConstraints, grainSize, loop); + BT_PROFILE("updateConstraintBatchIdsForMergesMt"); + UpdateConstraintBatchIdsForMergesLoop loop(constraintBatchIds, batches, numBatches); + int grainSize = 800; + btParallelFor(0, numConstraints, grainSize, loop); } - inline bool BatchCompare(const btBatchedConstraints::Range& a, const btBatchedConstraints::Range& b) { - int lenA = a.end - a.begin; - int lenB = b.end - b.begin; - return lenA > lenB; + int lenA = a.end - a.begin; + int lenB = b.end - b.begin; + return lenA > lenB; } - static void writeOutConstraintIndicesForRangeOfBatches(btBatchedConstraints* bc, - const int* constraintBatchIds, - int numConstraints, - int* constraintIdPerBatch, - int batchBegin, - int batchEnd - ) + const int* constraintBatchIds, + int numConstraints, + int* constraintIdPerBatch, + int batchBegin, + int batchEnd) { - BT_PROFILE("writeOutConstraintIndicesForRangeOfBatches"); - for ( int iCon = 0; iCon < numConstraints; ++iCon ) - { - int iBatch = constraintBatchIds[ iCon ]; - if (iBatch >= batchBegin && iBatch < batchEnd) - { - int iDestCon = constraintIdPerBatch[ iBatch ]; - constraintIdPerBatch[ iBatch ] = iDestCon + 1; - bc->m_constraintIndices[ iDestCon ] = iCon; - } - } + BT_PROFILE("writeOutConstraintIndicesForRangeOfBatches"); + for (int iCon = 0; iCon < numConstraints; ++iCon) + { + int iBatch = constraintBatchIds[iCon]; + if (iBatch >= batchBegin && iBatch < batchEnd) + { + int iDestCon = constraintIdPerBatch[iBatch]; + constraintIdPerBatch[iBatch] = iDestCon + 1; + bc->m_constraintIndices[iDestCon] = iCon; + } + } } - struct WriteOutConstraintIndicesLoop : public btIParallelForBody { - btBatchedConstraints* m_batchedConstraints; - const int* m_constraintBatchIds; - int m_numConstraints; - int* m_constraintIdPerBatch; - int m_maxNumBatchesPerPhase; - - WriteOutConstraintIndicesLoop( btBatchedConstraints* bc, const int* constraintBatchIds, int numConstraints, int* constraintIdPerBatch, int maxNumBatchesPerPhase ) - { - m_batchedConstraints = bc; - m_constraintBatchIds = constraintBatchIds; - m_numConstraints = numConstraints; - m_constraintIdPerBatch = constraintIdPerBatch; - m_maxNumBatchesPerPhase = maxNumBatchesPerPhase; - } - void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE - { - BT_PROFILE( "WriteOutConstraintIndicesLoop" ); - int batchBegin = iBegin * m_maxNumBatchesPerPhase; - int batchEnd = iEnd * m_maxNumBatchesPerPhase; - writeOutConstraintIndicesForRangeOfBatches(m_batchedConstraints, - m_constraintBatchIds, - m_numConstraints, - m_constraintIdPerBatch, - batchBegin, - batchEnd - ); - } + btBatchedConstraints* m_batchedConstraints; + const int* m_constraintBatchIds; + int m_numConstraints; + int* m_constraintIdPerBatch; + int m_maxNumBatchesPerPhase; + + WriteOutConstraintIndicesLoop(btBatchedConstraints* bc, const int* constraintBatchIds, int numConstraints, int* constraintIdPerBatch, int maxNumBatchesPerPhase) + { + m_batchedConstraints = bc; + m_constraintBatchIds = constraintBatchIds; + m_numConstraints = numConstraints; + m_constraintIdPerBatch = constraintIdPerBatch; + m_maxNumBatchesPerPhase = maxNumBatchesPerPhase; + } + void forLoop(int iBegin, int iEnd) const BT_OVERRIDE + { + BT_PROFILE("WriteOutConstraintIndicesLoop"); + int batchBegin = iBegin * m_maxNumBatchesPerPhase; + int batchEnd = iEnd * m_maxNumBatchesPerPhase; + writeOutConstraintIndicesForRangeOfBatches(m_batchedConstraints, + m_constraintBatchIds, + m_numConstraints, + m_constraintIdPerBatch, + batchBegin, + batchEnd); + } }; - static void writeOutConstraintIndicesMt(btBatchedConstraints* bc, - const int* constraintBatchIds, - int numConstraints, - int* constraintIdPerBatch, - int maxNumBatchesPerPhase, - int numPhases - ) + const int* constraintBatchIds, + int numConstraints, + int* constraintIdPerBatch, + int maxNumBatchesPerPhase, + int numPhases) { - BT_PROFILE("writeOutConstraintIndicesMt"); - bool inParallel = true; - if (inParallel) - { - WriteOutConstraintIndicesLoop loop( bc, constraintBatchIds, numConstraints, constraintIdPerBatch, maxNumBatchesPerPhase ); - btParallelFor( 0, numPhases, 1, loop ); - } - else - { - for ( int iCon = 0; iCon < numConstraints; ++iCon ) - { - int iBatch = constraintBatchIds[ iCon ]; - int iDestCon = constraintIdPerBatch[ iBatch ]; - constraintIdPerBatch[ iBatch ] = iDestCon + 1; - bc->m_constraintIndices[ iDestCon ] = iCon; - } - } + BT_PROFILE("writeOutConstraintIndicesMt"); + bool inParallel = true; + if (inParallel) + { + WriteOutConstraintIndicesLoop loop(bc, constraintBatchIds, numConstraints, constraintIdPerBatch, maxNumBatchesPerPhase); + btParallelFor(0, numPhases, 1, loop); + } + else + { + for (int iCon = 0; iCon < numConstraints; ++iCon) + { + int iBatch = constraintBatchIds[iCon]; + int iDestCon = constraintIdPerBatch[iBatch]; + constraintIdPerBatch[iBatch] = iDestCon + 1; + bc->m_constraintIndices[iDestCon] = iCon; + } + } } - static void writeGrainSizes(btBatchedConstraints* bc) { - typedef btBatchedConstraints::Range Range; - int numPhases = bc->m_phases.size(); - bc->m_phaseGrainSize.resizeNoInitialize(numPhases); - int numThreads = btGetTaskScheduler()->getNumThreads(); - for (int iPhase = 0; iPhase < numPhases; ++iPhase) - { - const Range& phase = bc->m_phases[ iPhase ]; - int numBatches = phase.end - phase.begin; - float grainSize = floor((0.25f*numBatches / float(numThreads)) + 0.0f); - bc->m_phaseGrainSize[ iPhase ] = btMax(1, int(grainSize)); - } + typedef btBatchedConstraints::Range Range; + int numPhases = bc->m_phases.size(); + bc->m_phaseGrainSize.resizeNoInitialize(numPhases); + int numThreads = btGetTaskScheduler()->getNumThreads(); + for (int iPhase = 0; iPhase < numPhases; ++iPhase) + { + const Range& phase = bc->m_phases[iPhase]; + int numBatches = phase.end - phase.begin; + float grainSize = floor((0.25f * numBatches / float(numThreads)) + 0.0f); + bc->m_phaseGrainSize[iPhase] = btMax(1, int(grainSize)); + } } - static void writeOutBatches(btBatchedConstraints* bc, - const int* constraintBatchIds, - int numConstraints, - const btBatchInfo* batches, - int* batchWork, - int maxNumBatchesPerPhase, - int numPhases -) + const int* constraintBatchIds, + int numConstraints, + const btBatchInfo* batches, + int* batchWork, + int maxNumBatchesPerPhase, + int numPhases) { - BT_PROFILE("writeOutBatches"); - typedef btBatchedConstraints::Range Range; - bc->m_constraintIndices.reserve( numConstraints ); - bc->m_batches.resizeNoInitialize( 0 ); - bc->m_phases.resizeNoInitialize( 0 ); - - //int maxNumBatches = numPhases * maxNumBatchesPerPhase; - { - int* constraintIdPerBatch = batchWork; // for each batch, keep an index into the next available slot in the m_constraintIndices array - int iConstraint = 0; - for (int iPhase = 0; iPhase < numPhases; ++iPhase) - { - int curPhaseBegin = bc->m_batches.size(); - int iBegin = iPhase * maxNumBatchesPerPhase; - int iEnd = iBegin + maxNumBatchesPerPhase; - for ( int i = iBegin; i < iEnd; ++i ) - { - const btBatchInfo& batch = batches[ i ]; - int curBatchBegin = iConstraint; - constraintIdPerBatch[ i ] = curBatchBegin; // record the start of each batch in m_constraintIndices array - int numConstraints = batch.numConstraints; - iConstraint += numConstraints; - if ( numConstraints > 0 ) - { - bc->m_batches.push_back( Range( curBatchBegin, iConstraint ) ); - } - } - // if any batches were emitted this phase, - if ( bc->m_batches.size() > curPhaseBegin ) - { - // output phase - bc->m_phases.push_back( Range( curPhaseBegin, bc->m_batches.size() ) ); - } - } - - btAssert(iConstraint == numConstraints); - bc->m_constraintIndices.resizeNoInitialize( numConstraints ); - writeOutConstraintIndicesMt( bc, constraintBatchIds, numConstraints, constraintIdPerBatch, maxNumBatchesPerPhase, numPhases ); - } - // for each phase - for (int iPhase = 0; iPhase < bc->m_phases.size(); ++iPhase) - { - // sort the batches from largest to smallest (can be helpful to some task schedulers) - const Range& curBatches = bc->m_phases[iPhase]; - bc->m_batches.quickSortInternal(BatchCompare, curBatches.begin, curBatches.end-1); - } - bc->m_phaseOrder.resize(bc->m_phases.size()); - for (int i = 0; i < bc->m_phases.size(); ++i) - { - bc->m_phaseOrder[i] = i; - } - writeGrainSizes(bc); + BT_PROFILE("writeOutBatches"); + typedef btBatchedConstraints::Range Range; + bc->m_constraintIndices.reserve(numConstraints); + bc->m_batches.resizeNoInitialize(0); + bc->m_phases.resizeNoInitialize(0); + + //int maxNumBatches = numPhases * maxNumBatchesPerPhase; + { + int* constraintIdPerBatch = batchWork; // for each batch, keep an index into the next available slot in the m_constraintIndices array + int iConstraint = 0; + for (int iPhase = 0; iPhase < numPhases; ++iPhase) + { + int curPhaseBegin = bc->m_batches.size(); + int iBegin = iPhase * maxNumBatchesPerPhase; + int iEnd = iBegin + maxNumBatchesPerPhase; + for (int i = iBegin; i < iEnd; ++i) + { + const btBatchInfo& batch = batches[i]; + int curBatchBegin = iConstraint; + constraintIdPerBatch[i] = curBatchBegin; // record the start of each batch in m_constraintIndices array + int numConstraints = batch.numConstraints; + iConstraint += numConstraints; + if (numConstraints > 0) + { + bc->m_batches.push_back(Range(curBatchBegin, iConstraint)); + } + } + // if any batches were emitted this phase, + if (bc->m_batches.size() > curPhaseBegin) + { + // output phase + bc->m_phases.push_back(Range(curPhaseBegin, bc->m_batches.size())); + } + } + + btAssert(iConstraint == numConstraints); + bc->m_constraintIndices.resizeNoInitialize(numConstraints); + writeOutConstraintIndicesMt(bc, constraintBatchIds, numConstraints, constraintIdPerBatch, maxNumBatchesPerPhase, numPhases); + } + // for each phase + for (int iPhase = 0; iPhase < bc->m_phases.size(); ++iPhase) + { + // sort the batches from largest to smallest (can be helpful to some task schedulers) + const Range& curBatches = bc->m_phases[iPhase]; + bc->m_batches.quickSortInternal(BatchCompare, curBatches.begin, curBatches.end - 1); + } + bc->m_phaseOrder.resize(bc->m_phases.size()); + for (int i = 0; i < bc->m_phases.size(); ++i) + { + bc->m_phaseOrder[i] = i; + } + writeGrainSizes(bc); } - // // PreallocatedMemoryHelper -- helper object for allocating a number of chunks of memory in a single contiguous block. // It is generally more efficient to do a single larger allocation than many smaller allocations. @@ -639,191 +605,184 @@ static void writeOutBatches(btBatchedConstraints* bc, template <int N> class PreallocatedMemoryHelper { - struct Chunk - { - void** ptr; - size_t size; - }; - Chunk m_chunks[N]; - int m_numChunks; + struct Chunk + { + void** ptr; + size_t size; + }; + Chunk m_chunks[N]; + int m_numChunks; + public: - PreallocatedMemoryHelper() {m_numChunks=0;} - void addChunk( void** ptr, size_t sz ) - { - btAssert( m_numChunks < N ); - if ( m_numChunks < N ) - { - Chunk& chunk = m_chunks[ m_numChunks ]; - chunk.ptr = ptr; - chunk.size = sz; - m_numChunks++; - } - } - size_t getSizeToAllocate() const - { - size_t totalSize = 0; - for (int i = 0; i < m_numChunks; ++i) - { - totalSize += m_chunks[i].size; - } - return totalSize; - } - void setChunkPointers(void* mem) const - { - size_t totalSize = 0; - for (int i = 0; i < m_numChunks; ++i) - { - const Chunk& chunk = m_chunks[ i ]; - char* chunkPtr = static_cast<char*>(mem) + totalSize; - *chunk.ptr = chunkPtr; - totalSize += chunk.size; - } - } + PreallocatedMemoryHelper() { m_numChunks = 0; } + void addChunk(void** ptr, size_t sz) + { + btAssert(m_numChunks < N); + if (m_numChunks < N) + { + Chunk& chunk = m_chunks[m_numChunks]; + chunk.ptr = ptr; + chunk.size = sz; + m_numChunks++; + } + } + size_t getSizeToAllocate() const + { + size_t totalSize = 0; + for (int i = 0; i < m_numChunks; ++i) + { + totalSize += m_chunks[i].size; + } + return totalSize; + } + void setChunkPointers(void* mem) const + { + size_t totalSize = 0; + for (int i = 0; i < m_numChunks; ++i) + { + const Chunk& chunk = m_chunks[i]; + char* chunkPtr = static_cast<char*>(mem) + totalSize; + *chunk.ptr = chunkPtr; + totalSize += chunk.size; + } + } }; - - static btVector3 findMaxDynamicConstraintExtent( - btVector3* bodyPositions, - bool* bodyDynamicFlags, - btBatchedConstraintInfo* conInfos, - int numConstraints, - int numBodies - ) + btVector3* bodyPositions, + bool* bodyDynamicFlags, + btBatchedConstraintInfo* conInfos, + int numConstraints, + int numBodies) { - BT_PROFILE("findMaxDynamicConstraintExtent"); - btVector3 consExtent = btVector3(1,1,1) * 0.001; - for (int iCon = 0; iCon < numConstraints; ++iCon) - { - const btBatchedConstraintInfo& con = conInfos[ iCon ]; - int iBody0 = con.bodyIds[0]; - int iBody1 = con.bodyIds[1]; - btAssert(iBody0 >= 0 && iBody0 < numBodies); - btAssert(iBody1 >= 0 && iBody1 < numBodies); - // is it a dynamic constraint? - if (bodyDynamicFlags[iBody0] && bodyDynamicFlags[iBody1]) - { - btVector3 delta = bodyPositions[iBody1] - bodyPositions[iBody0]; - consExtent.setMax(delta.absolute()); - } - } - return consExtent; + BT_PROFILE("findMaxDynamicConstraintExtent"); + btVector3 consExtent = btVector3(1, 1, 1) * 0.001; + for (int iCon = 0; iCon < numConstraints; ++iCon) + { + const btBatchedConstraintInfo& con = conInfos[iCon]; + int iBody0 = con.bodyIds[0]; + int iBody1 = con.bodyIds[1]; + btAssert(iBody0 >= 0 && iBody0 < numBodies); + btAssert(iBody1 >= 0 && iBody1 < numBodies); + // is it a dynamic constraint? + if (bodyDynamicFlags[iBody0] && bodyDynamicFlags[iBody1]) + { + btVector3 delta = bodyPositions[iBody1] - bodyPositions[iBody0]; + consExtent.setMax(delta.absolute()); + } + } + return consExtent; } - struct btIntVec3 { - int m_ints[ 3 ]; + int m_ints[3]; - SIMD_FORCE_INLINE const int& operator[](int i) const {return m_ints[i];} - SIMD_FORCE_INLINE int& operator[](int i) {return m_ints[i];} + SIMD_FORCE_INLINE const int& operator[](int i) const { return m_ints[i]; } + SIMD_FORCE_INLINE int& operator[](int i) { return m_ints[i]; } }; - struct AssignConstraintsToGridBatchesParams { - bool* bodyDynamicFlags; - btIntVec3* bodyGridCoords; - int numBodies; - btBatchedConstraintInfo* conInfos; - int* constraintBatchIds; - btIntVec3 gridChunkDim; - int maxNumBatchesPerPhase; - int numPhases; - int phaseMask; - - AssignConstraintsToGridBatchesParams() - { - memset(this, 0, sizeof(*this)); - } + bool* bodyDynamicFlags; + btIntVec3* bodyGridCoords; + int numBodies; + btBatchedConstraintInfo* conInfos; + int* constraintBatchIds; + btIntVec3 gridChunkDim; + int maxNumBatchesPerPhase; + int numPhases; + int phaseMask; + + AssignConstraintsToGridBatchesParams() + { + memset(this, 0, sizeof(*this)); + } }; - static void assignConstraintsToGridBatches(const AssignConstraintsToGridBatchesParams& params, int iConBegin, int iConEnd) { - BT_PROFILE("assignConstraintsToGridBatches"); - // (can be done in parallel) - for ( int iCon = iConBegin; iCon < iConEnd; ++iCon ) - { - const btBatchedConstraintInfo& con = params.conInfos[ iCon ]; - int iBody0 = con.bodyIds[ 0 ]; - int iBody1 = con.bodyIds[ 1 ]; - int iPhase = iCon; //iBody0; // pseudorandom choice to distribute evenly amongst phases - iPhase &= params.phaseMask; - int gridCoord[ 3 ]; - // is it a dynamic constraint? - if ( params.bodyDynamicFlags[ iBody0 ] && params.bodyDynamicFlags[ iBody1 ] ) - { - const btIntVec3& body0Coords = params.bodyGridCoords[iBody0]; - const btIntVec3& body1Coords = params.bodyGridCoords[iBody1]; - // for each dimension x,y,z, - for (int i = 0; i < 3; ++i) - { - int coordMin = btMin(body0Coords.m_ints[i], body1Coords.m_ints[i]); - int coordMax = btMax(body0Coords.m_ints[i], body1Coords.m_ints[i]); - if (coordMin != coordMax) - { - btAssert( coordMax == coordMin + 1 ); - if ((coordMin&1) == 0) - { - iPhase &= ~(1 << i); // force bit off - } - else - { - iPhase |= (1 << i); // force bit on - iPhase &= params.phaseMask; - } - } - gridCoord[ i ] = coordMin; - } - } - else - { - if ( !params.bodyDynamicFlags[ iBody0 ] ) - { - iBody0 = con.bodyIds[ 1 ]; - } - btAssert(params.bodyDynamicFlags[ iBody0 ]); - const btIntVec3& body0Coords = params.bodyGridCoords[iBody0]; - // for each dimension x,y,z, - for ( int i = 0; i < 3; ++i ) - { - gridCoord[ i ] = body0Coords.m_ints[ i ]; - } - } - // calculate chunk coordinates - int chunkCoord[ 3 ]; - btIntVec3 gridChunkDim = params.gridChunkDim; - // for each dimension x,y,z, - for ( int i = 0; i < 3; ++i ) - { - int coordOffset = ( iPhase >> i ) & 1; - chunkCoord[ i ] = (gridCoord[ i ] - coordOffset)/2; - btClamp( chunkCoord[ i ], 0, gridChunkDim[ i ] - 1); - btAssert( chunkCoord[ i ] < gridChunkDim[ i ] ); - } - int iBatch = iPhase * params.maxNumBatchesPerPhase + chunkCoord[ 0 ] + chunkCoord[ 1 ] * gridChunkDim[ 0 ] + chunkCoord[ 2 ] * gridChunkDim[ 0 ] * gridChunkDim[ 1 ]; - btAssert(iBatch >= 0 && iBatch < params.maxNumBatchesPerPhase*params.numPhases); - params.constraintBatchIds[ iCon ] = iBatch; - } + BT_PROFILE("assignConstraintsToGridBatches"); + // (can be done in parallel) + for (int iCon = iConBegin; iCon < iConEnd; ++iCon) + { + const btBatchedConstraintInfo& con = params.conInfos[iCon]; + int iBody0 = con.bodyIds[0]; + int iBody1 = con.bodyIds[1]; + int iPhase = iCon; //iBody0; // pseudorandom choice to distribute evenly amongst phases + iPhase &= params.phaseMask; + int gridCoord[3]; + // is it a dynamic constraint? + if (params.bodyDynamicFlags[iBody0] && params.bodyDynamicFlags[iBody1]) + { + const btIntVec3& body0Coords = params.bodyGridCoords[iBody0]; + const btIntVec3& body1Coords = params.bodyGridCoords[iBody1]; + // for each dimension x,y,z, + for (int i = 0; i < 3; ++i) + { + int coordMin = btMin(body0Coords.m_ints[i], body1Coords.m_ints[i]); + int coordMax = btMax(body0Coords.m_ints[i], body1Coords.m_ints[i]); + if (coordMin != coordMax) + { + btAssert(coordMax == coordMin + 1); + if ((coordMin & 1) == 0) + { + iPhase &= ~(1 << i); // force bit off + } + else + { + iPhase |= (1 << i); // force bit on + iPhase &= params.phaseMask; + } + } + gridCoord[i] = coordMin; + } + } + else + { + if (!params.bodyDynamicFlags[iBody0]) + { + iBody0 = con.bodyIds[1]; + } + btAssert(params.bodyDynamicFlags[iBody0]); + const btIntVec3& body0Coords = params.bodyGridCoords[iBody0]; + // for each dimension x,y,z, + for (int i = 0; i < 3; ++i) + { + gridCoord[i] = body0Coords.m_ints[i]; + } + } + // calculate chunk coordinates + int chunkCoord[3]; + btIntVec3 gridChunkDim = params.gridChunkDim; + // for each dimension x,y,z, + for (int i = 0; i < 3; ++i) + { + int coordOffset = (iPhase >> i) & 1; + chunkCoord[i] = (gridCoord[i] - coordOffset) / 2; + btClamp(chunkCoord[i], 0, gridChunkDim[i] - 1); + btAssert(chunkCoord[i] < gridChunkDim[i]); + } + int iBatch = iPhase * params.maxNumBatchesPerPhase + chunkCoord[0] + chunkCoord[1] * gridChunkDim[0] + chunkCoord[2] * gridChunkDim[0] * gridChunkDim[1]; + btAssert(iBatch >= 0 && iBatch < params.maxNumBatchesPerPhase * params.numPhases); + params.constraintBatchIds[iCon] = iBatch; + } } - struct AssignConstraintsToGridBatchesLoop : public btIParallelForBody { - const AssignConstraintsToGridBatchesParams* m_params; - - AssignConstraintsToGridBatchesLoop( const AssignConstraintsToGridBatchesParams& params ) - { - m_params = ¶ms; - } - void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE - { - assignConstraintsToGridBatches(*m_params, iBegin, iEnd); - } + const AssignConstraintsToGridBatchesParams* m_params; + + AssignConstraintsToGridBatchesLoop(const AssignConstraintsToGridBatchesParams& params) + { + m_params = ¶ms; + } + void forLoop(int iBegin, int iEnd) const BT_OVERRIDE + { + assignConstraintsToGridBatches(*m_params, iBegin, iEnd); + } }; - // // setupSpatialGridBatchesMt -- generate batches using a uniform 3D grid // @@ -853,276 +812,269 @@ to 4. With fewer phases, there are more constraints per phase and this makes it */ // static void setupSpatialGridBatchesMt( - btBatchedConstraints* batchedConstraints, - btAlignedObjectArray<char>* scratchMemory, - btConstraintArray* constraints, - const btAlignedObjectArray<btSolverBody>& bodies, - int minBatchSize, - int maxBatchSize, - bool use2DGrid -) + btBatchedConstraints* batchedConstraints, + btAlignedObjectArray<char>* scratchMemory, + btConstraintArray* constraints, + const btAlignedObjectArray<btSolverBody>& bodies, + int minBatchSize, + int maxBatchSize, + bool use2DGrid) { - BT_PROFILE("setupSpatialGridBatchesMt"); - const int numPhases = 8; - int numConstraints = constraints->size(); - int numConstraintRows = constraints->size(); - - const int maxGridChunkCount = 128; - int allocNumBatchesPerPhase = maxGridChunkCount; - int minNumBatchesPerPhase = 16; - int allocNumBatches = allocNumBatchesPerPhase * numPhases; - - btVector3* bodyPositions = NULL; - bool* bodyDynamicFlags = NULL; - btIntVec3* bodyGridCoords = NULL; - btBatchInfo* batches = NULL; - int* batchWork = NULL; - btBatchedConstraintInfo* conInfos = NULL; - int* constraintBatchIds = NULL; - int* constraintRowBatchIds = NULL; - { - PreallocatedMemoryHelper<10> memHelper; - memHelper.addChunk( (void**) &bodyPositions, sizeof( btVector3 ) * bodies.size() ); - memHelper.addChunk( (void**) &bodyDynamicFlags, sizeof( bool ) * bodies.size() ); - memHelper.addChunk( (void**) &bodyGridCoords, sizeof( btIntVec3 ) * bodies.size() ); - memHelper.addChunk( (void**) &batches, sizeof( btBatchInfo )* allocNumBatches ); - memHelper.addChunk( (void**) &batchWork, sizeof( int )* allocNumBatches ); - memHelper.addChunk( (void**) &conInfos, sizeof( btBatchedConstraintInfo ) * numConstraints ); - memHelper.addChunk( (void**) &constraintBatchIds, sizeof( int ) * numConstraints ); - memHelper.addChunk( (void**) &constraintRowBatchIds, sizeof( int ) * numConstraintRows ); - size_t scratchSize = memHelper.getSizeToAllocate(); - // if we need to reallocate - if (scratchMemory->capacity() < scratchSize) - { - // allocate 6.25% extra to avoid repeated reallocs - scratchMemory->reserve( scratchSize + scratchSize/16 ); - } - scratchMemory->resizeNoInitialize( scratchSize ); - char* memPtr = &scratchMemory->at(0); - memHelper.setChunkPointers( memPtr ); - } - - numConstraints = initBatchedConstraintInfo(conInfos, constraints); - - // compute bounding box around all dynamic bodies - // (could be done in parallel) - btVector3 bboxMin(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT); - btVector3 bboxMax = -bboxMin; - //int dynamicBodyCount = 0; - for (int i = 0; i < bodies.size(); ++i) - { - const btSolverBody& body = bodies[i]; - btVector3 bodyPos = body.getWorldTransform().getOrigin(); - bool isDynamic = ( body.internalGetInvMass().x() > btScalar( 0 ) ); - bodyPositions[i] = bodyPos; - bodyDynamicFlags[i] = isDynamic; - if (isDynamic) - { - //dynamicBodyCount++; - bboxMin.setMin(bodyPos); - bboxMax.setMax(bodyPos); - } - } - - // find max extent of all dynamic constraints - // (could be done in parallel) - btVector3 consExtent = findMaxDynamicConstraintExtent(bodyPositions, bodyDynamicFlags, conInfos, numConstraints, bodies.size()); - - btVector3 gridExtent = bboxMax - bboxMin; - - btVector3 gridCellSize = consExtent; - int gridDim[3]; - gridDim[ 0 ] = int( 1.0 + gridExtent.x() / gridCellSize.x() ); - gridDim[ 1 ] = int( 1.0 + gridExtent.y() / gridCellSize.y() ); - gridDim[ 2 ] = int( 1.0 + gridExtent.z() / gridCellSize.z() ); - - // if we can collapse an axis, it will cut our number of phases in half which could be more efficient - int phaseMask = 7; - bool collapseAxis = use2DGrid; - if ( collapseAxis ) - { - // pick the smallest axis to collapse, leaving us with the greatest number of cells in our grid - int iAxisToCollapse = 0; - int axisDim = gridDim[iAxisToCollapse]; - //for each dimension - for ( int i = 0; i < 3; ++i ) - { - if (gridDim[i] < axisDim) - { - iAxisToCollapse = i; - axisDim = gridDim[i]; - } - } - // collapse it - gridCellSize[iAxisToCollapse] = gridExtent[iAxisToCollapse] * 2.0f; - phaseMask &= ~(1 << iAxisToCollapse); - } - - int numGridChunks = 0; - btIntVec3 gridChunkDim; // each chunk is 2x2x2 group of cells - while (true) - { - gridDim[0] = int( 1.0 + gridExtent.x() / gridCellSize.x() ); - gridDim[1] = int( 1.0 + gridExtent.y() / gridCellSize.y() ); - gridDim[2] = int( 1.0 + gridExtent.z() / gridCellSize.z() ); - gridChunkDim[ 0 ] = btMax( 1, ( gridDim[ 0 ] + 0 ) / 2 ); - gridChunkDim[ 1 ] = btMax( 1, ( gridDim[ 1 ] + 0 ) / 2 ); - gridChunkDim[ 2 ] = btMax( 1, ( gridDim[ 2 ] + 0 ) / 2 ); - numGridChunks = gridChunkDim[ 0 ] * gridChunkDim[ 1 ] * gridChunkDim[ 2 ]; - float nChunks = float(gridChunkDim[0]) * float(gridChunkDim[1]) * float(gridChunkDim[2]); // suceptible to integer overflow - if ( numGridChunks <= maxGridChunkCount && nChunks <= maxGridChunkCount ) - { - break; - } - gridCellSize *= 1.25; // should roughly cut numCells in half - } - btAssert(numGridChunks <= maxGridChunkCount ); - int maxNumBatchesPerPhase = numGridChunks; - - // for each dynamic body, compute grid coords - btVector3 invGridCellSize = btVector3(1,1,1)/gridCellSize; - // (can be done in parallel) - for (int iBody = 0; iBody < bodies.size(); ++iBody) - { - btIntVec3& coords = bodyGridCoords[iBody]; - if (bodyDynamicFlags[iBody]) - { - btVector3 v = ( bodyPositions[ iBody ] - bboxMin )*invGridCellSize; - coords.m_ints[0] = int(v.x()); - coords.m_ints[1] = int(v.y()); - coords.m_ints[2] = int(v.z()); - btAssert(coords.m_ints[0] >= 0 && coords.m_ints[0] < gridDim[0]); - btAssert(coords.m_ints[1] >= 0 && coords.m_ints[1] < gridDim[1]); - btAssert(coords.m_ints[2] >= 0 && coords.m_ints[2] < gridDim[2]); - } - else - { - coords.m_ints[0] = -1; - coords.m_ints[1] = -1; - coords.m_ints[2] = -1; - } - } - - for (int iPhase = 0; iPhase < numPhases; ++iPhase) - { - int batchBegin = iPhase * maxNumBatchesPerPhase; - int batchEnd = batchBegin + maxNumBatchesPerPhase; - for ( int iBatch = batchBegin; iBatch < batchEnd; ++iBatch ) - { - btBatchInfo& batch = batches[ iBatch ]; - batch = btBatchInfo(); - } - } - - { - AssignConstraintsToGridBatchesParams params; - params.bodyDynamicFlags = bodyDynamicFlags; - params.bodyGridCoords = bodyGridCoords; - params.numBodies = bodies.size(); - params.conInfos = conInfos; - params.constraintBatchIds = constraintBatchIds; - params.gridChunkDim = gridChunkDim; - params.maxNumBatchesPerPhase = maxNumBatchesPerPhase; - params.numPhases = numPhases; - params.phaseMask = phaseMask; - bool inParallel = true; - if (inParallel) - { - AssignConstraintsToGridBatchesLoop loop(params); - int grainSize = 250; - btParallelFor(0, numConstraints, grainSize, loop); - } - else - { - assignConstraintsToGridBatches( params, 0, numConstraints ); - } - } - for ( int iCon = 0; iCon < numConstraints; ++iCon ) - { - const btBatchedConstraintInfo& con = conInfos[ iCon ]; - int iBatch = constraintBatchIds[ iCon ]; - btBatchInfo& batch = batches[iBatch]; - batch.numConstraints += con.numConstraintRows; - } - - for (int iPhase = 0; iPhase < numPhases; ++iPhase) - { - // if phase is legit, - if (iPhase == (iPhase&phaseMask)) - { - int iBeginBatch = iPhase * maxNumBatchesPerPhase; - int iEndBatch = iBeginBatch + maxNumBatchesPerPhase; - mergeSmallBatches( batches, iBeginBatch, iEndBatch, minBatchSize, maxBatchSize ); - } - } - // all constraints have been assigned a batchId - updateConstraintBatchIdsForMergesMt(constraintBatchIds, numConstraints, batches, maxNumBatchesPerPhase*numPhases); - - if (numConstraintRows > numConstraints) - { - expandConstraintRowsMt(&constraintRowBatchIds[0], &constraintBatchIds[0], &conInfos[0], numConstraints, numConstraintRows); - } - else - { - constraintRowBatchIds = constraintBatchIds; - } - - writeOutBatches(batchedConstraints, constraintRowBatchIds, numConstraintRows, batches, batchWork, maxNumBatchesPerPhase, numPhases); - btAssert(batchedConstraints->validate(constraints, bodies)); + BT_PROFILE("setupSpatialGridBatchesMt"); + const int numPhases = 8; + int numConstraints = constraints->size(); + int numConstraintRows = constraints->size(); + + const int maxGridChunkCount = 128; + int allocNumBatchesPerPhase = maxGridChunkCount; + int minNumBatchesPerPhase = 16; + int allocNumBatches = allocNumBatchesPerPhase * numPhases; + + btVector3* bodyPositions = NULL; + bool* bodyDynamicFlags = NULL; + btIntVec3* bodyGridCoords = NULL; + btBatchInfo* batches = NULL; + int* batchWork = NULL; + btBatchedConstraintInfo* conInfos = NULL; + int* constraintBatchIds = NULL; + int* constraintRowBatchIds = NULL; + { + PreallocatedMemoryHelper<10> memHelper; + memHelper.addChunk((void**)&bodyPositions, sizeof(btVector3) * bodies.size()); + memHelper.addChunk((void**)&bodyDynamicFlags, sizeof(bool) * bodies.size()); + memHelper.addChunk((void**)&bodyGridCoords, sizeof(btIntVec3) * bodies.size()); + memHelper.addChunk((void**)&batches, sizeof(btBatchInfo) * allocNumBatches); + memHelper.addChunk((void**)&batchWork, sizeof(int) * allocNumBatches); + memHelper.addChunk((void**)&conInfos, sizeof(btBatchedConstraintInfo) * numConstraints); + memHelper.addChunk((void**)&constraintBatchIds, sizeof(int) * numConstraints); + memHelper.addChunk((void**)&constraintRowBatchIds, sizeof(int) * numConstraintRows); + size_t scratchSize = memHelper.getSizeToAllocate(); + // if we need to reallocate + if (scratchMemory->capacity() < scratchSize) + { + // allocate 6.25% extra to avoid repeated reallocs + scratchMemory->reserve(scratchSize + scratchSize / 16); + } + scratchMemory->resizeNoInitialize(scratchSize); + char* memPtr = &scratchMemory->at(0); + memHelper.setChunkPointers(memPtr); + } + + numConstraints = initBatchedConstraintInfo(conInfos, constraints); + + // compute bounding box around all dynamic bodies + // (could be done in parallel) + btVector3 bboxMin(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT); + btVector3 bboxMax = -bboxMin; + //int dynamicBodyCount = 0; + for (int i = 0; i < bodies.size(); ++i) + { + const btSolverBody& body = bodies[i]; + btVector3 bodyPos = body.getWorldTransform().getOrigin(); + bool isDynamic = (body.internalGetInvMass().x() > btScalar(0)); + bodyPositions[i] = bodyPos; + bodyDynamicFlags[i] = isDynamic; + if (isDynamic) + { + //dynamicBodyCount++; + bboxMin.setMin(bodyPos); + bboxMax.setMax(bodyPos); + } + } + + // find max extent of all dynamic constraints + // (could be done in parallel) + btVector3 consExtent = findMaxDynamicConstraintExtent(bodyPositions, bodyDynamicFlags, conInfos, numConstraints, bodies.size()); + + btVector3 gridExtent = bboxMax - bboxMin; + + btVector3 gridCellSize = consExtent; + int gridDim[3]; + gridDim[0] = int(1.0 + gridExtent.x() / gridCellSize.x()); + gridDim[1] = int(1.0 + gridExtent.y() / gridCellSize.y()); + gridDim[2] = int(1.0 + gridExtent.z() / gridCellSize.z()); + + // if we can collapse an axis, it will cut our number of phases in half which could be more efficient + int phaseMask = 7; + bool collapseAxis = use2DGrid; + if (collapseAxis) + { + // pick the smallest axis to collapse, leaving us with the greatest number of cells in our grid + int iAxisToCollapse = 0; + int axisDim = gridDim[iAxisToCollapse]; + //for each dimension + for (int i = 0; i < 3; ++i) + { + if (gridDim[i] < axisDim) + { + iAxisToCollapse = i; + axisDim = gridDim[i]; + } + } + // collapse it + gridCellSize[iAxisToCollapse] = gridExtent[iAxisToCollapse] * 2.0f; + phaseMask &= ~(1 << iAxisToCollapse); + } + + int numGridChunks = 0; + btIntVec3 gridChunkDim; // each chunk is 2x2x2 group of cells + while (true) + { + gridDim[0] = int(1.0 + gridExtent.x() / gridCellSize.x()); + gridDim[1] = int(1.0 + gridExtent.y() / gridCellSize.y()); + gridDim[2] = int(1.0 + gridExtent.z() / gridCellSize.z()); + gridChunkDim[0] = btMax(1, (gridDim[0] + 0) / 2); + gridChunkDim[1] = btMax(1, (gridDim[1] + 0) / 2); + gridChunkDim[2] = btMax(1, (gridDim[2] + 0) / 2); + numGridChunks = gridChunkDim[0] * gridChunkDim[1] * gridChunkDim[2]; + float nChunks = float(gridChunkDim[0]) * float(gridChunkDim[1]) * float(gridChunkDim[2]); // suceptible to integer overflow + if (numGridChunks <= maxGridChunkCount && nChunks <= maxGridChunkCount) + { + break; + } + gridCellSize *= 1.25; // should roughly cut numCells in half + } + btAssert(numGridChunks <= maxGridChunkCount); + int maxNumBatchesPerPhase = numGridChunks; + + // for each dynamic body, compute grid coords + btVector3 invGridCellSize = btVector3(1, 1, 1) / gridCellSize; + // (can be done in parallel) + for (int iBody = 0; iBody < bodies.size(); ++iBody) + { + btIntVec3& coords = bodyGridCoords[iBody]; + if (bodyDynamicFlags[iBody]) + { + btVector3 v = (bodyPositions[iBody] - bboxMin) * invGridCellSize; + coords.m_ints[0] = int(v.x()); + coords.m_ints[1] = int(v.y()); + coords.m_ints[2] = int(v.z()); + btAssert(coords.m_ints[0] >= 0 && coords.m_ints[0] < gridDim[0]); + btAssert(coords.m_ints[1] >= 0 && coords.m_ints[1] < gridDim[1]); + btAssert(coords.m_ints[2] >= 0 && coords.m_ints[2] < gridDim[2]); + } + else + { + coords.m_ints[0] = -1; + coords.m_ints[1] = -1; + coords.m_ints[2] = -1; + } + } + + for (int iPhase = 0; iPhase < numPhases; ++iPhase) + { + int batchBegin = iPhase * maxNumBatchesPerPhase; + int batchEnd = batchBegin + maxNumBatchesPerPhase; + for (int iBatch = batchBegin; iBatch < batchEnd; ++iBatch) + { + btBatchInfo& batch = batches[iBatch]; + batch = btBatchInfo(); + } + } + + { + AssignConstraintsToGridBatchesParams params; + params.bodyDynamicFlags = bodyDynamicFlags; + params.bodyGridCoords = bodyGridCoords; + params.numBodies = bodies.size(); + params.conInfos = conInfos; + params.constraintBatchIds = constraintBatchIds; + params.gridChunkDim = gridChunkDim; + params.maxNumBatchesPerPhase = maxNumBatchesPerPhase; + params.numPhases = numPhases; + params.phaseMask = phaseMask; + bool inParallel = true; + if (inParallel) + { + AssignConstraintsToGridBatchesLoop loop(params); + int grainSize = 250; + btParallelFor(0, numConstraints, grainSize, loop); + } + else + { + assignConstraintsToGridBatches(params, 0, numConstraints); + } + } + for (int iCon = 0; iCon < numConstraints; ++iCon) + { + const btBatchedConstraintInfo& con = conInfos[iCon]; + int iBatch = constraintBatchIds[iCon]; + btBatchInfo& batch = batches[iBatch]; + batch.numConstraints += con.numConstraintRows; + } + + for (int iPhase = 0; iPhase < numPhases; ++iPhase) + { + // if phase is legit, + if (iPhase == (iPhase & phaseMask)) + { + int iBeginBatch = iPhase * maxNumBatchesPerPhase; + int iEndBatch = iBeginBatch + maxNumBatchesPerPhase; + mergeSmallBatches(batches, iBeginBatch, iEndBatch, minBatchSize, maxBatchSize); + } + } + // all constraints have been assigned a batchId + updateConstraintBatchIdsForMergesMt(constraintBatchIds, numConstraints, batches, maxNumBatchesPerPhase * numPhases); + + if (numConstraintRows > numConstraints) + { + expandConstraintRowsMt(&constraintRowBatchIds[0], &constraintBatchIds[0], &conInfos[0], numConstraints, numConstraintRows); + } + else + { + constraintRowBatchIds = constraintBatchIds; + } + + writeOutBatches(batchedConstraints, constraintRowBatchIds, numConstraintRows, batches, batchWork, maxNumBatchesPerPhase, numPhases); + btAssert(batchedConstraints->validate(constraints, bodies)); } - static void setupSingleBatch( - btBatchedConstraints* bc, - int numConstraints -) + btBatchedConstraints* bc, + int numConstraints) { - BT_PROFILE("setupSingleBatch"); - typedef btBatchedConstraints::Range Range; - - bc->m_constraintIndices.resize( numConstraints ); - for ( int i = 0; i < numConstraints; ++i ) - { - bc->m_constraintIndices[ i ] = i; - } - - bc->m_batches.resizeNoInitialize( 0 ); - bc->m_phases.resizeNoInitialize( 0 ); - bc->m_phaseOrder.resizeNoInitialize( 0 ); - bc->m_phaseGrainSize.resizeNoInitialize( 0 ); - - if (numConstraints > 0) - { - bc->m_batches.push_back( Range( 0, numConstraints ) ); - bc->m_phases.push_back( Range( 0, 1 ) ); - bc->m_phaseOrder.push_back(0); - bc->m_phaseGrainSize.push_back(1); - } + BT_PROFILE("setupSingleBatch"); + typedef btBatchedConstraints::Range Range; + + bc->m_constraintIndices.resize(numConstraints); + for (int i = 0; i < numConstraints; ++i) + { + bc->m_constraintIndices[i] = i; + } + + bc->m_batches.resizeNoInitialize(0); + bc->m_phases.resizeNoInitialize(0); + bc->m_phaseOrder.resizeNoInitialize(0); + bc->m_phaseGrainSize.resizeNoInitialize(0); + + if (numConstraints > 0) + { + bc->m_batches.push_back(Range(0, numConstraints)); + bc->m_phases.push_back(Range(0, 1)); + bc->m_phaseOrder.push_back(0); + bc->m_phaseGrainSize.push_back(1); + } } - void btBatchedConstraints::setup( - btConstraintArray* constraints, - const btAlignedObjectArray<btSolverBody>& bodies, - BatchingMethod batchingMethod, - int minBatchSize, - int maxBatchSize, - btAlignedObjectArray<char>* scratchMemory - ) + btConstraintArray* constraints, + const btAlignedObjectArray<btSolverBody>& bodies, + BatchingMethod batchingMethod, + int minBatchSize, + int maxBatchSize, + btAlignedObjectArray<char>* scratchMemory) { - if (constraints->size() >= minBatchSize*4) - { - bool use2DGrid = batchingMethod == BATCHING_METHOD_SPATIAL_GRID_2D; - setupSpatialGridBatchesMt( this, scratchMemory, constraints, bodies, minBatchSize, maxBatchSize, use2DGrid ); - if (s_debugDrawBatches) - { - debugDrawAllBatches( this, constraints, bodies ); - } - } - else - { - setupSingleBatch( this, constraints->size() ); - } + if (constraints->size() >= minBatchSize * 4) + { + bool use2DGrid = batchingMethod == BATCHING_METHOD_SPATIAL_GRID_2D; + setupSpatialGridBatchesMt(this, scratchMemory, constraints, bodies, minBatchSize, maxBatchSize, use2DGrid); + if (s_debugDrawBatches) + { + debugDrawAllBatches(this, constraints, bodies); + } + } + else + { + setupSingleBatch(this, constraints->size()); + } } - - |