btSequentialImpulseConstraintSolverMt.cpp

Go to the documentation of this file.

/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  https://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 "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);
        {
 
                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);
 
        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);
                }
        }
 
        {
                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;
}