Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

more solver optimizations, can be disabled using solver->setSolverMode(SOLVER_RANDMIZE_ORDER)

Browse Source
This commit is contained in:
ejcoumans
2007-03-17 07:59:27 +00:00
parent 151cd4b9da
commit 2b87104184
9 changed files with 159 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files

180
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
View File

@@ -46,8 +46,12 @@ struct btOrderIndex
short int m_pointIndex;
};
#define SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS 16384
static btOrderIndex gOrder[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS];
static unsigned long btSeed2 = 0;
unsigned long btRand2()
{
@@ -111,7 +115,7 @@ bool MyContactDestroyedCallback(void* userPersistentData)
btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
:m_solverMode(SOLVER_RANDMIZE_ORDER)//SOLVER_USE_WARMSTARTING)//SOLVER_RANDMIZE_ORDER) //not using SOLVER_USE_WARMSTARTING
:m_solverMode(SOLVER_RANDMIZE_ORDER | SOLVER_CACHE_FRIENDLY) //not using SOLVER_USE_WARMSTARTING
{
gContactDestroyedCallback = &MyContactDestroyedCallback;
@@ -138,7 +142,7 @@ void initSolverBody(btSolverBody* solverBody, btRigidBody* rigidbody)
solverBody->m_angularVelocity = rigidbody->getAngularVelocity();
solverBody->m_centerOfMassPosition = rigidbody->getCenterOfMassPosition();
solverBody->m_friction = rigidbody->getFriction();
solverBody->m_invInertiaWorld = rigidbody->getInvInertiaTensorWorld();
// solverBody->m_invInertiaWorld = rigidbody->getInvInertiaTensorWorld();
solverBody->m_invMass = rigidbody->getInvMass();
solverBody->m_linearVelocity = rigidbody->getLinearVelocity();
solverBody->m_originalBody = rigidbody;
@@ -342,12 +346,29 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
{
if (!(numConstraints + numManifolds))
{
// printf("empty\n");
return 0.f;
}
BEGIN_PROFILE("refreshManifolds");
int i;
for (i=0;i<numManifolds;i++)
{
btPersistentManifold* manifold = manifoldPtr[i];
btRigidBody* rb0 = (btRigidBody*)manifold->getBody0();
btRigidBody* rb1 = (btRigidBody*)manifold->getBody1();
manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
}
END_PROFILE("refreshManifolds");
BEGIN_PROFILE("gatherSolverData");
int sizeofSB = sizeof(btSolverBody);
int sizeofSC = sizeof(btSolverConstraint);
@@ -362,7 +383,10 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
// unsigned char* stackMemory = stackAlloc->allocate(memsize);
tmpSolverBodyPool.reserve(numManifolds*2);
//todo: use stack allocator for this temp memory
int minReservation = numManifolds*2;
tmpSolverBodyPool.reserve(minReservation);
{
for (int i=0;i<numBodies;i++)
@@ -380,19 +404,18 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
}
tmpSolverConstraintPool.reserve(numManifolds*4+numConstraints);
tmpSolverFrictionConstraintPool.reserve(numManifolds*4);
tmpSolverConstraintPool.reserve(minReservation);
tmpSolverFrictionConstraintPool.reserve(minReservation);
{
int i;
for (i=0;i<numManifolds;i++)
{
btPersistentManifold* manifold = manifoldPtr[i];
btRigidBody* rb0 = (btRigidBody*)manifold->getBody0();
btRigidBody* rb1 = (btRigidBody*)manifold->getBody1();
manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
int solverBodyIdA;
int solverBodyIdB;
@@ -591,6 +614,30 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
}
}
btAlignedObjectArray<int> gOrderTmpConstraintPool;
btAlignedObjectArray<int> gOrderFrictionConstraintPool;
int numConstraintPool = tmpSolverConstraintPool.size();
int numFrictionPool = tmpSolverFrictionConstraintPool.size();
///todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
gOrderTmpConstraintPool.resize(numConstraintPool);
gOrderFrictionConstraintPool.resize(numFrictionPool);
{
int i;
for (i=0;i<numConstraintPool;i++)
{
gOrderTmpConstraintPool[i] = i;
}
for (i=0;i<numFrictionPool;i++)
{
gOrderFrictionConstraintPool[i] = i;
}
}
END_PROFILE("prepareConstraints");
@@ -598,41 +645,64 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
//should traverse the contacts random order...
int iteration;
int j;
{
for ( iteration = 0;iteration<info.m_numIterations;iteration++)
{
int j;
if (m_solverMode & SOLVER_RANDMIZE_ORDER)
{
if ((iteration & 7) == 0) {
for (j=0; j<numConstraintPool; ++j) {
int tmp = gOrderTmpConstraintPool[j];
int swapi = btRandInt2(j+1);
gOrderTmpConstraintPool[j] = gOrderTmpConstraintPool[swapi];
gOrderTmpConstraintPool[swapi] = tmp;
}
for (j=0; j<numFrictionPool; ++j) {
int tmp = gOrderFrictionConstraintPool[j];
int swapi = btRandInt2(j+1);
gOrderFrictionConstraintPool[j] = gOrderFrictionConstraintPool[swapi];
gOrderFrictionConstraintPool[swapi] = tmp;
}
}
}
for (j=0;j<numConstraints;j++)
{
btTypedConstraint* constraint = constraints[j];
///todo: use solver bodies, so we don't need to copy from/to btRigidBody
if (constraint->getRigidBodyA().getIslandTag() >= 0)
{
tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].writebackVelocity();
}
if (constraint->getRigidBodyB().getIslandTag() >= 0)
{
tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity();
}
constraint->solveConstraint(info.m_timeStep);
if (constraint->getRigidBodyA().getIslandTag() >= 0)
{
tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].readVelocity();
}
if (constraint->getRigidBodyB().getIslandTag() >= 0)
{
tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity();
}
}
{
int numPoolConstraints = tmpSolverConstraintPool.size();
for (j=0;j<numPoolConstraints;j++)
{
btSolverConstraint& solveManifold = tmpSolverConstraintPool[j];
switch (solveManifold.m_constraintType)
{
case btSolverConstraint::BT_SOLVER_CONTACT_1D:
{
resolveSingleCollisionCombinedCacheFriendly(tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,info);
break;
}
case btSolverConstraint::BT_SOLVER_FRICTION_1D:
{
break;
}
default:
{
//undefined constraint type?
btAssert(0);
}
};
btSolverConstraint& solveManifold = tmpSolverConstraintPool[gOrderTmpConstraintPool[j]];
resolveSingleCollisionCombinedCacheFriendly(tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,info);
}
}
@@ -640,29 +710,11 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
int numFrictionPoolConstraints = tmpSolverFrictionConstraintPool.size();
for (j=0;j<numFrictionPoolConstraints;j++)
{
btSolverConstraint& solveManifold = tmpSolverFrictionConstraintPool[j];
switch (solveManifold.m_constraintType)
{
case btSolverConstraint::BT_SOLVER_CONTACT_1D:
{
break;
}
case btSolverConstraint::BT_SOLVER_FRICTION_1D:
{
btSolverConstraint& solveManifold = tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]];
btScalar appliedNormalImpulse = tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
btScalar appliedNormalImpulse = tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
resolveSingleFrictionCacheFriendly(tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,info,appliedNormalImpulse);
break;
}
default:
{
//undefined constraint type?
btAssert(0);
}
};
resolveSingleFrictionCacheFriendly(tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,info,appliedNormalImpulse);
}
}
@@ -678,6 +730,18 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
END_PROFILE("solveConstraints");
// printf("tmpSolverConstraintPool.size() = %i\n",tmpSolverConstraintPool.size());
/*
printf("tmpSolverBodyPool.size() = %i\n",tmpSolverBodyPool.size());
printf("tmpSolverConstraintPool.size() = %i\n",tmpSolverConstraintPool.size());
printf("tmpSolverFrictionConstraintPool.size() = %i\n",tmpSolverFrictionConstraintPool.size());
printf("tmpSolverBodyPool.capacity() = %i\n",tmpSolverBodyPool.capacity());
printf("tmpSolverConstraintPool.capacity() = %i\n",tmpSolverConstraintPool.capacity());
printf("tmpSolverFrictionConstraintPool.capacity() = %i\n",tmpSolverFrictionConstraintPool.capacity());
*/
tmpSolverBodyPool.resize(0);
tmpSolverConstraintPool.resize(0);
@@ -691,10 +755,10 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
{
//return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
if (getSolverMode() & SOLVER_CACHE_FRIENDLY)
{
return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
}
BEGIN_PROFILE("prepareConstraints");

3
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
View File

@@ -46,7 +46,8 @@ public:
{
SOLVER_RANDMIZE_ORDER = 1,
SOLVER_FRICTION_SEPARATE = 2,
SOLVER_USE_WARMSTARTING = 4
SOLVER_USE_WARMSTARTING = 4,
SOLVER_CACHE_FRIENDLY = 8
};
btSequentialImpulseConstraintSolver();

15
src/BulletDynamics/ConstraintSolver/btsolverbody.h
View File

@@ -25,7 +25,6 @@ class btRigidBody;
ATTRIBUTE_ALIGNED16 (struct btSolverBody)
{
btMatrix3x3 m_invInertiaWorld;
btVector3 m_centerOfMassPosition;
btVector3 m_linearVelocity;
btVector3 m_angularVelocity;
@@ -33,7 +32,6 @@ ATTRIBUTE_ALIGNED16 (struct btSolverBody)
float m_invMass;
float m_friction;
float m_unused;
btVector3 m_unused2;
inline void getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const
{
@@ -56,13 +54,24 @@ ATTRIBUTE_ALIGNED16 (struct btSolverBody)
}
}
void readVelocity()
{
if (m_invMass)
{
m_linearVelocity = m_originalBody->getLinearVelocity();
m_angularVelocity = m_originalBody->getAngularVelocity();
}
}
inline void applyImpulse(const btVector3& impulse,const btVector3& rel_pos)
{
if (m_invMass)
{
m_linearVelocity += impulse * m_invMass;
btVector3 torqueImpulse = rel_pos.cross(impulse);
m_angularVelocity += m_invInertiaWorld * torqueImpulse;
// m_angularVelocity += m_invInertiaWorld * torqueImpulse;
}
}

2
src/BulletDynamics/ConstraintSolver/btsolverconstraint.h
View File

@@ -44,7 +44,7 @@ ATTRIBUTE_ALIGNED16 (struct btSolverConstraint)
int m_constraintType;
int m_frictionIndex;
int m_unusedPadding[2];
// int m_unusedPadding[2];
enum btSolverConstraintType
{

4
src/BulletDynamics/Dynamics/btRigidBody.cpp
View File

@@ -112,7 +112,7 @@ btRigidBody::btRigidBody( btScalar mass,const btTransform& worldTransform,btColl
//#define EXPERIMENTAL_JITTER_REMOVAL 1
#define EXPERIMENTAL_JITTER_REMOVAL 1
#ifdef EXPERIMENTAL_JITTER_REMOVAL
//Bullet 2.20b has experimental damping code to reduce jitter just before objects fall asleep/deactivate
//doesn't work very well yet (value 0 disabled this damping)
@@ -121,7 +121,7 @@ btScalar gClippedAngvelThresholdSqr = btScalar(0.01);
btScalar gClippedLinearThresholdSqr = btScalar(0.01);
#endif //EXPERIMENTAL_JITTER_REMOVAL
btScalar gJitterVelocityDampingFactor = btScalar(0.9);
btScalar gJitterVelocityDampingFactor = btScalar(0.7);
void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform)
{