Merge pull request #1579 from lunkhound/parallel-solver-wip3
Multithreaded constraint solver
This commit is contained in:
erwincoumans
GitHub
@@ -15,6 +15,8 @@ SET(BulletDynamics_SRCS
|
||||
ConstraintSolver/btHingeConstraint.cpp
|
||||
ConstraintSolver/btPoint2PointConstraint.cpp
|
||||
ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
|
||||
ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp
|
||||
ConstraintSolver/btBatchedConstraints.cpp
|
||||
ConstraintSolver/btNNCGConstraintSolver.cpp
|
||||
ConstraintSolver/btSliderConstraint.cpp
|
||||
ConstraintSolver/btSolve2LinearConstraint.cpp
|
||||
@@ -62,6 +64,7 @@ SET(ConstraintSolver_HDRS
|
||||
ConstraintSolver/btJacobianEntry.h
|
||||
ConstraintSolver/btPoint2PointConstraint.h
|
||||
ConstraintSolver/btSequentialImpulseConstraintSolver.h
|
||||
ConstraintSolver/btSequentialImpulseConstraintSolverMt.h
|
||||
ConstraintSolver/btNNCGConstraintSolver.h
|
||||
ConstraintSolver/btSliderConstraint.h
|
||||
ConstraintSolver/btSolve2LinearConstraint.h
|
||||
|
||||
1129
src/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp
Normal file
1129
src/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp
Normal file
File diff suppressed because it is too large
Load Diff
66
src/BulletDynamics/ConstraintSolver/btBatchedConstraints.h
Normal file
66
src/BulletDynamics/ConstraintSolver/btBatchedConstraints.h
Normal file
@@ -0,0 +1,66 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef BT_BATCHED_CONSTRAINTS_H
|
||||
#define BT_BATCHED_CONSTRAINTS_H
|
||||
|
||||
#include "LinearMath/btThreads.h"
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btSolverConstraint.h"
|
||||
|
||||
|
||||
class btIDebugDraw;
|
||||
|
||||
struct btBatchedConstraints
|
||||
{
|
||||
enum BatchingMethod
|
||||
{
|
||||
BATCHING_METHOD_SPATIAL_GRID_2D,
|
||||
BATCHING_METHOD_SPATIAL_GRID_3D,
|
||||
BATCHING_METHOD_COUNT
|
||||
};
|
||||
struct Range
|
||||
{
|
||||
int begin;
|
||||
int end;
|
||||
|
||||
Range() : begin( 0 ), end( 0 ) {}
|
||||
Range( int _beg, int _end ) : begin( _beg ), end( _end ) {}
|
||||
};
|
||||
|
||||
btAlignedObjectArray<int> m_constraintIndices;
|
||||
btAlignedObjectArray<Range> m_batches; // each batch is a range of indices in the m_constraintIndices array
|
||||
btAlignedObjectArray<Range> m_phases; // each phase is range of indices in the m_batches array
|
||||
btAlignedObjectArray<char> m_phaseGrainSize; // max grain size for each phase
|
||||
btAlignedObjectArray<int> m_phaseOrder; // phases can be done in any order, so we can randomize the order here
|
||||
btIDebugDraw* m_debugDrawer;
|
||||
|
||||
static bool s_debugDrawBatches;
|
||||
|
||||
btBatchedConstraints() {m_debugDrawer=NULL;}
|
||||
void setup( btConstraintArray* constraints,
|
||||
const btAlignedObjectArray<btSolverBody>& bodies,
|
||||
BatchingMethod batchingMethod,
|
||||
int minBatchSize,
|
||||
int maxBatchSize,
|
||||
btAlignedObjectArray<char>* scratchMemory
|
||||
);
|
||||
bool validate( btConstraintArray* constraints, const btAlignedObjectArray<btSolverBody>& bodies ) const;
|
||||
};
|
||||
|
||||
|
||||
#endif // BT_BATCHED_CONSTRAINTS_H
|
||||
|
||||
@@ -1258,6 +1258,256 @@ void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold**
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void btSequentialImpulseConstraintSolver::convertJoint(btSolverConstraint* currentConstraintRow,
|
||||
btTypedConstraint* constraint,
|
||||
const btTypedConstraint::btConstraintInfo1& info1,
|
||||
int solverBodyIdA,
|
||||
int solverBodyIdB,
|
||||
const btContactSolverInfo& infoGlobal
|
||||
)
|
||||
{
|
||||
const btRigidBody& rbA = constraint->getRigidBodyA();
|
||||
const btRigidBody& rbB = constraint->getRigidBodyB();
|
||||
|
||||
const btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
|
||||
const btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
|
||||
|
||||
int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
|
||||
if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations)
|
||||
m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
|
||||
|
||||
for (int j=0;j<info1.m_numConstraintRows;j++)
|
||||
{
|
||||
memset(¤tConstraintRow[j],0,sizeof(btSolverConstraint));
|
||||
currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY;
|
||||
currentConstraintRow[j].m_upperLimit = SIMD_INFINITY;
|
||||
currentConstraintRow[j].m_appliedImpulse = 0.f;
|
||||
currentConstraintRow[j].m_appliedPushImpulse = 0.f;
|
||||
currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
|
||||
currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
|
||||
currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations;
|
||||
}
|
||||
|
||||
// these vectors are already cleared in initSolverBody, no need to redundantly clear again
|
||||
btAssert(bodyAPtr->getDeltaLinearVelocity().isZero());
|
||||
btAssert(bodyAPtr->getDeltaAngularVelocity().isZero());
|
||||
btAssert(bodyAPtr->getPushVelocity().isZero());
|
||||
btAssert(bodyAPtr->getTurnVelocity().isZero());
|
||||
btAssert(bodyBPtr->getDeltaLinearVelocity().isZero());
|
||||
btAssert(bodyBPtr->getDeltaAngularVelocity().isZero());
|
||||
btAssert(bodyBPtr->getPushVelocity().isZero());
|
||||
btAssert(bodyBPtr->getTurnVelocity().isZero());
|
||||
//bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
|
||||
//bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
|
||||
//bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
|
||||
//bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
|
||||
//bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
|
||||
//bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
|
||||
//bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
|
||||
//bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
|
||||
|
||||
|
||||
btTypedConstraint::btConstraintInfo2 info2;
|
||||
info2.fps = 1.f/infoGlobal.m_timeStep;
|
||||
info2.erp = infoGlobal.m_erp;
|
||||
info2.m_J1linearAxis = currentConstraintRow->m_contactNormal1;
|
||||
info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
|
||||
info2.m_J2linearAxis = currentConstraintRow->m_contactNormal2;
|
||||
info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
|
||||
info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
|
||||
///the size of btSolverConstraint needs be a multiple of btScalar
|
||||
btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
|
||||
info2.m_constraintError = ¤tConstraintRow->m_rhs;
|
||||
currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
|
||||
info2.m_damping = infoGlobal.m_damping;
|
||||
info2.cfm = ¤tConstraintRow->m_cfm;
|
||||
info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit;
|
||||
info2.m_upperLimit = ¤tConstraintRow->m_upperLimit;
|
||||
info2.m_numIterations = infoGlobal.m_numIterations;
|
||||
constraint->getInfo2(&info2);
|
||||
|
||||
///finalize the constraint setup
|
||||
for (int j=0;j<info1.m_numConstraintRows;j++)
|
||||
{
|
||||
btSolverConstraint& solverConstraint = currentConstraintRow[j];
|
||||
|
||||
if (solverConstraint.m_upperLimit>=constraint->getBreakingImpulseThreshold())
|
||||
{
|
||||
solverConstraint.m_upperLimit = constraint->getBreakingImpulseThreshold();
|
||||
}
|
||||
|
||||
if (solverConstraint.m_lowerLimit<=-constraint->getBreakingImpulseThreshold())
|
||||
{
|
||||
solverConstraint.m_lowerLimit = -constraint->getBreakingImpulseThreshold();
|
||||
}
|
||||
|
||||
solverConstraint.m_originalContactPoint = constraint;
|
||||
|
||||
{
|
||||
const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
|
||||
solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor();
|
||||
}
|
||||
{
|
||||
const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
|
||||
solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor();
|
||||
}
|
||||
|
||||
{
|
||||
btVector3 iMJlA = solverConstraint.m_contactNormal1*rbA.getInvMass();
|
||||
btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
|
||||
btVector3 iMJlB = solverConstraint.m_contactNormal2*rbB.getInvMass();//sign of normal?
|
||||
btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
|
||||
|
||||
btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal1);
|
||||
sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
|
||||
sum += iMJlB.dot(solverConstraint.m_contactNormal2);
|
||||
sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
|
||||
btScalar fsum = btFabs(sum);
|
||||
btAssert(fsum > SIMD_EPSILON);
|
||||
btScalar sorRelaxation = 1.f;//todo: get from globalInfo?
|
||||
solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?sorRelaxation/sum : 0.f;
|
||||
}
|
||||
|
||||
{
|
||||
btScalar rel_vel;
|
||||
btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0,0,0);
|
||||
btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse : btVector3(0,0,0);
|
||||
|
||||
btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0,0,0);
|
||||
btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ?bodyBPtr->m_externalTorqueImpulse : btVector3(0,0,0);
|
||||
|
||||
btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA)
|
||||
+ solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()+externalTorqueImpulseA);
|
||||
|
||||
btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB)
|
||||
+ solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()+externalTorqueImpulseB);
|
||||
|
||||
rel_vel = vel1Dotn+vel2Dotn;
|
||||
btScalar restitution = 0.f;
|
||||
btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
|
||||
btScalar velocityError = restitution - rel_vel * info2.m_damping;
|
||||
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
|
||||
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
|
||||
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
|
||||
solverConstraint.m_appliedImpulse = 0.f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void btSequentialImpulseConstraintSolver::convertJoints(btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
BT_PROFILE("convertJoints");
|
||||
for (int j=0;j<numConstraints;j++)
|
||||
{
|
||||
btTypedConstraint* constraint = constraints[j];
|
||||
constraint->buildJacobian();
|
||||
constraint->internalSetAppliedImpulse(0.0f);
|
||||
}
|
||||
|
||||
int totalNumRows = 0;
|
||||
|
||||
m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
|
||||
//calculate the total number of contraint rows
|
||||
for (int i=0;i<numConstraints;i++)
|
||||
{
|
||||
btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
|
||||
btJointFeedback* fb = constraints[i]->getJointFeedback();
|
||||
if (fb)
|
||||
{
|
||||
fb->m_appliedForceBodyA.setZero();
|
||||
fb->m_appliedTorqueBodyA.setZero();
|
||||
fb->m_appliedForceBodyB.setZero();
|
||||
fb->m_appliedTorqueBodyB.setZero();
|
||||
}
|
||||
|
||||
if (constraints[i]->isEnabled())
|
||||
{
|
||||
constraints[i]->getInfo1(&info1);
|
||||
} else
|
||||
{
|
||||
info1.m_numConstraintRows = 0;
|
||||
info1.nub = 0;
|
||||
}
|
||||
totalNumRows += info1.m_numConstraintRows;
|
||||
}
|
||||
m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
|
||||
|
||||
|
||||
///setup the btSolverConstraints
|
||||
int currentRow = 0;
|
||||
|
||||
for (int i=0;i<numConstraints;i++)
|
||||
{
|
||||
const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
|
||||
|
||||
if (info1.m_numConstraintRows)
|
||||
{
|
||||
btAssert(currentRow<totalNumRows);
|
||||
|
||||
btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
|
||||
btTypedConstraint* constraint = constraints[i];
|
||||
btRigidBody& rbA = constraint->getRigidBodyA();
|
||||
btRigidBody& rbB = constraint->getRigidBodyB();
|
||||
|
||||
int solverBodyIdA = getOrInitSolverBody(rbA,infoGlobal.m_timeStep);
|
||||
int solverBodyIdB = getOrInitSolverBody(rbB,infoGlobal.m_timeStep);
|
||||
|
||||
convertJoint(currentConstraintRow, constraint, info1, solverBodyIdA, solverBodyIdB, infoGlobal);
|
||||
}
|
||||
currentRow+=info1.m_numConstraintRows;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void btSequentialImpulseConstraintSolver::convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
BT_PROFILE("convertBodies");
|
||||
for (int i = 0; i < numBodies; i++)
|
||||
{
|
||||
bodies[i]->setCompanionId(-1);
|
||||
}
|
||||
#if BT_THREADSAFE
|
||||
m_kinematicBodyUniqueIdToSolverBodyTable.resize( 0 );
|
||||
#endif // BT_THREADSAFE
|
||||
|
||||
m_tmpSolverBodyPool.reserve(numBodies+1);
|
||||
m_tmpSolverBodyPool.resize(0);
|
||||
|
||||
//btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
|
||||
//initSolverBody(&fixedBody,0);
|
||||
|
||||
for (int i=0;i<numBodies;i++)
|
||||
{
|
||||
int bodyId = getOrInitSolverBody(*bodies[i],infoGlobal.m_timeStep);
|
||||
|
||||
btRigidBody* body = btRigidBody::upcast(bodies[i]);
|
||||
if (body && body->getInvMass())
|
||||
{
|
||||
btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
|
||||
btVector3 gyroForce (0,0,0);
|
||||
if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT)
|
||||
{
|
||||
gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce);
|
||||
solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
|
||||
}
|
||||
if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD)
|
||||
{
|
||||
gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep);
|
||||
solverBody.m_externalTorqueImpulse += gyroForce;
|
||||
}
|
||||
if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY)
|
||||
{
|
||||
gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep);
|
||||
solverBody.m_externalTorqueImpulse += gyroForce;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
|
||||
{
|
||||
m_fixedBodyId = -1;
|
||||
@@ -1344,250 +1594,13 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
||||
#endif //BT_ADDITIONAL_DEBUG
|
||||
|
||||
|
||||
for (int i = 0; i < numBodies; i++)
|
||||
{
|
||||
bodies[i]->setCompanionId(-1);
|
||||
}
|
||||
#if BT_THREADSAFE
|
||||
m_kinematicBodyUniqueIdToSolverBodyTable.resize( 0 );
|
||||
#endif // BT_THREADSAFE
|
||||
|
||||
m_tmpSolverBodyPool.reserve(numBodies+1);
|
||||
m_tmpSolverBodyPool.resize(0);
|
||||
|
||||
//btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
|
||||
//initSolverBody(&fixedBody,0);
|
||||
|
||||
//convert all bodies
|
||||
convertBodies(bodies, numBodies, infoGlobal);
|
||||
|
||||
convertJoints(constraints, numConstraints, infoGlobal);
|
||||
|
||||
for (int i=0;i<numBodies;i++)
|
||||
{
|
||||
int bodyId = getOrInitSolverBody(*bodies[i],infoGlobal.m_timeStep);
|
||||
convertContacts(manifoldPtr,numManifolds,infoGlobal);
|
||||
|
||||
btRigidBody* body = btRigidBody::upcast(bodies[i]);
|
||||
if (body && body->getInvMass())
|
||||
{
|
||||
btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
|
||||
btVector3 gyroForce (0,0,0);
|
||||
if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT)
|
||||
{
|
||||
gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce);
|
||||
solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
|
||||
}
|
||||
if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD)
|
||||
{
|
||||
gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep);
|
||||
solverBody.m_externalTorqueImpulse += gyroForce;
|
||||
}
|
||||
if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY)
|
||||
{
|
||||
gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep);
|
||||
solverBody.m_externalTorqueImpulse += gyroForce;
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
if (1)
|
||||
{
|
||||
int j;
|
||||
for (j=0;j<numConstraints;j++)
|
||||
{
|
||||
btTypedConstraint* constraint = constraints[j];
|
||||
constraint->buildJacobian();
|
||||
constraint->internalSetAppliedImpulse(0.0f);
|
||||
}
|
||||
}
|
||||
|
||||
//btRigidBody* rb0=0,*rb1=0;
|
||||
|
||||
//if (1)
|
||||
{
|
||||
{
|
||||
|
||||
int totalNumRows = 0;
|
||||
int i;
|
||||
|
||||
m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
|
||||
//calculate the total number of contraint rows
|
||||
for (i=0;i<numConstraints;i++)
|
||||
{
|
||||
btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
|
||||
btJointFeedback* fb = constraints[i]->getJointFeedback();
|
||||
if (fb)
|
||||
{
|
||||
fb->m_appliedForceBodyA.setZero();
|
||||
fb->m_appliedTorqueBodyA.setZero();
|
||||
fb->m_appliedForceBodyB.setZero();
|
||||
fb->m_appliedTorqueBodyB.setZero();
|
||||
}
|
||||
|
||||
if (constraints[i]->isEnabled())
|
||||
{
|
||||
constraints[i]->getInfo1(&info1);
|
||||
} else
|
||||
{
|
||||
info1.m_numConstraintRows = 0;
|
||||
info1.nub = 0;
|
||||
}
|
||||
totalNumRows += info1.m_numConstraintRows;
|
||||
}
|
||||
m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
|
||||
|
||||
|
||||
///setup the btSolverConstraints
|
||||
int currentRow = 0;
|
||||
|
||||
for (i=0;i<numConstraints;i++)
|
||||
{
|
||||
const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
|
||||
|
||||
if (info1.m_numConstraintRows)
|
||||
{
|
||||
btAssert(currentRow<totalNumRows);
|
||||
|
||||
btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
|
||||
btTypedConstraint* constraint = constraints[i];
|
||||
btRigidBody& rbA = constraint->getRigidBodyA();
|
||||
btRigidBody& rbB = constraint->getRigidBodyB();
|
||||
|
||||
int solverBodyIdA = getOrInitSolverBody(rbA,infoGlobal.m_timeStep);
|
||||
int solverBodyIdB = getOrInitSolverBody(rbB,infoGlobal.m_timeStep);
|
||||
|
||||
btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
|
||||
btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
|
||||
|
||||
|
||||
|
||||
|
||||
int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
|
||||
if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations)
|
||||
m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
|
||||
|
||||
|
||||
int j;
|
||||
for ( j=0;j<info1.m_numConstraintRows;j++)
|
||||
{
|
||||
memset(¤tConstraintRow[j],0,sizeof(btSolverConstraint));
|
||||
currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY;
|
||||
currentConstraintRow[j].m_upperLimit = SIMD_INFINITY;
|
||||
currentConstraintRow[j].m_appliedImpulse = 0.f;
|
||||
currentConstraintRow[j].m_appliedPushImpulse = 0.f;
|
||||
currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
|
||||
currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
|
||||
currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations;
|
||||
}
|
||||
|
||||
bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
|
||||
bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
|
||||
bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
|
||||
bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
|
||||
bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
|
||||
bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
|
||||
bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
|
||||
bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
|
||||
|
||||
|
||||
btTypedConstraint::btConstraintInfo2 info2;
|
||||
info2.fps = 1.f/infoGlobal.m_timeStep;
|
||||
info2.erp = infoGlobal.m_erp;
|
||||
info2.m_J1linearAxis = currentConstraintRow->m_contactNormal1;
|
||||
info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
|
||||
info2.m_J2linearAxis = currentConstraintRow->m_contactNormal2;
|
||||
info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
|
||||
info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
|
||||
///the size of btSolverConstraint needs be a multiple of btScalar
|
||||
btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
|
||||
info2.m_constraintError = ¤tConstraintRow->m_rhs;
|
||||
currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
|
||||
info2.m_damping = infoGlobal.m_damping;
|
||||
info2.cfm = ¤tConstraintRow->m_cfm;
|
||||
info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit;
|
||||
info2.m_upperLimit = ¤tConstraintRow->m_upperLimit;
|
||||
info2.m_numIterations = infoGlobal.m_numIterations;
|
||||
constraints[i]->getInfo2(&info2);
|
||||
|
||||
///finalize the constraint setup
|
||||
for ( j=0;j<info1.m_numConstraintRows;j++)
|
||||
{
|
||||
btSolverConstraint& solverConstraint = currentConstraintRow[j];
|
||||
|
||||
if (solverConstraint.m_upperLimit>=constraints[i]->getBreakingImpulseThreshold())
|
||||
{
|
||||
solverConstraint.m_upperLimit = constraints[i]->getBreakingImpulseThreshold();
|
||||
}
|
||||
|
||||
if (solverConstraint.m_lowerLimit<=-constraints[i]->getBreakingImpulseThreshold())
|
||||
{
|
||||
solverConstraint.m_lowerLimit = -constraints[i]->getBreakingImpulseThreshold();
|
||||
}
|
||||
|
||||
solverConstraint.m_originalContactPoint = constraint;
|
||||
|
||||
{
|
||||
const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
|
||||
solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor();
|
||||
}
|
||||
{
|
||||
const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
|
||||
solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor();
|
||||
}
|
||||
|
||||
{
|
||||
btVector3 iMJlA = solverConstraint.m_contactNormal1*rbA.getInvMass();
|
||||
btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
|
||||
btVector3 iMJlB = solverConstraint.m_contactNormal2*rbB.getInvMass();//sign of normal?
|
||||
btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
|
||||
|
||||
btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal1);
|
||||
sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
|
||||
sum += iMJlB.dot(solverConstraint.m_contactNormal2);
|
||||
sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
|
||||
btScalar fsum = btFabs(sum);
|
||||
btAssert(fsum > SIMD_EPSILON);
|
||||
btScalar sorRelaxation = 1.f;//todo: get from globalInfo?
|
||||
solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?sorRelaxation/sum : 0.f;
|
||||
}
|
||||
|
||||
|
||||
|
||||
{
|
||||
btScalar rel_vel;
|
||||
btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0,0,0);
|
||||
btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse : btVector3(0,0,0);
|
||||
|
||||
btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0,0,0);
|
||||
btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ?bodyBPtr->m_externalTorqueImpulse : btVector3(0,0,0);
|
||||
|
||||
btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA)
|
||||
+ solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()+externalTorqueImpulseA);
|
||||
|
||||
btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB)
|
||||
+ solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()+externalTorqueImpulseB);
|
||||
|
||||
rel_vel = vel1Dotn+vel2Dotn;
|
||||
btScalar restitution = 0.f;
|
||||
btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
|
||||
btScalar velocityError = restitution - rel_vel * info2.m_damping;
|
||||
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
|
||||
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
|
||||
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
|
||||
solverConstraint.m_appliedImpulse = 0.f;
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
|
||||
}
|
||||
}
|
||||
|
||||
convertContacts(manifoldPtr,numManifolds,infoGlobal);
|
||||
|
||||
}
|
||||
|
||||
// btContactSolverInfo info = infoGlobal;
|
||||
|
||||
@@ -1627,6 +1640,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
||||
|
||||
btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/)
|
||||
{
|
||||
BT_PROFILE("solveSingleIteration");
|
||||
btScalar leastSquaresResidual = 0.f;
|
||||
|
||||
int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
|
||||
@@ -1805,6 +1819,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
|
||||
|
||||
void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
|
||||
{
|
||||
BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");
|
||||
int iteration;
|
||||
if (infoGlobal.m_splitImpulse)
|
||||
{
|
||||
@@ -1863,14 +1878,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
|
||||
return 0.f;
|
||||
}
|
||||
|
||||
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
|
||||
void btSequentialImpulseConstraintSolver::writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
|
||||
int i,j;
|
||||
|
||||
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
|
||||
{
|
||||
for (j=0;j<numPoolConstraints;j++)
|
||||
for (int j=iBegin; j<iEnd; j++)
|
||||
{
|
||||
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
|
||||
btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
|
||||
@@ -1886,10 +1896,11 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo
|
||||
}
|
||||
//do a callback here?
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
numPoolConstraints = m_tmpSolverNonContactConstraintPool.size();
|
||||
for (j=0;j<numPoolConstraints;j++)
|
||||
void btSequentialImpulseConstraintSolver::writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
for (int j=iBegin; j<iEnd; j++)
|
||||
{
|
||||
const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
|
||||
btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint;
|
||||
@@ -1909,10 +1920,12 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo
|
||||
constr->setEnabled(false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
for ( i=0;i<m_tmpSolverBodyPool.size();i++)
|
||||
void btSequentialImpulseConstraintSolver::writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
for (int i=iBegin; i<iEnd; i++)
|
||||
{
|
||||
btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
|
||||
if (body)
|
||||
@@ -1936,6 +1949,19 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo
|
||||
m_tmpSolverBodyPool[i].m_originalBody->setCompanionId(-1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
BT_PROFILE("solveGroupCacheFriendlyFinish");
|
||||
|
||||
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
|
||||
{
|
||||
writeBackContacts(0, m_tmpSolverContactConstraintPool.size(), infoGlobal);
|
||||
}
|
||||
|
||||
writeBackJoints(0, m_tmpSolverNonContactConstraintPool.size(), infoGlobal);
|
||||
writeBackBodies(0, m_tmpSolverBodyPool.size(), infoGlobal);
|
||||
|
||||
m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
|
||||
m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
|
||||
|
||||
@@ -95,6 +95,10 @@ protected:
|
||||
|
||||
void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
|
||||
|
||||
virtual void convertJoints(btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal);
|
||||
void convertJoint(btSolverConstraint* destConstraintRow, btTypedConstraint* srcConstraint, const btTypedConstraint::btConstraintInfo1& info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo& infoGlobal);
|
||||
|
||||
virtual void convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal);
|
||||
|
||||
btSimdScalar resolveSplitPenetrationSIMD(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint)
|
||||
{
|
||||
@@ -121,7 +125,9 @@ protected:
|
||||
|
||||
protected:
|
||||
|
||||
|
||||
void writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
|
||||
void writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
|
||||
void writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
|
||||
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
|
||||
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
|
||||
virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,154 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_MT_H
|
||||
#define BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_MT_H
|
||||
|
||||
#include "btSequentialImpulseConstraintSolver.h"
|
||||
#include "btBatchedConstraints.h"
|
||||
#include "LinearMath/btThreads.h"
|
||||
|
||||
///
|
||||
/// btSequentialImpulseConstraintSolverMt
|
||||
///
|
||||
/// A multithreaded variant of the sequential impulse constraint solver. The constraints to be solved are grouped into
|
||||
/// batches and phases where each batch of constraints within a given phase can be solved in parallel with the rest.
|
||||
/// Ideally we want as few phases as possible, and each phase should have many batches, and all of the batches should
|
||||
/// have about the same number of constraints.
|
||||
/// This method works best on a large island of many constraints.
|
||||
///
|
||||
/// Supports all of the features of the normal sequential impulse solver such as:
|
||||
/// - split penetration impulse
|
||||
/// - rolling friction
|
||||
/// - interleaving constraints
|
||||
/// - warmstarting
|
||||
/// - 2 friction directions
|
||||
/// - randomized constraint ordering
|
||||
/// - early termination when leastSquaresResidualThreshold is satisfied
|
||||
///
|
||||
/// When the SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS flag is enabled, unlike the normal SequentialImpulse solver,
|
||||
/// the rolling friction is interleaved as well.
|
||||
/// Interleaving the contact penetration constraints with friction reduces the number of parallel loops that need to be done,
|
||||
/// which reduces threading overhead so it can be a performance win, however, it does seem to produce a less stable simulation,
|
||||
/// at least on stacks of blocks.
|
||||
///
|
||||
/// When the SOLVER_RANDMIZE_ORDER flag is enabled, the ordering of phases, and the ordering of constraints within each batch
|
||||
/// is randomized, however it does not swap constraints between batches.
|
||||
/// This is to avoid regenerating the batches for each solver iteration which would be quite costly in performance.
|
||||
///
|
||||
/// Note that a non-zero leastSquaresResidualThreshold could possibly affect the determinism of the simulation
|
||||
/// if the task scheduler's parallelSum operation is non-deterministic. The parallelSum operation can be non-deterministic
|
||||
/// because floating point addition is not associative due to rounding errors.
|
||||
/// The task scheduler can and should ensure that the result of any parallelSum operation is deterministic.
|
||||
///
|
||||
ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolverMt : public btSequentialImpulseConstraintSolver
|
||||
{
|
||||
public:
|
||||
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) BT_OVERRIDE;
|
||||
virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) BT_OVERRIDE;
|
||||
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) BT_OVERRIDE;
|
||||
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
|
||||
|
||||
// temp struct used to collect info from persistent manifolds into a cache-friendly struct using multiple threads
|
||||
struct btContactManifoldCachedInfo
|
||||
{
|
||||
static const int MAX_NUM_CONTACT_POINTS = 4;
|
||||
|
||||
int numTouchingContacts;
|
||||
int solverBodyIds[ 2 ];
|
||||
int contactIndex;
|
||||
int rollingFrictionIndex;
|
||||
bool contactHasRollingFriction[ MAX_NUM_CONTACT_POINTS ];
|
||||
btManifoldPoint* contactPoints[ MAX_NUM_CONTACT_POINTS ];
|
||||
};
|
||||
// temp struct used for setting up joint constraints in parallel
|
||||
struct JointParams
|
||||
{
|
||||
int m_solverConstraint;
|
||||
int m_solverBodyA;
|
||||
int m_solverBodyB;
|
||||
};
|
||||
void internalInitMultipleJoints(btTypedConstraint** constraints, int iBegin, int iEnd);
|
||||
void internalConvertMultipleJoints( const btAlignedObjectArray<JointParams>& jointParamsArray, btTypedConstraint** constraints, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal );
|
||||
|
||||
// parameters to control batching
|
||||
static bool s_allowNestedParallelForLoops; // whether to allow nested parallel operations
|
||||
static int s_minimumContactManifoldsForBatching; // don't even try to batch if fewer manifolds than this
|
||||
static btBatchedConstraints::BatchingMethod s_contactBatchingMethod;
|
||||
static btBatchedConstraints::BatchingMethod s_jointBatchingMethod;
|
||||
static int s_minBatchSize; // desired number of constraints per batch
|
||||
static int s_maxBatchSize;
|
||||
|
||||
protected:
|
||||
static const int CACHE_LINE_SIZE = 64;
|
||||
|
||||
btBatchedConstraints m_batchedContactConstraints;
|
||||
btBatchedConstraints m_batchedJointConstraints;
|
||||
int m_numFrictionDirections;
|
||||
bool m_useBatching;
|
||||
bool m_useObsoleteJointConstraints;
|
||||
btAlignedObjectArray<btContactManifoldCachedInfo> m_manifoldCachedInfoArray;
|
||||
btAlignedObjectArray<int> m_rollingFrictionIndexTable; // lookup table mapping contact index to rolling friction index
|
||||
btSpinMutex m_bodySolverArrayMutex;
|
||||
char m_antiFalseSharingPadding[CACHE_LINE_SIZE]; // padding to keep mutexes in separate cachelines
|
||||
btSpinMutex m_kinematicBodyUniqueIdToSolverBodyTableMutex;
|
||||
btAlignedObjectArray<char> m_scratchMemory;
|
||||
|
||||
virtual void randomizeConstraintOrdering( int iteration, int numIterations );
|
||||
virtual btScalar resolveAllJointConstraints( int iteration );
|
||||
virtual btScalar resolveAllContactConstraints();
|
||||
virtual btScalar resolveAllContactFrictionConstraints();
|
||||
virtual btScalar resolveAllContactConstraintsInterleaved();
|
||||
virtual btScalar resolveAllRollingFrictionConstraints();
|
||||
|
||||
virtual void setupBatchedContactConstraints();
|
||||
virtual void setupBatchedJointConstraints();
|
||||
virtual void convertJoints(btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
|
||||
virtual void convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
|
||||
virtual void convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
|
||||
|
||||
int getOrInitSolverBodyThreadsafe(btCollisionObject& body, btScalar timeStep);
|
||||
void allocAllContactConstraints(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
|
||||
void setupAllContactConstraints(const btContactSolverInfo& infoGlobal);
|
||||
void randomizeBatchedConstraintOrdering( btBatchedConstraints* batchedConstraints );
|
||||
|
||||
public:
|
||||
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
btSequentialImpulseConstraintSolverMt();
|
||||
virtual ~btSequentialImpulseConstraintSolverMt();
|
||||
|
||||
btScalar resolveMultipleJointConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd, int iteration );
|
||||
btScalar resolveMultipleContactConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd );
|
||||
btScalar resolveMultipleContactSplitPenetrationImpulseConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd );
|
||||
btScalar resolveMultipleContactFrictionConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd );
|
||||
btScalar resolveMultipleContactRollingFrictionConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd );
|
||||
btScalar resolveMultipleContactConstraintsInterleaved( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd );
|
||||
|
||||
void internalCollectContactManifoldCachedInfo(btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifold, int numManifolds, const btContactSolverInfo& infoGlobal);
|
||||
void internalAllocContactConstraints(const btContactManifoldCachedInfo* cachedInfoArray, int numManifolds);
|
||||
void internalSetupContactConstraints(int iContact, const btContactSolverInfo& infoGlobal);
|
||||
void internalConvertBodies(btCollisionObject** bodies, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
|
||||
void internalWriteBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
|
||||
void internalWriteBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
|
||||
void internalWriteBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_MT_H
|
||||
|
||||
@@ -50,63 +50,6 @@ subject to the following restrictions:
|
||||
#include "LinearMath/btSerializer.h"
|
||||
|
||||
|
||||
struct InplaceSolverIslandCallbackMt : public btSimulationIslandManagerMt::IslandCallback
|
||||
{
|
||||
btContactSolverInfo* m_solverInfo;
|
||||
btConstraintSolver* m_solver;
|
||||
btIDebugDraw* m_debugDrawer;
|
||||
btDispatcher* m_dispatcher;
|
||||
|
||||
InplaceSolverIslandCallbackMt(
|
||||
btConstraintSolver* solver,
|
||||
btStackAlloc* stackAlloc,
|
||||
btDispatcher* dispatcher)
|
||||
:m_solverInfo(NULL),
|
||||
m_solver(solver),
|
||||
m_debugDrawer(NULL),
|
||||
m_dispatcher(dispatcher)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
InplaceSolverIslandCallbackMt& operator=(InplaceSolverIslandCallbackMt& other)
|
||||
{
|
||||
btAssert(0);
|
||||
(void)other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btIDebugDraw* debugDrawer)
|
||||
{
|
||||
btAssert(solverInfo);
|
||||
m_solverInfo = solverInfo;
|
||||
m_debugDrawer = debugDrawer;
|
||||
}
|
||||
|
||||
|
||||
virtual void processIsland( btCollisionObject** bodies,
|
||||
int numBodies,
|
||||
btPersistentManifold** manifolds,
|
||||
int numManifolds,
|
||||
btTypedConstraint** constraints,
|
||||
int numConstraints,
|
||||
int islandId
|
||||
)
|
||||
{
|
||||
m_solver->solveGroup( bodies,
|
||||
numBodies,
|
||||
manifolds,
|
||||
numManifolds,
|
||||
constraints,
|
||||
numConstraints,
|
||||
*m_solverInfo,
|
||||
m_debugDrawer,
|
||||
m_dispatcher
|
||||
);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
///
|
||||
/// btConstraintSolverPoolMt
|
||||
@@ -209,7 +152,12 @@ void btConstraintSolverPoolMt::reset()
|
||||
/// btDiscreteDynamicsWorldMt
|
||||
///
|
||||
|
||||
btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolverPoolMt* constraintSolver, btCollisionConfiguration* collisionConfiguration)
|
||||
btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher,
|
||||
btBroadphaseInterface* pairCache,
|
||||
btConstraintSolverPoolMt* constraintSolver,
|
||||
btConstraintSolver* constraintSolverMt,
|
||||
btCollisionConfiguration* collisionConfiguration
|
||||
)
|
||||
: btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration)
|
||||
{
|
||||
if (m_ownsIslandManager)
|
||||
@@ -217,31 +165,18 @@ btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher, b
|
||||
m_islandManager->~btSimulationIslandManager();
|
||||
btAlignedFree( m_islandManager);
|
||||
}
|
||||
{
|
||||
void* mem = btAlignedAlloc(sizeof(InplaceSolverIslandCallbackMt),16);
|
||||
m_solverIslandCallbackMt = new (mem) InplaceSolverIslandCallbackMt (m_constraintSolver, 0, dispatcher);
|
||||
}
|
||||
{
|
||||
void* mem = btAlignedAlloc(sizeof(btSimulationIslandManagerMt),16);
|
||||
btSimulationIslandManagerMt* im = new (mem) btSimulationIslandManagerMt();
|
||||
im->setMinimumSolverBatchSize( m_solverInfo.m_minimumSolverBatchSize );
|
||||
m_islandManager = im;
|
||||
}
|
||||
m_constraintSolverMt = constraintSolverMt;
|
||||
}
|
||||
|
||||
|
||||
btDiscreteDynamicsWorldMt::~btDiscreteDynamicsWorldMt()
|
||||
{
|
||||
if (m_solverIslandCallbackMt)
|
||||
{
|
||||
m_solverIslandCallbackMt->~InplaceSolverIslandCallbackMt();
|
||||
btAlignedFree(m_solverIslandCallbackMt);
|
||||
}
|
||||
if (m_ownsConstraintSolver)
|
||||
{
|
||||
m_constraintSolver->~btConstraintSolver();
|
||||
btAlignedFree(m_constraintSolver);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -249,12 +184,17 @@ void btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo
|
||||
{
|
||||
BT_PROFILE("solveConstraints");
|
||||
|
||||
m_solverIslandCallbackMt->setup(&solverInfo, getDebugDrawer());
|
||||
m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
|
||||
|
||||
/// solve all the constraints for this island
|
||||
btSimulationIslandManagerMt* im = static_cast<btSimulationIslandManagerMt*>(m_islandManager);
|
||||
im->buildAndProcessIslands( getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_constraints, m_solverIslandCallbackMt );
|
||||
btSimulationIslandManagerMt::SolverParams solverParams;
|
||||
solverParams.m_solverPool = m_constraintSolver;
|
||||
solverParams.m_solverMt = m_constraintSolverMt;
|
||||
solverParams.m_solverInfo = &solverInfo;
|
||||
solverParams.m_debugDrawer = m_debugDrawer;
|
||||
solverParams.m_dispatcher = getCollisionWorld()->getDispatcher();
|
||||
im->buildAndProcessIslands( getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_constraints, solverParams );
|
||||
|
||||
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
|
||||
}
|
||||
@@ -325,3 +265,14 @@ void btDiscreteDynamicsWorldMt::integrateTransforms( btScalar timeStep )
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int btDiscreteDynamicsWorldMt::stepSimulation( btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep )
|
||||
{
|
||||
int numSubSteps = btDiscreteDynamicsWorld::stepSimulation(timeStep, maxSubSteps, fixedTimeStep);
|
||||
if (btITaskScheduler* scheduler = btGetTaskScheduler())
|
||||
{
|
||||
// tell Bullet's threads to sleep, so other threads can run
|
||||
scheduler->sleepWorkerThreadsHint();
|
||||
}
|
||||
return numSubSteps;
|
||||
}
|
||||
|
||||
@@ -21,7 +21,6 @@ subject to the following restrictions:
|
||||
#include "btSimulationIslandManagerMt.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
|
||||
|
||||
struct InplaceSolverIslandCallbackMt;
|
||||
|
||||
///
|
||||
/// btConstraintSolverPoolMt - masquerades as a constraint solver, but really it is a threadsafe pool of them.
|
||||
@@ -88,7 +87,7 @@ private:
|
||||
ATTRIBUTE_ALIGNED16(class) btDiscreteDynamicsWorldMt : public btDiscreteDynamicsWorld
|
||||
{
|
||||
protected:
|
||||
InplaceSolverIslandCallbackMt* m_solverIslandCallbackMt;
|
||||
btConstraintSolver* m_constraintSolverMt;
|
||||
|
||||
virtual void solveConstraints(btContactSolverInfo& solverInfo) BT_OVERRIDE;
|
||||
|
||||
@@ -126,9 +125,12 @@ public:
|
||||
btDiscreteDynamicsWorldMt(btDispatcher* dispatcher,
|
||||
btBroadphaseInterface* pairCache,
|
||||
btConstraintSolverPoolMt* constraintSolver, // Note this should be a solver-pool for multi-threading
|
||||
btConstraintSolver* constraintSolverMt, // single multi-threaded solver for large islands (or NULL)
|
||||
btCollisionConfiguration* collisionConfiguration
|
||||
);
|
||||
virtual ~btDiscreteDynamicsWorldMt();
|
||||
|
||||
virtual int stepSimulation( btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep ) BT_OVERRIDE;
|
||||
};
|
||||
|
||||
#endif //BT_DISCRETE_DYNAMICS_WORLD_H
|
||||
|
||||
@@ -22,6 +22,7 @@ subject to the following restrictions:
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h" // for s_minimumContactManifoldsForBatching
|
||||
|
||||
//#include <stdio.h>
|
||||
#include "LinearMath/btQuickprof.h"
|
||||
@@ -275,7 +276,7 @@ btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::allocateIsland
|
||||
void btSimulationIslandManagerMt::buildIslands( btDispatcher* dispatcher, btCollisionWorld* collisionWorld )
|
||||
{
|
||||
|
||||
BT_PROFILE("islandUnionFindAndQuickSort");
|
||||
BT_PROFILE("buildIslands");
|
||||
|
||||
btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
|
||||
|
||||
@@ -544,59 +545,103 @@ void btSimulationIslandManagerMt::mergeIslands()
|
||||
}
|
||||
|
||||
|
||||
void btSimulationIslandManagerMt::serialIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback )
|
||||
void btSimulationIslandManagerMt::solveIsland(btConstraintSolver* solver, Island& island, const SolverParams& solverParams)
|
||||
{
|
||||
btPersistentManifold** manifolds = island.manifoldArray.size() ? &island.manifoldArray[ 0 ] : NULL;
|
||||
btTypedConstraint** constraintsPtr = island.constraintArray.size() ? &island.constraintArray[ 0 ] : NULL;
|
||||
solver->solveGroup( &island.bodyArray[ 0 ],
|
||||
island.bodyArray.size(),
|
||||
manifolds,
|
||||
island.manifoldArray.size(),
|
||||
constraintsPtr,
|
||||
island.constraintArray.size(),
|
||||
*solverParams.m_solverInfo,
|
||||
solverParams.m_debugDrawer,
|
||||
solverParams.m_dispatcher
|
||||
);
|
||||
}
|
||||
|
||||
|
||||
void btSimulationIslandManagerMt::serialIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams )
|
||||
{
|
||||
BT_PROFILE( "serialIslandDispatch" );
|
||||
// serial dispatch
|
||||
btAlignedObjectArray<Island*>& islands = *islandsPtr;
|
||||
btConstraintSolver* solver = solverParams.m_solverMt ? solverParams.m_solverMt : solverParams.m_solverPool;
|
||||
for ( int i = 0; i < islands.size(); ++i )
|
||||
{
|
||||
Island* island = islands[ i ];
|
||||
btPersistentManifold** manifolds = island->manifoldArray.size() ? &island->manifoldArray[ 0 ] : NULL;
|
||||
btTypedConstraint** constraintsPtr = island->constraintArray.size() ? &island->constraintArray[ 0 ] : NULL;
|
||||
callback->processIsland( &island->bodyArray[ 0 ],
|
||||
island->bodyArray.size(),
|
||||
manifolds,
|
||||
island->manifoldArray.size(),
|
||||
constraintsPtr,
|
||||
island->constraintArray.size(),
|
||||
island->id
|
||||
);
|
||||
solveIsland(solver, *islands[ i ], solverParams);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
struct UpdateIslandDispatcher : public btIParallelForBody
|
||||
{
|
||||
btAlignedObjectArray<btSimulationIslandManagerMt::Island*>* islandsPtr;
|
||||
btSimulationIslandManagerMt::IslandCallback* callback;
|
||||
btAlignedObjectArray<btSimulationIslandManagerMt::Island*>& m_islandsPtr;
|
||||
const btSimulationIslandManagerMt::SolverParams& m_solverParams;
|
||||
|
||||
UpdateIslandDispatcher(btAlignedObjectArray<btSimulationIslandManagerMt::Island*>& islandsPtr, const btSimulationIslandManagerMt::SolverParams& solverParams)
|
||||
: m_islandsPtr(islandsPtr), m_solverParams(solverParams)
|
||||
{}
|
||||
|
||||
void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
|
||||
{
|
||||
btConstraintSolver* solver = m_solverParams.m_solverPool;
|
||||
for ( int i = iBegin; i < iEnd; ++i )
|
||||
{
|
||||
btSimulationIslandManagerMt::Island* island = ( *islandsPtr )[ i ];
|
||||
btPersistentManifold** manifolds = island->manifoldArray.size() ? &island->manifoldArray[ 0 ] : NULL;
|
||||
btTypedConstraint** constraintsPtr = island->constraintArray.size() ? &island->constraintArray[ 0 ] : NULL;
|
||||
callback->processIsland( &island->bodyArray[ 0 ],
|
||||
island->bodyArray.size(),
|
||||
manifolds,
|
||||
island->manifoldArray.size(),
|
||||
constraintsPtr,
|
||||
island->constraintArray.size(),
|
||||
island->id
|
||||
);
|
||||
btSimulationIslandManagerMt::Island* island = m_islandsPtr[ i ];
|
||||
btSimulationIslandManagerMt::solveIsland( solver, *island, m_solverParams );
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
void btSimulationIslandManagerMt::parallelIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback )
|
||||
|
||||
void btSimulationIslandManagerMt::parallelIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams )
|
||||
{
|
||||
BT_PROFILE( "parallelIslandDispatch" );
|
||||
int grainSize = 1; // iterations per task
|
||||
UpdateIslandDispatcher dispatcher;
|
||||
dispatcher.islandsPtr = islandsPtr;
|
||||
dispatcher.callback = callback;
|
||||
btParallelFor( 0, islandsPtr->size(), grainSize, dispatcher );
|
||||
//
|
||||
// if there are islands with many contacts, it may be faster to submit these
|
||||
// large islands *serially* to a single parallel constraint solver, and then later
|
||||
// submit the remaining smaller islands in parallel to multiple sequential solvers.
|
||||
//
|
||||
// Some task schedulers do not deal well with nested parallelFor loops. One implementation
|
||||
// of OpenMP was actually slower than doing everything single-threaded. Intel TBB
|
||||
// on the other hand, seems to do a pretty respectable job with it.
|
||||
//
|
||||
// When solving islands in parallel, the worst case performance happens when there
|
||||
// is one very large island and then perhaps a smattering of very small
|
||||
// islands -- one worker thread takes the large island and the remaining workers
|
||||
// tear through the smaller islands and then sit idle waiting for the first worker
|
||||
// to finish. Solving islands in parallel works best when there are numerous small
|
||||
// islands, roughly equal in size.
|
||||
//
|
||||
// By contrast, the other approach -- the parallel constraint solver -- is only
|
||||
// able to deliver a worthwhile speedup when the island is large. For smaller islands,
|
||||
// it is difficult to extract a useful amount of parallelism -- the overhead of grouping
|
||||
// the constraints into batches and sending the batches to worker threads can nullify
|
||||
// any gains from parallelism.
|
||||
//
|
||||
|
||||
UpdateIslandDispatcher dispatcher(*islandsPtr, solverParams);
|
||||
// We take advantage of the fact the islands are sorted in order of decreasing size
|
||||
int iBegin = 0;
|
||||
if (solverParams.m_solverMt)
|
||||
{
|
||||
while ( iBegin < islandsPtr->size() )
|
||||
{
|
||||
btSimulationIslandManagerMt::Island* island = ( *islandsPtr )[ iBegin ];
|
||||
if ( island->manifoldArray.size() < btSequentialImpulseConstraintSolverMt::s_minimumContactManifoldsForBatching )
|
||||
{
|
||||
// OK to submit the rest of the array in parallel
|
||||
break;
|
||||
}
|
||||
// serial dispatch to parallel solver for large islands (if any)
|
||||
solveIsland(solverParams.m_solverMt, *island, solverParams);
|
||||
++iBegin;
|
||||
}
|
||||
}
|
||||
// parallel dispatch to sequential solvers for rest
|
||||
btParallelFor( iBegin, islandsPtr->size(), 1, dispatcher );
|
||||
}
|
||||
|
||||
|
||||
@@ -604,15 +649,14 @@ void btSimulationIslandManagerMt::parallelIslandDispatch( btAlignedObjectArray<I
|
||||
void btSimulationIslandManagerMt::buildAndProcessIslands( btDispatcher* dispatcher,
|
||||
btCollisionWorld* collisionWorld,
|
||||
btAlignedObjectArray<btTypedConstraint*>& constraints,
|
||||
IslandCallback* callback
|
||||
const SolverParams& solverParams
|
||||
)
|
||||
{
|
||||
BT_PROFILE("buildAndProcessIslands");
|
||||
btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
|
||||
|
||||
buildIslands(dispatcher,collisionWorld);
|
||||
|
||||
BT_PROFILE("processIslands");
|
||||
|
||||
if(!getSplitIslands())
|
||||
{
|
||||
btPersistentManifold** manifolds = dispatcher->getInternalManifoldPointer();
|
||||
@@ -644,14 +688,17 @@ void btSimulationIslandManagerMt::buildAndProcessIslands( btDispatcher* dispatch
|
||||
}
|
||||
}
|
||||
btTypedConstraint** constraintsPtr = constraints.size() ? &constraints[ 0 ] : NULL;
|
||||
callback->processIsland(&collisionObjects[0],
|
||||
collisionObjects.size(),
|
||||
manifolds,
|
||||
maxNumManifolds,
|
||||
constraintsPtr,
|
||||
constraints.size(),
|
||||
-1
|
||||
);
|
||||
btConstraintSolver* solver = solverParams.m_solverMt ? solverParams.m_solverMt : solverParams.m_solverPool;
|
||||
solver->solveGroup(&collisionObjects[0],
|
||||
collisionObjects.size(),
|
||||
manifolds,
|
||||
maxNumManifolds,
|
||||
constraintsPtr,
|
||||
constraints.size(),
|
||||
*solverParams.m_solverInfo,
|
||||
solverParams.m_debugDrawer,
|
||||
solverParams.m_dispatcher
|
||||
);
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -671,6 +718,6 @@ void btSimulationIslandManagerMt::buildAndProcessIslands( btDispatcher* dispatch
|
||||
mergeIslands();
|
||||
}
|
||||
// dispatch islands to solver
|
||||
m_islandDispatch( &m_activeIslands, callback );
|
||||
m_islandDispatch( &m_activeIslands, solverParams );
|
||||
}
|
||||
}
|
||||
|
||||
@@ -19,7 +19,9 @@ subject to the following restrictions:
|
||||
#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
|
||||
|
||||
class btTypedConstraint;
|
||||
|
||||
class btConstraintSolver;
|
||||
struct btContactSolverInfo;
|
||||
class btIDebugDraw;
|
||||
|
||||
///
|
||||
/// SimulationIslandManagerMt -- Multithread capable version of SimulationIslandManager
|
||||
@@ -45,22 +47,19 @@ public:
|
||||
|
||||
void append( const Island& other ); // add bodies, manifolds, constraints to my own
|
||||
};
|
||||
struct IslandCallback
|
||||
struct SolverParams
|
||||
{
|
||||
virtual ~IslandCallback() {};
|
||||
|
||||
virtual void processIsland( btCollisionObject** bodies,
|
||||
int numBodies,
|
||||
btPersistentManifold** manifolds,
|
||||
int numManifolds,
|
||||
btTypedConstraint** constraints,
|
||||
int numConstraints,
|
||||
int islandId
|
||||
) = 0;
|
||||
btConstraintSolver* m_solverPool;
|
||||
btConstraintSolver* m_solverMt;
|
||||
btContactSolverInfo* m_solverInfo;
|
||||
btIDebugDraw* m_debugDrawer;
|
||||
btDispatcher* m_dispatcher;
|
||||
};
|
||||
typedef void( *IslandDispatchFunc ) ( btAlignedObjectArray<Island*>* islands, IslandCallback* callback );
|
||||
static void serialIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback );
|
||||
static void parallelIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback );
|
||||
static void solveIsland(btConstraintSolver* solver, Island& island, const SolverParams& solverParams);
|
||||
|
||||
typedef void( *IslandDispatchFunc ) ( btAlignedObjectArray<Island*>* islands, const SolverParams& solverParams );
|
||||
static void serialIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams );
|
||||
static void parallelIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams );
|
||||
protected:
|
||||
btAlignedObjectArray<Island*> m_allocatedIslands; // owner of all Islands
|
||||
btAlignedObjectArray<Island*> m_activeIslands; // islands actively in use
|
||||
@@ -83,7 +82,11 @@ public:
|
||||
btSimulationIslandManagerMt();
|
||||
virtual ~btSimulationIslandManagerMt();
|
||||
|
||||
virtual void buildAndProcessIslands( btDispatcher* dispatcher, btCollisionWorld* collisionWorld, btAlignedObjectArray<btTypedConstraint*>& constraints, IslandCallback* callback );
|
||||
virtual void buildAndProcessIslands( btDispatcher* dispatcher,
|
||||
btCollisionWorld* collisionWorld,
|
||||
btAlignedObjectArray<btTypedConstraint*>& constraints,
|
||||
const SolverParams& solverParams
|
||||
);
|
||||
|
||||
virtual void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld);
|
||||
|
||||
@@ -106,5 +109,6 @@ public:
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
#endif //BT_SIMULATION_ISLAND_MANAGER_H
|
||||
|
||||
|
||||
Reference in New Issue
Block a user