btParallelConstraintSolver.cpp

Go to the documentation of this file.

/*
   Copyright (C) 2010 Sony Computer Entertainment Inc.
   All rights reserved.

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 "btParallelConstraintSolver.h"
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
#include "LinearMath/btPoolAllocator.h"
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
#include "BulletMultiThreaded/vectormath2bullet.h"

#include "LinearMath/btQuickprof.h"
#include "BulletMultiThreaded/btThreadSupportInterface.h"
#ifdef PFX_USE_FREE_VECTORMATH
#include "vecmath/vmInclude.h"
#else
#include "vectormath/vmInclude.h"
#endif //PFX_USE_FREE_VECTORMATH

#include "HeapManager.h"

#include "PlatformDefinitions.h"

//#include "PfxSimdUtils.h"
#include "LinearMath/btScalar.h"

#include "TrbStateVec.h"





#define TMP_BUFF_BYTES (15*1024*1024)
unsigned char ATTRIBUTE_ALIGNED128(tmp_buff[TMP_BUFF_BYTES]);



// Project Gauss Seidel or the equivalent Sequential Impulse
 inline void resolveSingleConstraintRowGeneric(PfxSolverBody& body1,PfxSolverBody& body2,const btSolverConstraint& c)
{

        btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
        const btScalar deltaVel1Dotn    =       c.m_contactNormal1.dot(getBtVector3(body1.mDeltaLinearVelocity))        + c.m_relpos1CrossNormal.dot(getBtVector3(body1.mDeltaAngularVelocity));
        const btScalar deltaVel2Dotn    =       c.m_contactNormal2.dot(getBtVector3(body2.mDeltaLinearVelocity))        + c.m_relpos2CrossNormal.dot(getBtVector3(body2.mDeltaAngularVelocity));
//      const btScalar delta_rel_vel    =       deltaVel1Dotn-deltaVel2Dotn;
        deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
        deltaImpulse    -=      deltaVel2Dotn*c.m_jacDiagABInv;

        const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
        if (sum < c.m_lowerLimit)
        {
                deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
                c.m_appliedImpulse = c.m_lowerLimit;
        }
        else if (sum > c.m_upperLimit) 
        {
                deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
                c.m_appliedImpulse = c.m_upperLimit;
        }
        else
        {
                c.m_appliedImpulse = sum;
        }
        

        if (body1.mMassInv)
        {
                btVector3 linearComponent = c.m_contactNormal1*body1.mMassInv;
                body1.mDeltaLinearVelocity += vmVector3(linearComponent.getX()*deltaImpulse,linearComponent.getY()*deltaImpulse,linearComponent.getZ()*deltaImpulse);
                btVector3 tmp=c.m_angularComponentA*(btVector3(deltaImpulse,deltaImpulse,deltaImpulse));
                body1.mDeltaAngularVelocity += vmVector3(tmp.getX(),tmp.getY(),tmp.getZ());
        }

        if (body2.mMassInv)
        {
                btVector3 linearComponent = c.m_contactNormal2*body2.mMassInv;
                body2.mDeltaLinearVelocity += vmVector3(linearComponent.getX()*deltaImpulse,linearComponent.getY()*deltaImpulse,linearComponent.getZ()*deltaImpulse);
                btVector3 tmp = c.m_angularComponentB*((btVector3(deltaImpulse,deltaImpulse,deltaImpulse)));//*m_angularFactor);
                body2.mDeltaAngularVelocity += vmVector3(tmp.getX(),tmp.getY(),tmp.getZ());
        }

        //body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
        //body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);

}

 
static SIMD_FORCE_INLINE
void pfxSolveLinearConstraintRow(btConstraintRow &constraint,
        vmVector3 &deltaLinearVelocityA,vmVector3 &deltaAngularVelocityA,
        float massInvA,const vmMatrix3 &inertiaInvA,const vmVector3 &rA,
        vmVector3 &deltaLinearVelocityB,vmVector3 &deltaAngularVelocityB,
        float massInvB,const vmMatrix3 &inertiaInvB,const vmVector3 &rB)
{
        const vmVector3 normal(btReadVector3(constraint.m_normal));
        btScalar deltaImpulse = constraint.m_rhs;
        vmVector3 dVA = deltaLinearVelocityA + cross(deltaAngularVelocityA,rA);
        vmVector3 dVB = deltaLinearVelocityB + cross(deltaAngularVelocityB,rB);
        deltaImpulse -= constraint.m_jacDiagInv * dot(normal,dVA-dVB);
        btScalar oldImpulse = constraint.m_accumImpulse;
        constraint.m_accumImpulse = btClamped(oldImpulse + deltaImpulse,constraint.m_lowerLimit,constraint.m_upperLimit);
        deltaImpulse = constraint.m_accumImpulse - oldImpulse;
        deltaLinearVelocityA += deltaImpulse * massInvA * normal;
        deltaAngularVelocityA += deltaImpulse * inertiaInvA * cross(rA,normal);
        deltaLinearVelocityB -= deltaImpulse * massInvB * normal;
        deltaAngularVelocityB -= deltaImpulse * inertiaInvB * cross(rB,normal);

}
 
void btSolveContactConstraint(
        btConstraintRow &constraintResponse,
        btConstraintRow &constraintFriction1,
        btConstraintRow &constraintFriction2,
        const vmVector3 &contactPointA,
        const vmVector3 &contactPointB,
        PfxSolverBody &solverBodyA,
        PfxSolverBody &solverBodyB,
        float friction
        )
{
        vmVector3 rA = rotate(solverBodyA.mOrientation,contactPointA);
        vmVector3 rB = rotate(solverBodyB.mOrientation,contactPointB);

        pfxSolveLinearConstraintRow(constraintResponse,
                solverBodyA.mDeltaLinearVelocity,solverBodyA.mDeltaAngularVelocity,solverBodyA.mMassInv,solverBodyA.mInertiaInv,rA,
                solverBodyB.mDeltaLinearVelocity,solverBodyB.mDeltaAngularVelocity,solverBodyB.mMassInv,solverBodyB.mInertiaInv,rB);

        float mf = friction*fabsf(constraintResponse.m_accumImpulse);
        constraintFriction1.m_lowerLimit = -mf;
        constraintFriction1.m_upperLimit =  mf;
        constraintFriction2.m_lowerLimit = -mf;
        constraintFriction2.m_upperLimit =  mf;

        pfxSolveLinearConstraintRow(constraintFriction1,
                solverBodyA.mDeltaLinearVelocity,solverBodyA.mDeltaAngularVelocity,solverBodyA.mMassInv,solverBodyA.mInertiaInv,rA,
                solverBodyB.mDeltaLinearVelocity,solverBodyB.mDeltaAngularVelocity,solverBodyB.mMassInv,solverBodyB.mInertiaInv,rB);

        pfxSolveLinearConstraintRow(constraintFriction2,
                solverBodyA.mDeltaLinearVelocity,solverBodyA.mDeltaAngularVelocity,solverBodyA.mMassInv,solverBodyA.mInertiaInv,rA,
                solverBodyB.mDeltaLinearVelocity,solverBodyB.mDeltaAngularVelocity,solverBodyB.mMassInv,solverBodyB.mInertiaInv,rB);
}


void CustomSolveConstraintsTaskParallel(
        const PfxParallelGroup *contactParallelGroup,const PfxParallelBatch *contactParallelBatches,
        PfxConstraintPair *contactPairs,uint32_t numContactPairs,
        btPersistentManifold* offsetContactManifolds,
        btConstraintRow*        offsetContactConstraintRows,
        const PfxParallelGroup *jointParallelGroup,const PfxParallelBatch *jointParallelBatches,
        PfxConstraintPair *jointPairs,uint32_t numJointPairs,
        btSolverConstraint* offsetSolverConstraints,
        TrbState *offsetRigStates,
        PfxSolverBody *offsetSolverBodies,
        uint32_t numRigidBodies,
        int iteration,unsigned int taskId,unsigned int numTasks,btBarrier *barrier)
{

        PfxSolverBody staticBody;
        staticBody.mMassInv = 0.f;
        staticBody.mDeltaAngularVelocity=vmVector3(0,0,0);
        staticBody.mDeltaLinearVelocity =vmVector3(0,0,0);


        for(int k=0;k<iteration+1;k++) {
                // Joint
                for(uint32_t phaseId=0;phaseId<jointParallelGroup->numPhases;phaseId++) {
                        for(uint32_t batchId=0;batchId<jointParallelGroup->numBatches[phaseId];batchId++) {
                                uint32_t numPairs = jointParallelGroup->numPairs[phaseId*PFX_MAX_SOLVER_BATCHES+batchId];
                                if(batchId%numTasks == taskId && numPairs > 0) {
                                        const PfxParallelBatch &batch = jointParallelBatches[phaseId*PFX_MAX_SOLVER_BATCHES+batchId];
                                        for(uint32_t i=0;i<numPairs;i++) {
                                                PfxConstraintPair &pair = jointPairs[batch.pairIndices[i]];
                                                uint16_t iA = pfxGetRigidBodyIdA(pair);
                                                uint16_t iB = pfxGetRigidBodyIdB(pair);

                                                
                                                PfxSolverBody &solverBodyA = iA != 65535 ? offsetSolverBodies[iA] : staticBody;
                                                PfxSolverBody &solverBodyB = iB != 65535 ? offsetSolverBodies[iB] : staticBody;

                                                if(k==0) {
                                                        
                                                }
                                                else {
                                                        btSolverConstraint* constraintRow = &offsetSolverConstraints[pfxGetContactId1(pair)];
                                                        int numRows = pfxGetNumConstraints(pair);
                                                        int i;
                                                        for (i=0;i<numRows;i++)
                                                        {
                                                                resolveSingleConstraintRowGeneric(solverBodyA,solverBodyB,constraintRow[i]);
                                                        }
                                                        
                                                }
                                        }
                                }
                        }

                        barrier->sync();
                }

                // Contact
                for(uint32_t phaseId=0;phaseId<contactParallelGroup->numPhases;phaseId++) {
                        for(uint32_t batchId=0;batchId<contactParallelGroup->numBatches[phaseId];batchId++) {
                                uint32_t numPairs = contactParallelGroup->numPairs[phaseId*PFX_MAX_SOLVER_BATCHES+batchId];
                                if(batchId%numTasks == taskId && numPairs > 0) {
                                        const PfxParallelBatch &batch = contactParallelBatches[phaseId*PFX_MAX_SOLVER_BATCHES+batchId];
                                        for(uint32_t i=0;i<numPairs;i++) {
                                                PfxConstraintPair &pair = contactPairs[batch.pairIndices[i]];
                                                uint16_t iA = pfxGetRigidBodyIdA(pair);
                                                uint16_t iB = pfxGetRigidBodyIdB(pair);

                                                uint32_t contactIndex = pfxGetConstraintId1(pair);
                                                btPersistentManifold& contact = offsetContactManifolds[contactIndex];
                                                btConstraintRow* contactConstraintRows = &offsetContactConstraintRows[contactIndex*12];

                                                PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
                                                PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
                                                
                                                for(int j=0;j<contact.getNumContacts();j++) {
                                                        btManifoldPoint& cp = contact.getContactPoint(j);
                                                        
                                                        if(k==0) {
                                                                vmVector3 rA = rotate(solverBodyA.mOrientation,btReadVector3(cp.m_localPointA));
                                                                vmVector3 rB = rotate(solverBodyB.mOrientation,btReadVector3(cp.m_localPointB));
                                                                
                                                                float imp[3] = 
                                                                {
                                                                        cp.m_appliedImpulse,
                                                                        cp.m_appliedImpulseLateral1,
                                                                        cp.m_appliedImpulseLateral2
                                                                };
                                                                for(int k=0;k<3;k++) 
                                                                {
                                                                        vmVector3 normal = btReadVector3(contactConstraintRows[j*3+k].m_normal);
                                                                        contactConstraintRows[j*3+k].m_accumImpulse = imp[k];
                                                                        float deltaImpulse = contactConstraintRows[j*3+k].m_accumImpulse;
                                                                        solverBodyA.mDeltaLinearVelocity += deltaImpulse * solverBodyA.mMassInv * normal;
                                                                        solverBodyA.mDeltaAngularVelocity += deltaImpulse * solverBodyA.mInertiaInv * cross(rA,normal);
                                                                        solverBodyB.mDeltaLinearVelocity -= deltaImpulse * solverBodyB.mMassInv * normal;
                                                                        solverBodyB.mDeltaAngularVelocity -= deltaImpulse * solverBodyB.mInertiaInv * cross(rB,normal);
                                                                }
                                                        }
                                                        else {
                                                                btSolveContactConstraint(
                                                                        contactConstraintRows[j*3],
                                                                        contactConstraintRows[j*3+1],
                                                                        contactConstraintRows[j*3+2],
                                                                        btReadVector3(cp.m_localPointA),
                                                                        btReadVector3(cp.m_localPointB),
                                                                        solverBodyA,
                                                                        solverBodyB,
                                                                        cp.m_combinedFriction
                                                                        );
                                                        }
                                                }
                                        }
                                }
                        }

                        if (barrier)
                                barrier->sync();
                }
        }
}

void CustomPostSolverTask(
        TrbState *states,
        PfxSolverBody *solverBodies,
        uint32_t numRigidBodies)
{
        for(uint32_t i=0;i<numRigidBodies;i++) {
                TrbState &state = states[i];
                PfxSolverBody &solverBody = solverBodies[i];
                state.setLinearVelocity(state.getLinearVelocity()+solverBody.mDeltaLinearVelocity);
                state.setAngularVelocity(state.getAngularVelocity()+solverBody.mDeltaAngularVelocity);
        }
}

void*   SolverlsMemoryFunc()
{
        //don't create local store memory, just return 0
        return 0;
}


static SIMD_FORCE_INLINE
void pfxGetPlaneSpace(const vmVector3& n, vmVector3& p, vmVector3& q)
{
        if(fabsf(n[2]) > 0.707f) {
                // choose p in y-z plane
                float a = n[1]*n[1] + n[2]*n[2];
                float k = 1.0f/sqrtf(a);
                p[0] = 0;
                p[1] = -n[2]*k;
                p[2] = n[1]*k;
                // set q = n x p
                q[0] = a*k;
                q[1] = -n[0]*p[2];
                q[2] = n[0]*p[1];
        }
        else {
                // choose p in x-y plane
                float a = n[0]*n[0] + n[1]*n[1];
                float k = 1.0f/sqrtf(a);
                p[0] = -n[1]*k;
                p[1] = n[0]*k;
                p[2] = 0;
                // set q = n x p
                q[0] = -n[2]*p[1];
                q[1] = n[2]*p[0];
                q[2] = a*k;
        }
}



#define PFX_CONTACT_SLOP 0.001f

void btSetupContactConstraint(
        btConstraintRow &constraintResponse,
        btConstraintRow &constraintFriction1,
        btConstraintRow &constraintFriction2,
        float penetrationDepth,
        float restitution,
        float friction,
        const vmVector3 &contactNormal,
        const vmVector3 &contactPointA,
        const vmVector3 &contactPointB,
        const TrbState &stateA,
        const TrbState &stateB,
        PfxSolverBody &solverBodyA,
        PfxSolverBody &solverBodyB,
        const vmVector3& linVelA, 
        const vmVector3& angVelA,
        const vmVector3& linVelB, 
        const vmVector3& angVelB,

        float separateBias,
        float timeStep
        )
{
        vmVector3 rA = rotate(solverBodyA.mOrientation,contactPointA);
        vmVector3 rB = rotate(solverBodyB.mOrientation,contactPointB);

        vmMatrix3 K = vmMatrix3::scale(vmVector3(solverBodyA.mMassInv + solverBodyB.mMassInv)) - 
                        crossMatrix(rA) * solverBodyA.mInertiaInv * crossMatrix(rA) - 
                        crossMatrix(rB) * solverBodyB.mInertiaInv * crossMatrix(rB);
        
        //use the velocities without the applied (gravity and external) forces for restitution computation
        vmVector3 vArestitution = linVelA + cross(angVelA,rA);
        vmVector3 vBrestitution = linVelB + cross(angVelB,rB);
        vmVector3 vABrestitution = vArestitution-vBrestitution;

        vmVector3 vA = stateA.getLinearVelocity() + cross(stateA.getAngularVelocity(),rA);
        vmVector3 vB = stateB.getLinearVelocity() + cross(stateB.getAngularVelocity(),rB);
        vmVector3 vAB = vA-vB;


        vmVector3 tangent1,tangent2;
        btPlaneSpace1(contactNormal,tangent1,tangent2);

//      constraintResponse.m_accumImpulse = 0.f;
//      constraintFriction1.m_accumImpulse = 0.f;
//      constraintFriction2.m_accumImpulse = 0.f;

        // Contact Constraint
        {
                vmVector3 normal = contactNormal;

                float denom = dot(K*normal,normal);

                constraintResponse.m_rhs = -(1.0f+restitution)*dot(vAB,normal); // velocity error
                constraintResponse.m_rhs -= (separateBias * btMin(0.0f,penetrationDepth+PFX_CONTACT_SLOP)) / timeStep; // position error
                constraintResponse.m_rhs /= denom;
                constraintResponse.m_jacDiagInv = 1.0f/denom;
                constraintResponse.m_lowerLimit = 0.0f;
                constraintResponse.m_upperLimit = SIMD_INFINITY;
                btStoreVector3(normal,constraintResponse.m_normal);
        }

        // Friction Constraint 1
        {
                vmVector3 normal = tangent1;

                float denom = dot(K*normal,normal);

                constraintFriction1.m_jacDiagInv = 1.0f/denom;
                constraintFriction1.m_rhs = -dot(vAB,normal);
                constraintFriction1.m_rhs *= constraintFriction1.m_jacDiagInv;
                constraintFriction1.m_lowerLimit = 0.0f;
                constraintFriction1.m_upperLimit = SIMD_INFINITY;
                btStoreVector3(normal,constraintFriction1.m_normal);
        }
        
        // Friction Constraint 2
        {
                vmVector3 normal = tangent2;

                float denom = dot(K*normal,normal);

                constraintFriction2.m_jacDiagInv = 1.0f/denom;
                constraintFriction2.m_rhs = -dot(vAB,normal);
                constraintFriction2.m_rhs *= constraintFriction2.m_jacDiagInv;
                constraintFriction2.m_lowerLimit = 0.0f;
                constraintFriction2.m_upperLimit = SIMD_INFINITY;
                btStoreVector3(normal,constraintFriction2.m_normal);
        }
}


void CustomSetupContactConstraintsTask(
        PfxConstraintPair *contactPairs,uint32_t numContactPairs,
        btPersistentManifold*   offsetContactManifolds,
        btConstraintRow* offsetContactConstraintRows,
        TrbState *offsetRigStates,
        PfxSolverBody *offsetSolverBodies,
        uint32_t numRigidBodies,
        float separateBias,
        float timeStep)
{
        for(uint32_t i=0;i<numContactPairs;i++) {
                PfxConstraintPair &pair = contactPairs[i];
                if(!pfxGetActive(pair) || pfxGetNumConstraints(pair) == 0 ||
                        ((pfxGetMotionMaskA(pair)&PFX_MOTION_MASK_STATIC) && (pfxGetMotionMaskB(pair)&PFX_MOTION_MASK_STATIC)) ) {
                        continue;
                }

                uint16_t iA = pfxGetRigidBodyIdA(pair);
                uint16_t iB = pfxGetRigidBodyIdB(pair);

                int id = pfxGetConstraintId1(pair);
                btPersistentManifold& contact = offsetContactManifolds[id];
                btConstraintRow* contactConstraintRows = &offsetContactConstraintRows[id*12];

                TrbState &stateA = offsetRigStates[iA];
//              PfxRigBody &bodyA = offsetRigBodies[iA];
                PfxSolverBody &solverBodyA = offsetSolverBodies[iA];

                TrbState &stateB = offsetRigStates[iB];
//              PfxRigBody &bodyB = offsetRigBodies[iB];
                PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
                
                float restitution = 0.5f * (solverBodyA.restitution + solverBodyB.restitution);
                //if(contact.getDuration() > 1) restitution = 0.0f;
                
                float friction = sqrtf(solverBodyA.friction * solverBodyB.friction);

                for(int j=0;j<contact.getNumContacts();j++) {
                        btManifoldPoint& cp = contact.getContactPoint(j);

                        //pass the velocities without the applied (gravity and external) forces for restitution computation
                        const btRigidBody* rbA = btRigidBody::upcast(contact.getBody0());
                        const btRigidBody* rbB = btRigidBody::upcast(contact.getBody1());

                        btVector3 linVelA, linVelB;
                        btVector3 angVelA, angVelB;
                        
                        if (rbA && (rbA->getInvMass()>0.f))
                        {
                                linVelA = rbA->getLinearVelocity();
                                angVelA = rbA->getAngularVelocity();
                        } else
                        {
                                linVelA.setValue(0,0,0);
                                angVelA.setValue(0,0,0);
                        }

                        if (rbB && (rbB->getInvMass()>0.f))
                        {
                                linVelB = rbB->getLinearVelocity();
                                angVelB = rbB->getAngularVelocity();
                        } else
                        {
                                linVelB.setValue(0,0,0);
                                angVelB.setValue(0,0,0);
                        }



                        btSetupContactConstraint(
                                contactConstraintRows[j*3],
                                contactConstraintRows[j*3+1],
                                contactConstraintRows[j*3+2],
                                cp.getDistance(),
                                restitution,
                                friction,
                                btReadVector3(cp.m_normalWorldOnB),//.mConstraintRow[0].m_normal),
                                btReadVector3(cp.m_localPointA),
                                btReadVector3(cp.m_localPointB),
                                stateA,
                                stateB,
                                solverBodyA,
                                solverBodyB,
                                (const vmVector3&)linVelA, (const vmVector3&)angVelA,
                                (const vmVector3&)linVelB, (const vmVector3&)angVelB,
                                separateBias,
                                timeStep
                                );
                }

                //contact.setCompositeFriction(friction);
        }
}


void CustomWritebackContactConstraintsTask(
        PfxConstraintPair *contactPairs,uint32_t numContactPairs,
        btPersistentManifold*   offsetContactManifolds,
        btConstraintRow* offsetContactConstraintRows,
        TrbState *offsetRigStates,
        PfxSolverBody *offsetSolverBodies,
        uint32_t numRigidBodies,
        float separateBias,
        float timeStep)
{
        for(uint32_t i=0;i<numContactPairs;i++) {
                PfxConstraintPair &pair = contactPairs[i];
                if(!pfxGetActive(pair) || pfxGetNumConstraints(pair) == 0 ||
                        ((pfxGetMotionMaskA(pair)&PFX_MOTION_MASK_STATIC) && (pfxGetMotionMaskB(pair)&PFX_MOTION_MASK_STATIC)) ) {
                        continue;
                }
                int id = pfxGetConstraintId1(pair);
                btPersistentManifold& contact = offsetContactManifolds[id];
                btConstraintRow* contactConstraintRows = &offsetContactConstraintRows[id*12];
                for(int j=0;j<contact.getNumContacts();j++) {
                        btManifoldPoint& cp = contact.getContactPoint(j);
                        cp.m_appliedImpulse = contactConstraintRows[j*3+0].m_accumImpulse;
                        cp.m_appliedImpulseLateral1 = contactConstraintRows[j*3+1].m_accumImpulse;
                        cp.m_appliedImpulseLateral2 = contactConstraintRows[j*3+2].m_accumImpulse;
                }
                //contact.setCompositeFriction(friction);
        }
}

void    SolverThreadFunc(void* userPtr,void* lsMemory)
{
        btConstraintSolverIO* io = (btConstraintSolverIO*)(userPtr);//arg->io);
        btCriticalSection* criticalsection = io->setupContactConstraints.criticalSection;
        

        //CustomCriticalSection *criticalsection = &io->m_cs;
        switch(io->cmd) {

                case PFX_CONSTRAINT_SOLVER_CMD_SOLVE_CONSTRAINTS:
                CustomSolveConstraintsTaskParallel(
                        io->solveConstraints.contactParallelGroup,
                        io->solveConstraints.contactParallelBatches,
                        io->solveConstraints.contactPairs,
                        io->solveConstraints.numContactPairs,
                        io->solveConstraints.offsetContactManifolds,
                        io->solveConstraints.offsetContactConstraintRows,

                        io->solveConstraints.jointParallelGroup,
                        io->solveConstraints.jointParallelBatches,
                        io->solveConstraints.jointPairs,
                        io->solveConstraints.numJointPairs,
                        io->solveConstraints.offsetSolverConstraints,
                        io->solveConstraints.offsetRigStates1,
                        io->solveConstraints.offsetSolverBodies,
                        io->solveConstraints.numRigidBodies,
                        io->solveConstraints.iteration,

                        io->solveConstraints.taskId,
                        io->maxTasks1,
                        io->solveConstraints.barrier
                        );
                break;

                case PFX_CONSTRAINT_SOLVER_CMD_POST_SOLVER:
                        CustomPostSolverTask(   io->postSolver.states,io->postSolver.solverBodies,      io->postSolver.numRigidBodies);
                        break;


                case PFX_CONSTRAINT_SOLVER_CMD_SETUP_CONTACT_CONSTRAINTS:
                {
                        bool empty = false;
                        while(!empty) {
                                int start,batch;
                                
                                criticalsection->lock();

                                start = (int)criticalsection->getSharedParam(0);
                                batch = (int)criticalsection->getSharedParam(1);

                                //PFX_PRINTF("taskId %d start %d num %d\n",arg->taskId,start,batch);

                                // ���̃o�b�t�@���Z�b�g
                                int nextStart = start + batch;
                                int rest = btMax((int)io->setupContactConstraints.numContactPairs1 - nextStart,0);
                                int nextBatch = (rest > batch)?batch:rest;

                                criticalsection->setSharedParam(0,nextStart);
                criticalsection->setSharedParam(1,nextBatch);

                                criticalsection->unlock();
                                
                                if(batch > 0) {
                                        CustomSetupContactConstraintsTask(
                                                io->setupContactConstraints.offsetContactPairs+start,batch,
                                                io->setupContactConstraints.offsetContactManifolds,
                                                io->setupContactConstraints.offsetContactConstraintRows,
                                                io->setupContactConstraints.offsetRigStates,
//                                              io->setupContactConstraints.offsetRigBodies,
                                                io->setupContactConstraints.offsetSolverBodies,
                                                io->setupContactConstraints.numRigidBodies,
                                                io->setupContactConstraints.separateBias,
                                                io->setupContactConstraints.timeStep);
                                }
                                else {
                                        empty = true;
                                }
                        }
                }
                break;

                case PFX_CONSTRAINT_SOLVER_CMD_WRITEBACK_APPLIED_IMPULSES_CONTACT_CONSTRAINTS:
                {
                        bool empty = false;
                        while(!empty) {
                                int start,batch;
                                
                                criticalsection->lock();

                                start = (int)criticalsection->getSharedParam(0);
                                batch = (int)criticalsection->getSharedParam(1);

                                //PFX_PRINTF("taskId %d start %d num %d\n",arg->taskId,start,batch);

                                // ���̃o�b�t�@���Z�b�g
                                int nextStart = start + batch;
                                int rest = btMax((int)io->setupContactConstraints.numContactPairs1 - nextStart,0);
                                int nextBatch = (rest > batch)?batch:rest;

                                criticalsection->setSharedParam(0,nextStart);
                criticalsection->setSharedParam(1,nextBatch);

                                criticalsection->unlock();
                                
                                if(batch > 0) {
                                        CustomWritebackContactConstraintsTask(
                                                io->setupContactConstraints.offsetContactPairs+start,batch,
                                                io->setupContactConstraints.offsetContactManifolds,
                                                io->setupContactConstraints.offsetContactConstraintRows,
                                                io->setupContactConstraints.offsetRigStates,
//                                              io->setupContactConstraints.offsetRigBodies,
                                                io->setupContactConstraints.offsetSolverBodies,
                                                io->setupContactConstraints.numRigidBodies,
                                                io->setupContactConstraints.separateBias,
                                                io->setupContactConstraints.timeStep);
                                }
                                else {
                                        empty = true;
                                }
                        }
                }
                break;

                default:
                        {
                                btAssert(0);
                        }
        }

}


void CustomSetupContactConstraintsNew(
        PfxConstraintPair *contactPairs1,uint32_t numContactPairs,
        btPersistentManifold *offsetContactManifolds,
        btConstraintRow* offsetContactConstraintRows,
        TrbState *offsetRigStates,
        PfxSolverBody *offsetSolverBodies,
        uint32_t numRigidBodies,
        float separationBias,
        float timeStep,
        class btThreadSupportInterface* threadSupport,
        btCriticalSection* criticalSection,
        btConstraintSolverIO *io ,
        uint8_t cmd
        )
{
        int maxTasks = threadSupport->getNumTasks();

        int div = (int)maxTasks * 4;
        int batch = ((int)numContactPairs + div - 1) / div;
#ifdef __PPU__
                BulletPE2ConstraintSolverSpursSupport* spursThread = (BulletPE2ConstraintSolverSpursSupport*) threadSupport;
#endif
        if (criticalSection)
        {
                criticalSection->setSharedParam(0,0);
                criticalSection->setSharedParam(1,btMin(batch,64)); // batched number
        } else
        {
#ifdef __PPU__
                spursThread->setSharedParam(0,0);
                spursThread->setSharedParam(1,btMin(batch,64)); // batched number
#endif //__PPU__
        }

        for(int t=0;t<maxTasks;t++) {
                io[t].cmd = cmd;
                io[t].setupContactConstraints.offsetContactPairs = contactPairs1;
                io[t].setupContactConstraints.numContactPairs1 = numContactPairs;
                io[t].setupContactConstraints.offsetRigStates = offsetRigStates;
                io[t].setupContactConstraints.offsetContactManifolds = offsetContactManifolds;  
                io[t].setupContactConstraints.offsetContactConstraintRows = offsetContactConstraintRows;
                io[t].setupContactConstraints.offsetSolverBodies = offsetSolverBodies;
                io[t].setupContactConstraints.numRigidBodies = numRigidBodies;
                io[t].setupContactConstraints.separateBias = separationBias;
                io[t].setupContactConstraints.timeStep = timeStep;
                io[t].setupContactConstraints.criticalSection = criticalSection;
                io[t].maxTasks1 = maxTasks;
#ifdef __PPU__
                io[t].barrierAddr2 = (unsigned int)spursThread->getBarrierAddress();
                io[t].criticalsectionAddr2 = (unsigned int)spursThread->getCriticalSectionAddress();
#endif
        

//#define SEQUENTIAL_SETUP
#ifdef SEQUENTIAL_SETUP
                CustomSetupContactConstraintsTask(contactPairs1,numContactPairs,offsetContactManifolds,offsetRigStates,offsetSolverBodies,numRigidBodies,separationBias,timeStep);
#else
                threadSupport->sendRequest(1,(ppu_address_t)&io[t],t);
#endif

        }
#ifndef SEQUENTIAL_SETUP
        unsigned int arg0,arg1;
        for(int t=0;t<maxTasks;t++) {
                arg0 = t;
                threadSupport->waitForResponse(&arg0,&arg1);
        }
#endif //SEQUENTIAL_SETUP

}


void CustomSplitConstraints(
        PfxConstraintPair *pairs,uint32_t numPairs,
        PfxParallelGroup &group,PfxParallelBatch *batches,
        uint32_t numTasks,
        uint32_t numRigidBodies,
        void *poolBuff,
        uint32_t poolBytes
        )
{
        HeapManager pool((unsigned char*)poolBuff,poolBytes);

        // �X�e�[�g�`�F�b�N�p�r�b�g�t���O�e�[�u��
        int bufSize = sizeof(uint8_t)*numRigidBodies;
        bufSize = ((bufSize+127)>>7)<<7; // 128 bytes alignment
        uint8_t *bodyTable = (uint8_t*)pool.allocate(bufSize,HeapManager::ALIGN128);

        // �y�A�`�F�b�N�p�r�b�g�t���O�e�[�u��
        uint32_t *pairTable;
        size_t allocSize = sizeof(uint32_t)*((numPairs+31)/32);
        pairTable = (uint32_t*)pool.allocate(allocSize);
        memset(pairTable,0,allocSize);

        // �ڕW�Ƃ��镪����
        uint32_t targetCount = btMax(uint32_t(PFX_MIN_SOLVER_PAIRS),btMin(numPairs / (numTasks*2),uint32_t(PFX_MAX_SOLVER_PAIRS)));
        uint32_t startIndex = 0;
        
        uint32_t phaseId;
        uint32_t batchId;
        uint32_t totalCount=0;
        
        uint32_t maxBatches = btMin(numTasks,uint32_t(PFX_MAX_SOLVER_BATCHES));
        
        for(phaseId=0;phaseId<PFX_MAX_SOLVER_PHASES&&totalCount<numPairs;phaseId++) {
                bool startIndexCheck = true;
                
                group.numBatches[phaseId] = 0;
                
                uint32_t i = startIndex;
                
        // �`�F�b�N�p�r�b�g�t���O�e�[�u�����N���A
                memset(bodyTable,0xff,bufSize);
                
                for(batchId=0;i<numPairs&&totalCount<numPairs&&batchId<maxBatches;batchId++) {
                        uint32_t pairCount=0;
                        
                        PfxParallelBatch &batch = batches[phaseId*PFX_MAX_SOLVER_BATCHES+batchId];
                        uint32_t pairId = 0;
                        
                        for(;i<numPairs&&pairCount<targetCount;i++) {
                                uint32_t idxP = i>>5;
                                uint32_t maskP = 1L << (i & 31);
                                
                                //pair is already assigned to a phase/batch
                                if(pairTable[idxP] & maskP) {
                                        continue;
                                }
                                
                                uint32_t idxA = pfxGetRigidBodyIdA(pairs[i]);
                                uint32_t idxB = pfxGetRigidBodyIdB(pairs[i]);

                                // �����Ƃ��A�N�e�B�u�łȂ��A�܂��͏Փ˓_���O�̃y�A�͓o�^�Ώۂ���͂���
                                if(!pfxGetActive(pairs[i]) || pfxGetNumConstraints(pairs[i]) == 0 ||
                                        ((pfxGetMotionMaskA(pairs[i])&PFX_MOTION_MASK_STATIC) && (pfxGetMotionMaskB(pairs[i])&PFX_MOTION_MASK_STATIC)) ) {
                                        if(startIndexCheck) 
                                                startIndex++;
                                        //assign pair -> skip it because it has no constraints
                                        pairTable[idxP] |= maskP;
                                        totalCount++;
                                        continue;
                                }
                                
                                // �ˑ����̃`�F�b�N
                                if( (bodyTable[idxA] != batchId && bodyTable[idxA] != 0xff) || 
                                        (bodyTable[idxB] != batchId && bodyTable[idxB] != 0xff) ) {
                                        startIndexCheck = false;
                                        //bodies of the pair are already assigned to another batch within this phase
                                        continue;
                                }
                                
                                // �ˑ�������e�[�u���ɓo�^
                                if(pfxGetMotionMaskA(pairs[i])&PFX_MOTION_MASK_DYNAMIC) 
                                                bodyTable[idxA] = batchId;
                                if(pfxGetMotionMaskB(pairs[i])&PFX_MOTION_MASK_DYNAMIC) 
                                                bodyTable[idxB] = batchId;
                                
                                if(startIndexCheck) 
                                        startIndex++;
                                
                                pairTable[idxP] |= maskP;
                                //add the pair 'i' to the current batch
                                batch.pairIndices[pairId++] = i;
                                pairCount++;
                        }

                        group.numPairs[phaseId*PFX_MAX_SOLVER_BATCHES+batchId] = (uint16_t)pairId;
                        totalCount += pairCount;
                }

                group.numBatches[phaseId] = batchId;
        }

        group.numPhases = phaseId;

        pool.clear();
}



void CustomSolveConstraintsParallel(
        PfxConstraintPair *contactPairs,uint32_t numContactPairs,
        
        PfxConstraintPair *jointPairs,uint32_t numJointPairs,
        btPersistentManifold* offsetContactManifolds,
        btConstraintRow* offsetContactConstraintRows,
        btSolverConstraint* offsetSolverConstraints,
        TrbState *offsetRigStates,
        PfxSolverBody *offsetSolverBodies,
        uint32_t numRigidBodies,
        struct btConstraintSolverIO* io,
        class btThreadSupportInterface* threadSupport,
        int iteration,
        void* poolBuf,
        int poolBytes,
        class btBarrier* barrier)
        {

        int maxTasks = threadSupport->getNumTasks();
//      config.taskManager->setTaskEntry(PFX_SOLVER_ENTRY);

        HeapManager pool((unsigned char*)poolBuf,poolBytes);

        {
                PfxParallelGroup *cgroup = (PfxParallelGroup*)pool.allocate(sizeof(PfxParallelGroup));
                PfxParallelBatch *cbatches = (PfxParallelBatch*)pool.allocate(sizeof(PfxParallelBatch)*(PFX_MAX_SOLVER_PHASES*PFX_MAX_SOLVER_BATCHES),128);
                PfxParallelGroup *jgroup = (PfxParallelGroup*)pool.allocate(sizeof(PfxParallelGroup));
                PfxParallelBatch *jbatches = (PfxParallelBatch*)pool.allocate(sizeof(PfxParallelBatch)*(PFX_MAX_SOLVER_PHASES*PFX_MAX_SOLVER_BATCHES),128);
                
                uint32_t tmpBytes = poolBytes - 2 * (sizeof(PfxParallelGroup) + sizeof(PfxParallelBatch)*(PFX_MAX_SOLVER_PHASES*PFX_MAX_SOLVER_BATCHES) + 128);
                void *tmpBuff = pool.allocate(tmpBytes);
                
                {
                        BT_PROFILE("CustomSplitConstraints");
                        CustomSplitConstraints(contactPairs,numContactPairs,*cgroup,cbatches,maxTasks,numRigidBodies,tmpBuff,tmpBytes);
                        CustomSplitConstraints(jointPairs,numJointPairs,*jgroup,jbatches,maxTasks,numRigidBodies,tmpBuff,tmpBytes);
                }

                {
                        BT_PROFILE("PFX_CONSTRAINT_SOLVER_CMD_SOLVE_CONSTRAINTS");
//#define SOLVE_SEQUENTIAL
#ifdef SOLVE_SEQUENTIAL
                CustomSolveConstraintsTask(
                        io->solveConstraints.contactParallelGroup,
                        io->solveConstraints.contactParallelBatches,
                        io->solveConstraints.contactPairs,
                        io->solveConstraints.numContactPairs,
                        io->solveConstraints.offsetContactManifolds,

                        io->solveConstraints.jointParallelGroup,
                        io->solveConstraints.jointParallelBatches,
                        io->solveConstraints.jointPairs,
                        io->solveConstraints.numJointPairs,
                        io->solveConstraints.offsetSolverConstraints,

                        io->solveConstraints.offsetRigStates1,
                        io->solveConstraints.offsetSolverBodies,
                        io->solveConstraints.numRigidBodies,
                        io->solveConstraints.iteration,0,1,0);//arg->taskId,1,0);//,arg->maxTasks,arg->barrier);
#else
                for(int t=0;t<maxTasks;t++) {
                        io[t].cmd = PFX_CONSTRAINT_SOLVER_CMD_SOLVE_CONSTRAINTS;
                        io[t].solveConstraints.contactParallelGroup = cgroup;
                        io[t].solveConstraints.contactParallelBatches = cbatches;
                        io[t].solveConstraints.contactPairs = contactPairs;
                        io[t].solveConstraints.numContactPairs = numContactPairs;
                        io[t].solveConstraints.offsetContactManifolds = offsetContactManifolds;
                        io[t].solveConstraints.offsetContactConstraintRows = offsetContactConstraintRows;
                        io[t].solveConstraints.jointParallelGroup = jgroup;
                        io[t].solveConstraints.jointParallelBatches = jbatches;
                        io[t].solveConstraints.jointPairs = jointPairs;
                        io[t].solveConstraints.numJointPairs = numJointPairs;
                        io[t].solveConstraints.offsetSolverConstraints = offsetSolverConstraints;
                        io[t].solveConstraints.offsetRigStates1 = offsetRigStates;
                        io[t].solveConstraints.offsetSolverBodies = offsetSolverBodies;
                        io[t].solveConstraints.numRigidBodies = numRigidBodies;
                        io[t].solveConstraints.iteration = iteration;
                        io[t].solveConstraints.taskId = t;
                        io[t].solveConstraints.barrier = barrier;

                io[t].maxTasks1 = maxTasks;
#ifdef __PPU__
                BulletPE2ConstraintSolverSpursSupport* spursThread = (BulletPE2ConstraintSolverSpursSupport*) threadSupport;
                io[t].barrierAddr2 = (unsigned int) spursThread->getBarrierAddress();
                io[t].criticalsectionAddr2 = (unsigned int)spursThread->getCriticalSectionAddress();
#endif

                        threadSupport->sendRequest(1,(ppu_address_t)&io[t],t);
                }

                unsigned int arg0,arg1;
                for(int t=0;t<maxTasks;t++) {
                        arg0 = t;
                        threadSupport->waitForResponse(&arg0,&arg1);
                }
#endif
                }
                pool.clear();
        }

        {
                        BT_PROFILE("PFX_CONSTRAINT_SOLVER_CMD_POST_SOLVER");
                int batch = ((int)numRigidBodies + maxTasks - 1) / maxTasks;
                int rest = (int)numRigidBodies;
                int start = 0;

                for(int t=0;t<maxTasks;t++) {
                        int num = (rest - batch ) > 0 ? batch : rest;
                        io[t].cmd = PFX_CONSTRAINT_SOLVER_CMD_POST_SOLVER;
                        io[t].postSolver.states = offsetRigStates + start;
                        io[t].postSolver.solverBodies = offsetSolverBodies + start;
                        io[t].postSolver.numRigidBodies = (uint32_t)num;
                io[t].maxTasks1 = maxTasks;
#ifdef __PPU__
                BulletPE2ConstraintSolverSpursSupport* spursThread = (BulletPE2ConstraintSolverSpursSupport*) threadSupport;
                io[t].barrierAddr2 = (unsigned int)spursThread->getBarrierAddress();
                io[t].criticalsectionAddr2 = (unsigned int)spursThread->getCriticalSectionAddress();
#endif

#ifdef SOLVE_SEQUENTIAL
                        CustomPostSolverTask(   io[t].postSolver.states,io[t].postSolver.solverBodies,  io[t].postSolver.numRigidBodies);
#else
                        threadSupport->sendRequest(1,(ppu_address_t)&io[t],t);
#endif
                        rest -= num;
                        start += num;
                }

                unsigned int arg0,arg1;
                for(int t=0;t<maxTasks;t++) {
#ifndef SOLVE_SEQUENTIAL
                        arg0 = t;
                        threadSupport->waitForResponse(&arg0,&arg1);
#endif
                }
        }

}



void BPE_customConstraintSolverSequentialNew(unsigned int new_num, PfxBroadphasePair *new_pairs1 ,
                                                                        btPersistentManifold* offsetContactManifolds,
                                                                        PfxConstraintRow*       offsetContactConstraintRows,
                                                                          TrbState* states,int numRigidBodies, 
                                                                          struct PfxSolverBody* solverBodies, 
                                                                          PfxConstraintPair* jointPairs, unsigned int numJoints,
                                                                          btSolverConstraint* offsetSolverConstraints,
                                                                          float separateBias,
                                                                          float timeStep,
                                                                          int iteration,
                                                                          btThreadSupportInterface* solverThreadSupport,
                                                                          btCriticalSection* criticalSection,
                                                                          struct btConstraintSolverIO* solverIO,
                                                                          btBarrier* barrier
                                                                          )
{

        {
                BT_PROFILE("pfxSetupConstraints");

                for(uint32_t i=0;i<numJoints;i++) {
                        // ���̍X�V
                        PfxConstraintPair &pair = jointPairs[i];
                        int idA = pfxGetRigidBodyIdA(pair);

                        if (idA != 65535)
                        {
                                pfxSetMotionMaskA(pair,states[pfxGetRigidBodyIdA(pair)].getMotionMask());
                        }
                        else
                        {
                                pfxSetMotionMaskA(pair,PFX_MOTION_MASK_STATIC);
                        }
                        int idB = pfxGetRigidBodyIdB(pair);
                        if (idB!= 65535)
                        {
                                pfxSetMotionMaskB(pair,states[pfxGetRigidBodyIdB(pair)].getMotionMask());
                        } else
                        {
                                pfxSetMotionMaskB(pair,PFX_MOTION_MASK_STATIC);
                        }
                }

//              CustomSetupJointConstraintsSeq(                 jointPairs,numJoints,joints,                    states,                 solverBodies,                   numRigidBodies,                 timeStep);

#ifdef SEQUENTIAL_SETUP
                CustomSetupContactConstraintsSeqNew(
                        (PfxConstraintPair*)new_pairs1,new_num,contacts,
                        states,
                        solverBodies,
                        numRigidBodies,
                        separateBias,
                        timeStep);
#else

                CustomSetupContactConstraintsNew(
                        (PfxConstraintPair*)new_pairs1,new_num,
                        offsetContactManifolds,
                        offsetContactConstraintRows,
                        states,
                        solverBodies,
                        numRigidBodies,
                        separateBias,
                        timeStep,
                        solverThreadSupport,
                        criticalSection,solverIO,
                        PFX_CONSTRAINT_SOLVER_CMD_SETUP_CONTACT_CONSTRAINTS
                        );

#endif //SEQUENTIAL_SETUP

        }
        {
                BT_PROFILE("pfxSolveConstraints");

//#define SEQUENTIAL
#ifdef SEQUENTIAL
                CustomSolveConstraintsSeq(
                        (PfxConstraintPair*)new_pairs1,new_num,contacts,
                        jointPairs,numJoints,
                        states,
                        solverBodies,
                        numRigidBodies,
                        separateBias,
                        timeStep,
                        iteration);
#else //SEQUENTIAL
                CustomSolveConstraintsParallel(
                        (PfxConstraintPair*)new_pairs1,new_num,
                        jointPairs,numJoints,
                        offsetContactManifolds,
                        offsetContactConstraintRows,
                        offsetSolverConstraints,
                        states,
                        solverBodies,
                        numRigidBodies,
                        solverIO, solverThreadSupport,
                        iteration,
                        tmp_buff,
                        TMP_BUFF_BYTES,
                        barrier
                        );

#endif //SEQUENTIAL
        }

        {
                BT_PROFILE("writeback appliedImpulses");

                CustomSetupContactConstraintsNew(
                        (PfxConstraintPair*)new_pairs1,new_num,
                        offsetContactManifolds,
                        offsetContactConstraintRows,
                        states,
                        solverBodies,
                        numRigidBodies,
                        separateBias,
                        timeStep,
                        solverThreadSupport,
                        criticalSection,solverIO,
                        PFX_CONSTRAINT_SOLVER_CMD_WRITEBACK_APPLIED_IMPULSES_CONTACT_CONSTRAINTS
                        );
        }

}


struct  btParallelSolverMemoryCache
{
        btAlignedObjectArray<TrbState>  m_mystates;
        btAlignedObjectArray<PfxSolverBody>  m_mysolverbodies;
        btAlignedObjectArray<PfxBroadphasePair> m_mypairs;
        btAlignedObjectArray<PfxConstraintPair> m_jointPairs;
        btAlignedObjectArray<PfxConstraintRow> m_constraintRows;
        
};


btConstraintSolverIO* createSolverIO(int numThreads)
{
        return new btConstraintSolverIO[numThreads];
}

btParallelConstraintSolver::btParallelConstraintSolver(btThreadSupportInterface* solverThreadSupport)
{
        
        m_solverThreadSupport = solverThreadSupport;//createSolverThreadSupport(maxNumThreads);
        m_solverIO = createSolverIO(m_solverThreadSupport->getNumTasks());

        m_barrier = m_solverThreadSupport->createBarrier();
        m_criticalSection = m_solverThreadSupport->createCriticalSection();

        m_memoryCache = new btParallelSolverMemoryCache();
}
        
btParallelConstraintSolver::~btParallelConstraintSolver()
{
        delete m_memoryCache;
        delete m_solverIO;
        m_solverThreadSupport->deleteBarrier(m_barrier);
        m_solverThreadSupport->deleteCriticalSection(m_criticalSection);
}



btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int numRigidBodies,btPersistentManifold** manifoldPtr,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
{
        
/*      int sz = sizeof(PfxSolverBody);
        int sz2 = sizeof(vmVector3);
        int sz3 = sizeof(vmMatrix3);
        int sz4 = sizeof(vmQuat);
        int sz5 = sizeof(btConstraintRow);
        int sz6 = sizeof(btSolverConstraint);
        int sz7 = sizeof(TrbState);
*/

        btPersistentManifold* offsetContactManifolds= (btPersistentManifold*) dispatcher->getInternalManifoldPool()->getPoolAddress();

                
        m_memoryCache->m_mysolverbodies.resize(numRigidBodies);
        m_memoryCache->m_mystates.resize(numRigidBodies);

        {
                        BT_PROFILE("create states and solver bodies");
        for (int i=0;i<numRigidBodies;i++)
        {
                btCollisionObject* obj = bodies1[i];
                obj->setCompanionId(i);

                PfxSolverBody& solverBody = m_memoryCache->m_mysolverbodies[i];
                btRigidBody* rb = btRigidBody::upcast(obj);
                TrbState& state = m_memoryCache->m_mystates[i];
        
                state.reset();
                const btQuaternion& orgOri = obj->getWorldTransform().getRotation();
                vmQuat orn(orgOri.getX(),orgOri.getY(),orgOri.getZ(),orgOri.getW());
                state.setPosition(getVmVector3(obj->getWorldTransform().getOrigin()));
                state.setOrientation(orn);
                state.setPosition(state.getPosition());
                state.setRigidBodyId(i);
                state.setAngularDamping(0);
                state.setLinearDamping(0);
                
                
                solverBody.mOrientation = state.getOrientation();
                solverBody.mDeltaLinearVelocity = vmVector3(0.0f);
                solverBody.mDeltaAngularVelocity = vmVector3(0.0f);
                solverBody.friction = obj->getFriction();
                solverBody.restitution = obj->getRestitution();
                
                state.resetSleepCount();
                
                //if(state.getMotionMask()&PFX_MOTION_MASK_DYNAMIC) {
                if (rb && (rb->getInvMass()>0.f))
                {
                        btVector3 angVelPlusForces = rb->getAngularVelocity()+rb->getTotalTorque()*rb->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
                        btVector3 linVelPlusForces = rb->getLinearVelocity()+rb->getTotalForce()*rb->getInvMass()*infoGlobal.m_timeStep;

                        state.setAngularVelocity(btReadVector3(angVelPlusForces));
                        state.setLinearVelocity(btReadVector3(linVelPlusForces));

                        state.setMotionType(PfxMotionTypeActive);
                        vmMatrix3 ori(solverBody.mOrientation);
                        vmMatrix3 localInvInertia = vmMatrix3::identity();
                        localInvInertia.setCol(0,vmVector3(rb->getInvInertiaDiagLocal().getX(),0,0));
                        localInvInertia.setCol(1,vmVector3(0, rb->getInvInertiaDiagLocal().getY(),0));
                        localInvInertia.setCol(2,vmVector3(0,0, rb->getInvInertiaDiagLocal().getZ()));

                        solverBody.mMassInv = rb->getInvMass();
                        solverBody.mInertiaInv = ori * localInvInertia * transpose(ori);
                } else
                {
                        state.setAngularVelocity(vmVector3(0));
                        state.setLinearVelocity(vmVector3(0));
                
                        state.setMotionType(PfxMotionTypeFixed);
                        m_memoryCache->m_mysolverbodies[i].mMassInv = 0.f;
                        m_memoryCache->m_mysolverbodies[i].mInertiaInv = vmMatrix3(0.0f);
                }

        }
        }



        int totalPoints = 0;
#ifndef USE_C_ARRAYS
        m_memoryCache->m_mypairs.resize(numManifolds);
        //4 points per manifold and 3 rows per point makes 12 rows per manifold
        m_memoryCache->m_constraintRows.resize(numManifolds*12);
        m_memoryCache->m_jointPairs.resize(numConstraints);
#endif//USE_C_ARRAYS

        int actualNumManifolds= 0;
        {
                BT_PROFILE("convert manifolds");
                for (int i1=0;i1<numManifolds;i1++)
                {
                        if (manifoldPtr[i1]->getNumContacts()>0)
                        {
                                btPersistentManifold* m = manifoldPtr[i1];
                                btCollisionObject* obA = (btCollisionObject*)m->getBody0();
                                btCollisionObject* obB = (btCollisionObject*)m->getBody1();
                                bool obAisActive = !obA->isStaticOrKinematicObject() && obA->isActive();
                                bool obBisActive = !obB->isStaticOrKinematicObject() && obB->isActive();

                                if (!obAisActive && !obBisActive)
                                        continue;


                                //int contactId = i1;//actualNumManifolds;
                                
                                PfxBroadphasePair& pair = m_memoryCache->m_mypairs[actualNumManifolds];
                                //init those
                //              float compFric = obA->getFriction()*obB->getFriction();//@todo
                                int idA = obA->getCompanionId();
                                int idB = obB->getCompanionId();
                                
                                m->m_companionIdA = idA;
                                m->m_companionIdB = idB;
                                
                                
                        //      if ((mysolverbodies[idA].mMassInv!=0)&&(mysolverbodies[idB].mMassInv!=0))
                        //              continue;
                                int numPosPoints=0;
                                for (int p=0;p<m->getNumContacts();p++)
                                {
                                        //btManifoldPoint& pt = m->getContactPoint(p);
                                        //float dist = pt.getDistance();
                                        //if (dist<0.001)
                                                numPosPoints++;
                                }

                                
                                totalPoints+=numPosPoints;
                                pfxSetRigidBodyIdA(pair,idA);
                                pfxSetRigidBodyIdB(pair,idB);
                                pfxSetMotionMaskA(pair,m_memoryCache->m_mystates[idA].getMotionMask());
                                pfxSetMotionMaskB(pair,m_memoryCache->m_mystates[idB].getMotionMask());
                                pfxSetActive(pair,numPosPoints>0);
                                
                                pfxSetBroadphaseFlag(pair,0);
                                int contactId = m-offsetContactManifolds;
                                //likely the contact pool is not contiguous, make sure to allocate large enough contact pool
                                btAssert(contactId>=0);
                                btAssert(contactId<dispatcher->getInternalManifoldPool()->getMaxCount());
                                
                                pfxSetContactId(pair,contactId);
                                pfxSetNumConstraints(pair,numPosPoints);//manifoldPtr[i]->getNumContacts());
                                actualNumManifolds++;
                        }

                }
        }

        PfxConstraintPair* jointPairs=0;
        jointPairs = numConstraints? &m_memoryCache->m_jointPairs[0]:0;
        int actualNumJoints=0;


        btSolverConstraint* offsetSolverConstraints = 0;

        //if (1)
        {
                
                {
                        BT_PROFILE("convert constraints");

                        int totalNumRows = 0;
                        int i;
                        
                        m_tmpConstraintSizesPool.resize(numConstraints);
                        //calculate the total number of contraint rows
                        for (i=0;i<numConstraints;i++)
                        {
                                btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
                                constraints[i]->getInfo1(&info1);
                                totalNumRows += info1.m_numConstraintRows;
                        }
                        m_tmpSolverNonContactConstraintPool.resize(totalNumRows);
                        offsetSolverConstraints =totalNumRows? &m_tmpSolverNonContactConstraintPool[0]:0;

                        
                        int currentRow = 0;

                        for (i=0;i<numConstraints;i++)
                        {
                                const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
                                
                                if (info1.m_numConstraintRows)
                                {
                                        btAssert(currentRow<totalNumRows);
                                        btTypedConstraint* constraint = constraints[i];
                                        btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];

                                        btRigidBody& rbA = constraint->getRigidBodyA();
                                        btRigidBody& rbB = constraint->getRigidBodyB();
                        
                                        int idA = constraint->getRigidBodyA().getCompanionId();
                                        int idB = constraint->getRigidBodyB().getCompanionId();
                        
                                        
                                        int j;
                                        for ( j=0;j<info1.m_numConstraintRows;j++)
                                        {
                                                memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));
                                                currentConstraintRow[j].m_lowerLimit = -FLT_MAX;
                                                currentConstraintRow[j].m_upperLimit = FLT_MAX;
                                                currentConstraintRow[j].m_appliedImpulse = 0.f;
                                                currentConstraintRow[j].m_appliedPushImpulse = 0.f;
                                                currentConstraintRow[j].m_solverBodyIdA = idA;
                                                currentConstraintRow[j].m_solverBodyIdB = idB;
                                        }

                                



                                        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
                                        btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
                                        info2.m_constraintError = &currentConstraintRow->m_rhs;
                                        currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
                                        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);

                                        
                                

                                        for ( j=0;j<info1.m_numConstraintRows;j++)
                                        {
                                                btSolverConstraint& solverConstraint = currentConstraintRow[j];
                                                solverConstraint.m_originalContactPoint = constraint;

                                                solverConstraint.m_solverBodyIdA = idA;
                                                solverConstraint.m_solverBodyIdB = idB;

                                                {
                                                        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);

                                                        solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
                                                }


                                                {
                                                        btScalar rel_vel;
                                                        btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
                                                        btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());

                                                        rel_vel = vel1Dotn+vel2Dotn;

                                                        btScalar restitution = 0.f;
                                                        btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
                                                        btScalar        velocityError = restitution - rel_vel;// * damping;
                                                        btScalar        penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
                                                        btScalar        velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
                                                        solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
                                                        solverConstraint.m_appliedImpulse = 0.f;

                                                }
                                        }

                                        PfxConstraintPair& pair = jointPairs[actualNumJoints];
                                        
                                        int numConstraintRows= info1.m_numConstraintRows;
                                        pfxSetNumConstraints(pair,numConstraintRows);
                                        


                                        pfxSetRigidBodyIdA(pair,idA);
                                        pfxSetRigidBodyIdB(pair,idB);
                                        //is this needed?
                                        if (idA>=0)
                                                pfxSetMotionMaskA(pair,m_memoryCache->m_mystates[idA].getMotionMask());
                                        if (idB>=0)
                                                pfxSetMotionMaskB(pair,m_memoryCache->m_mystates[idB].getMotionMask());

                                        pfxSetActive(pair,true);
                                        int id = currentConstraintRow-offsetSolverConstraints;
                                        pfxSetContactId(pair,id);
                                        actualNumJoints++;


                                }
                                currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
                        }
                }
        }


        
        float separateBias=0.1;//info.m_erp;//or m_erp2?
        float timeStep=infoGlobal.m_timeStep;
        int iteration=infoGlobal.m_numIterations;

        //create a pair for each constraints, copy over info etc
        
        


        
        {
                BT_PROFILE("compute num contacts");
                int totalContacts =0;

                for (int i=0;i<actualNumManifolds;i++)
                {
                        PfxConstraintPair* pair = &m_memoryCache->m_mypairs[i];
                        totalContacts += pfxGetNumConstraints(*pair);
                }
                //printf("numManifolds = %d\n",numManifolds);
                //printf("totalContacts=%d\n",totalContacts);
        }
        


//      printf("actualNumManifolds=%d\n",actualNumManifolds);
        {
                BT_PROFILE("BPE_customConstraintSolverSequentialNew");
                if (numRigidBodies>0 && (actualNumManifolds+actualNumJoints)>0)
                {
//                      PFX_PRINTF("num points = %d\n",totalPoints);
//                      PFX_PRINTF("num points PFX = %d\n",total);
                        
                        
                        PfxConstraintRow* contactRows = actualNumManifolds? &m_memoryCache->m_constraintRows[0] : 0;
                         PfxBroadphasePair* actualPairs = m_memoryCache->m_mypairs.size() ? &m_memoryCache->m_mypairs[0] : 0;
                        BPE_customConstraintSolverSequentialNew(
                                actualNumManifolds,
                                actualPairs,
                                offsetContactManifolds,
                                contactRows,
                                &m_memoryCache->m_mystates[0],numRigidBodies,
                                &m_memoryCache->m_mysolverbodies[0],
                                jointPairs,actualNumJoints,
                                offsetSolverConstraints,
                                separateBias,timeStep,iteration,
                                m_solverThreadSupport,m_criticalSection,m_solverIO,m_barrier);
                }
        }

        //copy results back to bodies
        {
                BT_PROFILE("copy back");
                for (int i=0;i<numRigidBodies;i++)
                {
                        btCollisionObject* obj = bodies1[i];
                        btRigidBody* rb = btRigidBody::upcast(obj);
                        TrbState& state = m_memoryCache->m_mystates[i];
                        if (rb && (rb->getInvMass()>0.f))
                        {
                                rb->setLinearVelocity(btVector3(state.getLinearVelocity().getX(),state.getLinearVelocity().getY(),state.getLinearVelocity().getZ()));
                                rb->setAngularVelocity(btVector3(state.getAngularVelocity().getX(),state.getAngularVelocity().getY(),state.getAngularVelocity().getZ()));
                        }
                }
        }


        return 0.f;
}