parallel solver: various changes
- threading: adding btSequentialImpulseConstraintSolverMt - task scheduler: added parallelSum so that parallel solver can compute residuals - CommonRigidBodyMTBase: add slider for solver least squares residual and allow multithreading without needing OpenMP, TBB, or PPL - taskScheduler: don't wait for workers to sleep/signal at the end of each parallel block - parallel solver: convertContacts split into an allocContactConstraints and setupContactConstraints stage, the latter of which is done in parallel - parallel solver: rolling friction is now interleaved along with normal friction - parallel solver: batchified split impulse solving + some cleanup - parallel solver: sorting batches from largest to smallest - parallel solver: added parallel batch creation - parallel solver: added warmstartingWriteBackContacts func + other cleanup - task scheduler: truncate low bits to preserve determinism with parallelSum - parallel solver: reducing dynamic mem allocs and trying to parallelize more of the batch setup - parallel solver: parallelize updating constraint batch ids for merging - parallel solver: adding debug visualization - task scheduler: make TBB task scheduler parallelSum deterministic - parallel solver: split batch gen code into separate file; allow selection of batch gen method - task scheduler: add sleepWorkerThreadsHint() at end of simulation - parallel solver: added grain size per phase - task Scheduler: fix for strange threading issue; also no need for main thread to wait for workers to sleep - base constraint solver: break out joint setup into separate function for profiling/overriding - parallel solver: allow different batching method for contacts vs joints - base constraint solver: add convertJoint and convertBodies to make it possible to parallelize joint and body conversion - parallel solver: convert joints and bodies in parallel now - parallel solver: speed up batch creation with run-length encoding - parallel solver: batch gen: run-length expansion in parallel; collect constraint info in parallel - parallel solver: adding spatial grid batching method - parallel solver: enhancements to spatial grid batching - sequential solver: moving code for writing back into functions that derived classes can call - parallel solver: do write back of bodies and joints in parallel - parallel solver: removed all batching methods except for spatial grid (others were ineffective) - parallel solver: added 2D or 3D grid batching options; and a bit of cleanup - move btDefaultTaskScheduler into LinearMath project
This commit is contained in:
Lunkhound
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
|
||||
|
||||
Reference in New Issue
Block a user