diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/ConstraintSolver')
15 files changed, 3424 insertions, 399 deletions
diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp new file mode 100644 index 0000000000..c82ba87f9f --- /dev/null +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp @@ -0,0 +1,1128 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "btBatchedConstraints.h" + +#include "LinearMath/btIDebugDraw.h" +#include "LinearMath/btMinMax.h" +#include "LinearMath/btStackAlloc.h" +#include "LinearMath/btQuickprof.h" + +#include <string.h> //for memset + +const int kNoMerge = -1; + +bool btBatchedConstraints::s_debugDrawBatches = false; + + +struct btBatchedConstraintInfo +{ + int constraintIndex; + int numConstraintRows; + int bodyIds[2]; +}; + + +struct btBatchInfo +{ + int numConstraints; + int mergeIndex; + + 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; +} + + +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); + } + } +} + + +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); + } + } +} + + +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); + } + } +} + + +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 ) ); + } +} + + +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; +} + + +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; + } + } +}; + + +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; +} + + +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; + } + } + } +} + + +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; + } + } +} + + +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); + } +}; + + +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); +} + + +static void initBatchedConstraintInfoArray(btAlignedObjectArray<btBatchedConstraintInfo>* outConInfos, btConstraintArray* constraints) +{ + BT_PROFILE("initBatchedConstraintInfoArray"); + btAlignedObjectArray<btBatchedConstraintInfo>& conInfos = *outConInfos; + int numConstraints = constraints->size(); + conInfos.resizeNoInitialize(numConstraints); + + 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; + } + } + } +} + + +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; + } + } +} + + +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 ); + } +}; + + +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); +} + + +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; +} + + +static void writeOutConstraintIndicesForRangeOfBatches(btBatchedConstraints* bc, + 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; + } + } +} + + +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 + ); + } +}; + + +static void writeOutConstraintIndicesMt(btBatchedConstraints* bc, + 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; + } + } +} + + +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)); + } +} + + +static void writeOutBatches(btBatchedConstraints* bc, + 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); +} + + +// +// 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. +// +// Example Usage: +// +// btVector3* bodyPositions = NULL; +// btBatchedConstraintInfo* conInfos = NULL; +// { +// PreallocatedMemoryHelper<8> memHelper; +// memHelper.addChunk( (void**) &bodyPositions, sizeof( btVector3 ) * bodies.size() ); +// memHelper.addChunk( (void**) &conInfos, sizeof( btBatchedConstraintInfo ) * numConstraints ); +// void* memPtr = malloc( memHelper.getSizeToAllocate() ); // allocate the memory +// memHelper.setChunkPointers( memPtr ); // update pointers to chunks +// } +template <int N> +class PreallocatedMemoryHelper +{ + 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; + } + } +}; + + + +static btVector3 findMaxDynamicConstraintExtent( + 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; +} + + +struct btIntVec3 +{ + 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];} +}; + + +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)); + } +}; + + +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; + } +} + + +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); + } +}; + + +// +// setupSpatialGridBatchesMt -- generate batches using a uniform 3D grid +// +/* + +Bodies are treated as 3D points at their center of mass. We only consider dynamic bodies at this stage, +because only dynamic bodies are mutated when a constraint is solved, thus subject to race conditions. + +1. Compute a bounding box around all dynamic bodies +2. Compute the maximum extent of all dynamic constraints. Each dynamic constraint is treated as a line segment, and we need the size of + box that will fully enclose any single dynamic constraint + +3. Establish the cell size of our grid, the cell size in each dimension must be at least as large as the dynamic constraints max-extent, + so that no dynamic constraint can span more than 2 cells of our grid on any axis of the grid. The cell size should be adjusted + larger in order to keep the total number of cells from being excessively high + +Key idea: Given that each constraint spans 1 or 2 grid cells in each dimension, we can handle all constraints by processing + in chunks of 2x2x2 cells with 8 different 1-cell offsets ((0,0,0),(0,0,1),(0,1,0),(0,1,1),(1,0,0)...). + For each of the 8 offsets, we create a phase, and for each 2x2x2 chunk with dynamic constraints becomes a batch in that phase. + +4. Once the grid is established, we can calculate for each constraint which phase and batch it belongs in. + +5. Do a merge small batches on the batches of each phase separately, to try to even out the sizes of batches + +Optionally, we can "collapse" one dimension of our 3D grid to turn it into a 2D grid, which reduces the number of phases +to 4. With fewer phases, there are more constraints per phase and this makes it easier to create batches of a useful size. +*/ +// +static void setupSpatialGridBatchesMt( + 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)); +} + + +static void setupSingleBatch( + 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); + } +} + + +void btBatchedConstraints::setup( + 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() ); + } +} + + diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.h b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.h new file mode 100644 index 0000000000..0fd8f31dd4 --- /dev/null +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.h @@ -0,0 +1,66 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_BATCHED_CONSTRAINTS_H +#define BT_BATCHED_CONSTRAINTS_H + +#include "LinearMath/btThreads.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "BulletDynamics/ConstraintSolver/btSolverBody.h" +#include "BulletDynamics/ConstraintSolver/btSolverConstraint.h" + + +class btIDebugDraw; + +struct btBatchedConstraints +{ + enum BatchingMethod + { + BATCHING_METHOD_SPATIAL_GRID_2D, + BATCHING_METHOD_SPATIAL_GRID_3D, + BATCHING_METHOD_COUNT + }; + struct Range + { + int begin; + int end; + + Range() : begin( 0 ), end( 0 ) {} + Range( int _beg, int _end ) : begin( _beg ), end( _end ) {} + }; + + btAlignedObjectArray<int> m_constraintIndices; + btAlignedObjectArray<Range> m_batches; // each batch is a range of indices in the m_constraintIndices array + btAlignedObjectArray<Range> m_phases; // each phase is range of indices in the m_batches array + btAlignedObjectArray<char> m_phaseGrainSize; // max grain size for each phase + btAlignedObjectArray<int> m_phaseOrder; // phases can be done in any order, so we can randomize the order here + btIDebugDraw* m_debugDrawer; + + static bool s_debugDrawBatches; + + btBatchedConstraints() {m_debugDrawer=NULL;} + void setup( btConstraintArray* constraints, + const btAlignedObjectArray<btSolverBody>& bodies, + BatchingMethod batchingMethod, + int minBatchSize, + int maxBatchSize, + btAlignedObjectArray<char>* scratchMemory + ); + bool validate( btConstraintArray* constraints, const btAlignedObjectArray<btSolverBody>& bodies ) const; +}; + + +#endif // BT_BATCHED_CONSTRAINTS_H + diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btConstraintSolver.h index 890afe6da4..0491639f70 100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btConstraintSolver.h +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btConstraintSolver.h @@ -34,7 +34,8 @@ enum btConstraintSolverType { BT_SEQUENTIAL_IMPULSE_SOLVER=1, BT_MLCP_SOLVER=2, - BT_NNCG_SOLVER=4 + BT_NNCG_SOLVER=4, + BT_MULTIBODY_SOLVER=8, }; class btConstraintSolver diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 28d0c1dd48..93865cbc59 100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -29,7 +29,8 @@ enum btSolverMode SOLVER_CACHE_FRIENDLY = 128, SOLVER_SIMD = 256, SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS = 512, - SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024 + SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024, + SOLVER_DISABLE_IMPLICIT_CONE_FRICTION = 2048 }; struct btContactSolverInfoData diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index fa17254ec3..c38b8353f0 100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -855,8 +855,8 @@ int btGeneric6DofConstraint::get_limit_motor_info2( tag_vel, info->fps * limot->m_stopERP); info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity; - info->m_lowerLimit[srow] = -limot->m_maxMotorForce; - info->m_upperLimit[srow] = limot->m_maxMotorForce; + info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps; + info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps; } } if(limit) diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h index bea8629c32..b2ad45f749 100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -77,7 +77,7 @@ public: { m_accumulatedImpulse = 0.f; m_targetVelocity = 0; - m_maxMotorForce = 0.1f; + m_maxMotorForce = 6.0f; m_maxLimitForce = 300.0f; m_loLimit = 1.0f; m_hiLimit = -1.0f; diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp index f0976ee493..540dcd18f7 100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp @@ -719,8 +719,8 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2( tag_vel, info->fps * limot->m_motorERP); info->m_constraintError[srow] = mot_fact * limot->m_targetVelocity; - info->m_lowerLimit[srow] = -limot->m_maxMotorForce; - info->m_upperLimit[srow] = limot->m_maxMotorForce; + info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps; + info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps; info->cfm[srow] = limot->m_motorCFM; srow += info->rowskip; ++count; @@ -769,8 +769,8 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2( mot_fact = 0; } info->m_constraintError[srow] = mot_fact * targetvelocity * (rotational ? -1 : 1); - info->m_lowerLimit[srow] = -limot->m_maxMotorForce; - info->m_upperLimit[srow] = limot->m_maxMotorForce; + info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps; + info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps; info->cfm[srow] = limot->m_motorCFM; srow += info->rowskip; ++count; @@ -797,6 +797,12 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2( btScalar cfm = BT_ZERO; btScalar mA = BT_ONE / m_rbA.getInvMass(); btScalar mB = BT_ONE / m_rbB.getInvMass(); + if (rotational) { + btScalar rrA = (m_calculatedTransformA.getOrigin() - transA.getOrigin()).length2(); + btScalar rrB = (m_calculatedTransformB.getOrigin() - transB.getOrigin()).length2(); + if (m_rbA.getInvMass()) mA = mA * rrA + 1 / (m_rbA.getInvInertiaTensorWorld() * ax1).length(); + if (m_rbB.getInvMass()) mB = mB * rrB + 1 / (m_rbB.getInvInertiaTensorWorld() * ax1).length(); + } btScalar m = mA > mB ? mB : mA; btScalar angularfreq = sqrt(ks / m); @@ -815,7 +821,18 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2( btScalar fd = -kd * (vel) * (rotational ? -1 : 1) * dt; btScalar f = (fs+fd); - info->m_constraintError[srow] = (vel + f * (rotational ? -1 : 1)) ; + // after the spring force affecting the body(es) the new velocity will be + // vel + f / m * (rotational ? -1 : 1) + // so in theory this should be set here for m_constraintError + // (with m_constraintError we set a desired velocity for the affected body(es)) + // however in practice any value is fine as long as it is greater then the "proper" velocity, + // because the m_lowerLimit and the m_upperLimit will determinate the strength of the final pulling force + // so it is much simpler (and more robust) just to simply use inf (with the proper sign) + // you may also wonder what if the current velocity (vel) so high that the pulling force will not change its direction (in this iteration) + // will we not request a velocity with the wrong direction ? + // and the answare is not, because in practice during the solving the current velocity is subtracted from the m_constraintError + // so the sign of the force that is really matters + info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY); btScalar minf = f < fd ? f : fd; btScalar maxf = f < fd ? fd : f; diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h index 66d1769583..1b8d0eace9 100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h @@ -107,7 +107,7 @@ public: m_motorCFM = 0.f; m_enableMotor = false; m_targetVelocity = 0; - m_maxMotorForce = 0.1f; + m_maxMotorForce = 6.0f; m_servoMotor = false; m_servoTarget = 0; m_enableSpring = false; diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp index 6f765884ec..3f875989ea 100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp @@ -131,7 +131,7 @@ void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* inf btScalar force = delta * m_springStiffness[i]; btScalar velFactor = info->fps * m_springDamping[i] / btScalar(info->m_numIterations); m_linearLimits.m_targetVelocity[i] = velFactor * force; - m_linearLimits.m_maxMotorForce[i] = btFabs(force) / info->fps; + m_linearLimits.m_maxMotorForce[i] = btFabs(force); } } for(i = 0; i < 3; i++) @@ -146,7 +146,7 @@ void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* inf btScalar force = -delta * m_springStiffness[i+3]; btScalar velFactor = info->fps * m_springDamping[i+3] / btScalar(info->m_numIterations); m_angularLimits[i].m_targetVelocity = velFactor * force; - m_angularLimits[i].m_maxMotorForce = btFabs(force) / info->fps; + m_angularLimits[i].m_maxMotorForce = btFabs(force); } } } diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index b0d57a3e87..63174a6ec0 100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -21,6 +21,7 @@ subject to the following restrictions: #include "btSequentialImpulseConstraintSolver.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" + #include "LinearMath/btIDebugDraw.h" #include "LinearMath/btCpuFeatureUtility.h" @@ -42,11 +43,11 @@ int gNumSplitImpulseRecoveries = 0; //#define VERBOSE_RESIDUAL_PRINTF 1 ///This is the scalar reference implementation of solving a single constraint row, the innerloop of the Projected Gauss Seidel/Sequential Impulse constraint solver ///Below are optional SSE2 and SSE4/FMA3 versions. We assume most hardware has SSE2. For SSE4/FMA3 we perform a CPU feature check. -static btSimdScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) +static btScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm; - const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); - const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); + const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(bodyA.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA.internalGetDeltaAngularVelocity()); + const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(bodyB.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB.internalGetDeltaAngularVelocity()); // const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; @@ -68,18 +69,18 @@ static btSimdScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolver c.m_appliedImpulse = sum; } - body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); - body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); + bodyA.internalApplyImpulse(c.m_contactNormal1*bodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); + bodyB.internalApplyImpulse(c.m_contactNormal2*bodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); - return deltaImpulse; + return deltaImpulse*(1./c.m_jacDiagABInv); } -static btSimdScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) +static btScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm; - const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); - const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); + const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(bodyA.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA.internalGetDeltaAngularVelocity()); + const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(bodyB.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB.internalGetDeltaAngularVelocity()); deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; @@ -93,10 +94,10 @@ static btSimdScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSol { c.m_appliedImpulse = sum; } - body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); - body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); + bodyA.internalApplyImpulse(c.m_contactNormal1*bodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); + bodyB.internalApplyImpulse(c.m_contactNormal2*bodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); - return deltaImpulse; + return deltaImpulse*(1./c.m_jacDiagABInv); } @@ -149,14 +150,14 @@ static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 ) #endif // Project Gauss Seidel or the equivalent Sequential Impulse -static btSimdScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) +static btScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); btSimdScalar deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse), _mm_set1_ps(c.m_cfm))); - __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128)); - __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128)); + __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128)); + __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128)); deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv))); deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv))); btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse); @@ -169,27 +170,27 @@ static btSimdScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& body1, __m128 upperMinApplied = _mm_sub_ps(upperLimit1, cpAppliedImp); deltaImpulse = _mm_or_ps(_mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied)); c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1)); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128); - __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128, body2.internalGetInvMass().mVec128); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128); + __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128, bodyB.internalGetInvMass().mVec128); __m128 impulseMagnitude = deltaImpulse; - body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude)); - body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude)); - body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude)); - body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude)); - return deltaImpulse; + bodyA.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude)); + bodyA.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude)); + bodyB.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude)); + bodyB.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude)); + return deltaImpulse.m_floats[0]/c.m_jacDiagABInv; } // Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3 -static btSimdScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) +static btScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { #if defined (BT_ALLOW_SSE4) __m128 tmp = _mm_set_ps1(c.m_jacDiagABInv); __m128 deltaImpulse = _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm); const __m128 lowerLimit = _mm_set_ps1(c.m_lowerLimit); const __m128 upperLimit = _mm_set_ps1(c.m_upperLimit); - const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128)); - const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128)); + const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128)); + const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128)); deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse); deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse); tmp = _mm_add_ps(c.m_appliedImpulse, deltaImpulse); // sum @@ -197,26 +198,27 @@ static btSimdScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& const __m128 maskUpper = _mm_cmpgt_ps(upperLimit, tmp); deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), _mm_blendv_ps(_mm_sub_ps(upperLimit, c.m_appliedImpulse), deltaImpulse, maskUpper), maskLower); c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, _mm_blendv_ps(upperLimit, tmp, maskUpper), maskLower); - body1.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128), deltaImpulse, body1.internalGetDeltaLinearVelocity().mVec128); - body1.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, body1.internalGetDeltaAngularVelocity().mVec128); - body2.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128), deltaImpulse, body2.internalGetDeltaLinearVelocity().mVec128); - body2.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, body2.internalGetDeltaAngularVelocity().mVec128); - return deltaImpulse; + bodyA.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128), deltaImpulse, bodyA.internalGetDeltaLinearVelocity().mVec128); + bodyA.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, bodyA.internalGetDeltaAngularVelocity().mVec128); + bodyB.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128), deltaImpulse, bodyB.internalGetDeltaLinearVelocity().mVec128); + bodyB.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, bodyB.internalGetDeltaAngularVelocity().mVec128); + btSimdScalar deltaImp = deltaImpulse; + return deltaImp.m_floats[0]*(1./c.m_jacDiagABInv); #else - return gResolveSingleConstraintRowGeneric_sse2(body1,body2,c); + return gResolveSingleConstraintRowGeneric_sse2(bodyA,bodyB,c); #endif } -static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) +static btScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); btSimdScalar deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse), _mm_set1_ps(c.m_cfm))); - __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128)); - __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128)); + __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128)); + __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128)); deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv))); deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv))); btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse); @@ -226,39 +228,40 @@ static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& bod __m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp); deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse)); c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum)); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128); - __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128); + __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128); __m128 impulseMagnitude = deltaImpulse; - body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude)); - body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude)); - body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude)); - body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude)); - return deltaImpulse; + bodyA.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude)); + bodyA.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude)); + bodyB.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude)); + bodyB.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude)); + return deltaImpulse.m_floats[0]/c.m_jacDiagABInv; } // Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3 -static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) +static btScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { #ifdef BT_ALLOW_SSE4 __m128 tmp = _mm_set_ps1(c.m_jacDiagABInv); __m128 deltaImpulse = _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm); const __m128 lowerLimit = _mm_set_ps1(c.m_lowerLimit); - const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128)); - const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128)); + const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128)); + const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128)); deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse); deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse); tmp = _mm_add_ps(c.m_appliedImpulse, deltaImpulse); const __m128 mask = _mm_cmpgt_ps(tmp, lowerLimit); deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), deltaImpulse, mask); c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, tmp, mask); - body1.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128), deltaImpulse, body1.internalGetDeltaLinearVelocity().mVec128); - body1.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, body1.internalGetDeltaAngularVelocity().mVec128); - body2.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128), deltaImpulse, body2.internalGetDeltaLinearVelocity().mVec128); - body2.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, body2.internalGetDeltaAngularVelocity().mVec128); - return deltaImpulse; + bodyA.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128), deltaImpulse, bodyA.internalGetDeltaLinearVelocity().mVec128); + bodyA.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, bodyA.internalGetDeltaAngularVelocity().mVec128); + bodyB.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128), deltaImpulse, bodyB.internalGetDeltaLinearVelocity().mVec128); + bodyB.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, bodyB.internalGetDeltaAngularVelocity().mVec128); + btSimdScalar deltaImp = deltaImpulse; + return deltaImp.m_floats[0]*(1./c.m_jacDiagABInv); #else - return gResolveSingleConstraintRowLowerLimit_sse2(body1,body2,c); + return gResolveSingleConstraintRowLowerLimit_sse2(bodyA,bodyB,c); #endif //BT_ALLOW_SSE4 } @@ -267,32 +270,32 @@ static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBo -btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& c) { - return m_resolveSingleConstraintRowGeneric(body1, body2, c); + return m_resolveSingleConstraintRowGeneric(bodyA, bodyB, c); } // Project Gauss Seidel or the equivalent Sequential Impulse -btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& c) { - return m_resolveSingleConstraintRowGeneric(body1, body2, c); + return m_resolveSingleConstraintRowGeneric(bodyA, bodyB, c); } -btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& c) { - return m_resolveSingleConstraintRowLowerLimit(body1, body2, c); + return m_resolveSingleConstraintRowLowerLimit(bodyA, bodyB, c); } -btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& c) { - return m_resolveSingleConstraintRowLowerLimit(body1, body2, c); + return m_resolveSingleConstraintRowLowerLimit(bodyA, bodyB, c); } -static btSimdScalar gResolveSplitPenetrationImpulse_scalar_reference( - btSolverBody& body1, - btSolverBody& body2, +static btScalar gResolveSplitPenetrationImpulse_scalar_reference( + btSolverBody& bodyA, + btSolverBody& bodyB, const btSolverConstraint& c) { btScalar deltaImpulse = 0.f; @@ -301,8 +304,8 @@ static btSimdScalar gResolveSplitPenetrationImpulse_scalar_reference( { gNumSplitImpulseRecoveries++; deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm; - const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity()); - const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity()); + const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(bodyA.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(bodyA.internalGetTurnVelocity()); + const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(bodyB.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(bodyB.internalGetTurnVelocity()); deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; @@ -316,13 +319,13 @@ static btSimdScalar gResolveSplitPenetrationImpulse_scalar_reference( { c.m_appliedPushImpulse = sum; } - body1.internalApplyPushImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); - body2.internalApplyPushImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); + bodyA.internalApplyPushImpulse(c.m_contactNormal1*bodyA.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); + bodyB.internalApplyPushImpulse(c.m_contactNormal2*bodyB.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); } - return deltaImpulse; + return deltaImpulse*(1./c.m_jacDiagABInv); } -static btSimdScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +static btScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& c) { #ifdef USE_SIMD if (!c.m_rhsPenetration) @@ -334,8 +337,8 @@ static btSimdScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody& body1,btS __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse),_mm_set1_ps(c.m_cfm))); - __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128)); - __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128)); + __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,bodyA.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,bodyA.internalGetTurnVelocity().mVec128)); + __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,bodyB.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,bodyB.internalGetTurnVelocity().mVec128)); deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); @@ -345,16 +348,17 @@ static btSimdScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody& body1,btS __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); c.m_appliedPushImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128); - __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128,body2.internalGetInvMass().mVec128); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,bodyA.internalGetInvMass().mVec128); + __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128,bodyB.internalGetInvMass().mVec128); __m128 impulseMagnitude = deltaImpulse; - body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); - body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); - body2.internalGetPushVelocity().mVec128 = _mm_add_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); - body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); - return deltaImpulse; + bodyA.internalGetPushVelocity().mVec128 = _mm_add_ps(bodyA.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); + bodyA.internalGetTurnVelocity().mVec128 = _mm_add_ps(bodyA.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); + bodyB.internalGetPushVelocity().mVec128 = _mm_add_ps(bodyB.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); + bodyB.internalGetTurnVelocity().mVec128 = _mm_add_ps(bodyB.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); + btSimdScalar deltaImp = deltaImpulse; + return deltaImp.m_floats[0] * (1. / c.m_jacDiagABInv); #else - return gResolveSplitPenetrationImpulse_scalar_reference(body1,body2,c); + return gResolveSplitPenetrationImpulse_scalar_reference(bodyA,bodyB,c); #endif } @@ -548,7 +552,7 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody; - btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody; + btRigidBody* bodyA = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody; solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; @@ -572,12 +576,12 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr solverConstraint.m_angularComponentA .setZero(); } - if (body1) + if (bodyA) { solverConstraint.m_contactNormal2 = -normalAxis; btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal2); solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentB = body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor(); + solverConstraint.m_angularComponentB = bodyA->getInvInertiaTensorWorld()*ftorqueAxis1*bodyA->getAngularFactor(); } else { solverConstraint.m_contactNormal2.setZero(); @@ -594,10 +598,10 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); denom0 = body0->getInvMass() + normalAxis.dot(vec); } - if (body1) + if (bodyA) { vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = body1->getInvMass() + normalAxis.dot(vec); + denom1 = bodyA->getInvMass() + normalAxis.dot(vec); } btScalar denom = relaxation/(denom0+denom1); solverConstraint.m_jacDiagABInv = denom; @@ -609,8 +613,8 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr btScalar rel_vel; btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0)) + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0)); - btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0)) - + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0)); + btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyA?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0)) + + solverConstraint.m_relpos2CrossNormal.dot(bodyA?solverBodyB.m_angularVelocity:btVector3(0,0,0)); rel_vel = vel1Dotn+vel2Dotn; @@ -662,7 +666,7 @@ void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint( btSo btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody; - btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody; + btRigidBody* bodyA = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody; solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; @@ -681,13 +685,13 @@ void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint( btSo { btVector3 ftorqueAxis1 = normalAxis1; solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_angularComponentB = bodyA ? bodyA->getInvInertiaTensorWorld()*ftorqueAxis1*bodyA->getAngularFactor() : btVector3(0,0,0); } { btVector3 iMJaA = body0?body0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0); - btVector3 iMJaB = body1?body1->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0); + btVector3 iMJaB = bodyA?bodyA->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0); btScalar sum = 0; sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); @@ -700,8 +704,8 @@ void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint( btSo btScalar rel_vel; btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0)) + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0)); - btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0)) - + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0)); + btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyA?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0)) + + solverConstraint.m_relpos2CrossNormal.dot(bodyA?solverBodyB.m_angularVelocity:btVector3(0,0,0)); rel_vel = vel1Dotn+vel2Dotn; @@ -738,23 +742,21 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& { #if BT_THREADSAFE int solverBodyId = -1; - if ( !body.isStaticOrKinematicObject() ) + bool isRigidBodyType = btRigidBody::upcast( &body ) != NULL; + if ( isRigidBodyType && !body.isStaticOrKinematicObject() ) { // dynamic body // Dynamic bodies can only be in one island, so it's safe to write to the companionId solverBodyId = body.getCompanionId(); if ( solverBodyId < 0 ) { - if ( btRigidBody* rb = btRigidBody::upcast( &body ) ) - { - solverBodyId = m_tmpSolverBodyPool.size(); - btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody( &solverBody, &body, timeStep ); - body.setCompanionId( solverBodyId ); - } + solverBodyId = m_tmpSolverBodyPool.size(); + btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); + initSolverBody( &solverBody, &body, timeStep ); + body.setCompanionId( solverBodyId ); } } - else if (body.isKinematicObject()) + else if (isRigidBodyType && body.isKinematicObject()) { // // NOTE: must test for kinematic before static because some kinematic objects also @@ -774,7 +776,6 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& if ( solverBodyId == INVALID_SOLVER_BODY_ID ) { // create a table entry for this body - btRigidBody* rb = btRigidBody::upcast( &body ); solverBodyId = m_tmpSolverBodyPool.size(); btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); initSolverBody( &solverBody, &body, timeStep ); @@ -783,6 +784,13 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& } else { + bool isMultiBodyType = (body.getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK); + // Incorrectly set collision object flags can degrade performance in various ways. + if (!isMultiBodyType) + { + btAssert( body.isStaticOrKinematicObject() ); + } + //it could be a multibody link collider // all fixed bodies (inf mass) get mapped to a single solver id if ( m_fixedBodyId < 0 ) { @@ -792,7 +800,7 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& } solverBodyId = m_fixedBodyId; } - btAssert( solverBodyId < m_tmpSolverBodyPool.size() ); + btAssert( solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size() ); return solverBodyId; #else // BT_THREADSAFE @@ -1258,6 +1266,256 @@ void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold** } } + +void btSequentialImpulseConstraintSolver::convertJoint(btSolverConstraint* currentConstraintRow, + btTypedConstraint* constraint, + const btTypedConstraint::btConstraintInfo1& info1, + int solverBodyIdA, + int solverBodyIdB, + const btContactSolverInfo& infoGlobal + ) +{ + const btRigidBody& rbA = constraint->getRigidBodyA(); + const btRigidBody& rbB = constraint->getRigidBodyB(); + + const btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; + const btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; + + int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations; + if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations) + m_maxOverrideNumSolverIterations = overrideNumSolverIterations; + + for (int j=0;j<info1.m_numConstraintRows;j++) + { + memset(¤tConstraintRow[j],0,sizeof(btSolverConstraint)); + currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY; + currentConstraintRow[j].m_upperLimit = SIMD_INFINITY; + currentConstraintRow[j].m_appliedImpulse = 0.f; + currentConstraintRow[j].m_appliedPushImpulse = 0.f; + currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA; + currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB; + currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations; + } + + // these vectors are already cleared in initSolverBody, no need to redundantly clear again + btAssert(bodyAPtr->getDeltaLinearVelocity().isZero()); + btAssert(bodyAPtr->getDeltaAngularVelocity().isZero()); + btAssert(bodyAPtr->getPushVelocity().isZero()); + btAssert(bodyAPtr->getTurnVelocity().isZero()); + btAssert(bodyBPtr->getDeltaLinearVelocity().isZero()); + btAssert(bodyBPtr->getDeltaAngularVelocity().isZero()); + btAssert(bodyBPtr->getPushVelocity().isZero()); + btAssert(bodyBPtr->getTurnVelocity().isZero()); + //bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); + //bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); + //bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f); + //bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f); + //bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); + //bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); + //bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f); + //bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f); + + + btTypedConstraint::btConstraintInfo2 info2; + info2.fps = 1.f/infoGlobal.m_timeStep; + info2.erp = infoGlobal.m_erp; + info2.m_J1linearAxis = currentConstraintRow->m_contactNormal1; + info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal; + info2.m_J2linearAxis = currentConstraintRow->m_contactNormal2; + info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal; + info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this + ///the size of btSolverConstraint needs be a multiple of btScalar + btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint)); + info2.m_constraintError = ¤tConstraintRow->m_rhs; + currentConstraintRow->m_cfm = infoGlobal.m_globalCfm; + info2.m_damping = infoGlobal.m_damping; + info2.cfm = ¤tConstraintRow->m_cfm; + info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit; + info2.m_upperLimit = ¤tConstraintRow->m_upperLimit; + info2.m_numIterations = infoGlobal.m_numIterations; + constraint->getInfo2(&info2); + + ///finalize the constraint setup + for (int j=0;j<info1.m_numConstraintRows;j++) + { + btSolverConstraint& solverConstraint = currentConstraintRow[j]; + + if (solverConstraint.m_upperLimit>=constraint->getBreakingImpulseThreshold()) + { + solverConstraint.m_upperLimit = constraint->getBreakingImpulseThreshold(); + } + + if (solverConstraint.m_lowerLimit<=-constraint->getBreakingImpulseThreshold()) + { + solverConstraint.m_lowerLimit = -constraint->getBreakingImpulseThreshold(); + } + + solverConstraint.m_originalContactPoint = constraint; + + { + const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal; + solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor(); + } + { + const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal; + solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor(); + } + + { + btVector3 iMJlA = solverConstraint.m_contactNormal1*rbA.getInvMass(); + btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal; + btVector3 iMJlB = solverConstraint.m_contactNormal2*rbB.getInvMass();//sign of normal? + btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal; + + btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal1); + sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); + sum += iMJlB.dot(solverConstraint.m_contactNormal2); + sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); + btScalar fsum = btFabs(sum); + btAssert(fsum > SIMD_EPSILON); + btScalar sorRelaxation = 1.f;//todo: get from globalInfo? + solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?sorRelaxation/sum : 0.f; + } + + { + btScalar rel_vel; + btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0,0,0); + btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse : btVector3(0,0,0); + + btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0,0,0); + btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ?bodyBPtr->m_externalTorqueImpulse : btVector3(0,0,0); + + btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA) + + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()+externalTorqueImpulseA); + + btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB) + + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()+externalTorqueImpulseB); + + rel_vel = vel1Dotn+vel2Dotn; + btScalar restitution = 0.f; + btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2 + btScalar velocityError = restitution - rel_vel * info2.m_damping; + btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; + solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint.m_appliedImpulse = 0.f; + } + } +} + + +void btSequentialImpulseConstraintSolver::convertJoints(btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal) +{ + BT_PROFILE("convertJoints"); + for (int j=0;j<numConstraints;j++) + { + btTypedConstraint* constraint = constraints[j]; + constraint->buildJacobian(); + constraint->internalSetAppliedImpulse(0.0f); + } + + int totalNumRows = 0; + + m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints); + //calculate the total number of contraint rows + for (int i=0;i<numConstraints;i++) + { + btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; + btJointFeedback* fb = constraints[i]->getJointFeedback(); + if (fb) + { + fb->m_appliedForceBodyA.setZero(); + fb->m_appliedTorqueBodyA.setZero(); + fb->m_appliedForceBodyB.setZero(); + fb->m_appliedTorqueBodyB.setZero(); + } + + if (constraints[i]->isEnabled()) + { + constraints[i]->getInfo1(&info1); + } else + { + info1.m_numConstraintRows = 0; + info1.nub = 0; + } + totalNumRows += info1.m_numConstraintRows; + } + m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows); + + + ///setup the btSolverConstraints + int currentRow = 0; + + for (int i=0;i<numConstraints;i++) + { + const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; + + if (info1.m_numConstraintRows) + { + btAssert(currentRow<totalNumRows); + + btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow]; + btTypedConstraint* constraint = constraints[i]; + btRigidBody& rbA = constraint->getRigidBodyA(); + btRigidBody& rbB = constraint->getRigidBodyB(); + + int solverBodyIdA = getOrInitSolverBody(rbA,infoGlobal.m_timeStep); + int solverBodyIdB = getOrInitSolverBody(rbB,infoGlobal.m_timeStep); + + convertJoint(currentConstraintRow, constraint, info1, solverBodyIdA, solverBodyIdB, infoGlobal); + } + currentRow+=info1.m_numConstraintRows; + } +} + + +void btSequentialImpulseConstraintSolver::convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) +{ + BT_PROFILE("convertBodies"); + for (int i = 0; i < numBodies; i++) + { + bodies[i]->setCompanionId(-1); + } +#if BT_THREADSAFE + m_kinematicBodyUniqueIdToSolverBodyTable.resize( 0 ); +#endif // BT_THREADSAFE + + m_tmpSolverBodyPool.reserve(numBodies+1); + m_tmpSolverBodyPool.resize(0); + + //btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); + //initSolverBody(&fixedBody,0); + + for (int i=0;i<numBodies;i++) + { + int bodyId = getOrInitSolverBody(*bodies[i],infoGlobal.m_timeStep); + + btRigidBody* body = btRigidBody::upcast(bodies[i]); + if (body && body->getInvMass()) + { + btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; + btVector3 gyroForce (0,0,0); + if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT) + { + gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce); + solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep; + } + if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD) + { + gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep); + solverBody.m_externalTorqueImpulse += gyroForce; + } + if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY) + { + gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep); + solverBody.m_externalTorqueImpulse += gyroForce; + + } + } + } +} + + btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) { m_fixedBodyId = -1; @@ -1344,254 +1602,14 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol #endif //BT_ADDITIONAL_DEBUG - for (int i = 0; i < numBodies; i++) - { - bodies[i]->setCompanionId(-1); - } -#if BT_THREADSAFE - m_kinematicBodyUniqueIdToSolverBodyTable.resize( 0 ); -#endif // BT_THREADSAFE - - m_tmpSolverBodyPool.reserve(numBodies+1); - m_tmpSolverBodyPool.resize(0); - - //btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); - //initSolverBody(&fixedBody,0); - //convert all bodies + convertBodies(bodies, numBodies, infoGlobal); + convertJoints(constraints, numConstraints, infoGlobal); - for (int i=0;i<numBodies;i++) - { - int bodyId = getOrInitSolverBody(*bodies[i],infoGlobal.m_timeStep); - - btRigidBody* body = btRigidBody::upcast(bodies[i]); - if (body && body->getInvMass()) - { - btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; - btVector3 gyroForce (0,0,0); - if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT) - { - gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce); - solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep; - } - if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD) - { - gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep); - solverBody.m_externalTorqueImpulse += gyroForce; - } - if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY) - { - gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep); - solverBody.m_externalTorqueImpulse += gyroForce; - - } - - - } - } - - if (1) - { - int j; - for (j=0;j<numConstraints;j++) - { - btTypedConstraint* constraint = constraints[j]; - constraint->buildJacobian(); - constraint->internalSetAppliedImpulse(0.0f); - } - } - - //btRigidBody* rb0=0,*rb1=0; - - //if (1) - { - { - - int totalNumRows = 0; - int i; - - m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints); - //calculate the total number of contraint rows - for (i=0;i<numConstraints;i++) - { - btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; - btJointFeedback* fb = constraints[i]->getJointFeedback(); - if (fb) - { - fb->m_appliedForceBodyA.setZero(); - fb->m_appliedTorqueBodyA.setZero(); - fb->m_appliedForceBodyB.setZero(); - fb->m_appliedTorqueBodyB.setZero(); - } - - if (constraints[i]->isEnabled()) - { - } - if (constraints[i]->isEnabled()) - { - constraints[i]->getInfo1(&info1); - } else - { - info1.m_numConstraintRows = 0; - info1.nub = 0; - } - totalNumRows += info1.m_numConstraintRows; - } - m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows); - - - ///setup the btSolverConstraints - int currentRow = 0; - - for (i=0;i<numConstraints;i++) - { - const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; - - if (info1.m_numConstraintRows) - { - btAssert(currentRow<totalNumRows); - - btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow]; - btTypedConstraint* constraint = constraints[i]; - btRigidBody& rbA = constraint->getRigidBodyA(); - btRigidBody& rbB = constraint->getRigidBodyB(); - - int solverBodyIdA = getOrInitSolverBody(rbA,infoGlobal.m_timeStep); - int solverBodyIdB = getOrInitSolverBody(rbB,infoGlobal.m_timeStep); - - btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; - btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; + convertContacts(manifoldPtr,numManifolds,infoGlobal); - - - int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations; - if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations) - m_maxOverrideNumSolverIterations = overrideNumSolverIterations; - - - int j; - for ( j=0;j<info1.m_numConstraintRows;j++) - { - memset(¤tConstraintRow[j],0,sizeof(btSolverConstraint)); - currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY; - currentConstraintRow[j].m_upperLimit = SIMD_INFINITY; - currentConstraintRow[j].m_appliedImpulse = 0.f; - currentConstraintRow[j].m_appliedPushImpulse = 0.f; - currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA; - currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB; - currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations; - } - - bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); - bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); - bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f); - bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f); - bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); - bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); - bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f); - bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f); - - - btTypedConstraint::btConstraintInfo2 info2; - info2.fps = 1.f/infoGlobal.m_timeStep; - info2.erp = infoGlobal.m_erp; - info2.m_J1linearAxis = currentConstraintRow->m_contactNormal1; - info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal; - info2.m_J2linearAxis = currentConstraintRow->m_contactNormal2; - info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal; - info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this - ///the size of btSolverConstraint needs be a multiple of btScalar - btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint)); - info2.m_constraintError = ¤tConstraintRow->m_rhs; - currentConstraintRow->m_cfm = infoGlobal.m_globalCfm; - info2.m_damping = infoGlobal.m_damping; - info2.cfm = ¤tConstraintRow->m_cfm; - info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit; - info2.m_upperLimit = ¤tConstraintRow->m_upperLimit; - info2.m_numIterations = infoGlobal.m_numIterations; - constraints[i]->getInfo2(&info2); - - ///finalize the constraint setup - for ( j=0;j<info1.m_numConstraintRows;j++) - { - btSolverConstraint& solverConstraint = currentConstraintRow[j]; - - if (solverConstraint.m_upperLimit>=constraints[i]->getBreakingImpulseThreshold()) - { - solverConstraint.m_upperLimit = constraints[i]->getBreakingImpulseThreshold(); - } - - if (solverConstraint.m_lowerLimit<=-constraints[i]->getBreakingImpulseThreshold()) - { - solverConstraint.m_lowerLimit = -constraints[i]->getBreakingImpulseThreshold(); - } - - solverConstraint.m_originalContactPoint = constraint; - - { - const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal; - solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor(); - } - { - const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal; - solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor(); - } - - { - btVector3 iMJlA = solverConstraint.m_contactNormal1*rbA.getInvMass(); - btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal; - btVector3 iMJlB = solverConstraint.m_contactNormal2*rbB.getInvMass();//sign of normal? - btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal; - - btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal1); - sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); - sum += iMJlB.dot(solverConstraint.m_contactNormal2); - sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); - btScalar fsum = btFabs(sum); - btAssert(fsum > SIMD_EPSILON); - btScalar sorRelaxation = 1.f;//todo: get from globalInfo? - solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?sorRelaxation/sum : 0.f; - } - - - - { - btScalar rel_vel; - btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0,0,0); - btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse : btVector3(0,0,0); - - btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0,0,0); - btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ?bodyBPtr->m_externalTorqueImpulse : btVector3(0,0,0); - - btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA) - + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()+externalTorqueImpulseA); - - btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB) - + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()+externalTorqueImpulseB); - - rel_vel = vel1Dotn+vel2Dotn; - btScalar restitution = 0.f; - btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2 - btScalar velocityError = restitution - rel_vel * info2.m_damping; - btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; - solverConstraint.m_appliedImpulse = 0.f; - - - } - } - } - currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows; - } - } - - convertContacts(manifoldPtr,numManifolds,infoGlobal); - - } - // btContactSolverInfo info = infoGlobal; @@ -1630,6 +1648,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/) { + BT_PROFILE("solveSingleIteration"); btScalar leastSquaresResidual = 0.f; int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); @@ -1675,7 +1694,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration if (iteration < constraint.m_overrideNumSolverIterations) { btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint); - leastSquaresResidual += residual*residual; + leastSquaresResidual = btMax(leastSquaresResidual, residual*residual); } } @@ -1706,7 +1725,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration { const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]]; btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - leastSquaresResidual += residual*residual; + leastSquaresResidual = btMax(leastSquaresResidual, residual*residual); totalImpulse = solveManifold.m_appliedImpulse; } @@ -1723,7 +1742,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - leastSquaresResidual += residual*residual; + leastSquaresResidual = btMax(leastSquaresResidual, residual*residual); } } @@ -1738,7 +1757,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - leastSquaresResidual += residual*residual; + leastSquaresResidual = btMax(leastSquaresResidual, residual*residual); } } } @@ -1755,7 +1774,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration { const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - leastSquaresResidual += residual*residual; + leastSquaresResidual = btMax(leastSquaresResidual, residual*residual); } @@ -1774,7 +1793,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - leastSquaresResidual += residual*residual; + leastSquaresResidual = btMax(leastSquaresResidual, residual*residual); } } } @@ -1796,7 +1815,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); - leastSquaresResidual += residual*residual; + leastSquaresResidual = btMax(leastSquaresResidual, residual*residual); } } @@ -1808,6 +1827,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) { + BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations"); int iteration; if (infoGlobal.m_splitImpulse) { @@ -1823,7 +1843,7 @@ void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIte const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - leastSquaresResidual += residual*residual; + leastSquaresResidual = btMax(leastSquaresResidual, residual*residual); } } if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration>=(infoGlobal.m_numIterations-1)) @@ -1866,14 +1886,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations( return 0.f; } -btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal) +void btSequentialImpulseConstraintSolver::writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) { - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - int i,j; - - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - for (j=0;j<numPoolConstraints;j++) + for (int j=iBegin; j<iEnd; j++) { const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j]; btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint; @@ -1889,10 +1904,11 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo } //do a callback here? } - } +} - numPoolConstraints = m_tmpSolverNonContactConstraintPool.size(); - for (j=0;j<numPoolConstraints;j++) +void btSequentialImpulseConstraintSolver::writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) +{ + for (int j=iBegin; j<iEnd; j++) { const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j]; btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint; @@ -1912,10 +1928,12 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo constr->setEnabled(false); } } +} - - for ( i=0;i<m_tmpSolverBodyPool.size();i++) +void btSequentialImpulseConstraintSolver::writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) +{ + for (int i=iBegin; i<iEnd; i++) { btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody; if (body) @@ -1939,6 +1957,19 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo m_tmpSolverBodyPool[i].m_originalBody->setCompanionId(-1); } } +} + +btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal) +{ + BT_PROFILE("solveGroupCacheFriendlyFinish"); + + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + writeBackContacts(0, m_tmpSolverContactConstraintPool.size(), infoGlobal); + } + + writeBackJoints(0, m_tmpSolverNonContactConstraintPool.size(), infoGlobal); + writeBackBodies(0, m_tmpSolverBodyPool.size(), infoGlobal); m_tmpSolverContactConstraintPool.resizeNoInitialize(0); m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0); diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 16c7eb74c1..b834c3dac3 100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -4,8 +4,8 @@ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. @@ -27,7 +27,7 @@ class btCollisionObject; #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" #include "BulletDynamics/ConstraintSolver/btConstraintSolver.h" -typedef btSimdScalar(*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&); +typedef btScalar(*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&); ///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method. ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolver : public btConstraintSolver @@ -64,44 +64,48 @@ protected: void setupFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB, btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2, - btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, + btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0., btScalar cfmSlip=0.); void setupTorsionalFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB, btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2, - btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, + btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.); btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0., btScalar cfmSlip=0.); btSolverConstraint& addTorsionalFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,btScalar torsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f); - - void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, + + void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2); static void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode); - void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB, + void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal); ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction unsigned long m_btSeed2; - + btScalar restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold); virtual void convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal); void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal); + virtual void convertJoints(btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal); + void convertJoint(btSolverConstraint* currentConstraintRow, btTypedConstraint* constraint, const btTypedConstraint::btConstraintInfo1& info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo& infoGlobal); + + virtual void convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal); - btSimdScalar resolveSplitPenetrationSIMD(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint) + btScalar resolveSplitPenetrationSIMD(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint) { return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint ); } - btSimdScalar resolveSplitPenetrationImpulseCacheFriendly(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint) + btScalar resolveSplitPenetrationImpulseCacheFriendly(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint) { return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint ); } @@ -110,18 +114,20 @@ protected: int getOrInitSolverBody(btCollisionObject& body,btScalar timeStep); void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep); - btSimdScalar resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); - btSimdScalar resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); - btSimdScalar resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); - btSimdScalar resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); - btSimdScalar resolveSplitPenetrationImpulse(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint) + btScalar resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); + btScalar resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); + btScalar resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); + btScalar resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); + btScalar resolveSplitPenetrationImpulse(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint) { return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint ); } - + protected: - - + + void writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); + void writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); + void writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal); virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); @@ -133,15 +139,15 @@ protected: public: BT_DECLARE_ALIGNED_ALLOCATOR(); - + btSequentialImpulseConstraintSolver(); virtual ~btSequentialImpulseConstraintSolver(); virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher); - + ///clear internal cached data and reset random seed virtual void reset(); - + unsigned long btRand2(); int btRandInt2 (int n); @@ -155,7 +161,7 @@ public: return m_btSeed2; } - + virtual btConstraintSolverType getSolverType() const { return BT_SEQUENTIAL_IMPULSE_SOLVER; diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp new file mode 100644 index 0000000000..4306c37e49 --- /dev/null +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp @@ -0,0 +1,1621 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "btSequentialImpulseConstraintSolverMt.h" + +#include "LinearMath/btQuickprof.h" + +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" + +#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" + + + +bool btSequentialImpulseConstraintSolverMt::s_allowNestedParallelForLoops = false; // some task schedulers don't like nested loops +int btSequentialImpulseConstraintSolverMt::s_minimumContactManifoldsForBatching = 250; +int btSequentialImpulseConstraintSolverMt::s_minBatchSize = 50; +int btSequentialImpulseConstraintSolverMt::s_maxBatchSize = 100; +btBatchedConstraints::BatchingMethod btSequentialImpulseConstraintSolverMt::s_contactBatchingMethod = btBatchedConstraints::BATCHING_METHOD_SPATIAL_GRID_2D; +btBatchedConstraints::BatchingMethod btSequentialImpulseConstraintSolverMt::s_jointBatchingMethod = btBatchedConstraints::BATCHING_METHOD_SPATIAL_GRID_2D; + + +btSequentialImpulseConstraintSolverMt::btSequentialImpulseConstraintSolverMt() +{ + m_numFrictionDirections = 1; + m_useBatching = false; + m_useObsoleteJointConstraints = false; +} + + +btSequentialImpulseConstraintSolverMt::~btSequentialImpulseConstraintSolverMt() +{ +} + + +void btSequentialImpulseConstraintSolverMt::setupBatchedContactConstraints() +{ + BT_PROFILE("setupBatchedContactConstraints"); + m_batchedContactConstraints.setup( &m_tmpSolverContactConstraintPool, + m_tmpSolverBodyPool, + s_contactBatchingMethod, + s_minBatchSize, + s_maxBatchSize, + &m_scratchMemory + ); +} + + +void btSequentialImpulseConstraintSolverMt::setupBatchedJointConstraints() +{ + BT_PROFILE("setupBatchedJointConstraints"); + m_batchedJointConstraints.setup( &m_tmpSolverNonContactConstraintPool, + m_tmpSolverBodyPool, + s_jointBatchingMethod, + s_minBatchSize, + s_maxBatchSize, + &m_scratchMemory + ); +} + + +void btSequentialImpulseConstraintSolverMt::internalSetupContactConstraints(int iContactConstraint, const btContactSolverInfo& infoGlobal) +{ + btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[iContactConstraint]; + + btVector3 rel_pos1; + btVector3 rel_pos2; + btScalar relaxation; + + int solverBodyIdA = contactConstraint.m_solverBodyIdA; + int solverBodyIdB = contactConstraint.m_solverBodyIdB; + + btSolverBody* solverBodyA = &m_tmpSolverBodyPool[ solverBodyIdA ]; + btSolverBody* solverBodyB = &m_tmpSolverBodyPool[ solverBodyIdB ]; + + btRigidBody* colObj0 = solverBodyA->m_originalBody; + btRigidBody* colObj1 = solverBodyB->m_originalBody; + + btManifoldPoint& cp = *static_cast<btManifoldPoint*>( contactConstraint.m_originalContactPoint ); + + const btVector3& pos1 = cp.getPositionWorldOnA(); + const btVector3& pos2 = cp.getPositionWorldOnB(); + + rel_pos1 = pos1 - solverBodyA->getWorldTransform().getOrigin(); + rel_pos2 = pos2 - solverBodyB->getWorldTransform().getOrigin(); + + btVector3 vel1; + btVector3 vel2; + + solverBodyA->getVelocityInLocalPointNoDelta( rel_pos1, vel1 ); + solverBodyB->getVelocityInLocalPointNoDelta( rel_pos2, vel2 ); + + btVector3 vel = vel1 - vel2; + btScalar rel_vel = cp.m_normalWorldOnB.dot( vel ); + + setupContactConstraint( contactConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2 ); + + // setup rolling friction constraints + int rollingFrictionIndex = m_rollingFrictionIndexTable[iContactConstraint]; + if (rollingFrictionIndex >= 0) + { + btSolverConstraint& spinningFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[ rollingFrictionIndex ]; + btAssert( spinningFrictionConstraint.m_frictionIndex == iContactConstraint ); + setupTorsionalFrictionConstraint( spinningFrictionConstraint, + cp.m_normalWorldOnB, + solverBodyIdA, + solverBodyIdB, + cp, + cp.m_combinedSpinningFriction, + rel_pos1, + rel_pos2, + colObj0, + colObj1, + relaxation, + 0.0f, + 0.0f + ); + btVector3 axis[2]; + btPlaneSpace1( cp.m_normalWorldOnB, axis[0], axis[1] ); + axis[0].normalize(); + axis[1].normalize(); + + applyAnisotropicFriction( colObj0, axis[0], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION ); + applyAnisotropicFriction( colObj1, axis[0], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION ); + applyAnisotropicFriction( colObj0, axis[1], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION ); + applyAnisotropicFriction( colObj1, axis[1], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION ); + // put the largest axis first + if (axis[1].length2() > axis[0].length2()) + { + btSwap(axis[0], axis[1]); + } + const btScalar kRollingFrictionThreshold = 0.001f; + for (int i = 0; i < 2; ++i) + { + int iRollingFric = rollingFrictionIndex + 1 + i; + btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[ iRollingFric ]; + btAssert(rollingFrictionConstraint.m_frictionIndex == iContactConstraint); + btVector3 dir = axis[i]; + if ( dir.length() > kRollingFrictionThreshold ) + { + setupTorsionalFrictionConstraint( rollingFrictionConstraint, + dir, + solverBodyIdA, + solverBodyIdB, + cp, + cp.m_combinedRollingFriction, + rel_pos1, + rel_pos2, + colObj0, + colObj1, + relaxation, + 0.0f, + 0.0f + ); + } + else + { + rollingFrictionConstraint.m_frictionIndex = -1; // disable constraint + } + } + } + + // setup friction constraints + // setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip); + { + ///Bullet has several options to set the friction directions + ///By default, each contact has only a single friction direction that is recomputed automatically very frame + ///based on the relative linear velocity. + ///If the relative velocity it zero, it will automatically compute a friction direction. + + ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS. + ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction. + /// + ///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity. + /// + ///The user can manually override the friction directions for certain contacts using a contact callback, + ///and set the cp.m_lateralFrictionInitialized to true + ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2) + ///this will give a conveyor belt effect + /// + btSolverConstraint* frictionConstraint1 = &m_tmpSolverContactFrictionConstraintPool[contactConstraint.m_frictionIndex]; + btAssert(frictionConstraint1->m_frictionIndex == iContactConstraint); + + btSolverConstraint* frictionConstraint2 = NULL; + if ( infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS ) + { + frictionConstraint2 = &m_tmpSolverContactFrictionConstraintPool[contactConstraint.m_frictionIndex + 1]; + btAssert( frictionConstraint2->m_frictionIndex == iContactConstraint ); + } + + if ( !( infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING ) || !( cp.m_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED ) ) + { + cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; + btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); + if ( !( infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION ) && lat_rel_vel > SIMD_EPSILON ) + { + cp.m_lateralFrictionDir1 *= 1.f / btSqrt( lat_rel_vel ); + applyAnisotropicFriction( colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION ); + applyAnisotropicFriction( colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION ); + setupFrictionConstraint( *frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal ); + + if ( frictionConstraint2 ) + { + cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross( cp.m_normalWorldOnB ); + cp.m_lateralFrictionDir2.normalize();//?? + applyAnisotropicFriction( colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION ); + applyAnisotropicFriction( colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION ); + setupFrictionConstraint( *frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal ); + } + } + else + { + btPlaneSpace1( cp.m_normalWorldOnB, cp.m_lateralFrictionDir1, cp.m_lateralFrictionDir2 ); + + applyAnisotropicFriction( colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION ); + applyAnisotropicFriction( colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION ); + setupFrictionConstraint( *frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal ); + + if ( frictionConstraint2 ) + { + applyAnisotropicFriction( colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION ); + applyAnisotropicFriction( colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION ); + setupFrictionConstraint( *frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal ); + } + + if ( ( infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS ) && ( infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION ) ) + { + cp.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED; + } + } + } + else + { + setupFrictionConstraint( *frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM ); + if ( frictionConstraint2 ) + { + setupFrictionConstraint( *frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM ); + } + } + } + + setFrictionConstraintImpulse( contactConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal ); +} + + +struct SetupContactConstraintsLoop : public btIParallelForBody +{ + btSequentialImpulseConstraintSolverMt* m_solver; + const btBatchedConstraints* m_bc; + const btContactSolverInfo* m_infoGlobal; + + SetupContactConstraintsLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc, const btContactSolverInfo& infoGlobal ) + { + m_solver = solver; + m_bc = bc; + m_infoGlobal = &infoGlobal; + } + void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + BT_PROFILE( "SetupContactConstraintsLoop" ); + for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch ) + { + const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ]; + for (int i = batch.begin; i < batch.end; ++i) + { + int iContact = m_bc->m_constraintIndices[i]; + m_solver->internalSetupContactConstraints( iContact, *m_infoGlobal ); + } + } + } +}; + + +void btSequentialImpulseConstraintSolverMt::setupAllContactConstraints(const btContactSolverInfo& infoGlobal) +{ + BT_PROFILE( "setupAllContactConstraints" ); + if ( m_useBatching ) + { + const btBatchedConstraints& batchedCons = m_batchedContactConstraints; + SetupContactConstraintsLoop loop( this, &batchedCons, infoGlobal ); + for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase ) + { + int iPhase = batchedCons.m_phaseOrder[ iiPhase ]; + const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ]; + int grainSize = 1; + btParallelFor( phase.begin, phase.end, grainSize, loop ); + } + } + else + { + for ( int i = 0; i < m_tmpSolverContactConstraintPool.size(); ++i ) + { + internalSetupContactConstraints( i, infoGlobal ); + } + } +} + + +int btSequentialImpulseConstraintSolverMt::getOrInitSolverBodyThreadsafe(btCollisionObject& body,btScalar timeStep) +{ + // + // getOrInitSolverBody is threadsafe only for a single thread per solver (with potentially multiple solvers) + // + // getOrInitSolverBodyThreadsafe -- attempts to be fully threadsafe (however may affect determinism) + // + int solverBodyId = -1; + bool isRigidBodyType = btRigidBody::upcast( &body ) != NULL; + if ( isRigidBodyType && !body.isStaticOrKinematicObject() ) + { + // dynamic body + // Dynamic bodies can only be in one island, so it's safe to write to the companionId + solverBodyId = body.getCompanionId(); + if ( solverBodyId < 0 ) + { + m_bodySolverArrayMutex.lock(); + // now that we have the lock, check again + solverBodyId = body.getCompanionId(); + if ( solverBodyId < 0 ) + { + solverBodyId = m_tmpSolverBodyPool.size(); + btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); + initSolverBody( &solverBody, &body, timeStep ); + body.setCompanionId( solverBodyId ); + } + m_bodySolverArrayMutex.unlock(); + } + } + else if (isRigidBodyType && body.isKinematicObject()) + { + // + // NOTE: must test for kinematic before static because some kinematic objects also + // identify as "static" + // + // Kinematic bodies can be in multiple islands at once, so it is a + // race condition to write to them, so we use an alternate method + // to record the solverBodyId + int uniqueId = body.getWorldArrayIndex(); + const int INVALID_SOLVER_BODY_ID = -1; + if (m_kinematicBodyUniqueIdToSolverBodyTable.size() <= uniqueId ) + { + m_kinematicBodyUniqueIdToSolverBodyTableMutex.lock(); + // now that we have the lock, check again + if ( m_kinematicBodyUniqueIdToSolverBodyTable.size() <= uniqueId ) + { + m_kinematicBodyUniqueIdToSolverBodyTable.resize( uniqueId + 1, INVALID_SOLVER_BODY_ID ); + } + m_kinematicBodyUniqueIdToSolverBodyTableMutex.unlock(); + } + solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[ uniqueId ]; + // if no table entry yet, + if ( INVALID_SOLVER_BODY_ID == solverBodyId ) + { + // need to acquire both locks + m_kinematicBodyUniqueIdToSolverBodyTableMutex.lock(); + m_bodySolverArrayMutex.lock(); + // now that we have the lock, check again + solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[ uniqueId ]; + if ( INVALID_SOLVER_BODY_ID == solverBodyId ) + { + // create a table entry for this body + solverBodyId = m_tmpSolverBodyPool.size(); + btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); + initSolverBody( &solverBody, &body, timeStep ); + m_kinematicBodyUniqueIdToSolverBodyTable[ uniqueId ] = solverBodyId; + } + m_bodySolverArrayMutex.unlock(); + m_kinematicBodyUniqueIdToSolverBodyTableMutex.unlock(); + } + } + else + { + // all fixed bodies (inf mass) get mapped to a single solver id + if ( m_fixedBodyId < 0 ) + { + m_bodySolverArrayMutex.lock(); + // now that we have the lock, check again + if ( m_fixedBodyId < 0 ) + { + m_fixedBodyId = m_tmpSolverBodyPool.size(); + btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); + initSolverBody( &fixedBody, 0, timeStep ); + } + m_bodySolverArrayMutex.unlock(); + } + solverBodyId = m_fixedBodyId; + } + btAssert( solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size() ); + return solverBodyId; +} + + +void btSequentialImpulseConstraintSolverMt::internalCollectContactManifoldCachedInfo(btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) +{ + BT_PROFILE("internalCollectContactManifoldCachedInfo"); + for (int i = 0; i < numManifolds; ++i) + { + btContactManifoldCachedInfo* cachedInfo = &cachedInfoArray[i]; + btPersistentManifold* manifold = manifoldPtr[i]; + btCollisionObject* colObj0 = (btCollisionObject*) manifold->getBody0(); + btCollisionObject* colObj1 = (btCollisionObject*) manifold->getBody1(); + + int solverBodyIdA = getOrInitSolverBodyThreadsafe( *colObj0, infoGlobal.m_timeStep ); + int solverBodyIdB = getOrInitSolverBodyThreadsafe( *colObj1, infoGlobal.m_timeStep ); + + cachedInfo->solverBodyIds[ 0 ] = solverBodyIdA; + cachedInfo->solverBodyIds[ 1 ] = solverBodyIdB; + cachedInfo->numTouchingContacts = 0; + + btSolverBody* solverBodyA = &m_tmpSolverBodyPool[ solverBodyIdA ]; + btSolverBody* solverBodyB = &m_tmpSolverBodyPool[ solverBodyIdB ]; + + // A contact manifold between 2 static object should not exist! + // check the collision flags of your objects if this assert fires. + // Incorrectly set collision object flags can degrade performance in various ways. + btAssert( !m_tmpSolverBodyPool[ solverBodyIdA ].m_invMass.isZero() || !m_tmpSolverBodyPool[ solverBodyIdB ].m_invMass.isZero() ); + + int iContact = 0; + for ( int j = 0; j < manifold->getNumContacts(); j++ ) + { + btManifoldPoint& cp = manifold->getContactPoint( j ); + + if ( cp.getDistance() <= manifold->getContactProcessingThreshold() ) + { + cachedInfo->contactPoints[ iContact ] = &cp; + cachedInfo->contactHasRollingFriction[ iContact ] = ( cp.m_combinedRollingFriction > 0.f ); + iContact++; + } + } + cachedInfo->numTouchingContacts = iContact; + } +} + + +struct CollectContactManifoldCachedInfoLoop : public btIParallelForBody +{ + btSequentialImpulseConstraintSolverMt* m_solver; + btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* m_cachedInfoArray; + btPersistentManifold** m_manifoldPtr; + const btContactSolverInfo* m_infoGlobal; + + CollectContactManifoldCachedInfoLoop( btSequentialImpulseConstraintSolverMt* solver, btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifoldPtr, const btContactSolverInfo& infoGlobal ) + { + m_solver = solver; + m_cachedInfoArray = cachedInfoArray; + m_manifoldPtr = manifoldPtr; + m_infoGlobal = &infoGlobal; + } + void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + m_solver->internalCollectContactManifoldCachedInfo( m_cachedInfoArray + iBegin, m_manifoldPtr + iBegin, iEnd - iBegin, *m_infoGlobal ); + } +}; + + +void btSequentialImpulseConstraintSolverMt::internalAllocContactConstraints(const btContactManifoldCachedInfo* cachedInfoArray, int numManifolds) +{ + BT_PROFILE("internalAllocContactConstraints"); + // possibly parallel part + for ( int iManifold = 0; iManifold < numManifolds; ++iManifold ) + { + const btContactManifoldCachedInfo& cachedInfo = cachedInfoArray[ iManifold ]; + int contactIndex = cachedInfo.contactIndex; + int frictionIndex = contactIndex * m_numFrictionDirections; + int rollingFrictionIndex = cachedInfo.rollingFrictionIndex; + for ( int i = 0; i < cachedInfo.numTouchingContacts; i++ ) + { + btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[contactIndex]; + contactConstraint.m_solverBodyIdA = cachedInfo.solverBodyIds[ 0 ]; + contactConstraint.m_solverBodyIdB = cachedInfo.solverBodyIds[ 1 ]; + contactConstraint.m_originalContactPoint = cachedInfo.contactPoints[ i ]; + + // allocate the friction constraints + contactConstraint.m_frictionIndex = frictionIndex; + for ( int iDir = 0; iDir < m_numFrictionDirections; ++iDir ) + { + btSolverConstraint& frictionConstraint = m_tmpSolverContactFrictionConstraintPool[frictionIndex]; + frictionConstraint.m_frictionIndex = contactIndex; + frictionIndex++; + } + + // allocate rolling friction constraints + if ( cachedInfo.contactHasRollingFriction[ i ] ) + { + m_rollingFrictionIndexTable[ contactIndex ] = rollingFrictionIndex; + // allocate 3 (although we may use only 2 sometimes) + for ( int i = 0; i < 3; i++ ) + { + m_tmpSolverContactRollingFrictionConstraintPool[ rollingFrictionIndex ].m_frictionIndex = contactIndex; + rollingFrictionIndex++; + } + } + else + { + // indicate there is no rolling friction for this contact point + m_rollingFrictionIndexTable[ contactIndex ] = -1; + } + contactIndex++; + } + } +} + + +struct AllocContactConstraintsLoop : public btIParallelForBody +{ + btSequentialImpulseConstraintSolverMt* m_solver; + const btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* m_cachedInfoArray; + + AllocContactConstraintsLoop( btSequentialImpulseConstraintSolverMt* solver, btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* cachedInfoArray ) + { + m_solver = solver; + m_cachedInfoArray = cachedInfoArray; + } + void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + m_solver->internalAllocContactConstraints( m_cachedInfoArray + iBegin, iEnd - iBegin ); + } +}; + + +void btSequentialImpulseConstraintSolverMt::allocAllContactConstraints(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) +{ + BT_PROFILE( "allocAllContactConstraints" ); + btAlignedObjectArray<btContactManifoldCachedInfo> cachedInfoArray; // = m_manifoldCachedInfoArray; + cachedInfoArray.resizeNoInitialize( numManifolds ); + if (/* DISABLES CODE */ (false)) + { + // sequential + internalCollectContactManifoldCachedInfo(&cachedInfoArray[ 0 ], manifoldPtr, numManifolds, infoGlobal); + } + else + { + // may alter ordering of bodies which affects determinism + CollectContactManifoldCachedInfoLoop loop( this, &cachedInfoArray[ 0 ], manifoldPtr, infoGlobal ); + int grainSize = 200; + btParallelFor( 0, numManifolds, grainSize, loop ); + } + + { + // serial part + int numContacts = 0; + int numRollingFrictionConstraints = 0; + for ( int iManifold = 0; iManifold < numManifolds; ++iManifold ) + { + btContactManifoldCachedInfo& cachedInfo = cachedInfoArray[ iManifold ]; + cachedInfo.contactIndex = numContacts; + cachedInfo.rollingFrictionIndex = numRollingFrictionConstraints; + numContacts += cachedInfo.numTouchingContacts; + for (int i = 0; i < cachedInfo.numTouchingContacts; ++i) + { + if (cachedInfo.contactHasRollingFriction[i]) + { + numRollingFrictionConstraints += 3; + } + } + } + { + BT_PROFILE( "allocPools" ); + if ( m_tmpSolverContactConstraintPool.capacity() < numContacts ) + { + // if we need to reallocate, reserve some extra so we don't have to reallocate again next frame + int extraReserve = numContacts / 16; + m_tmpSolverContactConstraintPool.reserve( numContacts + extraReserve ); + m_rollingFrictionIndexTable.reserve( numContacts + extraReserve ); + m_tmpSolverContactFrictionConstraintPool.reserve( ( numContacts + extraReserve )*m_numFrictionDirections ); + m_tmpSolverContactRollingFrictionConstraintPool.reserve( numRollingFrictionConstraints + extraReserve ); + } + m_tmpSolverContactConstraintPool.resizeNoInitialize( numContacts ); + m_rollingFrictionIndexTable.resizeNoInitialize( numContacts ); + m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize( numContacts*m_numFrictionDirections ); + m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize( numRollingFrictionConstraints ); + } + } + { + AllocContactConstraintsLoop loop(this, &cachedInfoArray[0]); + int grainSize = 200; + btParallelFor( 0, numManifolds, grainSize, loop ); + } +} + + +void btSequentialImpulseConstraintSolverMt::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) +{ + if (!m_useBatching) + { + btSequentialImpulseConstraintSolver::convertContacts(manifoldPtr, numManifolds, infoGlobal); + return; + } + BT_PROFILE( "convertContacts" ); + if (numManifolds > 0) + { + if ( m_fixedBodyId < 0 ) + { + m_fixedBodyId = m_tmpSolverBodyPool.size(); + btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); + initSolverBody( &fixedBody, 0, infoGlobal.m_timeStep ); + } + allocAllContactConstraints( manifoldPtr, numManifolds, infoGlobal ); + if ( m_useBatching ) + { + setupBatchedContactConstraints(); + } + setupAllContactConstraints( infoGlobal ); + } +} + + +void btSequentialImpulseConstraintSolverMt::internalInitMultipleJoints( btTypedConstraint** constraints, int iBegin, int iEnd ) +{ + BT_PROFILE("internalInitMultipleJoints"); + for ( int i = iBegin; i < iEnd; i++ ) + { + btTypedConstraint* constraint = constraints[i]; + btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; + if (constraint->isEnabled()) + { + constraint->buildJacobian(); + constraint->internalSetAppliedImpulse( 0.0f ); + btJointFeedback* fb = constraint->getJointFeedback(); + if ( fb ) + { + fb->m_appliedForceBodyA.setZero(); + fb->m_appliedTorqueBodyA.setZero(); + fb->m_appliedForceBodyB.setZero(); + fb->m_appliedTorqueBodyB.setZero(); + } + constraint->getInfo1( &info1 ); + } + else + { + info1.m_numConstraintRows = 0; + info1.nub = 0; + } + } +} + + +struct InitJointsLoop : public btIParallelForBody +{ + btSequentialImpulseConstraintSolverMt* m_solver; + btTypedConstraint** m_constraints; + + InitJointsLoop( btSequentialImpulseConstraintSolverMt* solver, btTypedConstraint** constraints ) + { + m_solver = solver; + m_constraints = constraints; + } + void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + m_solver->internalInitMultipleJoints( m_constraints, iBegin, iEnd ); + } +}; + + +void btSequentialImpulseConstraintSolverMt::internalConvertMultipleJoints( const btAlignedObjectArray<JointParams>& jointParamsArray, btTypedConstraint** constraints, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal ) +{ + BT_PROFILE("internalConvertMultipleJoints"); + for ( int i = iBegin; i < iEnd; ++i ) + { + const JointParams& jointParams = jointParamsArray[ i ]; + int currentRow = jointParams.m_solverConstraint; + if ( currentRow != -1 ) + { + const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[ i ]; + btAssert( currentRow < m_tmpSolverNonContactConstraintPool.size() ); + btAssert( info1.m_numConstraintRows > 0 ); + + btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[ currentRow ]; + btTypedConstraint* constraint = constraints[ i ]; + + convertJoint( currentConstraintRow, constraint, info1, jointParams.m_solverBodyA, jointParams.m_solverBodyB, infoGlobal ); + } + } +} + + +struct ConvertJointsLoop : public btIParallelForBody +{ + btSequentialImpulseConstraintSolverMt* m_solver; + const btAlignedObjectArray<btSequentialImpulseConstraintSolverMt::JointParams>& m_jointParamsArray; + btTypedConstraint** m_srcConstraints; + const btContactSolverInfo& m_infoGlobal; + + ConvertJointsLoop( btSequentialImpulseConstraintSolverMt* solver, + const btAlignedObjectArray<btSequentialImpulseConstraintSolverMt::JointParams>& jointParamsArray, + btTypedConstraint** srcConstraints, + const btContactSolverInfo& infoGlobal + ) : + m_jointParamsArray(jointParamsArray), + m_infoGlobal(infoGlobal) + { + m_solver = solver; + m_srcConstraints = srcConstraints; + } + void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + m_solver->internalConvertMultipleJoints( m_jointParamsArray, m_srcConstraints, iBegin, iEnd, m_infoGlobal ); + } +}; + + +void btSequentialImpulseConstraintSolverMt::convertJoints(btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal) +{ + if ( !m_useBatching ) + { + btSequentialImpulseConstraintSolver::convertJoints(constraints, numConstraints, infoGlobal); + return; + } + BT_PROFILE("convertJoints"); + bool parallelJointSetup = true; + m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints); + if (parallelJointSetup) + { + InitJointsLoop loop(this, constraints); + int grainSize = 40; + btParallelFor(0, numConstraints, grainSize, loop); + } + else + { + internalInitMultipleJoints( constraints, 0, numConstraints ); + } + + int totalNumRows = 0; + btAlignedObjectArray<JointParams> jointParamsArray; + jointParamsArray.resizeNoInitialize(numConstraints); + + //calculate the total number of contraint rows + for (int i=0;i<numConstraints;i++) + { + btTypedConstraint* constraint = constraints[ i ]; + + JointParams& params = jointParamsArray[ i ]; + const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; + + if (info1.m_numConstraintRows) + { + params.m_solverConstraint = totalNumRows; + params.m_solverBodyA = getOrInitSolverBody( constraint->getRigidBodyA(), infoGlobal.m_timeStep ); + params.m_solverBodyB = getOrInitSolverBody( constraint->getRigidBodyB(), infoGlobal.m_timeStep ); + } + else + { + params.m_solverConstraint = -1; + } + totalNumRows += info1.m_numConstraintRows; + } + m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows); + + ///setup the btSolverConstraints + if ( parallelJointSetup ) + { + ConvertJointsLoop loop(this, jointParamsArray, constraints, infoGlobal); + int grainSize = 20; + btParallelFor(0, numConstraints, grainSize, loop); + } + else + { + internalConvertMultipleJoints( jointParamsArray, constraints, 0, numConstraints, infoGlobal ); + } + setupBatchedJointConstraints(); +} + + +void btSequentialImpulseConstraintSolverMt::internalConvertBodies(btCollisionObject** bodies, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) +{ + BT_PROFILE("internalConvertBodies"); + for (int i=iBegin; i < iEnd; i++) + { + btCollisionObject* obj = bodies[i]; + obj->setCompanionId(i); + btSolverBody& solverBody = m_tmpSolverBodyPool[i]; + initSolverBody(&solverBody, obj, infoGlobal.m_timeStep); + + btRigidBody* body = btRigidBody::upcast(obj); + if (body && body->getInvMass()) + { + btVector3 gyroForce (0,0,0); + if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT) + { + gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce); + solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep; + } + if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD) + { + gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep); + solverBody.m_externalTorqueImpulse += gyroForce; + } + if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY) + { + gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep); + solverBody.m_externalTorqueImpulse += gyroForce; + } + } + } +} + + +struct ConvertBodiesLoop : public btIParallelForBody +{ + btSequentialImpulseConstraintSolverMt* m_solver; + btCollisionObject** m_bodies; + int m_numBodies; + const btContactSolverInfo& m_infoGlobal; + + ConvertBodiesLoop( btSequentialImpulseConstraintSolverMt* solver, + btCollisionObject** bodies, + int numBodies, + const btContactSolverInfo& infoGlobal + ) : + m_infoGlobal(infoGlobal) + { + m_solver = solver; + m_bodies = bodies; + m_numBodies = numBodies; + } + void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + m_solver->internalConvertBodies( m_bodies, iBegin, iEnd, m_infoGlobal ); + } +}; + + +void btSequentialImpulseConstraintSolverMt::convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) +{ + BT_PROFILE("convertBodies"); + m_kinematicBodyUniqueIdToSolverBodyTable.resize( 0 ); + + m_tmpSolverBodyPool.resizeNoInitialize(numBodies+1); + + m_fixedBodyId = numBodies; + { + btSolverBody& fixedBody = m_tmpSolverBodyPool[ m_fixedBodyId ]; + initSolverBody( &fixedBody, NULL, infoGlobal.m_timeStep ); + } + + bool parallelBodySetup = true; + if (parallelBodySetup) + { + ConvertBodiesLoop loop(this, bodies, numBodies, infoGlobal); + int grainSize = 40; + btParallelFor(0, numBodies, grainSize, loop); + } + else + { + internalConvertBodies( bodies, 0, numBodies, infoGlobal ); + } +} + + +btScalar btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlySetup( + btCollisionObject** bodies, + int numBodies, + btPersistentManifold** manifoldPtr, + int numManifolds, + btTypedConstraint** constraints, + int numConstraints, + const btContactSolverInfo& infoGlobal, + btIDebugDraw* debugDrawer + ) +{ + m_numFrictionDirections = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1; + m_useBatching = false; + if ( numManifolds >= s_minimumContactManifoldsForBatching && + (s_allowNestedParallelForLoops || !btThreadsAreRunning()) + ) + { + m_useBatching = true; + m_batchedContactConstraints.m_debugDrawer = debugDrawer; + m_batchedJointConstraints.m_debugDrawer = debugDrawer; + } + btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies, + numBodies, + manifoldPtr, + numManifolds, + constraints, + numConstraints, + infoGlobal, + debugDrawer + ); + return 0.0f; +} + + +btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactSplitPenetrationImpulseConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd ) +{ + btScalar leastSquaresResidual = 0.f; + for ( int iiCons = batchBegin; iiCons < batchEnd; ++iiCons ) + { + int iCons = consIndices[ iiCons ]; + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[ iCons ]; + btSolverBody& bodyA = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdA ]; + btSolverBody& bodyB = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdB ]; + btScalar residual = resolveSplitPenetrationImpulse( bodyA, bodyB, solveManifold ); + leastSquaresResidual += residual*residual; + } + return leastSquaresResidual; +} + + +struct ContactSplitPenetrationImpulseSolverLoop : public btIParallelSumBody +{ + btSequentialImpulseConstraintSolverMt* m_solver; + const btBatchedConstraints* m_bc; + + ContactSplitPenetrationImpulseSolverLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc ) + { + m_solver = solver; + m_bc = bc; + } + btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + BT_PROFILE( "ContactSplitPenetrationImpulseSolverLoop" ); + btScalar sum = 0; + for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch ) + { + const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ]; + sum += m_solver->resolveMultipleContactSplitPenetrationImpulseConstraints( m_bc->m_constraintIndices, batch.begin, batch.end ); + } + return sum; + } +}; + + +void btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +{ + BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations"); + if (infoGlobal.m_splitImpulse) + { + for ( int iteration = 0; iteration < infoGlobal.m_numIterations; iteration++ ) + { + btScalar leastSquaresResidual = 0.f; + if (m_useBatching) + { + const btBatchedConstraints& batchedCons = m_batchedContactConstraints; + ContactSplitPenetrationImpulseSolverLoop loop( this, &batchedCons ); + btScalar leastSquaresResidual = 0.f; + for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase ) + { + int iPhase = batchedCons.m_phaseOrder[ iiPhase ]; + const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ]; + int grainSize = batchedCons.m_phaseGrainSize[iPhase]; + leastSquaresResidual += btParallelSum( phase.begin, phase.end, grainSize, loop ); + } + } + else + { + // non-batched + leastSquaresResidual = resolveMultipleContactSplitPenetrationImpulseConstraints(m_orderTmpConstraintPool, 0, m_tmpSolverContactConstraintPool.size()); + } + if ( leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= ( infoGlobal.m_numIterations - 1 ) ) + { +#ifdef VERBOSE_RESIDUAL_PRINTF + printf( "residual = %f at iteration #%d\n", leastSquaresResidual, iteration ); +#endif + break; + } + } + } +} + + +btScalar btSequentialImpulseConstraintSolverMt::solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +{ + if ( !m_useBatching ) + { + return btSequentialImpulseConstraintSolver::solveSingleIteration( iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer ); + } + BT_PROFILE( "solveSingleIterationMt" ); + btScalar leastSquaresResidual = 0.f; + + if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) + { + if (1) // uncomment this for a bit less random ((iteration & 7) == 0) + { + randomizeConstraintOrdering(iteration, infoGlobal.m_numIterations); + } + } + + { + ///solve all joint constraints + leastSquaresResidual += resolveAllJointConstraints(iteration); + + if (iteration< infoGlobal.m_numIterations) + { + // this loop is only used for cone-twist constraints, + // it would be nice to skip this loop if none of the constraints need it + if ( m_useObsoleteJointConstraints ) + { + for ( int j = 0; j<numConstraints; j++ ) + { + if ( constraints[ j ]->isEnabled() ) + { + int bodyAid = getOrInitSolverBody( constraints[ j ]->getRigidBodyA(), infoGlobal.m_timeStep ); + int bodyBid = getOrInitSolverBody( constraints[ j ]->getRigidBodyB(), infoGlobal.m_timeStep ); + btSolverBody& bodyA = m_tmpSolverBodyPool[ bodyAid ]; + btSolverBody& bodyB = m_tmpSolverBodyPool[ bodyBid ]; + constraints[ j ]->solveConstraintObsolete( bodyA, bodyB, infoGlobal.m_timeStep ); + } + } + } + + if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) + { + // solve all contact, contact-friction, and rolling friction constraints interleaved + leastSquaresResidual += resolveAllContactConstraintsInterleaved(); + } + else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS + { + // don't interleave them + // solve all contact constraints + leastSquaresResidual += resolveAllContactConstraints(); + + // solve all contact friction constraints + leastSquaresResidual += resolveAllContactFrictionConstraints(); + + // solve all rolling friction constraints + leastSquaresResidual += resolveAllRollingFrictionConstraints(); + } + } + } + return leastSquaresResidual; +} + + +btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleJointConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd, int iteration ) +{ + btScalar leastSquaresResidual = 0.f; + for ( int iiCons = batchBegin; iiCons < batchEnd; ++iiCons ) + { + int iCons = consIndices[ iiCons ]; + const btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[ iCons ]; + if ( iteration < constraint.m_overrideNumSolverIterations ) + { + btSolverBody& bodyA = m_tmpSolverBodyPool[ constraint.m_solverBodyIdA ]; + btSolverBody& bodyB = m_tmpSolverBodyPool[ constraint.m_solverBodyIdB ]; + btScalar residual = resolveSingleConstraintRowGeneric( bodyA, bodyB, constraint ); + leastSquaresResidual += residual*residual; + } + } + return leastSquaresResidual; +} + + +btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd ) +{ + btScalar leastSquaresResidual = 0.f; + for ( int iiCons = batchBegin; iiCons < batchEnd; ++iiCons ) + { + int iCons = consIndices[ iiCons ]; + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[ iCons ]; + btSolverBody& bodyA = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdA ]; + btSolverBody& bodyB = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdB ]; + btScalar residual = resolveSingleConstraintRowLowerLimit( bodyA, bodyB, solveManifold ); + leastSquaresResidual += residual*residual; + } + return leastSquaresResidual; +} + + +btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactFrictionConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd ) +{ + btScalar leastSquaresResidual = 0.f; + for ( int iiCons = batchBegin; iiCons < batchEnd; ++iiCons ) + { + int iContact = consIndices[ iiCons ]; + btScalar totalImpulse = m_tmpSolverContactConstraintPool[ iContact ].m_appliedImpulse; + + // apply sliding friction + if ( totalImpulse > 0.0f ) + { + int iBegin = iContact * m_numFrictionDirections; + int iEnd = iBegin + m_numFrictionDirections; + for ( int iFriction = iBegin; iFriction < iEnd; ++iFriction ) + { + btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[ iFriction++ ]; + btAssert( solveManifold.m_frictionIndex == iContact ); + + solveManifold.m_lowerLimit = -( solveManifold.m_friction*totalImpulse ); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + + btSolverBody& bodyA = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdA ]; + btSolverBody& bodyB = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdB ]; + btScalar residual = resolveSingleConstraintRowGeneric( bodyA, bodyB, solveManifold ); + leastSquaresResidual += residual*residual; + } + } + } + return leastSquaresResidual; +} + + +btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactRollingFrictionConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd ) +{ + btScalar leastSquaresResidual = 0.f; + for ( int iiCons = batchBegin; iiCons < batchEnd; ++iiCons ) + { + int iContact = consIndices[ iiCons ]; + int iFirstRollingFriction = m_rollingFrictionIndexTable[ iContact ]; + if ( iFirstRollingFriction >= 0 ) + { + btScalar totalImpulse = m_tmpSolverContactConstraintPool[ iContact ].m_appliedImpulse; + // apply rolling friction + if ( totalImpulse > 0.0f ) + { + int iBegin = iFirstRollingFriction; + int iEnd = iBegin + 3; + for ( int iRollingFric = iBegin; iRollingFric < iEnd; ++iRollingFric ) + { + btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[ iRollingFric ]; + if ( rollingFrictionConstraint.m_frictionIndex != iContact ) + { + break; + } + btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; + if ( rollingFrictionMagnitude > rollingFrictionConstraint.m_friction ) + { + rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; + } + + rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; + rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; + + btScalar residual = resolveSingleConstraintRowGeneric( m_tmpSolverBodyPool[ rollingFrictionConstraint.m_solverBodyIdA ], m_tmpSolverBodyPool[ rollingFrictionConstraint.m_solverBodyIdB ], rollingFrictionConstraint ); + leastSquaresResidual += residual*residual; + } + } + } + } + return leastSquaresResidual; +} + + +btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactConstraintsInterleaved( const btAlignedObjectArray<int>& contactIndices, + int batchBegin, + int batchEnd + ) +{ + btScalar leastSquaresResidual = 0.f; + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + + for ( int iiCons = batchBegin; iiCons < batchEnd; iiCons++ ) + { + btScalar totalImpulse = 0; + int iContact = contactIndices[ iiCons ]; + // apply penetration constraint + { + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[ iContact ]; + btScalar residual = resolveSingleConstraintRowLowerLimit( m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdA ], m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdB ], solveManifold ); + leastSquaresResidual += residual*residual; + totalImpulse = solveManifold.m_appliedImpulse; + } + + // apply sliding friction + if ( totalImpulse > 0.0f ) + { + int iBegin = iContact * m_numFrictionDirections; + int iEnd = iBegin + m_numFrictionDirections; + for ( int iFriction = iBegin; iFriction < iEnd; ++iFriction ) + { + btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[ iFriction ]; + btAssert( solveManifold.m_frictionIndex == iContact ); + + solveManifold.m_lowerLimit = -( solveManifold.m_friction*totalImpulse ); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + + btSolverBody& bodyA = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdA ]; + btSolverBody& bodyB = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdB ]; + btScalar residual = resolveSingleConstraintRowGeneric( bodyA, bodyB, solveManifold ); + leastSquaresResidual += residual*residual; + } + } + + // apply rolling friction + int iFirstRollingFriction = m_rollingFrictionIndexTable[ iContact ]; + if ( totalImpulse > 0.0f && iFirstRollingFriction >= 0) + { + int iBegin = iFirstRollingFriction; + int iEnd = iBegin + 3; + for ( int iRollingFric = iBegin; iRollingFric < iEnd; ++iRollingFric ) + { + btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[ iRollingFric ]; + if ( rollingFrictionConstraint.m_frictionIndex != iContact ) + { + break; + } + btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; + if ( rollingFrictionMagnitude > rollingFrictionConstraint.m_friction ) + { + rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; + } + + rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; + rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; + + btScalar residual = resolveSingleConstraintRowGeneric( m_tmpSolverBodyPool[ rollingFrictionConstraint.m_solverBodyIdA ], m_tmpSolverBodyPool[ rollingFrictionConstraint.m_solverBodyIdB ], rollingFrictionConstraint ); + leastSquaresResidual += residual*residual; + } + } + } + return leastSquaresResidual; +} + + +void btSequentialImpulseConstraintSolverMt::randomizeBatchedConstraintOrdering( btBatchedConstraints* batchedConstraints ) +{ + btBatchedConstraints& bc = *batchedConstraints; + // randomize ordering of phases + for ( int ii = 1; ii < bc.m_phaseOrder.size(); ++ii ) + { + int iSwap = btRandInt2( ii + 1 ); + bc.m_phaseOrder.swap( ii, iSwap ); + } + + // for each batch, + for ( int iBatch = 0; iBatch < bc.m_batches.size(); ++iBatch ) + { + // randomize ordering of constraints within the batch + const btBatchedConstraints::Range& batch = bc.m_batches[ iBatch ]; + for ( int iiCons = batch.begin; iiCons < batch.end; ++iiCons ) + { + int iSwap = batch.begin + btRandInt2( iiCons - batch.begin + 1 ); + btAssert(iSwap >= batch.begin && iSwap < batch.end); + bc.m_constraintIndices.swap( iiCons, iSwap ); + } + } +} + + +void btSequentialImpulseConstraintSolverMt::randomizeConstraintOrdering(int iteration, int numIterations) +{ + // randomize ordering of joint constraints + randomizeBatchedConstraintOrdering( &m_batchedJointConstraints ); + + //contact/friction constraints are not solved more than numIterations + if ( iteration < numIterations ) + { + randomizeBatchedConstraintOrdering( &m_batchedContactConstraints ); + } +} + + +struct JointSolverLoop : public btIParallelSumBody +{ + btSequentialImpulseConstraintSolverMt* m_solver; + const btBatchedConstraints* m_bc; + int m_iteration; + + JointSolverLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc, int iteration ) + { + m_solver = solver; + m_bc = bc; + m_iteration = iteration; + } + btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + BT_PROFILE( "JointSolverLoop" ); + btScalar sum = 0; + for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch ) + { + const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ]; + sum += m_solver->resolveMultipleJointConstraints( m_bc->m_constraintIndices, batch.begin, batch.end, m_iteration ); + } + return sum; + } +}; + + +btScalar btSequentialImpulseConstraintSolverMt::resolveAllJointConstraints(int iteration) +{ + BT_PROFILE( "resolveAllJointConstraints" ); + const btBatchedConstraints& batchedCons = m_batchedJointConstraints; + JointSolverLoop loop( this, &batchedCons, iteration ); + btScalar leastSquaresResidual = 0.f; + for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase ) + { + int iPhase = batchedCons.m_phaseOrder[ iiPhase ]; + const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ]; + int grainSize = 1; + leastSquaresResidual += btParallelSum( phase.begin, phase.end, grainSize, loop ); + } + return leastSquaresResidual; +} + + +struct ContactSolverLoop : public btIParallelSumBody +{ + btSequentialImpulseConstraintSolverMt* m_solver; + const btBatchedConstraints* m_bc; + + ContactSolverLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc ) + { + m_solver = solver; + m_bc = bc; + } + btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + BT_PROFILE( "ContactSolverLoop" ); + btScalar sum = 0; + for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch ) + { + const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ]; + sum += m_solver->resolveMultipleContactConstraints( m_bc->m_constraintIndices, batch.begin, batch.end ); + } + return sum; + } +}; + + +btScalar btSequentialImpulseConstraintSolverMt::resolveAllContactConstraints() +{ + BT_PROFILE( "resolveAllContactConstraints" ); + const btBatchedConstraints& batchedCons = m_batchedContactConstraints; + ContactSolverLoop loop( this, &batchedCons ); + btScalar leastSquaresResidual = 0.f; + for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase ) + { + int iPhase = batchedCons.m_phaseOrder[ iiPhase ]; + const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ]; + int grainSize = batchedCons.m_phaseGrainSize[iPhase]; + leastSquaresResidual += btParallelSum( phase.begin, phase.end, grainSize, loop ); + } + return leastSquaresResidual; +} + + +struct ContactFrictionSolverLoop : public btIParallelSumBody +{ + btSequentialImpulseConstraintSolverMt* m_solver; + const btBatchedConstraints* m_bc; + + ContactFrictionSolverLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc ) + { + m_solver = solver; + m_bc = bc; + } + btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + BT_PROFILE( "ContactFrictionSolverLoop" ); + btScalar sum = 0; + for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch ) + { + const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ]; + sum += m_solver->resolveMultipleContactFrictionConstraints( m_bc->m_constraintIndices, batch.begin, batch.end ); + } + return sum; + } +}; + + +btScalar btSequentialImpulseConstraintSolverMt::resolveAllContactFrictionConstraints() +{ + BT_PROFILE( "resolveAllContactFrictionConstraints" ); + const btBatchedConstraints& batchedCons = m_batchedContactConstraints; + ContactFrictionSolverLoop loop( this, &batchedCons ); + btScalar leastSquaresResidual = 0.f; + for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase ) + { + int iPhase = batchedCons.m_phaseOrder[ iiPhase ]; + const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ]; + int grainSize = batchedCons.m_phaseGrainSize[iPhase]; + leastSquaresResidual += btParallelSum( phase.begin, phase.end, grainSize, loop ); + } + return leastSquaresResidual; +} + + +struct InterleavedContactSolverLoop : public btIParallelSumBody +{ + btSequentialImpulseConstraintSolverMt* m_solver; + const btBatchedConstraints* m_bc; + + InterleavedContactSolverLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc ) + { + m_solver = solver; + m_bc = bc; + } + btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + BT_PROFILE( "InterleavedContactSolverLoop" ); + btScalar sum = 0; + for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch ) + { + const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ]; + sum += m_solver->resolveMultipleContactConstraintsInterleaved( m_bc->m_constraintIndices, batch.begin, batch.end ); + } + return sum; + } +}; + + +btScalar btSequentialImpulseConstraintSolverMt::resolveAllContactConstraintsInterleaved() +{ + BT_PROFILE( "resolveAllContactConstraintsInterleaved" ); + const btBatchedConstraints& batchedCons = m_batchedContactConstraints; + InterleavedContactSolverLoop loop( this, &batchedCons ); + btScalar leastSquaresResidual = 0.f; + for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase ) + { + int iPhase = batchedCons.m_phaseOrder[ iiPhase ]; + const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ]; + int grainSize = 1; + leastSquaresResidual += btParallelSum( phase.begin, phase.end, grainSize, loop ); + } + return leastSquaresResidual; +} + + +struct ContactRollingFrictionSolverLoop : public btIParallelSumBody +{ + btSequentialImpulseConstraintSolverMt* m_solver; + const btBatchedConstraints* m_bc; + + ContactRollingFrictionSolverLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc ) + { + m_solver = solver; + m_bc = bc; + } + btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + BT_PROFILE( "ContactFrictionSolverLoop" ); + btScalar sum = 0; + for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch ) + { + const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ]; + sum += m_solver->resolveMultipleContactRollingFrictionConstraints( m_bc->m_constraintIndices, batch.begin, batch.end ); + } + return sum; + } +}; + + +btScalar btSequentialImpulseConstraintSolverMt::resolveAllRollingFrictionConstraints() +{ + BT_PROFILE( "resolveAllRollingFrictionConstraints" ); + btScalar leastSquaresResidual = 0.f; + // + // We do not generate batches for rolling friction constraints. We assume that + // one of two cases is true: + // + // 1. either most bodies in the simulation have rolling friction, in which case we can use the + // batches for contacts and use a lookup table to translate contact indices to rolling friction + // (ignoring any contact indices that don't map to a rolling friction constraint). As long as + // most contacts have a corresponding rolling friction constraint, this should parallelize well. + // + // -OR- + // + // 2. few bodies in the simulation have rolling friction, so it is not worth trying to use the + // batches from contacts as most of the contacts won't have corresponding rolling friction + // constraints and most threads would end up doing very little work. Most of the time would + // go to threading overhead, so we don't bother with threading. + // + int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); + if (numRollingFrictionPoolConstraints >= m_tmpSolverContactConstraintPool.size()) + { + // use batching if there are many rolling friction constraints + const btBatchedConstraints& batchedCons = m_batchedContactConstraints; + ContactRollingFrictionSolverLoop loop( this, &batchedCons ); + btScalar leastSquaresResidual = 0.f; + for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase ) + { + int iPhase = batchedCons.m_phaseOrder[ iiPhase ]; + const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ]; + int grainSize = 1; + leastSquaresResidual += btParallelSum( phase.begin, phase.end, grainSize, loop ); + } + } + else + { + // no batching, also ignores SOLVER_RANDMIZE_ORDER + for ( int j = 0; j < numRollingFrictionPoolConstraints; j++ ) + { + btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[ j ]; + if ( rollingFrictionConstraint.m_frictionIndex >= 0 ) + { + btScalar totalImpulse = m_tmpSolverContactConstraintPool[ rollingFrictionConstraint.m_frictionIndex ].m_appliedImpulse; + if ( totalImpulse > 0.0f ) + { + btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; + if ( rollingFrictionMagnitude > rollingFrictionConstraint.m_friction ) + rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; + + rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; + rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; + + btScalar residual = resolveSingleConstraintRowGeneric( m_tmpSolverBodyPool[ rollingFrictionConstraint.m_solverBodyIdA ], m_tmpSolverBodyPool[ rollingFrictionConstraint.m_solverBodyIdB ], rollingFrictionConstraint ); + leastSquaresResidual += residual*residual; + } + } + } + } + return leastSquaresResidual; +} + + +void btSequentialImpulseConstraintSolverMt::internalWriteBackContacts( int iBegin, int iEnd, const btContactSolverInfo& infoGlobal ) +{ + BT_PROFILE("internalWriteBackContacts"); + writeBackContacts(iBegin, iEnd, infoGlobal); + //for ( int iContact = iBegin; iContact < iEnd; ++iContact) + //{ + // const btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[ iContact ]; + // btManifoldPoint* pt = (btManifoldPoint*) contactConstraint.m_originalContactPoint; + // btAssert( pt ); + // pt->m_appliedImpulse = contactConstraint.m_appliedImpulse; + // pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[ contactConstraint.m_frictionIndex ].m_appliedImpulse; + // if ( m_numFrictionDirections == 2 ) + // { + // pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[ contactConstraint.m_frictionIndex + 1 ].m_appliedImpulse; + // } + //} +} + + +void btSequentialImpulseConstraintSolverMt::internalWriteBackJoints( int iBegin, int iEnd, const btContactSolverInfo& infoGlobal ) +{ + BT_PROFILE("internalWriteBackJoints"); + writeBackJoints(iBegin, iEnd, infoGlobal); +} + + +void btSequentialImpulseConstraintSolverMt::internalWriteBackBodies( int iBegin, int iEnd, const btContactSolverInfo& infoGlobal ) +{ + BT_PROFILE("internalWriteBackBodies"); + writeBackBodies( iBegin, iEnd, infoGlobal ); +} + + +struct WriteContactPointsLoop : public btIParallelForBody +{ + btSequentialImpulseConstraintSolverMt* m_solver; + const btContactSolverInfo* m_infoGlobal; + + WriteContactPointsLoop( btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal ) + { + m_solver = solver; + m_infoGlobal = &infoGlobal; + } + void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + m_solver->internalWriteBackContacts( iBegin, iEnd, *m_infoGlobal ); + } +}; + + +struct WriteJointsLoop : public btIParallelForBody +{ + btSequentialImpulseConstraintSolverMt* m_solver; + const btContactSolverInfo* m_infoGlobal; + + WriteJointsLoop( btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal ) + { + m_solver = solver; + m_infoGlobal = &infoGlobal; + } + void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + m_solver->internalWriteBackJoints( iBegin, iEnd, *m_infoGlobal ); + } +}; + + +struct WriteBodiesLoop : public btIParallelForBody +{ + btSequentialImpulseConstraintSolverMt* m_solver; + const btContactSolverInfo* m_infoGlobal; + + WriteBodiesLoop( btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal ) + { + m_solver = solver; + m_infoGlobal = &infoGlobal; + } + void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + m_solver->internalWriteBackBodies( iBegin, iEnd, *m_infoGlobal ); + } +}; + + +btScalar btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) +{ + BT_PROFILE("solveGroupCacheFriendlyFinish"); + + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + WriteContactPointsLoop loop( this, infoGlobal ); + int grainSize = 500; + btParallelFor( 0, m_tmpSolverContactConstraintPool.size(), grainSize, loop ); + } + + { + WriteJointsLoop loop( this, infoGlobal ); + int grainSize = 400; + btParallelFor( 0, m_tmpSolverNonContactConstraintPool.size(), grainSize, loop ); + } + { + WriteBodiesLoop loop( this, infoGlobal ); + int grainSize = 100; + btParallelFor( 0, m_tmpSolverBodyPool.size(), grainSize, loop ); + } + + m_tmpSolverContactConstraintPool.resizeNoInitialize(0); + m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0); + m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0); + m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0); + + m_tmpSolverBodyPool.resizeNoInitialize(0); + return 0.f; +} + diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h new file mode 100644 index 0000000000..55d53474c4 --- /dev/null +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h @@ -0,0 +1,154 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_MT_H +#define BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_MT_H + +#include "btSequentialImpulseConstraintSolver.h" +#include "btBatchedConstraints.h" +#include "LinearMath/btThreads.h" + +/// +/// btSequentialImpulseConstraintSolverMt +/// +/// A multithreaded variant of the sequential impulse constraint solver. The constraints to be solved are grouped into +/// batches and phases where each batch of constraints within a given phase can be solved in parallel with the rest. +/// Ideally we want as few phases as possible, and each phase should have many batches, and all of the batches should +/// have about the same number of constraints. +/// This method works best on a large island of many constraints. +/// +/// Supports all of the features of the normal sequential impulse solver such as: +/// - split penetration impulse +/// - rolling friction +/// - interleaving constraints +/// - warmstarting +/// - 2 friction directions +/// - randomized constraint ordering +/// - early termination when leastSquaresResidualThreshold is satisfied +/// +/// When the SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS flag is enabled, unlike the normal SequentialImpulse solver, +/// the rolling friction is interleaved as well. +/// Interleaving the contact penetration constraints with friction reduces the number of parallel loops that need to be done, +/// which reduces threading overhead so it can be a performance win, however, it does seem to produce a less stable simulation, +/// at least on stacks of blocks. +/// +/// When the SOLVER_RANDMIZE_ORDER flag is enabled, the ordering of phases, and the ordering of constraints within each batch +/// is randomized, however it does not swap constraints between batches. +/// This is to avoid regenerating the batches for each solver iteration which would be quite costly in performance. +/// +/// Note that a non-zero leastSquaresResidualThreshold could possibly affect the determinism of the simulation +/// if the task scheduler's parallelSum operation is non-deterministic. The parallelSum operation can be non-deterministic +/// because floating point addition is not associative due to rounding errors. +/// The task scheduler can and should ensure that the result of any parallelSum operation is deterministic. +/// +ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolverMt : public btSequentialImpulseConstraintSolver +{ +public: + virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) BT_OVERRIDE; + virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) BT_OVERRIDE; + virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) BT_OVERRIDE; + virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) BT_OVERRIDE; + + // temp struct used to collect info from persistent manifolds into a cache-friendly struct using multiple threads + struct btContactManifoldCachedInfo + { + static const int MAX_NUM_CONTACT_POINTS = 4; + + int numTouchingContacts; + int solverBodyIds[ 2 ]; + int contactIndex; + int rollingFrictionIndex; + bool contactHasRollingFriction[ MAX_NUM_CONTACT_POINTS ]; + btManifoldPoint* contactPoints[ MAX_NUM_CONTACT_POINTS ]; + }; + // temp struct used for setting up joint constraints in parallel + struct JointParams + { + int m_solverConstraint; + int m_solverBodyA; + int m_solverBodyB; + }; + void internalInitMultipleJoints(btTypedConstraint** constraints, int iBegin, int iEnd); + void internalConvertMultipleJoints( const btAlignedObjectArray<JointParams>& jointParamsArray, btTypedConstraint** constraints, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal ); + + // parameters to control batching + static bool s_allowNestedParallelForLoops; // whether to allow nested parallel operations + static int s_minimumContactManifoldsForBatching; // don't even try to batch if fewer manifolds than this + static btBatchedConstraints::BatchingMethod s_contactBatchingMethod; + static btBatchedConstraints::BatchingMethod s_jointBatchingMethod; + static int s_minBatchSize; // desired number of constraints per batch + static int s_maxBatchSize; + +protected: + static const int CACHE_LINE_SIZE = 64; + + btBatchedConstraints m_batchedContactConstraints; + btBatchedConstraints m_batchedJointConstraints; + int m_numFrictionDirections; + bool m_useBatching; + bool m_useObsoleteJointConstraints; + btAlignedObjectArray<btContactManifoldCachedInfo> m_manifoldCachedInfoArray; + btAlignedObjectArray<int> m_rollingFrictionIndexTable; // lookup table mapping contact index to rolling friction index + btSpinMutex m_bodySolverArrayMutex; + char m_antiFalseSharingPadding[CACHE_LINE_SIZE]; // padding to keep mutexes in separate cachelines + btSpinMutex m_kinematicBodyUniqueIdToSolverBodyTableMutex; + btAlignedObjectArray<char> m_scratchMemory; + + virtual void randomizeConstraintOrdering( int iteration, int numIterations ); + virtual btScalar resolveAllJointConstraints( int iteration ); + virtual btScalar resolveAllContactConstraints(); + virtual btScalar resolveAllContactFrictionConstraints(); + virtual btScalar resolveAllContactConstraintsInterleaved(); + virtual btScalar resolveAllRollingFrictionConstraints(); + + virtual void setupBatchedContactConstraints(); + virtual void setupBatchedJointConstraints(); + virtual void convertJoints(btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal) BT_OVERRIDE; + virtual void convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) BT_OVERRIDE; + virtual void convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) BT_OVERRIDE; + + int getOrInitSolverBodyThreadsafe(btCollisionObject& body, btScalar timeStep); + void allocAllContactConstraints(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal); + void setupAllContactConstraints(const btContactSolverInfo& infoGlobal); + void randomizeBatchedConstraintOrdering( btBatchedConstraints* batchedConstraints ); + +public: + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btSequentialImpulseConstraintSolverMt(); + virtual ~btSequentialImpulseConstraintSolverMt(); + + btScalar resolveMultipleJointConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd, int iteration ); + btScalar resolveMultipleContactConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd ); + btScalar resolveMultipleContactSplitPenetrationImpulseConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd ); + btScalar resolveMultipleContactFrictionConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd ); + btScalar resolveMultipleContactRollingFrictionConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd ); + btScalar resolveMultipleContactConstraintsInterleaved( const btAlignedObjectArray<int>& contactIndices, int batchBegin, int batchEnd ); + + void internalCollectContactManifoldCachedInfo(btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal); + void internalAllocContactConstraints(const btContactManifoldCachedInfo* cachedInfoArray, int numManifolds); + void internalSetupContactConstraints(int iContactConstraint, const btContactSolverInfo& infoGlobal); + void internalConvertBodies(btCollisionObject** bodies, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); + void internalWriteBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); + void internalWriteBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); + void internalWriteBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); +}; + + + + +#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_MT_H + diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp index d63cef0316..d63cef0316 100644..100755 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h index 1957f08a96..1957f08a96 100644..100755 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h |