Update bullet to Master 12409f1118a7c7a266f9071350c70789dfe73bb9

39 files changed, 5423 insertions, 771 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 = &params;
+    }
+    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(&currentConstraintRow[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 = &currentConstraintRow->m_rhs;
+	currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
+	info2.m_damping = infoGlobal.m_damping;
+	info2.cfm = &currentConstraintRow->m_cfm;
+	info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
+	info2.m_upperLimit = &currentConstraintRow->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(&currentConstraintRow[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 = &currentConstraintRow->m_rhs;
-					currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
-					info2.m_damping = infoGlobal.m_damping;
-					info2.cfm = &currentConstraintRow->m_cfm;
-					info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
-					info2.m_upperLimit = &currentConstraintRow->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
diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
index a196d4522e..b9944c138b 100644
--- a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
+++ b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
@@ -842,6 +842,9 @@ public:
 
 		btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
 
+		if(!m_dispatcher->needsCollision(m_me, otherObj))
+			return false;
+
 		//call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179
 		if (m_dispatcher->needsResponse(m_me,otherObj))
 		{
@@ -1342,9 +1345,12 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 					btVector3 axis = tr.getBasis().getColumn(0);
 					btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
 					btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
-					btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
-					btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
-					getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0));
+					if (minTh <= maxTh)
+					{
+						btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
+						btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
+						getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0));
+					}
 					axis = tr.getBasis().getColumn(1);
 					btScalar ay = p6DOF->getAngle(1);
 					btScalar az = p6DOF->getAngle(2);
@@ -1533,6 +1539,8 @@ void	btDiscreteDynamicsWorld::serialize(btSerializer* serializer)
 
 	serializeRigidBodies(serializer);
 
+	serializeContactManifolds(serializer);
+
 	serializer->finishSerialization();
 }
 
diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp
index 1d10bad922..d705bf2381 100644
--- a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp
+++ b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp
@@ -50,63 +50,6 @@ subject to the following restrictions:
 #include "LinearMath/btSerializer.h"
 
 
-struct InplaceSolverIslandCallbackMt : public btSimulationIslandManagerMt::IslandCallback
-{
-	btContactSolverInfo*	m_solverInfo;
-	btConstraintSolver*		m_solver;
-	btIDebugDraw*			m_debugDrawer;
-	btDispatcher*			m_dispatcher;
-
-	InplaceSolverIslandCallbackMt(
-		btConstraintSolver*	solver,
-		btStackAlloc* stackAlloc,
-		btDispatcher* dispatcher)
-		:m_solverInfo(NULL),
-		m_solver(solver),
-		m_debugDrawer(NULL),
-		m_dispatcher(dispatcher)
-	{
-
-	}
-
-	InplaceSolverIslandCallbackMt& operator=(InplaceSolverIslandCallbackMt& other)
-	{
-		btAssert(0);
-		(void)other;
-		return *this;
-	}
-
-	SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btIDebugDraw* debugDrawer)
-	{
-		btAssert(solverInfo);
-		m_solverInfo = solverInfo;
-		m_debugDrawer = debugDrawer;
-	}
-
-
-	virtual	void	processIsland( btCollisionObject** bodies,
-                                   int numBodies,
-                                   btPersistentManifold** manifolds,
-                                   int numManifolds,
-                                   btTypedConstraint** constraints,
-                                   int numConstraints,
-                                   int islandId
-                                   )
-	{
-        m_solver->solveGroup( bodies,
-                              numBodies,
-                              manifolds,
-                              numManifolds,
-                              constraints,
-                              numConstraints,
-                              *m_solverInfo,
-                              m_debugDrawer,
-                              m_dispatcher
-                              );
-    }
-
-};
-
 
 ///
 /// btConstraintSolverPoolMt
@@ -209,7 +152,12 @@ void btConstraintSolverPoolMt::reset()
 /// btDiscreteDynamicsWorldMt
 ///
 
-btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolverPoolMt* constraintSolver, btCollisionConfiguration* collisionConfiguration)
+btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher,
+    btBroadphaseInterface* pairCache,
+    btConstraintSolverPoolMt* constraintSolver,
+    btConstraintSolver* constraintSolverMt,
+    btCollisionConfiguration* collisionConfiguration
+)
 : btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration)
 {
 	if (m_ownsIslandManager)
@@ -217,31 +165,18 @@ btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher, b
 		m_islandManager->~btSimulationIslandManager();
 		btAlignedFree( m_islandManager);
 	}
-    {
-		void* mem = btAlignedAlloc(sizeof(InplaceSolverIslandCallbackMt),16);
-		m_solverIslandCallbackMt = new (mem) InplaceSolverIslandCallbackMt (m_constraintSolver, 0, dispatcher);
-    }
 	{
 		void* mem = btAlignedAlloc(sizeof(btSimulationIslandManagerMt),16);
 		btSimulationIslandManagerMt* im = new (mem) btSimulationIslandManagerMt();
         im->setMinimumSolverBatchSize( m_solverInfo.m_minimumSolverBatchSize );
         m_islandManager = im;
 	}
+    m_constraintSolverMt = constraintSolverMt;
 }
 
 
 btDiscreteDynamicsWorldMt::~btDiscreteDynamicsWorldMt()
 {
-	if (m_solverIslandCallbackMt)
-	{
-		m_solverIslandCallbackMt->~InplaceSolverIslandCallbackMt();
-		btAlignedFree(m_solverIslandCallbackMt);
-	}
-	if (m_ownsConstraintSolver)
-	{
-		m_constraintSolver->~btConstraintSolver();
-		btAlignedFree(m_constraintSolver);
-	}
 }
 
 
@@ -249,12 +184,17 @@ void btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo
 {
 	BT_PROFILE("solveConstraints");
 
-	m_solverIslandCallbackMt->setup(&solverInfo, getDebugDrawer());
 	m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
 
 	/// solve all the constraints for this island
     btSimulationIslandManagerMt* im = static_cast<btSimulationIslandManagerMt*>(m_islandManager);
-    im->buildAndProcessIslands( getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_constraints, m_solverIslandCallbackMt );
+    btSimulationIslandManagerMt::SolverParams solverParams;
+    solverParams.m_solverPool = m_constraintSolver;
+    solverParams.m_solverMt = m_constraintSolverMt;
+    solverParams.m_solverInfo = &solverInfo;
+    solverParams.m_debugDrawer = m_debugDrawer;
+    solverParams.m_dispatcher = getCollisionWorld()->getDispatcher();
+    im->buildAndProcessIslands( getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_constraints, solverParams );
 
 	m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
 }
@@ -325,3 +265,14 @@ void btDiscreteDynamicsWorldMt::integrateTransforms( btScalar timeStep )
     }
 }
 
+
+int	btDiscreteDynamicsWorldMt::stepSimulation( btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep )
+{
+    int numSubSteps = btDiscreteDynamicsWorld::stepSimulation(timeStep, maxSubSteps, fixedTimeStep);
+    if (btITaskScheduler* scheduler = btGetTaskScheduler())
+    {
+        // tell Bullet's threads to sleep, so other threads can run
+        scheduler->sleepWorkerThreadsHint();
+    }
+    return numSubSteps;
+}
diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h
index 2f144cdda4..667fe5800e 100644
--- a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h
+++ b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h
@@ -21,7 +21,6 @@ subject to the following restrictions:
 #include "btSimulationIslandManagerMt.h"
 #include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
 
-struct InplaceSolverIslandCallbackMt;
 
 ///
 /// btConstraintSolverPoolMt - masquerades as a constraint solver, but really it is a threadsafe pool of them.
@@ -88,7 +87,7 @@ private:
 ATTRIBUTE_ALIGNED16(class) btDiscreteDynamicsWorldMt : public btDiscreteDynamicsWorld
 {
 protected:
-    InplaceSolverIslandCallbackMt* m_solverIslandCallbackMt;
+    btConstraintSolver* m_constraintSolverMt;
 
     virtual void solveConstraints(btContactSolverInfo& solverInfo) BT_OVERRIDE;
 
@@ -126,9 +125,12 @@ public:
 	btDiscreteDynamicsWorldMt(btDispatcher* dispatcher,
         btBroadphaseInterface* pairCache,
         btConstraintSolverPoolMt* constraintSolver,   // Note this should be a solver-pool for multi-threading
+        btConstraintSolver* constraintSolverMt,    // single multi-threaded solver for large islands (or NULL)
         btCollisionConfiguration* collisionConfiguration
     );
 	virtual ~btDiscreteDynamicsWorldMt();
+
+    virtual int	stepSimulation( btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep ) BT_OVERRIDE;
 };
 
 #endif //BT_DISCRETE_DYNAMICS_WORLD_H
diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp b/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp
index 99b34353c7..fc54f0ba6e 100644
--- a/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp
+++ b/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp
@@ -22,6 +22,7 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 #include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h"  // for s_minimumContactManifoldsForBatching
 
 //#include <stdio.h>
 #include "LinearMath/btQuickprof.h"
@@ -275,7 +276,7 @@ btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::allocateIsland
 void btSimulationIslandManagerMt::buildIslands( btDispatcher* dispatcher, btCollisionWorld* collisionWorld )
 {
 
-	BT_PROFILE("islandUnionFindAndQuickSort");
+	BT_PROFILE("buildIslands");
 	
 	btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
 
@@ -314,13 +315,11 @@ void btSimulationIslandManagerMt::buildIslands( btDispatcher* dispatcher, btColl
 			btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
 			if (colObj0->getIslandTag() == islandId)
 			{
-				if (colObj0->getActivationState()== ACTIVE_TAG)
-				{
-					allSleeping = false;
-				}
-				if (colObj0->getActivationState()== DISABLE_DEACTIVATION)
+				if (colObj0->getActivationState()== ACTIVE_TAG ||
+				   colObj0->getActivationState()== DISABLE_DEACTIVATION)
 				{
 					allSleeping = false;
+					break;
 				}
 			}
 		}
@@ -546,59 +545,103 @@ void btSimulationIslandManagerMt::mergeIslands()
 }
 
 
-void btSimulationIslandManagerMt::serialIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback )
+void btSimulationIslandManagerMt::solveIsland(btConstraintSolver* solver, Island& island, const SolverParams& solverParams)
+{
+    btPersistentManifold** manifolds = island.manifoldArray.size() ? &island.manifoldArray[ 0 ] : NULL;
+    btTypedConstraint** constraintsPtr = island.constraintArray.size() ? &island.constraintArray[ 0 ] : NULL;
+    solver->solveGroup( &island.bodyArray[ 0 ],
+        island.bodyArray.size(),
+        manifolds,
+        island.manifoldArray.size(),
+        constraintsPtr,
+        island.constraintArray.size(),
+        *solverParams.m_solverInfo,
+        solverParams.m_debugDrawer,
+        solverParams.m_dispatcher
+    );
+}
+
+
+void btSimulationIslandManagerMt::serialIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams )
 {
     BT_PROFILE( "serialIslandDispatch" );
     // serial dispatch
     btAlignedObjectArray<Island*>& islands = *islandsPtr;
+    btConstraintSolver* solver = solverParams.m_solverMt ? solverParams.m_solverMt : solverParams.m_solverPool;
     for ( int i = 0; i < islands.size(); ++i )
     {
-        Island* island = islands[ i ];
-        btPersistentManifold** manifolds = island->manifoldArray.size() ? &island->manifoldArray[ 0 ] : NULL;
-        btTypedConstraint** constraintsPtr = island->constraintArray.size() ? &island->constraintArray[ 0 ] : NULL;
-        callback->processIsland( &island->bodyArray[ 0 ],
-                                 island->bodyArray.size(),
-                                 manifolds,
-                                 island->manifoldArray.size(),
-                                 constraintsPtr,
-                                 island->constraintArray.size(),
-                                 island->id
-                                 );
+        solveIsland(solver, *islands[ i ], solverParams);
     }
 }
 
+
 struct UpdateIslandDispatcher : public btIParallelForBody
 {
-    btAlignedObjectArray<btSimulationIslandManagerMt::Island*>* islandsPtr;
-    btSimulationIslandManagerMt::IslandCallback* callback;
+    btAlignedObjectArray<btSimulationIslandManagerMt::Island*>& m_islandsPtr;
+    const btSimulationIslandManagerMt::SolverParams& m_solverParams;
+
+    UpdateIslandDispatcher(btAlignedObjectArray<btSimulationIslandManagerMt::Island*>& islandsPtr, const btSimulationIslandManagerMt::SolverParams& solverParams)
+        : m_islandsPtr(islandsPtr), m_solverParams(solverParams)
+    {}
 
     void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
     {
+        btConstraintSolver* solver = m_solverParams.m_solverPool;
         for ( int i = iBegin; i < iEnd; ++i )
         {
-            btSimulationIslandManagerMt::Island* island = ( *islandsPtr )[ i ];
-            btPersistentManifold** manifolds = island->manifoldArray.size() ? &island->manifoldArray[ 0 ] : NULL;
-            btTypedConstraint** constraintsPtr = island->constraintArray.size() ? &island->constraintArray[ 0 ] : NULL;
-            callback->processIsland( &island->bodyArray[ 0 ],
-                island->bodyArray.size(),
-                manifolds,
-                island->manifoldArray.size(),
-                constraintsPtr,
-                island->constraintArray.size(),
-                island->id
-            );
+            btSimulationIslandManagerMt::Island* island = m_islandsPtr[ i ];
+            btSimulationIslandManagerMt::solveIsland( solver, *island, m_solverParams );
         }
     }
 };
 
-void btSimulationIslandManagerMt::parallelIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback )
+
+void btSimulationIslandManagerMt::parallelIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams )
 {
     BT_PROFILE( "parallelIslandDispatch" );
-    int grainSize = 1;  // iterations per task
-    UpdateIslandDispatcher dispatcher;
-    dispatcher.islandsPtr = islandsPtr;
-    dispatcher.callback = callback;
-    btParallelFor( 0, islandsPtr->size(), grainSize, dispatcher );
+    //
+    // if there are islands with many contacts, it may be faster to submit these
+    // large islands *serially* to a single parallel constraint solver, and then later
+    // submit the remaining smaller islands in parallel to multiple sequential solvers.
+    //
+    // Some task schedulers do not deal well with nested parallelFor loops. One implementation
+    // of OpenMP was actually slower than doing everything single-threaded. Intel TBB
+    // on the other hand, seems to do a pretty respectable job with it.
+    //
+    // When solving islands in parallel, the worst case performance happens when there
+    // is one very large island and then perhaps a smattering of very small
+    // islands -- one worker thread takes the large island and the remaining workers
+    // tear through the smaller islands and then sit idle waiting for the first worker
+    // to finish. Solving islands in parallel works best when there are numerous small
+    // islands, roughly equal in size.
+    //
+    // By contrast, the other approach -- the parallel constraint solver -- is only
+    // able to deliver a worthwhile speedup when the island is large. For smaller islands,
+    // it is difficult to extract a useful amount of parallelism -- the overhead of grouping
+    // the constraints into batches and sending the batches to worker threads can nullify
+    // any gains from parallelism.
+    //
+
+    UpdateIslandDispatcher dispatcher(*islandsPtr, solverParams);
+    // We take advantage of the fact the islands are sorted in order of decreasing size
+    int iBegin = 0;
+    if (solverParams.m_solverMt)
+    {
+        while ( iBegin < islandsPtr->size() )
+        {
+            btSimulationIslandManagerMt::Island* island = ( *islandsPtr )[ iBegin ];
+            if ( island->manifoldArray.size() < btSequentialImpulseConstraintSolverMt::s_minimumContactManifoldsForBatching )
+            {
+                // OK to submit the rest of the array in parallel
+                break;
+            }
+            // serial dispatch to parallel solver for large islands (if any)
+            solveIsland(solverParams.m_solverMt, *island, solverParams);
+            ++iBegin;
+        }
+    }
+    // parallel dispatch to sequential solvers for rest
+    btParallelFor( iBegin, islandsPtr->size(), 1, dispatcher );
 }
 
 
@@ -606,15 +649,14 @@ void btSimulationIslandManagerMt::parallelIslandDispatch( btAlignedObjectArray<I
 void btSimulationIslandManagerMt::buildAndProcessIslands( btDispatcher* dispatcher,
                                                         btCollisionWorld* collisionWorld,
                                                         btAlignedObjectArray<btTypedConstraint*>& constraints,
-                                                        IslandCallback* callback
+                                                        const SolverParams& solverParams
                                                         )
 {
+	BT_PROFILE("buildAndProcessIslands");
 	btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
 
 	buildIslands(dispatcher,collisionWorld);
 
-	BT_PROFILE("processIslands");
-
 	if(!getSplitIslands())
 	{
         btPersistentManifold** manifolds = dispatcher->getInternalManifoldPointer();
@@ -646,14 +688,17 @@ void btSimulationIslandManagerMt::buildAndProcessIslands( btDispatcher* dispatch
             }
         }
         btTypedConstraint** constraintsPtr = constraints.size() ? &constraints[ 0 ] : NULL;
-		callback->processIsland(&collisionObjects[0],
-                                 collisionObjects.size(),
-                                 manifolds,
-                                 maxNumManifolds,
-                                 constraintsPtr,
-                                 constraints.size(),
-                                 -1
-                                 );
+        btConstraintSolver* solver = solverParams.m_solverMt ? solverParams.m_solverMt : solverParams.m_solverPool;
+        solver->solveGroup(&collisionObjects[0],
+                           collisionObjects.size(),
+                           manifolds,
+                           maxNumManifolds,
+                           constraintsPtr,
+                           constraints.size(),
+                           *solverParams.m_solverInfo,
+                           solverParams.m_debugDrawer,
+                           solverParams.m_dispatcher
+                           );
 	}
 	else
 	{
@@ -673,6 +718,6 @@ void btSimulationIslandManagerMt::buildAndProcessIslands( btDispatcher* dispatch
             mergeIslands();
         }
         // dispatch islands to solver
-        m_islandDispatch( &m_activeIslands, callback );
+        m_islandDispatch( &m_activeIslands, solverParams );
 	}
 }
diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h b/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h
index 9a781aaef1..563577a6f4 100644
--- a/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h
+++ b/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h
@@ -19,7 +19,9 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
 
 class btTypedConstraint;
-
+class btConstraintSolver;
+struct btContactSolverInfo;
+class btIDebugDraw;
 
 ///
 /// SimulationIslandManagerMt -- Multithread capable version of SimulationIslandManager
@@ -45,22 +47,19 @@ public:
 
         void append( const Island& other );  // add bodies, manifolds, constraints to my own
     };
-    struct	IslandCallback
+    struct SolverParams
     {
-        virtual ~IslandCallback() {};
-
-        virtual	void processIsland( btCollisionObject** bodies,
-                                    int numBodies,
-                                    btPersistentManifold** manifolds,
-                                    int numManifolds,
-                                    btTypedConstraint** constraints,
-                                    int numConstraints,
-                                    int islandId
-                                    ) = 0;
+        btConstraintSolver*		m_solverPool;
+        btConstraintSolver*		m_solverMt;
+        btContactSolverInfo*	m_solverInfo;
+        btIDebugDraw*			m_debugDrawer;
+        btDispatcher*			m_dispatcher;
     };
-    typedef void( *IslandDispatchFunc ) ( btAlignedObjectArray<Island*>* islands, IslandCallback* callback );
-    static void serialIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback );
-    static void parallelIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback );
+    static void solveIsland(btConstraintSolver* solver, Island& island, const SolverParams& solverParams);
+
+    typedef void( *IslandDispatchFunc ) ( btAlignedObjectArray<Island*>* islands, const SolverParams& solverParams );
+    static void serialIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams );
+    static void parallelIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams );
 protected:
     btAlignedObjectArray<Island*> m_allocatedIslands;  // owner of all Islands
     btAlignedObjectArray<Island*> m_activeIslands;  // islands actively in use
@@ -83,7 +82,11 @@ public:
 	btSimulationIslandManagerMt();
 	virtual ~btSimulationIslandManagerMt();
 
-    virtual void buildAndProcessIslands( btDispatcher* dispatcher, btCollisionWorld* collisionWorld, btAlignedObjectArray<btTypedConstraint*>& constraints, IslandCallback* callback );
+    virtual void buildAndProcessIslands( btDispatcher* dispatcher,
+        btCollisionWorld* collisionWorld,
+        btAlignedObjectArray<btTypedConstraint*>& constraints,
+        const SolverParams& solverParams
+    );
 
 	virtual void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld);
 
@@ -106,5 +109,6 @@ public:
     }
 };
 
+
 #endif //BT_SIMULATION_ISLAND_MANAGER_H
 
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp
index 62865e0c78..0e85b55728 100644
--- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp
+++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp
@@ -112,14 +112,15 @@ btMultiBody::btMultiBody(int n_links,
 		m_userObjectPointer(0),
 		m_userIndex2(-1),
 		m_userIndex(-1),
+		m_companionId(-1),
 		m_linearDamping(0.04f),
 		m_angularDamping(0.04f),
 		m_useGyroTerm(true),
-			m_maxAppliedImpulse(1000.f),
+		m_maxAppliedImpulse(1000.f),
 		m_maxCoordinateVelocity(100.f),
-			m_hasSelfCollision(true),		
+		m_hasSelfCollision(true),		
 		__posUpdated(false),
-			m_dofCount(0),
+		m_dofCount(0),
 		m_posVarCnt(0),
 		m_useRK4(false), 	
 		m_useGlobalVelocities(false),
@@ -136,6 +137,9 @@ btMultiBody::btMultiBody(int n_links,
 
     m_baseForce.setValue(0, 0, 0);
     m_baseTorque.setValue(0, 0, 0);
+
+	clearConstraintForces();
+	clearForcesAndTorques();
 }
 
 btMultiBody::~btMultiBody()
@@ -740,13 +744,13 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 	const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
 	const btScalar DAMPING_K2_ANGULAR= m_angularDamping;
 
-    btVector3 base_vel = getBaseVel();
-    btVector3 base_omega = getBaseOmega();
+	const btVector3 base_vel = getBaseVel();
+	const btVector3 base_omega = getBaseOmega();
 
     // Temporary matrices/vectors -- use scratch space from caller
     // so that we don't have to keep reallocating every frame
 
-    scratch_r.resize(2*m_dofCount + 6);				//multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount
+    scratch_r.resize(2*m_dofCount + 7);				//multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount
     scratch_v.resize(8*num_links + 6);
     scratch_m.resize(4*num_links + 4);
 
@@ -777,7 +781,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
     // hhat is NOT stored for the base (but ahat is)    
 	btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
 	btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr;
-	v_ptr += num_links * 2 + 2;
+	v_ptr += num_links * 2 + 2; 
 	//
     // Y_i, invD_i
     btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
@@ -815,13 +819,13 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
     }
 	else 
 	{
-		btVector3 baseForce = isConstraintPass? m_baseConstraintForce : m_baseForce;
-		btVector3 baseTorque = isConstraintPass? m_baseConstraintTorque : m_baseTorque;
+		const btVector3& baseForce = isConstraintPass? m_baseConstraintForce : m_baseForce;
+		const btVector3& baseTorque = isConstraintPass? m_baseConstraintTorque : m_baseTorque;
 		//external forces		
 		zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));	
 
 		//adding damping terms (only)
-		btScalar linDampMult = 1., angDampMult = 1.;
+		const btScalar linDampMult = 1., angDampMult = 1.;
 		zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().safeNorm()),
 								   linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().safeNorm()));
 
@@ -963,16 +967,15 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 			//
 			Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof]
 			- m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
-			- spatCoriolisAcc[i].dot(hDof)
-			;
-		}
+			- spatCoriolisAcc[i].dot(hDof);
 
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
-		{
-			btScalar *D_row = &D[dof * m_links[i].m_dofCount];			
+		}
+ 		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{	
+			btScalar *D_row = &D[dof * m_links[i].m_dofCount];
 			for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
 			{
-				btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
+				const btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
 				D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2);
 			}
 		}
@@ -983,14 +986,20 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 			case btMultibodyLink::ePrismatic:
 			case btMultibodyLink::eRevolute:
 			{
-				invDi[0] = 1.0f / D[0];
+				if (D[0]>=SIMD_EPSILON)
+				{
+					invDi[0] = 1.0f / D[0];
+				} else
+				{
+					invDi[0] = 0;
+				}
 				break;
 			}
 			case btMultibodyLink::eSpherical:
 			case btMultibodyLink::ePlanar:
 			{
-				btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
-				btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
+				const btMatrix3x3 D3x3(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
+				const btMatrix3x3 invD3x3(D3x3.inverse());
 
 				//unroll the loop?
 				for(int row = 0; row < 3; ++row)
@@ -1016,7 +1025,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 
 			for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
 			{				
-				btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
+				const btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
 				//				
 				spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof];
 			}
@@ -1027,7 +1036,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 		//determine (h*D^{-1}) * h^{T}
 		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
 		{			
-			btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
+			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
 			//
 			dyadTemp -= symmetricSpatialOuterProduct(hDof, spatForceVecTemps[dof]);
 		}
@@ -1048,7 +1057,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 
 		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
 		{				
-			btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
+			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
 			//
 			spatForceVecTemps[0] += hDof * invD_times_Y[dof];			
 		}
@@ -1099,7 +1108,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 		
 		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
 		{
-			btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
+			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
 			//			
 			Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof);			
 		}
@@ -1159,12 +1168,12 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
     }
 
     // transform base accelerations back to the world frame.
-    btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
+	const btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
 	output[0] = omegadot_out[0];
 	output[1] = omegadot_out[1];
 	output[2] = omegadot_out[2];
 
-    btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear()));
+	const btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear()));
 	output[3] = vdot_out[0];
 	output[4] = vdot_out[1];
 	output[5] = vdot_out[2];
@@ -1266,12 +1275,29 @@ void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bo
     if (num_links == 0) 
 	{
 		// in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
-        result[0] = rhs_bot[0] / m_baseInertia[0];
-        result[1] = rhs_bot[1] / m_baseInertia[1];
+   
+	if ((m_baseInertia[0] >= SIMD_EPSILON) && (m_baseInertia[1] >= SIMD_EPSILON) && (m_baseInertia[2] >= SIMD_EPSILON))
+	{
+		result[0] = rhs_bot[0] / m_baseInertia[0];
+		result[1] = rhs_bot[1] / m_baseInertia[1];
         result[2] = rhs_bot[2] / m_baseInertia[2];
-        result[3] = rhs_top[0] / m_baseMass;
-        result[4] = rhs_top[1] / m_baseMass;
-        result[5] = rhs_top[2] / m_baseMass;
+	} else
+	{
+		result[0] = 0;
+		result[1] = 0;
+		result[2] = 0;
+	}
+	if (m_baseMass>=SIMD_EPSILON)
+	{
+        	result[3] = rhs_top[0] / m_baseMass;
+        	result[4] = rhs_top[1] / m_baseMass;
+        	result[5] = rhs_top[2] / m_baseMass;
+	} else
+	{
+		result[3] = 0;
+		result[4] = 0;
+		result[5] = 0;
+	}
     } else 
 	{
 		if (!m_cachedInertiaValid)
@@ -1322,9 +1348,21 @@ void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionV
     if (num_links == 0) 
 	{
 		// in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
-		result.setAngular(rhs.getAngular() / m_baseInertia);
-		result.setLinear(rhs.getLinear() / m_baseMass);		
-    } else 
+		if ((m_baseInertia[0] >= SIMD_EPSILON) && (m_baseInertia[1] >= SIMD_EPSILON) && (m_baseInertia[2] >= SIMD_EPSILON))
+        	{
+			result.setAngular(rhs.getAngular() / m_baseInertia);       
+        	} else  
+        	{       
+                	result.setAngular(btVector3(0,0,0));
+        	}
+        	if (m_baseMass>=SIMD_EPSILON)
+        	{       
+			result.setLinear(rhs.getLinear() / m_baseMass);               	 	
+        	} else  
+        	{       
+                	result.setLinear(btVector3(0,0,0));
+        	}
+    	} else 
 	{
 		/// Special routine for calculating the inverse of a spatial inertia matrix
 		///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
@@ -1808,6 +1846,7 @@ void btMultiBody::fillConstraintJacobianMultiDof(int link,
 
 void btMultiBody::wakeUp()
 {
+	m_sleepTimer = 0;
     m_awake = true;
 }
 
@@ -1956,7 +1995,11 @@ int	btMultiBody::calculateSerializeBufferSize()	const
 const char*	btMultiBody::serialize(void* dataBuffer, class btSerializer* serializer) const
 {
 		btMultiBodyData* mbd = (btMultiBodyData*) dataBuffer;
-		getBaseWorldTransform().serialize(mbd->m_baseWorldTransform);
+		getBasePos().serialize(mbd->m_baseWorldPosition);
+		getWorldToBaseRot().inverse().serialize(mbd->m_baseWorldOrientation);
+		getBaseVel().serialize(mbd->m_baseLinearVelocity);
+		getBaseOmega().serialize(mbd->m_baseAngularVelocity);
+
 		mbd->m_baseMass = this->getBaseMass();
 		getBaseInertia().serialize(mbd->m_baseInertia);
 		{
@@ -1982,6 +2025,12 @@ const char*	btMultiBody::serialize(void* dataBuffer, class btSerializer* seriali
 				memPtr->m_posVarCount = getLink(i).m_posVarCount;
 				
 				getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia);
+
+				getLink(i).m_absFrameTotVelocity.m_topVec.serialize(memPtr->m_absFrameTotVelocityTop);
+				getLink(i).m_absFrameTotVelocity.m_bottomVec.serialize(memPtr->m_absFrameTotVelocityBottom);
+				getLink(i).m_absFrameLocVelocity.m_topVec.serialize(memPtr->m_absFrameLocVelocityTop);
+				getLink(i).m_absFrameLocVelocity.m_bottomVec.serialize(memPtr->m_absFrameLocVelocityBottom);
+
 				memPtr->m_linkMass = getLink(i).m_mass;
 				memPtr->m_parentIndex = getLink(i).m_parent;
 				memPtr->m_jointDamping = getLink(i).m_jointDamping;
@@ -1991,7 +2040,7 @@ const char*	btMultiBody::serialize(void* dataBuffer, class btSerializer* seriali
 				memPtr->m_jointMaxForce = getLink(i).m_jointMaxForce;
 				memPtr->m_jointMaxVelocity = getLink(i).m_jointMaxVelocity;
 
-				getLink(i).m_eVector.serialize(memPtr->m_parentComToThisComOffset);
+				getLink(i).m_eVector.serialize(memPtr->m_parentComToThisPivotOffset);
 				getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset);
 				getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis);
 				btAssert(memPtr->m_dofCount<=3);
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h
index 655165ac18..5cd00e5173 100644
--- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h
+++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h
@@ -702,15 +702,18 @@ private:
 	int	m_companionId;
 	btScalar	m_linearDamping;
 	btScalar	m_angularDamping;
-	bool	m_useGyroTerm;
+	bool		m_useGyroTerm;
 	btScalar	m_maxAppliedImpulse;
 	btScalar	m_maxCoordinateVelocity;
 	bool		m_hasSelfCollision;
 	
-		bool __posUpdated;
-		int m_dofCount, m_posVarCnt;
+	bool __posUpdated;
+	int m_dofCount, m_posVarCnt;
+
 	bool m_useRK4, m_useGlobalVelocities;
-	
+	//for global velocities, see 8.3.2B Proposed resolution in Jakub Stepien PhD Thesis
+	//https://drive.google.com/file/d/0Bz3vEa19XOYGNWdZWGpMdUdqVmZ5ZVBOaEh4ZnpNaUxxZFNV/view?usp=sharing
+
 	///the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal usage only
 	bool m_internalNeedsJointFeedback;
 };
@@ -718,12 +721,17 @@ private:
 struct btMultiBodyLinkDoubleData
 {
 	btQuaternionDoubleData	m_zeroRotParentToThis;
-	btVector3DoubleData		m_parentComToThisComOffset;
+	btVector3DoubleData		m_parentComToThisPivotOffset;
 	btVector3DoubleData		m_thisPivotToThisComOffset;
 	btVector3DoubleData		m_jointAxisTop[6];
 	btVector3DoubleData		m_jointAxisBottom[6];
 
 	btVector3DoubleData		m_linkInertia;   // inertia of the base (in local frame; diagonal)
+	btVector3DoubleData		m_absFrameTotVelocityTop;
+	btVector3DoubleData		m_absFrameTotVelocityBottom;
+	btVector3DoubleData		m_absFrameLocVelocityTop;
+	btVector3DoubleData		m_absFrameLocVelocityBottom;
+
 	double					m_linkMass;
 	int						m_parentIndex;
 	int						m_jointType;
@@ -751,11 +759,16 @@ struct btMultiBodyLinkDoubleData
 struct btMultiBodyLinkFloatData
 {
 	btQuaternionFloatData	m_zeroRotParentToThis;
-	btVector3FloatData		m_parentComToThisComOffset;
+	btVector3FloatData		m_parentComToThisPivotOffset;
 	btVector3FloatData		m_thisPivotToThisComOffset;
 	btVector3FloatData		m_jointAxisTop[6];
 	btVector3FloatData		m_jointAxisBottom[6];
-	btVector3FloatData	m_linkInertia;   // inertia of the base (in local frame; diagonal)
+	btVector3FloatData		m_linkInertia;   // inertia of the base (in local frame; diagonal)
+	btVector3FloatData		m_absFrameTotVelocityTop;
+	btVector3FloatData		m_absFrameTotVelocityBottom;
+	btVector3FloatData		m_absFrameLocVelocityTop;
+	btVector3FloatData		m_absFrameLocVelocityBottom;
+
 	int						m_dofCount;
 	float				m_linkMass;
 	int					m_parentIndex;
@@ -784,29 +797,38 @@ struct btMultiBodyLinkFloatData
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct	btMultiBodyDoubleData
 {
-	btTransformDoubleData m_baseWorldTransform;
+	btVector3DoubleData m_baseWorldPosition;
+	btQuaternionDoubleData m_baseWorldOrientation;
+	btVector3DoubleData m_baseLinearVelocity;
+	btVector3DoubleData m_baseAngularVelocity;
 	btVector3DoubleData m_baseInertia;   // inertia of the base (in local frame; diagonal)
 	double	m_baseMass;
+	int		m_numLinks;
+	char	m_padding[4];
 
 	char	*m_baseName;
 	btMultiBodyLinkDoubleData	*m_links;
 	btCollisionObjectDoubleData	*m_baseCollider;
-	char	*m_paddingPtr;
-	int		m_numLinks;
-	char	m_padding[4];
+	
+	
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct	btMultiBodyFloatData
 {
-	char	*m_baseName;
-	btMultiBodyLinkFloatData	*m_links;
-	btCollisionObjectFloatData	*m_baseCollider;
-	btTransformFloatData m_baseWorldTransform;
+	btVector3FloatData m_baseWorldPosition;
+	btQuaternionFloatData m_baseWorldOrientation;
+	btVector3FloatData m_baseLinearVelocity;
+	btVector3FloatData m_baseAngularVelocity;
+
 	btVector3FloatData m_baseInertia;   // inertia of the base (in local frame; diagonal)
-	
 	float	m_baseMass;
 	int		m_numLinks;
+
+	char	*m_baseName;
+	btMultiBodyLinkFloatData	*m_links;
+	btCollisionObjectFloatData	*m_baseCollider;
+
 };
 
 
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
index d52852dd8e..9f61874b83 100644
--- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
+++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
@@ -253,7 +253,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
         {
             vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
             if (angConstraint) {
-                denom0 = rb0->getInvMass() + constraintNormalAng.dot(vec);
+				denom0 = constraintNormalAng.dot(solverConstraint.m_angularComponentA);
             }
             else {
                 denom0 = rb0->getInvMass() + constraintNormalLin.dot(vec);
@@ -277,7 +277,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
         {
             vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
             if (angConstraint) {
-                denom1 = rb1->getInvMass() + constraintNormalAng.dot(vec);
+				denom1 = constraintNormalAng.dot(-solverConstraint.m_angularComponentB);
             }
             else {
                 denom1 = rb1->getInvMass() + constraintNormalLin.dot(vec);
@@ -315,7 +315,8 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
         }
         else if(rb0)
         {
-            rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
+			rel_vel += rb0->getLinearVelocity().dot(solverConstraint.m_contactNormal1);
+			rel_vel += rb0->getAngularVelocity().dot(solverConstraint.m_relpos1CrossNormal);
         }
         if (multiBodyB)
         {
@@ -327,7 +328,8 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
         }
         else if(rb1)
         {
-            rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
+			rel_vel += rb1->getLinearVelocity().dot(solverConstraint.m_contactNormal2);
+			rel_vel += rb1->getAngularVelocity().dot(solverConstraint.m_relpos2CrossNormal);
         }
         
         solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h
index 83521b9501..a2ae571273 100644
--- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h
+++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h
@@ -119,6 +119,14 @@ public:
 		return m_bodyB;
 	}
 
+	int getLinkA() const
+	{
+		return m_linkA;
+	}
+	int getLinkB() const
+	{
+		return m_linkB;
+	}
 	void	internalSetAppliedImpulse(int dof, btScalar appliedImpulse)
 	{
 		btAssert(dof>=0);
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
index 1e2d074096..cd84826e1a 100644
--- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
+++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
@@ -39,7 +39,7 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 		btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index];
 		
 		btScalar residual = resolveSingleConstraintRowGeneric(constraint);
-		leastSquaredResidual += residual*residual;
+		leastSquaredResidual = btMax(leastSquaredResidual,residual*residual);
 
 		if(constraint.m_multiBodyA) 
 			constraint.m_multiBodyA->setPosUpdated(false);
@@ -60,36 +60,101 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 			residual = resolveSingleConstraintRowGeneric(constraint);
 		}
 
-		leastSquaredResidual += residual*residual;
+		leastSquaredResidual = btMax(leastSquaredResidual,residual*residual);
  
 		if(constraint.m_multiBodyA) 
 			constraint.m_multiBodyA->setPosUpdated(false);
 		if(constraint.m_multiBodyB) 
 			constraint.m_multiBodyB->setPosUpdated(false);
 	}
-	
-	//solve featherstone frictional contact
 
-	for (int j1=0;j1<this->m_multiBodyFrictionContactConstraints.size();j1++)
+	//solve featherstone frictional contact
+	if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode&SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0))
 	{
-		if (iteration < infoGlobal.m_numIterations)
+		for (int j1 = 0; j1<this->m_multiBodyTorsionalFrictionContactConstraints.size(); j1++)
+		{
+			if (iteration < infoGlobal.m_numIterations)
+			{
+				int index = j1;//iteration&1? j1 : m_multiBodyTorsionalFrictionContactConstraints.size()-1-j1;
+
+				btMultiBodySolverConstraint& frictionConstraint = m_multiBodyTorsionalFrictionContactConstraints[index];
+				btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
+				//adjust friction limits here
+				if (totalImpulse>btScalar(0))
+				{
+					frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
+					frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
+					btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint);
+					leastSquaredResidual = btMax(leastSquaredResidual , residual*residual);
+
+					if (frictionConstraint.m_multiBodyA)
+						frictionConstraint.m_multiBodyA->setPosUpdated(false);
+					if (frictionConstraint.m_multiBodyB)
+						frictionConstraint.m_multiBodyB->setPosUpdated(false);
+				}
+			}
+		}
+
+		for (int j1 = 0; j1 < this->m_multiBodyFrictionContactConstraints.size(); j1++)
 		{
-			int index = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
+			if (iteration < infoGlobal.m_numIterations)
+			{
+				int index = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
+				btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index];
+
+				btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
+				j1++;
+				int index2 = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
+				btMultiBodySolverConstraint& frictionConstraintB = m_multiBodyFrictionContactConstraints[index2];
+				btAssert(frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex);
+
+				if (frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex)
+				{
+					frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
+					frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
+					frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction*totalImpulse);
+					frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction*totalImpulse;
+					btScalar residual = resolveConeFrictionConstraintRows(frictionConstraint, frictionConstraintB);
+					leastSquaredResidual = btMax(leastSquaredResidual, residual*residual);
+					
+					if (frictionConstraintB.m_multiBodyA)
+						frictionConstraintB.m_multiBodyA->setPosUpdated(false);
+					if (frictionConstraintB.m_multiBodyB)
+						frictionConstraintB.m_multiBodyB->setPosUpdated(false);
+				
+					if (frictionConstraint.m_multiBodyA)
+						frictionConstraint.m_multiBodyA->setPosUpdated(false);
+					if (frictionConstraint.m_multiBodyB)
+						frictionConstraint.m_multiBodyB->setPosUpdated(false);
+				}
+			}
+		}
 
-			btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index];
-			btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
-			//adjust friction limits here
-			if (totalImpulse>btScalar(0))
+		
+	}
+	else
+	{
+		for (int j1 = 0; j1<this->m_multiBodyFrictionContactConstraints.size(); j1++)
+		{
+			if (iteration < infoGlobal.m_numIterations)
 			{
-				frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
-				frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
-				btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint);
-				leastSquaredResidual += residual*residual;
-
-				if(frictionConstraint.m_multiBodyA) 
-					frictionConstraint.m_multiBodyA->setPosUpdated(false);
-				if(frictionConstraint.m_multiBodyB) 
-					frictionConstraint.m_multiBodyB->setPosUpdated(false);
+				int index = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
+
+				btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index];
+				btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
+				//adjust friction limits here
+				if (totalImpulse>btScalar(0))
+				{
+					frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
+					frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
+					btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint);
+					leastSquaredResidual = btMax(leastSquaredResidual, residual*residual);
+
+					if (frictionConstraint.m_multiBodyA)
+						frictionConstraint.m_multiBodyA->setPosUpdated(false);
+					if (frictionConstraint.m_multiBodyB)
+						frictionConstraint.m_multiBodyB->setPosUpdated(false);
+				}
 			}
 		}
 	}
@@ -101,6 +166,8 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionOb
 	m_multiBodyNonContactConstraints.resize(0);
 	m_multiBodyNormalContactConstraints.resize(0);
 	m_multiBodyFrictionContactConstraints.resize(0);
+	m_multiBodyTorsionalFrictionContactConstraints.resize(0);
+	
 	m_data.m_jacobians.resize(0);
 	m_data.m_deltaVelocitiesUnitImpulse.resize(0);
 	m_data.m_deltaVelocities.resize(0);
@@ -128,82 +195,267 @@ void	btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar im
 btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c)
 {
 
-	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
-	btScalar deltaVelADotn=0;
-	btScalar deltaVelBDotn=0;
+	btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm;
+	btScalar deltaVelADotn = 0;
+	btScalar deltaVelBDotn = 0;
 	btSolverBody* bodyA = 0;
 	btSolverBody* bodyB = 0;
-	int ndofA=0;
-	int ndofB=0;
+	int ndofA = 0;
+	int ndofB = 0;
 
 	if (c.m_multiBodyA)
 	{
-		ndofA  = c.m_multiBodyA->getNumDofs() + 6;
-		for (int i = 0; i < ndofA; ++i) 
-			deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
-	} else if(c.m_solverBodyIdA >= 0)
+		ndofA = c.m_multiBodyA->getNumDofs() + 6;
+		for (int i = 0; i < ndofA; ++i)
+			deltaVelADotn += m_data.m_jacobians[c.m_jacAindex + i] * m_data.m_deltaVelocities[c.m_deltaVelAindex + i];
+	}
+	else if (c.m_solverBodyIdA >= 0)
 	{
 		bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA];
-		deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) 	+ c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
+		deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
 	}
 
 	if (c.m_multiBodyB)
 	{
-		ndofB  = c.m_multiBodyB->getNumDofs() + 6;
-		for (int i = 0; i < ndofB; ++i) 
-			deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
-	} else if(c.m_solverBodyIdB >= 0)
+		ndofB = c.m_multiBodyB->getNumDofs() + 6;
+		for (int i = 0; i < ndofB; ++i)
+			deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex + i] * m_data.m_deltaVelocities[c.m_deltaVelBindex + i];
+	}
+	else if (c.m_solverBodyIdB >= 0)
 	{
 		bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB];
-		deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity())  + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
+		deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
 	}
 
-	
-	deltaImpulse	-=	deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
-	deltaImpulse	-=	deltaVelBDotn*c.m_jacDiagABInv;
+
+	deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
+	deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv;
 	const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
-	
+
 	if (sum < c.m_lowerLimit)
 	{
-		deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
+		deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
 		c.m_appliedImpulse = c.m_lowerLimit;
 	}
-	else if (sum > c.m_upperLimit) 
+	else if (sum > c.m_upperLimit)
 	{
-		deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
+		deltaImpulse = c.m_upperLimit - c.m_appliedImpulse;
 		c.m_appliedImpulse = c.m_upperLimit;
 	}
 	else
 	{
 		c.m_appliedImpulse = sum;
 	}
-
+	
 	if (c.m_multiBodyA)
 	{
-		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
+		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse, c.m_deltaVelAindex, ndofA);
 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
 		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
-		c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
+		c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse);
 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
-	} else if(c.m_solverBodyIdA >= 0)
+	}
+	else if (c.m_solverBodyIdA >= 0)
 	{
-		bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
+		bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
 
 	}
 	if (c.m_multiBodyB)
 	{
-		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
+		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse, c.m_deltaVelBindex, ndofB);
 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
 		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
-		c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
+		c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse);
 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
-	} else if(c.m_solverBodyIdB >= 0)
+	}
+	else if (c.m_solverBodyIdB >= 0)
 	{
-		bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
+		bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
 	}
-	return deltaImpulse;
+    btScalar deltaVel =deltaImpulse/c.m_jacDiagABInv;
+	return deltaVel;
+}
+
+
+btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1,const btMultiBodySolverConstraint& cB)
+{
+	int ndofA=0;
+	int ndofB=0;
+	btSolverBody* bodyA = 0;
+	btSolverBody* bodyB = 0;
+	btScalar deltaImpulseB = 0.f;
+	btScalar sumB = 0.f;
+	{
+		deltaImpulseB = cB.m_rhs-btScalar(cB.m_appliedImpulse)*cB.m_cfm;
+		btScalar deltaVelADotn=0;
+		btScalar deltaVelBDotn=0;
+		if (cB.m_multiBodyA)
+		{
+			ndofA  = cB.m_multiBodyA->getNumDofs() + 6;
+			for (int i = 0; i < ndofA; ++i) 
+				deltaVelADotn += m_data.m_jacobians[cB.m_jacAindex+i] * m_data.m_deltaVelocities[cB.m_deltaVelAindex+i];
+		} else if(cB.m_solverBodyIdA >= 0)
+		{
+			bodyA = &m_tmpSolverBodyPool[cB.m_solverBodyIdA];
+			deltaVelADotn += cB.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) 	+ cB.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
+		}
+
+		if (cB.m_multiBodyB)
+		{
+			ndofB  = cB.m_multiBodyB->getNumDofs() + 6;
+			for (int i = 0; i < ndofB; ++i) 
+				deltaVelBDotn += m_data.m_jacobians[cB.m_jacBindex+i] * m_data.m_deltaVelocities[cB.m_deltaVelBindex+i];
+		} else if(cB.m_solverBodyIdB >= 0)
+		{
+			bodyB = &m_tmpSolverBodyPool[cB.m_solverBodyIdB];
+			deltaVelBDotn += cB.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity())  + cB.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
+		}
+
+	
+		deltaImpulseB	-=	deltaVelADotn*cB.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
+		deltaImpulseB	-=	deltaVelBDotn*cB.m_jacDiagABInv;
+		sumB = btScalar(cB.m_appliedImpulse) + deltaImpulseB;
+	}
+
+	btScalar deltaImpulseA = 0.f;
+	btScalar sumA = 0.f;
+	const btMultiBodySolverConstraint& cA = cA1;
+	{
+		{
+			deltaImpulseA = cA.m_rhs-btScalar(cA.m_appliedImpulse)*cA.m_cfm;
+			btScalar deltaVelADotn=0;
+			btScalar deltaVelBDotn=0;
+			if (cA.m_multiBodyA)
+			{
+				ndofA  = cA.m_multiBodyA->getNumDofs() + 6;
+				for (int i = 0; i < ndofA; ++i) 
+					deltaVelADotn += m_data.m_jacobians[cA.m_jacAindex+i] * m_data.m_deltaVelocities[cA.m_deltaVelAindex+i];
+			} else if(cA.m_solverBodyIdA >= 0)
+			{
+				bodyA = &m_tmpSolverBodyPool[cA.m_solverBodyIdA];
+				deltaVelADotn += cA.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) 	+ cA.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
+			}
+
+			if (cA.m_multiBodyB)
+			{
+				ndofB  = cA.m_multiBodyB->getNumDofs() + 6;
+				for (int i = 0; i < ndofB; ++i) 
+					deltaVelBDotn += m_data.m_jacobians[cA.m_jacBindex+i] * m_data.m_deltaVelocities[cA.m_deltaVelBindex+i];
+			} else if(cA.m_solverBodyIdB >= 0)
+			{
+				bodyB = &m_tmpSolverBodyPool[cA.m_solverBodyIdB];
+				deltaVelBDotn += cA.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity())  + cA.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
+			}
+
+	
+			deltaImpulseA	-=	deltaVelADotn*cA.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
+			deltaImpulseA	-=	deltaVelBDotn*cA.m_jacDiagABInv;
+			sumA = btScalar(cA.m_appliedImpulse) + deltaImpulseA;
+		}
+	}
+
+	if (sumA*sumA+sumB*sumB>=cA.m_lowerLimit*cB.m_lowerLimit)
+	{
+		btScalar angle = btAtan2(sumA,sumB);
+		btScalar sumAclipped = btFabs(cA.m_lowerLimit*btSin(angle));
+		btScalar sumBclipped = btFabs(cB.m_lowerLimit*btCos(angle));
+
+		
+		if (sumA < -sumAclipped)
+		{
+			deltaImpulseA = -sumAclipped - cA.m_appliedImpulse;
+			cA.m_appliedImpulse = -sumAclipped;
+		}
+		else if (sumA > sumAclipped)
+		{
+			deltaImpulseA = sumAclipped - cA.m_appliedImpulse;
+			cA.m_appliedImpulse = sumAclipped;
+		}
+		else
+		{
+			cA.m_appliedImpulse = sumA;
+		}
+
+		if (sumB < -sumBclipped)
+		{
+			deltaImpulseB = -sumBclipped - cB.m_appliedImpulse;
+			cB.m_appliedImpulse = -sumBclipped;
+		}
+		else if (sumB > sumBclipped)
+		{
+			deltaImpulseB = sumBclipped - cB.m_appliedImpulse;
+			cB.m_appliedImpulse = sumBclipped;
+		}
+		else
+		{
+			cB.m_appliedImpulse = sumB;
+		}
+		//deltaImpulseA = sumAclipped-cA.m_appliedImpulse;
+		//cA.m_appliedImpulse = sumAclipped;
+		//deltaImpulseB = sumBclipped-cB.m_appliedImpulse;
+		//cB.m_appliedImpulse = sumBclipped;
+	} 
+	else
+	{
+		cA.m_appliedImpulse = sumA;
+		cB.m_appliedImpulse = sumB;
+	}
+	
+	if (cA.m_multiBodyA)
+	{
+		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex],deltaImpulseA,cA.m_deltaVelAindex,ndofA);
+#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
+		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
+		cA.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex],deltaImpulseA);
+#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+	} else if(cA.m_solverBodyIdA >= 0)
+	{
+		bodyA->internalApplyImpulse(cA.m_contactNormal1*bodyA->internalGetInvMass(),cA.m_angularComponentA,deltaImpulseA);
+
+	}
+	if (cA.m_multiBodyB)
+	{
+		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex],deltaImpulseA,cA.m_deltaVelBindex,ndofB);
+#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
+		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
+		cA.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex],deltaImpulseA);
+#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+	} else if(cA.m_solverBodyIdB >= 0)
+	{
+		bodyB->internalApplyImpulse(cA.m_contactNormal2*bodyB->internalGetInvMass(),cA.m_angularComponentB,deltaImpulseA);
+	}
+
+	if (cB.m_multiBodyA)
+	{
+		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex],deltaImpulseB,cB.m_deltaVelAindex,ndofA);
+#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
+		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
+		cB.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex],deltaImpulseB);
+#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+	} else if(cB.m_solverBodyIdA >= 0)
+	{
+		bodyA->internalApplyImpulse(cB.m_contactNormal1*bodyA->internalGetInvMass(),cB.m_angularComponentA,deltaImpulseB);
+	}
+	if (cB.m_multiBodyB)
+	{
+		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex],deltaImpulseB,cB.m_deltaVelBindex,ndofB);
+#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
+		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
+		cB.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex],deltaImpulseB);
+#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+	} else if(cB.m_solverBodyIdB >= 0)
+	{
+		bodyB->internalApplyImpulse(cB.m_contactNormal2*bodyB->internalGetInvMass(),cB.m_angularComponentB,deltaImpulseB);
+	}
+
+    btScalar deltaVel =deltaImpulseA/cA.m_jacDiagABInv+deltaImpulseB/cB.m_jacDiagABInv;
+    return deltaVel;
 }
 
 
@@ -908,7 +1160,10 @@ btMultiBodySolverConstraint&	btMultiBodyConstraintSolver::addMultiBodyTorsionalF
                                                                                                      btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
 {
     BT_PROFILE("addMultiBodyRollingFrictionConstraint");
-    btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
+
+	bool useTorsionalAndConeFriction = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode&SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0));
+
+    btMultiBodySolverConstraint& solverConstraint = useTorsionalAndConeFriction? m_multiBodyTorsionalFrictionContactConstraints.expandNonInitializing() : m_multiBodyFrictionContactConstraints.expandNonInitializing();
     solverConstraint.m_orgConstraint = 0;
     solverConstraint.m_orgDofIndex = -1;
     
@@ -1151,6 +1406,7 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol
 
 btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
 {
+	//printf("btMultiBodyConstraintSolver::solveGroup: numBodies=%d, numConstraints=%d\n", numBodies, numConstraints);
 	return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
 }
 
@@ -1234,27 +1490,12 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv
 
 	if (c.m_multiBodyA)
 	{
-		
-		if(c.m_multiBodyA->isMultiDof())
-		{
-			c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
-		}
-		else
-		{
-			c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
-		}
+		c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
 	}
 	
 	if (c.m_multiBodyB)
 	{
-		if(c.m_multiBodyB->isMultiDof())
-		{
-			c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
-		}
-		else
-		{
-			c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
-		}
+		c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
 	}
 #endif
 
@@ -1416,6 +1657,8 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
 
 void  btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
 {
+	//printf("solveMultiBodyGroup: numBodies=%d, numConstraints=%d, numManifolds=%d, numMultiBodyConstraints=%d\n", numBodies, numConstraints, numManifolds, numMultiBodyConstraints);
+
 	//printf("solveMultiBodyGroup start\n");
 	m_tmpMultiBodyConstraints = multiBodyConstraints;
 	m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h
index 489347d874..29f484e1d8 100644
--- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h
+++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h
@@ -36,6 +36,7 @@ protected:
 
 	btMultiBodyConstraintArray			m_multiBodyNormalContactConstraints;
 	btMultiBodyConstraintArray			m_multiBodyFrictionContactConstraints;
+	btMultiBodyConstraintArray			m_multiBodyTorsionalFrictionContactConstraints;
 
 	btMultiBodyJacobianData				m_data;
 	
@@ -45,6 +46,9 @@ protected:
 
 	btScalar resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c);
 	
+	//solve 2 friction directions and clamp against the implicit friction cone
+	btScalar resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1, const btMultiBodySolverConstraint& cB);
+	
 
 	void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal);
     
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
index 9eacc22647..9c5f3ad8a9 100644
--- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
+++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
@@ -277,7 +277,11 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 		m_multiBodyConstraints.resize(0);
 	}
 
-	
+    void    setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver)
+    {
+        m_solver = solver;
+    }
+    
 	virtual	void	processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold**	manifolds,int numManifolds, int islandId)
 	{
 		if (islandId<0)
@@ -348,7 +352,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 				for (i=0;i<numCurMultiBodyConstraints;i++)
 					m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]);
 				
-				if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize)
+				if ((m_multiBodyConstraints.size()+m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize)
 				{
 					processConstraints();
 				} else
@@ -394,6 +398,22 @@ btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld ()
 	delete m_solverMultiBodyIslandCallback;
 }
 
+void    btMultiBodyDynamicsWorld::setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver)
+{
+    m_multiBodyConstraintSolver = solver;
+    m_solverMultiBodyIslandCallback->setMultiBodyConstraintSolver(solver);
+    btDiscreteDynamicsWorld::setConstraintSolver(solver);
+}
+
+void    btMultiBodyDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
+{
+    if (solver->getSolverType()==BT_MULTIBODY_SOLVER)
+    {
+        m_multiBodyConstraintSolver = (btMultiBodyConstraintSolver*)solver;
+    }
+    btDiscreteDynamicsWorld::setConstraintSolver(solver);
+}
+
 void	btMultiBodyDynamicsWorld::forwardKinematics()
 {
 
@@ -411,6 +431,8 @@ void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 
 	BT_PROFILE("solveConstraints");
 	
+	clearMultiBodyConstraintForces();
+
 	m_sortedConstraints.resize( m_constraints.size());
 	int i; 
 	for (i=0;i<getNumConstraints();i++)
@@ -433,8 +455,6 @@ void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 	m_solverMultiBodyIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer());
 	m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
 	
-	/// solve all the constraints for this island
-	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback);
 
 #ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
 	{
@@ -669,7 +689,9 @@ void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 		}
 	}
 
-	clearMultiBodyConstraintForces();
+	/// solve all the constraints for this island
+	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
+
 
 	m_solverMultiBodyIslandCallback->processConstraints();
 	
@@ -824,21 +846,24 @@ void	btMultiBodyDynamicsWorld::debugDrawWorld()
 			{
 				btMultiBody* bod = m_multiBodies[b];
 				bod->forwardKinematics(m_scratch_world_to_local1,m_scratch_local_origin1);
-				
-				getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
-
+		
+				if (mode  & btIDebugDraw::DBG_DrawFrames)
+				{
+					getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
+				}
 
 				for (int m = 0; m<bod->getNumLinks(); m++)
 				{
 					
 					const btTransform& tr = bod->getLink(m).m_cachedWorldTransform;
-
-					getDebugDrawer()->drawTransform(tr, 0.1);
-
+					if (mode  & btIDebugDraw::DBG_DrawFrames)
+					{
+						getDebugDrawer()->drawTransform(tr, 0.1);
+					}
 						//draw the joint axis
 					if (bod->getLink(m).m_jointType==btMultibodyLink::eRevolute)
 					{
-						btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_topVec);
+						btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_topVec)*0.1;
 					
 						btVector4 color(0,0,0,1);//1,1,1);
 						btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
@@ -847,7 +872,7 @@ void	btMultiBodyDynamicsWorld::debugDrawWorld()
 					}
 					if (bod->getLink(m).m_jointType==btMultibodyLink::eFixed)
 					{
-						btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
+						btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec)*0.1;
 					
 						btVector4 color(0,0,0,1);//1,1,1);
 						btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
@@ -856,7 +881,7 @@ void	btMultiBodyDynamicsWorld::debugDrawWorld()
 					}
 					if (bod->getLink(m).m_jointType==btMultibodyLink::ePrismatic)
 					{
-						btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
+						btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec)*0.1;
 					
 						btVector4 color(0,0,0,1);//1,1,1);
 						btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
@@ -970,6 +995,8 @@ void	btMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
 
 	serializeCollisionObjects(serializer);
 
+	serializeContactManifolds(serializer);
+
 	serializer->finishSerialization();
 }
 
@@ -988,4 +1015,17 @@ void	btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer)
 		}
 	}
 
-}
\ No newline at end of file
+	//serialize all multibody links (collision objects)
+	for (i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		if (colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+		{
+			int len = colObj->calculateSerializeBufferSize();
+			btChunk* chunk = serializer->allocate(len,1);
+			const char* structType = colObj->serialize(chunk->m_oldPtr, serializer);
+			serializer->finalizeChunk(chunk,structType,BT_MB_LINKCOLLIDER_CODE,colObj);
+		}
+	}
+
+}
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
index c0c132bbba..2fbf089d81 100644
--- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
+++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
@@ -109,6 +109,8 @@ public:
 	virtual void applyGravity();
 	
 	virtual	void	serialize(btSerializer* serializer);
+	virtual void	setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver);
+    virtual void    setConstraintSolver(btConstraintSolver* solver);
 
 };
 #endif //BT_MULTIBODY_DYNAMICS_WORLD_H
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp
index 1f94117aa9..af48e94a83 100644
--- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp
+++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp
@@ -65,13 +65,16 @@ int btMultiBodyFixedConstraint::getIslandIdA() const
 
 	if (m_bodyA)
 	{
-		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-		for (int i=0;i<m_bodyA->getNumLinks();i++)
+		if (m_linkA < 0)
 		{
-			if (m_bodyA->getLink(i).m_collider)
-				return m_bodyA->getLink(i).m_collider->getIslandTag();
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
 		}
 	}
 	return -1;
@@ -83,16 +86,17 @@ int btMultiBodyFixedConstraint::getIslandIdB() const
 		return m_rigidBodyB->getIslandTag();
 	if (m_bodyB)
 	{
-		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-
-		for (int i=0;i<m_bodyB->getNumLinks();i++)
+		if (m_linkB < 0)
 		{
-			col = m_bodyB->getLink(i).m_collider;
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
 			if (col)
 				return col->getIslandTag();
 		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
 	}
 	return -1;
 }
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp
index 5fdb7007d8..09ddd65cd8 100644
--- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp
+++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp
@@ -45,16 +45,18 @@ btMultiBodyGearConstraint::~btMultiBodyGearConstraint()
 
 int btMultiBodyGearConstraint::getIslandIdA() const
 {
-
 	if (m_bodyA)
 	{
-		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-		for (int i=0;i<m_bodyA->getNumLinks();i++)
+		if (m_linkA < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
 		{
-			if (m_bodyA->getLink(i).m_collider)
-				return m_bodyA->getLink(i).m_collider->getIslandTag();
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
 		}
 	}
 	return -1;
@@ -64,16 +66,17 @@ int btMultiBodyGearConstraint::getIslandIdB() const
 {
 	if (m_bodyB)
 	{
-		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-
-		for (int i=0;i<m_bodyB->getNumLinks();i++)
+		if (m_linkB < 0)
 		{
-			col = m_bodyB->getLink(i).m_collider;
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
 			if (col)
 				return col->getIslandTag();
 		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
 	}
 	return -1;
 }
@@ -134,6 +137,10 @@ void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray&
 		if (m_erp!=0)
 		{
 			btScalar currentPositionA = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
+			if (m_gearAuxLink >= 0)
+			{
+				currentPositionA -= m_bodyA->getJointPosMultiDof(m_gearAuxLink)[dof];
+			}
 			btScalar currentPositionB = m_gearRatio*m_bodyA->getJointPosMultiDof(m_linkB)[dof];
 			btScalar diff = currentPositionB+currentPositionA;
 			btScalar desiredPositionDiff = this->m_relativePositionTarget;
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
index 6d173b66a1..35c929f7ce 100644
--- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
+++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
@@ -53,17 +53,22 @@ btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
 {
 }
 
+
 int btMultiBodyJointLimitConstraint::getIslandIdA() const
 {
-	if(m_bodyA)
+
+	if (m_bodyA)
 	{
-		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-		for (int i=0;i<m_bodyA->getNumLinks();i++)
+		if (m_linkA < 0)
 		{
-			if (m_bodyA->getLink(i).m_collider)
-				return m_bodyA->getLink(i).m_collider->getIslandTag();
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
 		}
 	}
 	return -1;
@@ -71,18 +76,19 @@ int btMultiBodyJointLimitConstraint::getIslandIdA() const
 
 int btMultiBodyJointLimitConstraint::getIslandIdB() const
 {
-	if(m_bodyB)
+	if (m_bodyB)
 	{
-		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-
-		for (int i=0;i<m_bodyB->getNumLinks();i++)
+		if (m_linkB < 0)
 		{
-			col = m_bodyB->getLink(i).m_collider;
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
 			if (col)
 				return col->getIslandTag();
 		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
 	}
 	return -1;
 }
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
index e0921178e9..2a70ea97e5 100644
--- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
+++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
@@ -74,29 +74,37 @@ btMultiBodyJointMotor::~btMultiBodyJointMotor()
 
 int btMultiBodyJointMotor::getIslandIdA() const
 {
-	btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-	if (col)
-		return col->getIslandTag();
-	for (int i=0;i<m_bodyA->getNumLinks();i++)
+	if (this->m_linkA < 0)
 	{
-		if (m_bodyA->getLink(i).m_collider)
-			return m_bodyA->getLink(i).m_collider->getIslandTag();
+		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+		if (col)
+			return col->getIslandTag();
+	}
+	else
+	{
+		if (m_bodyA->getLink(m_linkA).m_collider)
+		{
+			return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
+		}
 	}
 	return -1;
 }
 
 int btMultiBodyJointMotor::getIslandIdB() const
 {
-	btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
-	if (col)
-		return col->getIslandTag();
-
-	for (int i=0;i<m_bodyB->getNumLinks();i++)
+	if (m_linkB < 0)
 	{
-		col = m_bodyB->getLink(i).m_collider;
+		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
 		if (col)
 			return col->getIslandTag();
 	}
+	else
+	{
+		if (m_bodyB->getLink(m_linkB).m_collider)
+		{
+			return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
+	}
 	return -1;
 }
 
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h
index 01828e5843..21c9e7a557 100644
--- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h
+++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h
@@ -182,6 +182,8 @@ btVector3 m_appliedConstraintForce;    // In WORLD frame
 		m_cachedRVector.setValue(0, 0, 0);
 		m_appliedForce.setValue( 0, 0, 0);
 		m_appliedTorque.setValue(0, 0, 0);
+		m_appliedConstraintForce.setValue(0,0,0);
+		m_appliedConstraintTorque.setValue(0,0,0);
 		//		
 		m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f;
 		m_jointPos[3] = 1.f;			//"quat.w"
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h
index 671e15d314..7092e62b5a 100644
--- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h
+++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h
@@ -19,6 +19,16 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 
 #include "btMultiBody.h"
+#include "LinearMath/btSerializer.h"
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btMultiBodyLinkColliderData btMultiBodyLinkColliderDoubleData
+#define btMultiBodyLinkColliderDataName "btMultiBodyLinkColliderDoubleData"
+#else
+#define btMultiBodyLinkColliderData btMultiBodyLinkColliderFloatData
+#define btMultiBodyLinkColliderDataName "btMultiBodyLinkColliderFloatData"
+#endif
+
 
 class btMultiBodyLinkCollider : public btCollisionObject
 {
@@ -119,7 +129,49 @@ public:
 		}
 		return true;
 	}
+
+	virtual	int	calculateSerializeBufferSize()	const;
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual	const char*	serialize(void* dataBuffer,  class btSerializer* serializer) const;
+
+};
+
+
+struct	btMultiBodyLinkColliderFloatData
+{
+	btCollisionObjectFloatData m_colObjData;
+	btMultiBodyFloatData	*m_multiBody;
+	int			m_link;
+	char		m_padding[4];
 };
 
+struct	btMultiBodyLinkColliderDoubleData
+{
+	btCollisionObjectDoubleData m_colObjData;
+	btMultiBodyDoubleData		*m_multiBody;
+	int			m_link;
+	char		m_padding[4];
+};
+
+SIMD_FORCE_INLINE	int	btMultiBodyLinkCollider::calculateSerializeBufferSize() const
+{
+	return sizeof(btMultiBodyLinkColliderData);
+}
+
+SIMD_FORCE_INLINE	const char*	btMultiBodyLinkCollider::serialize(void* dataBuffer,  class btSerializer* serializer) const
+{
+	btMultiBodyLinkColliderData* dataOut = (btMultiBodyLinkColliderData*)dataBuffer;
+	btCollisionObject::serialize(&dataOut->m_colObjData,serializer);
+	
+	dataOut->m_link = this->m_link;
+	dataOut->m_multiBody = (btMultiBodyData*)serializer->getUniquePointer(m_multiBody);
+
+	// Fill padding with zeros to appease msan.
+	memset(dataOut->m_padding, 0, sizeof(dataOut->m_padding));
+
+	return btMultiBodyLinkColliderDataName;
+}
+
 #endif //BT_FEATHERSTONE_LINK_COLLIDER_H
 
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp
new file mode 100644
index 0000000000..338e8af0ab
--- /dev/null
+++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp
@@ -0,0 +1,966 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2018 Google Inc. http://bulletphysics.org
+
+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 "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
+#include "BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h"
+
+#define DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+
+static bool interleaveContactAndFriction = false;
+
+struct btJointNode
+{
+	int jointIndex;          // pointer to enclosing dxJoint object
+	int otherBodyIndex;      // *other* body this joint is connected to
+	int nextJointNodeIndex;  //-1 for null
+	int constraintRowIndex;
+};
+
+// Helper function to compute a delta velocity in the constraint space.
+static btScalar computeDeltaVelocityInConstraintSpace(
+	const btVector3& angularDeltaVelocity,
+	const btVector3& contactNormal,
+	btScalar invMass,
+	const btVector3& angularJacobian,
+	const btVector3& linearJacobian)
+{
+	return angularDeltaVelocity.dot(angularJacobian) + contactNormal.dot(linearJacobian) * invMass;
+}
+
+// Faster version of computeDeltaVelocityInConstraintSpace that can be used when contactNormal and linearJacobian are
+// identical.
+static btScalar computeDeltaVelocityInConstraintSpace(
+	const btVector3& angularDeltaVelocity,
+	btScalar invMass,
+	const btVector3& angularJacobian)
+{
+	return angularDeltaVelocity.dot(angularJacobian) + invMass;
+}
+
+// Helper function to compute a delta velocity in the constraint space.
+static btScalar computeDeltaVelocityInConstraintSpace(const btScalar* deltaVelocity, const btScalar* jacobian, int size)
+{
+	btScalar result = 0;
+	for (int i = 0; i < size; ++i)
+		result += deltaVelocity[i] * jacobian[i];
+
+	return result;
+}
+
+static btScalar computeConstraintMatrixDiagElementMultiBody(
+	const btAlignedObjectArray<btSolverBody>& solverBodyPool,
+	const btMultiBodyJacobianData& data,
+	const btMultiBodySolverConstraint& constraint)
+{
+	btScalar ret = 0;
+
+	const btMultiBody* multiBodyA = constraint.m_multiBodyA;
+	const btMultiBody* multiBodyB = constraint.m_multiBodyB;
+
+	if (multiBodyA)
+	{
+		const btScalar* jacA = &data.m_jacobians[constraint.m_jacAindex];
+		const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex];
+		const int ndofA = multiBodyA->getNumDofs() + 6;
+		ret += computeDeltaVelocityInConstraintSpace(deltaA, jacA, ndofA);
+	}
+	else
+	{
+		const int solverBodyIdA = constraint.m_solverBodyIdA;
+		btAssert(solverBodyIdA != -1);
+		const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
+		const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0;
+		ret += computeDeltaVelocityInConstraintSpace(
+			constraint.m_relpos1CrossNormal,
+			invMassA,
+			constraint.m_angularComponentA);
+	}
+
+	if (multiBodyB)
+	{
+		const btScalar* jacB = &data.m_jacobians[constraint.m_jacBindex];
+		const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex];
+		const int ndofB = multiBodyB->getNumDofs() + 6;
+		ret += computeDeltaVelocityInConstraintSpace(deltaB, jacB, ndofB);
+	}
+	else
+	{
+		const int solverBodyIdB = constraint.m_solverBodyIdB;
+		btAssert(solverBodyIdB != -1);
+		const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
+		const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0;
+		ret += computeDeltaVelocityInConstraintSpace(
+			constraint.m_relpos2CrossNormal,
+			invMassB,
+			constraint.m_angularComponentB);
+	}
+
+	return ret;
+}
+
+static btScalar computeConstraintMatrixOffDiagElementMultiBody(
+	const btAlignedObjectArray<btSolverBody>& solverBodyPool,
+	const btMultiBodyJacobianData& data,
+	const btMultiBodySolverConstraint& constraint,
+	const btMultiBodySolverConstraint& offDiagConstraint)
+{
+	btScalar offDiagA = btScalar(0);
+
+	const btMultiBody* multiBodyA = constraint.m_multiBodyA;
+	const btMultiBody* multiBodyB = constraint.m_multiBodyB;
+	const btMultiBody* offDiagMultiBodyA = offDiagConstraint.m_multiBodyA;
+	const btMultiBody* offDiagMultiBodyB = offDiagConstraint.m_multiBodyB;
+
+	// Assumed at least one system is multibody
+	btAssert(multiBodyA || multiBodyB);
+	btAssert(offDiagMultiBodyA || offDiagMultiBodyB);
+
+	if (offDiagMultiBodyA)
+	{
+		const btScalar* offDiagJacA = &data.m_jacobians[offDiagConstraint.m_jacAindex];
+
+		if (offDiagMultiBodyA == multiBodyA)
+		{
+			const int ndofA = multiBodyA->getNumDofs() + 6;
+			const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex];
+			offDiagA += computeDeltaVelocityInConstraintSpace(deltaA, offDiagJacA, ndofA);
+		}
+		else if (offDiagMultiBodyA == multiBodyB)
+		{
+			const int ndofB = multiBodyB->getNumDofs() + 6;
+			const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex];
+			offDiagA += computeDeltaVelocityInConstraintSpace(deltaB, offDiagJacA, ndofB);
+		}
+	}
+	else
+	{
+		const int solverBodyIdA = constraint.m_solverBodyIdA;
+		const int solverBodyIdB = constraint.m_solverBodyIdB;
+
+		const int offDiagSolverBodyIdA = offDiagConstraint.m_solverBodyIdA;
+		btAssert(offDiagSolverBodyIdA != -1);
+
+		if (offDiagSolverBodyIdA == solverBodyIdA)
+		{
+			btAssert(solverBodyIdA != -1);
+			const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
+			const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0;
+			offDiagA += computeDeltaVelocityInConstraintSpace(
+				offDiagConstraint.m_relpos1CrossNormal,
+				offDiagConstraint.m_contactNormal1,
+				invMassA, constraint.m_angularComponentA,
+				constraint.m_contactNormal1);
+		}
+		else if (offDiagSolverBodyIdA == solverBodyIdB)
+		{
+			btAssert(solverBodyIdB != -1);
+			const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
+			const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0;
+			offDiagA += computeDeltaVelocityInConstraintSpace(
+				offDiagConstraint.m_relpos1CrossNormal,
+				offDiagConstraint.m_contactNormal1,
+				invMassB,
+				constraint.m_angularComponentB,
+				constraint.m_contactNormal2);
+		}
+	}
+
+	if (offDiagMultiBodyB)
+	{
+		const btScalar* offDiagJacB = &data.m_jacobians[offDiagConstraint.m_jacBindex];
+
+		if (offDiagMultiBodyB == multiBodyA)
+		{
+			const int ndofA = multiBodyA->getNumDofs() + 6;
+			const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex];
+			offDiagA += computeDeltaVelocityInConstraintSpace(deltaA, offDiagJacB, ndofA);
+		}
+		else if (offDiagMultiBodyB == multiBodyB)
+		{
+			const int ndofB = multiBodyB->getNumDofs() + 6;
+			const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex];
+			offDiagA += computeDeltaVelocityInConstraintSpace(deltaB, offDiagJacB, ndofB);
+		}
+	}
+	else
+	{
+		const int solverBodyIdA = constraint.m_solverBodyIdA;
+		const int solverBodyIdB = constraint.m_solverBodyIdB;
+
+		const int offDiagSolverBodyIdB = offDiagConstraint.m_solverBodyIdB;
+		btAssert(offDiagSolverBodyIdB != -1);
+
+		if (offDiagSolverBodyIdB == solverBodyIdA)
+		{
+			btAssert(solverBodyIdA != -1);
+			const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
+			const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0;
+			offDiagA += computeDeltaVelocityInConstraintSpace(
+				offDiagConstraint.m_relpos2CrossNormal,
+				offDiagConstraint.m_contactNormal2,
+				invMassA, constraint.m_angularComponentA,
+				constraint.m_contactNormal1);
+		}
+		else if (offDiagSolverBodyIdB == solverBodyIdB)
+		{
+			btAssert(solverBodyIdB != -1);
+			const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
+			const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0;
+			offDiagA += computeDeltaVelocityInConstraintSpace(
+				offDiagConstraint.m_relpos2CrossNormal,
+				offDiagConstraint.m_contactNormal2,
+				invMassB, constraint.m_angularComponentB,
+				constraint.m_contactNormal2);
+		}
+	}
+
+	return offDiagA;
+}
+
+void btMultiBodyMLCPConstraintSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
+{
+	createMLCPFastRigidBody(infoGlobal);
+	createMLCPFastMultiBody(infoGlobal);
+}
+
+void btMultiBodyMLCPConstraintSolver::createMLCPFastRigidBody(const btContactSolverInfo& infoGlobal)
+{
+	int numContactRows = interleaveContactAndFriction ? 3 : 1;
+
+	int numConstraintRows = m_allConstraintPtrArray.size();
+
+	if (numConstraintRows == 0)
+		return;
+
+	int n = numConstraintRows;
+	{
+		BT_PROFILE("init b (rhs)");
+		m_b.resize(numConstraintRows);
+		m_bSplit.resize(numConstraintRows);
+		m_b.setZero();
+		m_bSplit.setZero();
+		for (int i = 0; i < numConstraintRows; i++)
+		{
+			btScalar jacDiag = m_allConstraintPtrArray[i]->m_jacDiagABInv;
+			if (!btFuzzyZero(jacDiag))
+			{
+				btScalar rhs = m_allConstraintPtrArray[i]->m_rhs;
+				btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration;
+				m_b[i] = rhs / jacDiag;
+				m_bSplit[i] = rhsPenetration / jacDiag;
+			}
+		}
+	}
+
+	//	btScalar* w = 0;
+	//	int nub = 0;
+
+	m_lo.resize(numConstraintRows);
+	m_hi.resize(numConstraintRows);
+
+	{
+		BT_PROFILE("init lo/ho");
+
+		for (int i = 0; i < numConstraintRows; i++)
+		{
+			if (0)  //m_limitDependencies[i]>=0)
+			{
+				m_lo[i] = -BT_INFINITY;
+				m_hi[i] = BT_INFINITY;
+			}
+			else
+			{
+				m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
+				m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
+			}
+		}
+	}
+
+	//
+	int m = m_allConstraintPtrArray.size();
+
+	int numBodies = m_tmpSolverBodyPool.size();
+	btAlignedObjectArray<int> bodyJointNodeArray;
+	{
+		BT_PROFILE("bodyJointNodeArray.resize");
+		bodyJointNodeArray.resize(numBodies, -1);
+	}
+	btAlignedObjectArray<btJointNode> jointNodeArray;
+	{
+		BT_PROFILE("jointNodeArray.reserve");
+		jointNodeArray.reserve(2 * m_allConstraintPtrArray.size());
+	}
+
+	btMatrixXu& J3 = m_scratchJ3;
+	{
+		BT_PROFILE("J3.resize");
+		J3.resize(2 * m, 8);
+	}
+	btMatrixXu& JinvM3 = m_scratchJInvM3;
+	{
+		BT_PROFILE("JinvM3.resize/setZero");
+
+		JinvM3.resize(2 * m, 8);
+		JinvM3.setZero();
+		J3.setZero();
+	}
+	int cur = 0;
+	int rowOffset = 0;
+	btAlignedObjectArray<int>& ofs = m_scratchOfs;
+	{
+		BT_PROFILE("ofs resize");
+		ofs.resize(0);
+		ofs.resizeNoInitialize(m_allConstraintPtrArray.size());
+	}
+	{
+		BT_PROFILE("Compute J and JinvM");
+		int c = 0;
+
+		int numRows = 0;
+
+		for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
+		{
+			ofs[c] = rowOffset;
+			int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
+			int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
+			btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
+			btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
+
+			numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
+			if (orgBodyA)
+			{
+				{
+					int slotA = -1;
+					//find free jointNode slot for sbA
+					slotA = jointNodeArray.size();
+					jointNodeArray.expand();  //NonInitializing();
+					int prevSlot = bodyJointNodeArray[sbA];
+					bodyJointNodeArray[sbA] = slotA;
+					jointNodeArray[slotA].nextJointNodeIndex = prevSlot;
+					jointNodeArray[slotA].jointIndex = c;
+					jointNodeArray[slotA].constraintRowIndex = i;
+					jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1;
+				}
+				for (int row = 0; row < numRows; row++, cur++)
+				{
+					btVector3 normalInvMass = m_allConstraintPtrArray[i + row]->m_contactNormal1 * orgBodyA->getInvMass();
+					btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld();
+
+					for (int r = 0; r < 3; r++)
+					{
+						J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal1[r]);
+						J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal[r]);
+						JinvM3.setElem(cur, r, normalInvMass[r]);
+						JinvM3.setElem(cur, r + 4, relPosCrossNormalInvInertia[r]);
+					}
+					J3.setElem(cur, 3, 0);
+					JinvM3.setElem(cur, 3, 0);
+					J3.setElem(cur, 7, 0);
+					JinvM3.setElem(cur, 7, 0);
+				}
+			}
+			else
+			{
+				cur += numRows;
+			}
+			if (orgBodyB)
+			{
+				{
+					int slotB = -1;
+					//find free jointNode slot for sbA
+					slotB = jointNodeArray.size();
+					jointNodeArray.expand();  //NonInitializing();
+					int prevSlot = bodyJointNodeArray[sbB];
+					bodyJointNodeArray[sbB] = slotB;
+					jointNodeArray[slotB].nextJointNodeIndex = prevSlot;
+					jointNodeArray[slotB].jointIndex = c;
+					jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1;
+					jointNodeArray[slotB].constraintRowIndex = i;
+				}
+
+				for (int row = 0; row < numRows; row++, cur++)
+				{
+					btVector3 normalInvMassB = m_allConstraintPtrArray[i + row]->m_contactNormal2 * orgBodyB->getInvMass();
+					btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld();
+
+					for (int r = 0; r < 3; r++)
+					{
+						J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal2[r]);
+						J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal[r]);
+						JinvM3.setElem(cur, r, normalInvMassB[r]);
+						JinvM3.setElem(cur, r + 4, relPosInvInertiaB[r]);
+					}
+					J3.setElem(cur, 3, 0);
+					JinvM3.setElem(cur, 3, 0);
+					J3.setElem(cur, 7, 0);
+					JinvM3.setElem(cur, 7, 0);
+				}
+			}
+			else
+			{
+				cur += numRows;
+			}
+			rowOffset += numRows;
+		}
+	}
+
+	//compute JinvM = J*invM.
+	const btScalar* JinvM = JinvM3.getBufferPointer();
+
+	const btScalar* Jptr = J3.getBufferPointer();
+	{
+		BT_PROFILE("m_A.resize");
+		m_A.resize(n, n);
+	}
+
+	{
+		BT_PROFILE("m_A.setZero");
+		m_A.setZero();
+	}
+	int c = 0;
+	{
+		int numRows = 0;
+		BT_PROFILE("Compute A");
+		for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
+		{
+			int row__ = ofs[c];
+			int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
+			int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
+			//	btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
+			//	btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
+
+			numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
+
+			const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
+
+			{
+				int startJointNodeA = bodyJointNodeArray[sbA];
+				while (startJointNodeA >= 0)
+				{
+					int j0 = jointNodeArray[startJointNodeA].jointIndex;
+					int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex;
+					if (j0 < c)
+					{
+						int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows;
+						size_t ofsother = (m_allConstraintPtrArray[cr0]->m_solverBodyIdB == sbA) ? 8 * numRowsOther : 0;
+						//printf("%d joint i %d and j0: %d: ",count++,i,j0);
+						m_A.multiplyAdd2_p8r(JinvMrow,
+											 Jptr + 2 * 8 * (size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__, ofs[j0]);
+					}
+					startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex;
+				}
+			}
+
+			{
+				int startJointNodeB = bodyJointNodeArray[sbB];
+				while (startJointNodeB >= 0)
+				{
+					int j1 = jointNodeArray[startJointNodeB].jointIndex;
+					int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex;
+
+					if (j1 < c)
+					{
+						int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows;
+						size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8 * numRowsOther : 0;
+						m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)numRows,
+											 Jptr + 2 * 8 * (size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__, ofs[j1]);
+					}
+					startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex;
+				}
+			}
+		}
+
+		{
+			BT_PROFILE("compute diagonal");
+			// compute diagonal blocks of m_A
+
+			int row__ = 0;
+			int numJointRows = m_allConstraintPtrArray.size();
+
+			int jj = 0;
+			for (; row__ < numJointRows;)
+			{
+				//int sbA = m_allConstraintPtrArray[row__]->m_solverBodyIdA;
+				int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB;
+				//	btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
+				btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
+
+				const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows;
+
+				const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
+				const btScalar* Jrow = Jptr + 2 * 8 * (size_t)row__;
+				m_A.multiply2_p8r(JinvMrow, Jrow, infom, infom, row__, row__);
+				if (orgBodyB)
+				{
+					m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)infom, Jrow + 8 * (size_t)infom, infom, infom, row__, row__);
+				}
+				row__ += infom;
+				jj++;
+			}
+		}
+	}
+
+	if (1)
+	{
+		// add cfm to the diagonal of m_A
+		for (int i = 0; i < m_A.rows(); ++i)
+		{
+			m_A.setElem(i, i, m_A(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
+		}
+	}
+
+	///fill the upper triangle of the matrix, to make it symmetric
+	{
+		BT_PROFILE("fill the upper triangle ");
+		m_A.copyLowerToUpperTriangle();
+	}
+
+	{
+		BT_PROFILE("resize/init x");
+		m_x.resize(numConstraintRows);
+		m_xSplit.resize(numConstraintRows);
+
+		if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+		{
+			for (int i = 0; i < m_allConstraintPtrArray.size(); i++)
+			{
+				const btSolverConstraint& c = *m_allConstraintPtrArray[i];
+				m_x[i] = c.m_appliedImpulse;
+				m_xSplit[i] = c.m_appliedPushImpulse;
+			}
+		}
+		else
+		{
+			m_x.setZero();
+			m_xSplit.setZero();
+		}
+	}
+}
+
+void btMultiBodyMLCPConstraintSolver::createMLCPFastMultiBody(const btContactSolverInfo& infoGlobal)
+{
+	const int multiBodyNumConstraints = m_multiBodyAllConstraintPtrArray.size();
+
+	if (multiBodyNumConstraints == 0)
+		return;
+
+	// 1. Compute b
+	{
+		BT_PROFILE("init b (rhs)");
+
+		m_multiBodyB.resize(multiBodyNumConstraints);
+		m_multiBodyB.setZero();
+
+		for (int i = 0; i < multiBodyNumConstraints; ++i)
+		{
+			const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
+			const btScalar jacDiag = constraint.m_jacDiagABInv;
+
+			if (!btFuzzyZero(jacDiag))
+			{
+				// Note that rhsPenetration is currently always zero because the split impulse hasn't been implemented for multibody yet.
+				const btScalar rhs = constraint.m_rhs;
+				m_multiBodyB[i] = rhs / jacDiag;
+			}
+		}
+	}
+
+	// 2. Compute lo and hi
+	{
+		BT_PROFILE("init lo/ho");
+
+		m_multiBodyLo.resize(multiBodyNumConstraints);
+		m_multiBodyHi.resize(multiBodyNumConstraints);
+
+		for (int i = 0; i < multiBodyNumConstraints; ++i)
+		{
+			const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
+			m_multiBodyLo[i] = constraint.m_lowerLimit;
+			m_multiBodyHi[i] = constraint.m_upperLimit;
+		}
+	}
+
+	// 3. Construct A matrix by using the impulse testing
+	{
+		BT_PROFILE("Compute A");
+
+		{
+			BT_PROFILE("m_A.resize");
+			m_multiBodyA.resize(multiBodyNumConstraints, multiBodyNumConstraints);
+		}
+
+		for (int i = 0; i < multiBodyNumConstraints; ++i)
+		{
+			// Compute the diagonal of A, which is A(i, i)
+			const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
+			const btScalar diagA = computeConstraintMatrixDiagElementMultiBody(m_tmpSolverBodyPool, m_data, constraint);
+			m_multiBodyA.setElem(i, i, diagA);
+
+			// Computes the off-diagonals of A:
+			//   a. The rest of i-th row of A, from A(i, i+1) to A(i, n)
+			//   b. The rest of i-th column of A, from A(i+1, i) to A(n, i)
+			for (int j = i + 1; j < multiBodyNumConstraints; ++j)
+			{
+				const btMultiBodySolverConstraint& offDiagConstraint = *m_multiBodyAllConstraintPtrArray[j];
+				const btScalar offDiagA = computeConstraintMatrixOffDiagElementMultiBody(m_tmpSolverBodyPool, m_data, constraint, offDiagConstraint);
+
+				// Set the off-diagonal values of A. Note that A is symmetric.
+				m_multiBodyA.setElem(i, j, offDiagA);
+				m_multiBodyA.setElem(j, i, offDiagA);
+			}
+		}
+	}
+
+	// Add CFM to the diagonal of m_A
+	for (int i = 0; i < m_multiBodyA.rows(); ++i)
+	{
+		m_multiBodyA.setElem(i, i, m_multiBodyA(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
+	}
+
+	// 4. Initialize x
+	{
+		BT_PROFILE("resize/init x");
+
+		m_multiBodyX.resize(multiBodyNumConstraints);
+
+		if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+		{
+			for (int i = 0; i < multiBodyNumConstraints; ++i)
+			{
+				const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
+				m_multiBodyX[i] = constraint.m_appliedImpulse;
+			}
+		}
+		else
+		{
+			m_multiBodyX.setZero();
+		}
+	}
+}
+
+bool btMultiBodyMLCPConstraintSolver::solveMLCP(const btContactSolverInfo& infoGlobal)
+{
+	bool result = true;
+
+	if (m_A.rows() != 0)
+	{
+		// If using split impulse, we solve 2 separate (M)LCPs
+		if (infoGlobal.m_splitImpulse)
+		{
+			const btMatrixXu Acopy = m_A;
+			const btAlignedObjectArray<int> limitDependenciesCopy = m_limitDependencies;
+			// TODO(JS): Do we really need these copies when solveMLCP takes them as const?
+
+			result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations);
+			if (result)
+				result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo, m_hi, limitDependenciesCopy, infoGlobal.m_numIterations);
+		}
+		else
+		{
+			result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations);
+		}
+	}
+
+	if (!result)
+		return false;
+
+	if (m_multiBodyA.rows() != 0)
+	{
+		result = m_solver->solveMLCP(m_multiBodyA, m_multiBodyB, m_multiBodyX, m_multiBodyLo, m_multiBodyHi, m_multiBodyLimitDependencies, infoGlobal.m_numIterations);
+	}
+
+	return result;
+}
+
+btScalar btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlySetup(
+	btCollisionObject** bodies,
+	int numBodies,
+	btPersistentManifold** manifoldPtr,
+	int numManifolds,
+	btTypedConstraint** constraints,
+	int numConstraints,
+	const btContactSolverInfo& infoGlobal,
+	btIDebugDraw* debugDrawer)
+{
+	// 1. Setup for rigid-bodies
+	btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(
+		bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+
+	// 2. Setup for multi-bodies
+	//   a. Collect all different kinds of constraint as pointers into one array, m_allConstraintPtrArray
+	//   b. Set the index array for frictional contact constraints, m_limitDependencies
+	{
+		BT_PROFILE("gather constraint data");
+
+		int dindex = 0;
+
+		const int numRigidBodyConstraints = m_tmpSolverNonContactConstraintPool.size() + m_tmpSolverContactConstraintPool.size() + m_tmpSolverContactFrictionConstraintPool.size();
+		const int numMultiBodyConstraints = m_multiBodyNonContactConstraints.size() + m_multiBodyNormalContactConstraints.size() + m_multiBodyFrictionContactConstraints.size();
+
+		m_allConstraintPtrArray.resize(0);
+		m_multiBodyAllConstraintPtrArray.resize(0);
+
+		// i. Setup for rigid bodies
+
+		m_limitDependencies.resize(numRigidBodyConstraints);
+
+		for (int i = 0; i < m_tmpSolverNonContactConstraintPool.size(); ++i)
+		{
+			m_allConstraintPtrArray.push_back(&m_tmpSolverNonContactConstraintPool[i]);
+			m_limitDependencies[dindex++] = -1;
+		}
+
+		int firstContactConstraintOffset = dindex;
+
+		// The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
+		if (interleaveContactAndFriction)
+		{
+			for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
+			{
+				const int numFrictionPerContact = m_tmpSolverContactConstraintPool.size() == m_tmpSolverContactFrictionConstraintPool.size() ? 1 : 2;
+
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
+				m_limitDependencies[dindex++] = -1;
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact]);
+				int findex = (m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact].m_frictionIndex * (1 + numFrictionPerContact));
+				m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
+				if (numFrictionPerContact == 2)
+				{
+					m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact + 1]);
+					m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
+				}
+			}
+		}
+		else
+		{
+			for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
+			{
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
+				m_limitDependencies[dindex++] = -1;
+			}
+			for (int i = 0; i < m_tmpSolverContactFrictionConstraintPool.size(); i++)
+			{
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i]);
+				m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex + firstContactConstraintOffset;
+			}
+		}
+
+		if (!m_allConstraintPtrArray.size())
+		{
+			m_A.resize(0, 0);
+			m_b.resize(0);
+			m_x.resize(0);
+			m_lo.resize(0);
+			m_hi.resize(0);
+		}
+
+		// ii. Setup for multibodies
+
+		dindex = 0;
+
+		m_multiBodyLimitDependencies.resize(numMultiBodyConstraints);
+
+		for (int i = 0; i < m_multiBodyNonContactConstraints.size(); ++i)
+		{
+			m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNonContactConstraints[i]);
+			m_multiBodyLimitDependencies[dindex++] = -1;
+		}
+
+		firstContactConstraintOffset = dindex;
+
+		// The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
+		if (interleaveContactAndFriction)
+		{
+			for (int i = 0; i < m_multiBodyNormalContactConstraints.size(); ++i)
+			{
+				const int numtiBodyNumFrictionPerContact = m_multiBodyNormalContactConstraints.size() == m_multiBodyFrictionContactConstraints.size() ? 1 : 2;
+
+				m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNormalContactConstraints[i]);
+				m_multiBodyLimitDependencies[dindex++] = -1;
+
+				btMultiBodySolverConstraint& frictionContactConstraint1 = m_multiBodyFrictionContactConstraints[i * numtiBodyNumFrictionPerContact];
+				m_multiBodyAllConstraintPtrArray.push_back(&frictionContactConstraint1);
+
+				const int findex = (frictionContactConstraint1.m_frictionIndex * (1 + numtiBodyNumFrictionPerContact)) + firstContactConstraintOffset;
+
+				m_multiBodyLimitDependencies[dindex++] = findex;
+
+				if (numtiBodyNumFrictionPerContact == 2)
+				{
+					btMultiBodySolverConstraint& frictionContactConstraint2 = m_multiBodyFrictionContactConstraints[i * numtiBodyNumFrictionPerContact + 1];
+					m_multiBodyAllConstraintPtrArray.push_back(&frictionContactConstraint2);
+
+					m_multiBodyLimitDependencies[dindex++] = findex;
+				}
+			}
+		}
+		else
+		{
+			for (int i = 0; i < m_multiBodyNormalContactConstraints.size(); ++i)
+			{
+				m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNormalContactConstraints[i]);
+				m_multiBodyLimitDependencies[dindex++] = -1;
+			}
+			for (int i = 0; i < m_multiBodyFrictionContactConstraints.size(); ++i)
+			{
+				m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyFrictionContactConstraints[i]);
+				m_multiBodyLimitDependencies[dindex++] = m_multiBodyFrictionContactConstraints[i].m_frictionIndex + firstContactConstraintOffset;
+			}
+		}
+
+		if (!m_multiBodyAllConstraintPtrArray.size())
+		{
+			m_multiBodyA.resize(0, 0);
+			m_multiBodyB.resize(0);
+			m_multiBodyX.resize(0);
+			m_multiBodyLo.resize(0);
+			m_multiBodyHi.resize(0);
+		}
+	}
+
+	// Construct MLCP terms
+	{
+		BT_PROFILE("createMLCPFast");
+		createMLCPFast(infoGlobal);
+	}
+
+	return btScalar(0);
+}
+
+btScalar btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
+{
+	bool result = true;
+	{
+		BT_PROFILE("solveMLCP");
+		result = solveMLCP(infoGlobal);
+	}
+
+	// Fallback to btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations if the solution isn't valid.
+	if (!result)
+	{
+		m_fallback++;
+		return btMultiBodyConstraintSolver::solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+	}
+
+	{
+		BT_PROFILE("process MLCP results");
+
+		for (int i = 0; i < m_allConstraintPtrArray.size(); ++i)
+		{
+			const btSolverConstraint& c = *m_allConstraintPtrArray[i];
+
+			const btScalar deltaImpulse = m_x[i] - c.m_appliedImpulse;
+			c.m_appliedImpulse = m_x[i];
+
+			int sbA = c.m_solverBodyIdA;
+			int sbB = c.m_solverBodyIdB;
+
+			btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA];
+			btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB];
+
+			solverBodyA.internalApplyImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
+			solverBodyB.internalApplyImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
+
+			if (infoGlobal.m_splitImpulse)
+			{
+				const btScalar deltaPushImpulse = m_xSplit[i] - c.m_appliedPushImpulse;
+				solverBodyA.internalApplyPushImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaPushImpulse);
+				solverBodyB.internalApplyPushImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaPushImpulse);
+				c.m_appliedPushImpulse = m_xSplit[i];
+			}
+		}
+
+		for (int i = 0; i < m_multiBodyAllConstraintPtrArray.size(); ++i)
+		{
+			btMultiBodySolverConstraint& c = *m_multiBodyAllConstraintPtrArray[i];
+
+			const btScalar deltaImpulse = m_multiBodyX[i] - c.m_appliedImpulse;
+			c.m_appliedImpulse = m_multiBodyX[i];
+
+			btMultiBody* multiBodyA = c.m_multiBodyA;
+			if (multiBodyA)
+			{
+				const int ndofA = multiBodyA->getNumDofs() + 6;
+				applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse, c.m_deltaVelAindex, ndofA);
+#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+				//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
+				//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
+				multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse);
+#endif  // DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+			}
+			else
+			{
+				const int sbA = c.m_solverBodyIdA;
+				btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA];
+				solverBodyA.internalApplyImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
+			}
+
+			btMultiBody* multiBodyB = c.m_multiBodyB;
+			if (multiBodyB)
+			{
+				const int ndofB = multiBodyB->getNumDofs() + 6;
+				applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse, c.m_deltaVelBindex, ndofB);
+#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+				//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
+				//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
+				multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse);
+#endif  // DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+			}
+			else
+			{
+				const int sbB = c.m_solverBodyIdB;
+				btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB];
+				solverBodyB.internalApplyImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
+			}
+		}
+	}
+
+	return btScalar(0);
+}
+
+btMultiBodyMLCPConstraintSolver::btMultiBodyMLCPConstraintSolver(btMLCPSolverInterface* solver)
+	: m_solver(solver), m_fallback(0)
+{
+	// Do nothing
+}
+
+btMultiBodyMLCPConstraintSolver::~btMultiBodyMLCPConstraintSolver()
+{
+	// Do nothing
+}
+
+void btMultiBodyMLCPConstraintSolver::setMLCPSolver(btMLCPSolverInterface* solver)
+{
+	m_solver = solver;
+}
+
+int btMultiBodyMLCPConstraintSolver::getNumFallbacks() const
+{
+	return m_fallback;
+}
+
+void btMultiBodyMLCPConstraintSolver::setNumFallbacks(int num)
+{
+	m_fallback = num;
+}
+
+btConstraintSolverType btMultiBodyMLCPConstraintSolver::getSolverType() const
+{
+	return BT_MLCP_SOLVER;
+}
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h
new file mode 100644
index 0000000000..6be36ba142
--- /dev/null
+++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h
@@ -0,0 +1,187 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2018 Google Inc. http://bulletphysics.org
+
+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_MULTIBODY_MLCP_CONSTRAINT_SOLVER_H
+#define BT_MULTIBODY_MLCP_CONSTRAINT_SOLVER_H
+
+#include "LinearMath/btMatrixX.h"
+#include "LinearMath/btThreads.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+
+class btMLCPSolverInterface;
+class btMultiBody;
+
+class btMultiBodyMLCPConstraintSolver : public btMultiBodyConstraintSolver
+{
+protected:
+	/// \name MLCP Formulation for Rigid Bodies
+	/// \{
+
+	/// A matrix in the MLCP formulation
+	btMatrixXu m_A;
+
+	/// b vector in the MLCP formulation.
+	btVectorXu m_b;
+
+	/// Constraint impulse, which is an output of MLCP solving.
+	btVectorXu m_x;
+
+	/// Lower bound of constraint impulse, \c m_x.
+	btVectorXu m_lo;
+
+	/// Upper bound of constraint impulse, \c m_x.
+	btVectorXu m_hi;
+
+	/// \}
+
+	/// \name Cache Variables for Split Impulse for Rigid Bodies
+	/// When using 'split impulse' we solve two separate (M)LCPs
+	/// \{
+
+	/// Split impulse Cache vector corresponding to \c m_b.
+	btVectorXu m_bSplit;
+
+	/// Split impulse cache vector corresponding to \c m_x.
+	btVectorXu m_xSplit;
+
+	/// \}
+
+	/// \name MLCP Formulation for Multibodies
+	/// \{
+
+	/// A matrix in the MLCP formulation
+	btMatrixXu m_multiBodyA;
+
+	/// b vector in the MLCP formulation.
+	btVectorXu m_multiBodyB;
+
+	/// Constraint impulse, which is an output of MLCP solving.
+	btVectorXu m_multiBodyX;
+
+	/// Lower bound of constraint impulse, \c m_x.
+	btVectorXu m_multiBodyLo;
+
+	/// Upper bound of constraint impulse, \c m_x.
+	btVectorXu m_multiBodyHi;
+
+	/// \}
+
+	/// Indices of normal contact constraint associated with frictional contact constraint for rigid bodies.
+	///
+	/// This is used by the MLCP solver to update the upper bounds of frictional contact impulse given intermediate
+	/// normal contact impulse. For example, i-th element represents the index of a normal constraint that is
+	/// accosiated with i-th frictional contact constraint if i-th constraint is a frictional contact constraint.
+	/// Otherwise, -1.
+	btAlignedObjectArray<int> m_limitDependencies;
+
+	/// Indices of normal contact constraint associated with frictional contact constraint for multibodies.
+	///
+	/// This is used by the MLCP solver to update the upper bounds of frictional contact impulse given intermediate
+	/// normal contact impulse. For example, i-th element represents the index of a normal constraint that is
+	/// accosiated with i-th frictional contact constraint if i-th constraint is a frictional contact constraint.
+	/// Otherwise, -1.
+	btAlignedObjectArray<int> m_multiBodyLimitDependencies;
+
+	/// Array of all the rigid body constraints
+	btAlignedObjectArray<btSolverConstraint*> m_allConstraintPtrArray;
+
+	/// Array of all the multibody constraints
+	btAlignedObjectArray<btMultiBodySolverConstraint*> m_multiBodyAllConstraintPtrArray;
+
+	/// MLCP solver
+	btMLCPSolverInterface* m_solver;
+
+	/// Count of fallbacks of using btSequentialImpulseConstraintSolver, which happens when the MLCP solver fails.
+	int m_fallback;
+
+	/// \name MLCP Scratch Variables
+	/// The following scratch variables are not stateful -- contents are cleared prior to each use.
+	/// They are only cached here to avoid extra memory allocations and deallocations and to ensure
+	/// that multiple instances of the solver can be run in parallel.
+	///
+	/// \{
+
+	/// Cache variable for constraint Jacobian matrix.
+	btMatrixXu m_scratchJ3;
+
+	/// Cache variable for constraint Jacobian times inverse mass matrix.
+	btMatrixXu m_scratchJInvM3;
+
+	/// Cache variable for offsets.
+	btAlignedObjectArray<int> m_scratchOfs;
+
+	/// \}
+
+	/// Constructs MLCP terms, which are \c m_A, \c m_b, \c m_lo, and \c m_hi.
+	virtual void createMLCPFast(const btContactSolverInfo& infoGlobal);
+
+	/// Constructs MLCP terms for constraints of two rigid bodies
+	void createMLCPFastRigidBody(const btContactSolverInfo& infoGlobal);
+
+	/// Constructs MLCP terms for constraints of two multi-bodies or one rigid body and one multibody
+	void createMLCPFastMultiBody(const btContactSolverInfo& infoGlobal);
+
+	/// Solves MLCP and returns the success
+	virtual bool solveMLCP(const btContactSolverInfo& infoGlobal);
+
+	// Documentation inherited
+	btScalar solveGroupCacheFriendlySetup(
+		btCollisionObject** bodies,
+		int numBodies,
+		btPersistentManifold** manifoldPtr,
+		int numManifolds,
+		btTypedConstraint** constraints,
+		int numConstraints,
+		const btContactSolverInfo& infoGlobal,
+		btIDebugDraw* debugDrawer) BT_OVERRIDE;
+
+	// Documentation inherited
+	btScalar solveGroupCacheFriendlyIterations(
+		btCollisionObject** bodies,
+		int numBodies,
+		btPersistentManifold** manifoldPtr,
+		int numManifolds,
+		btTypedConstraint** constraints,
+		int numConstraints,
+		const btContactSolverInfo& infoGlobal,
+		btIDebugDraw* debugDrawer) BT_OVERRIDE;
+
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR()
+
+	/// Constructor
+	///
+	/// \param[in] solver MLCP solver. Assumed it's not null.
+	/// \param[in] maxLCPSize Maximum size of LCP to solve using MLCP solver. If the MLCP size exceeds this number, sequaltial impulse method will be used.
+	explicit btMultiBodyMLCPConstraintSolver(btMLCPSolverInterface* solver);
+
+	/// Destructor
+	virtual ~btMultiBodyMLCPConstraintSolver();
+
+	/// Sets MLCP solver. Assumed it's not null.
+	void setMLCPSolver(btMLCPSolverInterface* solver);
+
+	/// Returns the number of fallbacks of using btSequentialImpulseConstraintSolver, which happens when the MLCP
+	/// solver fails.
+	int getNumFallbacks() const;
+
+	/// Sets the number of fallbacks. This function may be used to reset the number to zero.
+	void setNumFallbacks(int num);
+
+	/// Returns the constraint solver type.
+	virtual btConstraintSolverType getSolverType() const;
+};
+
+#endif  // BT_MULTIBODY_MLCP_CONSTRAINT_SOLVER_H
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
index 125d52ad0b..2b59f0b7a6 100644
--- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
+++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
@@ -64,13 +64,16 @@ int btMultiBodyPoint2Point::getIslandIdA() const
 
 	if (m_bodyA)
 	{
-		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-		for (int i=0;i<m_bodyA->getNumLinks();i++)
+		if (m_linkA < 0)
 		{
-			if (m_bodyA->getLink(i).m_collider)
-				return m_bodyA->getLink(i).m_collider->getIslandTag();
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();			
 		}
 	}
 	return -1;
@@ -82,16 +85,17 @@ int btMultiBodyPoint2Point::getIslandIdB() const
 		return m_rigidBodyB->getIslandTag();
 	if (m_bodyB)
 	{
-		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-
-		for (int i=0;i<m_bodyB->getNumLinks();i++)
+		if (m_linkB < 0)
 		{
-			col = m_bodyB->getLink(i).m_collider;
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
 			if (col)
 				return col->getIslandTag();
 		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
 	}
 	return -1;
 }
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp
index 3b64b8183f..43f26f9833 100644
--- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp
+++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp
@@ -68,13 +68,16 @@ int btMultiBodySliderConstraint::getIslandIdA() const
 
 	if (m_bodyA)
 	{
-		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-		for (int i=0;i<m_bodyA->getNumLinks();i++)
+		if (m_linkA < 0)
 		{
-			if (m_bodyA->getLink(i).m_collider)
-				return m_bodyA->getLink(i).m_collider->getIslandTag();
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
 		}
 	}
 	return -1;
@@ -86,20 +89,20 @@ int btMultiBodySliderConstraint::getIslandIdB() const
 		return m_rigidBodyB->getIslandTag();
 	if (m_bodyB)
 	{
-		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-
-		for (int i=0;i<m_bodyB->getNumLinks();i++)
+		if (m_linkB < 0)
 		{
-			col = m_bodyB->getLink(i).m_collider;
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
 			if (col)
 				return col->getIslandTag();
 		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
 	}
 	return -1;
 }
-
 void btMultiBodySliderConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
 {
     // Convert local points back to world
diff --git a/thirdparty/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/thirdparty/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp
index a7b1688469..f299aa34e8 100644
--- a/thirdparty/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp
+++ b/thirdparty/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp
@@ -121,12 +121,19 @@ void	btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedT
 	btQuaternion rotatingOrn(right,-wheel.m_rotation);
 	btMatrix3x3 rotatingMat(rotatingOrn);
 
-	btMatrix3x3 basis2(
-		right[0],fwd[0],up[0],
-		right[1],fwd[1],up[1],
-		right[2],fwd[2],up[2]
-	);
-	
+    btMatrix3x3 basis2;
+    basis2[0][m_indexRightAxis] = -right[0];
+    basis2[1][m_indexRightAxis] = -right[1];
+    basis2[2][m_indexRightAxis] = -right[2];
+
+    basis2[0][m_indexUpAxis] = up[0];
+    basis2[1][m_indexUpAxis] = up[1];
+    basis2[2][m_indexUpAxis] = up[2];
+
+    basis2[0][m_indexForwardAxis] = fwd[0];
+    basis2[1][m_indexForwardAxis] = fwd[1];
+    basis2[2][m_indexForwardAxis] = fwd[2];
+
 	wheel.m_worldTransform.setBasis(steeringMat * rotatingMat * basis2);
 	wheel.m_worldTransform.setOrigin(
 		wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength
@@ -493,8 +500,8 @@ struct btWheelContactPoint
 
 };
 
-btScalar calcRollingFriction(btWheelContactPoint& contactPoint);
-btScalar calcRollingFriction(btWheelContactPoint& contactPoint)
+btScalar calcRollingFriction(btWheelContactPoint& contactPoint, int numWheelsOnGround);
+btScalar calcRollingFriction(btWheelContactPoint& contactPoint, int numWheelsOnGround)
 {
 
 	btScalar j1=0.f;
@@ -513,7 +520,7 @@ btScalar calcRollingFriction(btWheelContactPoint& contactPoint)
 	btScalar vrel = contactPoint.m_frictionDirectionWorld.dot(vel);
 
 	// calculate j that moves us to zero relative velocity
-	j1 = -vrel * contactPoint.m_jacDiagABInv;
+	j1 = -vrel * contactPoint.m_jacDiagABInv/btScalar(numWheelsOnGround);
 	btSetMin(j1, maxImpulse);
 	btSetMax(j1, -maxImpulse);
 
@@ -567,7 +574,7 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 					const btTransform& wheelTrans = getWheelTransformWS( i );
 
 					btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
-					m_axle[i] = btVector3(	
+					m_axle[i] = -btVector3(	
 						wheelBasis0[0][m_indexRightAxis],
 						wheelBasis0[1][m_indexRightAxis],
 						wheelBasis0[2][m_indexRightAxis]);
@@ -615,7 +622,8 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 					btScalar defaultRollingFrictionImpulse = 0.f;
 					btScalar maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse;
 					btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,m_forwardWS[wheel],maxImpulse);
-					rollingFriction = calcRollingFriction(contactPt);
+					btAssert(numWheelsOnGround > 0);
+					rollingFriction = calcRollingFriction(contactPt, numWheelsOnGround);
 				}
 			}