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

Add the option for the btSimulationIslandManager to avoid splitting islands (for constraint solving)

Browse Source 
Move the convertContact inside constraint solver to its own method
This commit is contained in:
erwin.coumans
2009-02-13 02:34:46 +00:00
parent 26d7757135
commit d886c06fa5
10 changed files with 452 additions and 363 deletions
Download Patch File Download Diff File Expand all files Collapse all files

98
Demos/BasicDemo/BasicDemo.cpp
View File

@@ -13,17 +13,27 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution. 3. This notice may not be removed or altered from any source distribution.
*/ */
#define LARGE_DEMO 0
#if LARGE_DEMO
///create 512 (8x8x8) dynamic object
#define ARRAY_SIZE_X 8
#define ARRAY_SIZE_Y 8
#define ARRAY_SIZE_Z 8
#else
///create 125 (5x5x5) dynamic object
#define ARRAY_SIZE_X 5
#define ARRAY_SIZE_Y 5
#define ARRAY_SIZE_Z 5
#endif
///create 125 (5x5x5) dynamic object
#define ARRAY_SIZE_X 5
#define ARRAY_SIZE_Y 5
#define ARRAY_SIZE_Z 5
//maximum number of objects (and allow user to shoot additional boxes) //maximum number of objects (and allow user to shoot additional boxes)
#define MAX_PROXIES (ARRAY_SIZE_X*ARRAY_SIZE_Y*ARRAY_SIZE_Z + 1024) #define MAX_PROXIES (ARRAY_SIZE_X*ARRAY_SIZE_Y*ARRAY_SIZE_Z + 1024)
///scaling of the objects (0.1 = 20 centimeter boxes ) ///scaling of the objects (0.1 = 20 centimeter boxes )
#define SCALING 0.1 //#define SCALING 0.1
#define SCALING 1
#define START_POS_X -5 #define START_POS_X -5
#define START_POS_Y -5 #define START_POS_Y -5
#define START_POS_Z -3 #define START_POS_Z -3
@@ -34,6 +44,9 @@ subject to the following restrictions:
#include "btBulletDynamicsCommon.h" #include "btBulletDynamicsCommon.h"
#include <stdio.h> //printf debugging #include <stdio.h> //printf debugging
#include "BulletDynamics/ConstraintSolver/btParallelBatchConstraintSolver.h"
#include "BulletDynamics/ConstraintSolver/btParallelBatchConstraintSolver2.h"
#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
void BasicDemo::clientMoveAndDisplay() void BasicDemo::clientMoveAndDisplay()
{ {
@@ -46,6 +59,10 @@ void BasicDemo::clientMoveAndDisplay()
if (m_dynamicsWorld) if (m_dynamicsWorld)
{ {
m_dynamicsWorld->stepSimulation(ms / 1000000.f); m_dynamicsWorld->stepSimulation(ms / 1000000.f);
if (m_idle)
{
CProfileManager::dumpAll();
}
//optional but useful: debug drawing //optional but useful: debug drawing
m_dynamicsWorld->debugDrawWorld(); m_dynamicsWorld->debugDrawWorld();
} }
@@ -75,6 +92,10 @@ void BasicDemo::displayCallback(void) {
} }
static btSequentialImpulseConstraintSolver* sDefSeqImpSolver = 0;
static btParallelBatchConstraintSolver* sParallelBatchSolver = 0;
static btParallelBatchConstraintSolver2* sParallelBatchSolver2 = 0;
@@ -94,22 +115,31 @@ void BasicDemo::initPhysics()
m_broadphase = new btDbvtBroadphase(); m_broadphase = new btDbvtBroadphase();
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded) ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver; sDefSeqImpSolver = new btSequentialImpulseConstraintSolver;
m_solver = sol; // create parallel batch solver for tests
sParallelBatchSolver = new btParallelBatchConstraintSolver();
sParallelBatchSolver2 = new btParallelBatchConstraintSolver2();
m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration); // start with parallel batch solver
m_solver = sParallelBatchSolver;
btDiscreteDynamicsWorld* ddw = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
m_dynamicsWorld = ddw;
ddw->getSimulationIslandManager()->setSplitIslands(false);
m_dynamicsWorld->setGravity(btVector3(0,-10,0)); m_dynamicsWorld->setGravity(btVector3(0,-10,0));
///create a few basic rigid bodies ///create a few basic rigid bodies
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.))); btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(30.),btScalar(1.),btScalar(30.)));
// btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
// btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50); // btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
m_collisionShapes.push_back(groundShape); m_collisionShapes.push_back(groundShape);
btTransform groundTransform; btTransform groundTransform;
groundTransform.setIdentity(); groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,-50,0)); groundTransform.setOrigin(btVector3(0,-5,0));
// groundTransform.setOrigin(btVector3(0,-50,0));
//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
{ {
@@ -215,7 +245,9 @@ void BasicDemo::exitPhysics()
delete m_dynamicsWorld; delete m_dynamicsWorld;
delete m_solver; m_solver = 0;
delete sDefSeqImpSolver;
delete sParallelBatchSolver;
delete m_broadphase; delete m_broadphase;
@@ -228,4 +260,48 @@ void BasicDemo::exitPhysics()
void BasicDemo::keyboardCallback(unsigned char key, int x, int y)
{
(void)x;
(void)y;
switch (key)
{
case 'q' :
exitPhysics();
break;
case 'S' :
{
btConstraintSolver* curr_solver = m_dynamicsWorld->getConstraintSolver();
btDiscreteDynamicsWorld* pDdw = (btDiscreteDynamicsWorld*)m_dynamicsWorld;
if(curr_solver == sDefSeqImpSolver)
{
static bool toggle = true;
toggle=!toggle;
if (toggle)
{
pDdw->setConstraintSolver(sParallelBatchSolver);
pDdw->getSimulationIslandManager()->setSplitIslands(false);
printf("\nUsing ParallelBatch constraint solver\n");
} else
{
pDdw->setConstraintSolver(sParallelBatchSolver2);
pDdw->getSimulationIslandManager()->setSplitIslands(false);
printf("\nUsing ParallelBatch constraint solver2\n");
}
}
else
{
m_dynamicsWorld->setConstraintSolver(sDefSeqImpSolver);
pDdw->getSimulationIslandManager()->setSplitIslands(true);
printf("\nUsing default SequentialImpulse constraint solver\n");
}
}
break;
default :
break;
}
DemoApplication::keyboardCallback(key, x, y);
}

2
Demos/BasicDemo/BasicDemo.h
View File

@@ -58,6 +58,8 @@ class BasicDemo : public DemoApplication
virtual void displayCallback(); virtual void displayCallback();
virtual void keyboardCallback(unsigned char key, int x, int y);
static DemoApplication* Create() static DemoApplication* Create()
{ {
BasicDemo* demo = new BasicDemo; BasicDemo* demo = new BasicDemo;

6
Demos/OpenGL/DemoApplication.cpp
View File

@@ -1269,6 +1269,7 @@ void DemoApplication::clientResetScene()
numObjects = m_dynamicsWorld->getNumCollisionObjects(); numObjects = m_dynamicsWorld->getNumCollisionObjects();
} }
///create a copy of the array, not a reference!
btCollisionObjectArray copyArray = m_dynamicsWorld->getCollisionObjectArray(); btCollisionObjectArray copyArray = m_dynamicsWorld->getCollisionObjectArray();
for (i=0;i<copyArray.size();i++) for (i=0;i<copyArray.size();i++)
@@ -1297,8 +1298,8 @@ void DemoApplication::clientResetScene()
colObj->setDeactivationTime(0); colObj->setDeactivationTime(0);
//colObj->setActivationState(WANTS_DEACTIVATION); //colObj->setActivationState(WANTS_DEACTIVATION);
} }
//removed cached contact points //removed cached contact points (this is not necessary if all objects have been removed from the dynamics world)
//m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(colObj->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher()); m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(colObj->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher());
btRigidBody* body = btRigidBody::upcast(colObj); btRigidBody* body = btRigidBody::upcast(colObj);
if (body && !body->isStaticObject()) if (body && !body->isStaticObject())
@@ -1310,6 +1311,7 @@ void DemoApplication::clientResetScene()
} }
///reset some internal cached data in the broadphase
m_dynamicsWorld->getBroadphase()->resetPool(getDynamicsWorld()->getDispatcher()); m_dynamicsWorld->getBroadphase()->resetPool(getDynamicsWorld()->getDispatcher());
m_dynamicsWorld->getConstraintSolver()->reset(); m_dynamicsWorld->getConstraintSolver()->reset();

32
src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
View File

@@ -24,7 +24,8 @@ subject to the following restrictions:
//#include <stdio.h> //#include <stdio.h>
#include "LinearMath/btQuickprof.h" #include "LinearMath/btQuickprof.h"
btSimulationIslandManager::btSimulationIslandManager() btSimulationIslandManager::btSimulationIslandManager():
m_splitIslands(true)
{ {
} }
@@ -251,11 +252,11 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio
int i; int i;
int maxNumManifolds = dispatcher->getNumManifolds(); int maxNumManifolds = dispatcher->getNumManifolds();
#define SPLIT_ISLANDS 1 //#define SPLIT_ISLANDS 1
#ifdef SPLIT_ISLANDS //#ifdef SPLIT_ISLANDS
#endif //SPLIT_ISLANDS //#endif //SPLIT_ISLANDS
for (i=0;i<maxNumManifolds ;i++) for (i=0;i<maxNumManifolds ;i++)
@@ -279,11 +280,12 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio
{ {
colObj0->activate(); colObj0->activate();
} }
#ifdef SPLIT_ISLANDS if(m_splitIslands)
// //filtering for response {
//filtering for response
if (dispatcher->needsResponse(colObj0,colObj1)) if (dispatcher->needsResponse(colObj0,colObj1))
m_islandmanifold.push_back(manifold); m_islandmanifold.push_back(manifold);
#endif //SPLIT_ISLANDS }
} }
} }
} }
@@ -303,11 +305,14 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
BT_PROFILE("processIslands"); BT_PROFILE("processIslands");
#ifndef SPLIT_ISLANDS if(!m_splitIslands)
{
btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer(); btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
int maxNumManifolds = dispatcher->getNumManifolds();
callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1); callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
#else }
else
{
// Sort manifolds, based on islands // Sort manifolds, based on islands
// Sort the vector using predicate and std::sort // Sort the vector using predicate and std::sort
//std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate); //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
@@ -326,7 +331,7 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
// printf("Start Islands\n"); // printf("Start Islands\n");
//traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex) for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
@@ -370,7 +375,7 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
if (!islandSleeping) if (!islandSleeping)
{ {
callback->ProcessIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId); callback->ProcessIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId);
// printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds); // printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
} }
if (numIslandManifolds) if (numIslandManifolds)
@@ -380,7 +385,6 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
m_islandBodies.resize(0); m_islandBodies.resize(0);
} }
#endif //SPLIT_ISLANDS } // else if(!splitIslands)
} }

10
src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h
View File

@@ -35,6 +35,7 @@ class btSimulationIslandManager
btAlignedObjectArray<btPersistentManifold*> m_islandmanifold; btAlignedObjectArray<btPersistentManifold*> m_islandmanifold;
btAlignedObjectArray<btCollisionObject* > m_islandBodies; btAlignedObjectArray<btCollisionObject* > m_islandBodies;
bool m_splitIslands;
public: public:
btSimulationIslandManager(); btSimulationIslandManager();
@@ -65,6 +66,15 @@ public:
void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld); void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld);
bool getSplitIslands()
{
return m_splitIslands;
}
void setSplitIslands(bool doSplitIslands)
{
m_splitIslands = doSplitIslands;
}
}; };
#endif //SIMULATION_ISLAND_MANAGER_H #endif //SIMULATION_ISLAND_MANAGER_H

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

@@ -120,7 +120,6 @@ SIMD_FORCE_INLINE void btSequentialImpulseConstraintSolver::resolveSingleConstra
body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse); body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
} }
SIMD_FORCE_INLINE void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) SIMD_FORCE_INLINE void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{ {
#ifdef USE_SIMD #ifdef USE_SIMD
@@ -269,7 +268,7 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c
solverConstraint.m_appliedImpulse = 0.f; solverConstraint.m_appliedImpulse = 0.f;
// solverConstraint.m_appliedPushImpulse = 0.f; // solverConstraint.m_appliedPushImpulse = 0.f;
solverConstraint.m_penetration = 0.f;
{ {
btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal); btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
@@ -324,10 +323,8 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c
rel_vel = vel1Dotn+vel2Dotn; rel_vel = vel1Dotn+vel2Dotn;
btScalar positionalError = 0.f; btScalar positionalError = 0.f;
positionalError = 0;//-solverConstraint.m_penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
solverConstraint.m_restitution=0.f;
btSimdScalar velocityError = solverConstraint.m_restitution - rel_vel; btSimdScalar velocityError = - rel_vel;
btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv); btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
solverConstraint.m_rhs = velocityImpulse; solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_cfm = 0.f; solverConstraint.m_cfm = 0.f;
@@ -364,175 +361,12 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
} }
#include <stdio.h> #include <stdio.h>
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
{ {
BT_PROFILE("solveGroupCacheFriendlySetup");
(void)stackAlloc;
(void)debugDrawer;
if (!(numConstraints + numManifolds))
{
// printf("empty\n");
return 0.f;
}
if (1)
{
int j;
for (j=0;j<numConstraints;j++)
{
btTypedConstraint* constraint = constraints[j];
constraint->buildJacobian();
}
}
btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
initSolverBody(&fixedBody,0);
//btRigidBody* rb0=0,*rb1=0;
//if (1)
{
{
int totalNumRows = 0;
int i;
//calculate the total number of contraint rows
for (i=0;i<numConstraints;i++)
{
btTypedConstraint::btConstraintInfo1 info1;
constraints[i]->getInfo1(&info1);
totalNumRows += info1.m_numConstraintRows;
}
m_tmpSolverNonContactConstraintPool.resize(totalNumRows);
btTypedConstraint::btConstraintInfo1 info1;
info1.m_numConstraintRows = 0;
///setup the btSolverConstraints
int currentRow = 0;
for (i=0;i<numConstraints;i++,currentRow+=info1.m_numConstraintRows)
{
constraints[i]->getInfo1(&info1);
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);
int solverBodyIdB = getOrInitSolverBody(rbB);
btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
int j;
for ( j=0;j<info1.m_numConstraintRows;j++)
{
memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));
currentConstraintRow[j].m_lowerLimit = -FLT_MAX;
currentConstraintRow[j].m_upperLimit = FLT_MAX;
currentConstraintRow[j].m_appliedImpulse = 0.f;
currentConstraintRow[j].m_penetration = 0.f;
currentConstraintRow[j].m_appliedPushImpulse = 0.f;
currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
}
bodyAPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
bodyAPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
bodyBPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
bodyBPtr->m_deltaAngularVelocity.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_contactNormal;
info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
info2.m_J2linearAxis = 0;
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 = &currentConstraintRow->m_rhs;
info2.cfm = &currentConstraintRow->m_cfm;
info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
constraints[i]->getInfo2(&info2);
///finalize the constraint setup
for ( j=0;j<info1.m_numConstraintRows;j++)
{
btSolverConstraint& solverConstraint = currentConstraintRow[j];
{
const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1;
}
{
const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2;
}
{
btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass();
btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal?
btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
sum += iMJlB.dot(solverConstraint.m_contactNormal);
sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
}
///fix rhs
///todo: add force/torque accelerators
{
btScalar rel_vel;
btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());
rel_vel = vel1Dotn+vel2Dotn;
btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
solverConstraint.m_restitution = 0.f;
btScalar velocityError = solverConstraint.m_restitution - rel_vel;// * damping;
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
solverConstraint.m_appliedImpulse = 0.f;
}
}
}
}
}
{
int i;
btPersistentManifold* manifold = 0;
btCollisionObject* colObj0=0,*colObj1=0; btCollisionObject* colObj0=0,*colObj1=0;
for (i=0;i<numManifolds;i++)
{
manifold = manifoldPtr[i];
colObj0 = (btCollisionObject*)manifold->getBody0(); colObj0 = (btCollisionObject*)manifold->getBody0();
colObj1 = (btCollisionObject*)manifold->getBody1(); colObj1 = (btCollisionObject*)manifold->getBody1();
@@ -545,9 +379,6 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
solverBodyIdB = getOrInitSolverBody(*colObj1); solverBodyIdB = getOrInitSolverBody(*colObj1);
} }
if (solverBodyIdA == 0 && solverBodyIdB == 0)
continue;
btVector3 rel_pos1; btVector3 rel_pos1;
btVector3 rel_pos2; btVector3 rel_pos2;
btScalar relaxation; btScalar relaxation;
@@ -625,28 +456,22 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
rel_vel = cp.m_normalWorldOnB.dot(vel); rel_vel = cp.m_normalWorldOnB.dot(vel);
solverConstraint.m_penetration = cp.getDistance()+infoGlobal.m_linearSlop; btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
//solverConstraint.m_penetration = cp.getDistance();
solverConstraint.m_friction = cp.m_combinedFriction; solverConstraint.m_friction = cp.m_combinedFriction;
btScalar restitution = 0.f;
if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold) if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
{ {
solverConstraint.m_restitution = 0.f; restitution = 0.f;
} else } else
{ {
solverConstraint.m_restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
if (solverConstraint.m_restitution <= btScalar(0.)) if (restitution <= btScalar(0.))
{ {
solverConstraint.m_restitution = 0.f; restitution = 0.f;
}; };
} }
@@ -676,8 +501,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
rel_vel = vel1Dotn+vel2Dotn; rel_vel = vel1Dotn+vel2Dotn;
btScalar positionalError = 0.f; btScalar positionalError = 0.f;
positionalError = -solverConstraint.m_penetration * infoGlobal.m_erp/infoGlobal.m_timeStep; positionalError = -penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
btScalar velocityError = solverConstraint.m_restitution - rel_vel;// * damping; btScalar velocityError = restitution - rel_vel;// * damping;
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
@@ -687,15 +512,6 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
} }
#ifdef _USE_JACOBIAN
solverConstraint.m_jac = btJacobianEntry (
rel_pos1,rel_pos2,cp.m_normalWorldOnB,
rb0->getInvInertiaDiagLocal(),
rb0->getInvMass(),
rb1->getInvInertiaDiagLocal(),
rb1->getInvMass());
#endif //_USE_JACOBIAN
/////setup the friction constraints /////setup the friction constraints
@@ -785,6 +601,178 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
} }
} }
}
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
{
BT_PROFILE("solveGroupCacheFriendlySetup");
(void)stackAlloc;
(void)debugDrawer;
if (!(numConstraints + numManifolds))
{
// printf("empty\n");
return 0.f;
}
if (1)
{
int j;
for (j=0;j<numConstraints;j++)
{
btTypedConstraint* constraint = constraints[j];
constraint->buildJacobian();
}
}
btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
initSolverBody(&fixedBody,0);
//btRigidBody* rb0=0,*rb1=0;
//if (1)
{
{
int totalNumRows = 0;
int i;
//calculate the total number of contraint rows
for (i=0;i<numConstraints;i++)
{
btTypedConstraint::btConstraintInfo1 info1;
constraints[i]->getInfo1(&info1);
totalNumRows += info1.m_numConstraintRows;
}
m_tmpSolverNonContactConstraintPool.resize(totalNumRows);
btTypedConstraint::btConstraintInfo1 info1;
info1.m_numConstraintRows = 0;
///setup the btSolverConstraints
int currentRow = 0;
for (i=0;i<numConstraints;i++,currentRow+=info1.m_numConstraintRows)
{
constraints[i]->getInfo1(&info1);
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);
int solverBodyIdB = getOrInitSolverBody(rbB);
btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
int j;
for ( j=0;j<info1.m_numConstraintRows;j++)
{
memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));
currentConstraintRow[j].m_lowerLimit = -FLT_MAX;
currentConstraintRow[j].m_upperLimit = FLT_MAX;
currentConstraintRow[j].m_appliedImpulse = 0.f;
currentConstraintRow[j].m_appliedPushImpulse = 0.f;
currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
}
bodyAPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
bodyAPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
bodyBPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
bodyBPtr->m_deltaAngularVelocity.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_contactNormal;
info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
info2.m_J2linearAxis = 0;
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 = &currentConstraintRow->m_rhs;
info2.cfm = &currentConstraintRow->m_cfm;
info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
constraints[i]->getInfo2(&info2);
///finalize the constraint setup
for ( j=0;j<info1.m_numConstraintRows;j++)
{
btSolverConstraint& solverConstraint = currentConstraintRow[j];
{
const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1;
}
{
const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2;
}
{
btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass();
btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal?
btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
sum += iMJlB.dot(solverConstraint.m_contactNormal);
sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
}
///fix rhs
///todo: add force/torque accelerators
{
btScalar rel_vel;
btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());
rel_vel = vel1Dotn+vel2Dotn;
btScalar restitution = 0.f;
btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
btScalar velocityError = restitution - rel_vel;// * damping;
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
solverConstraint.m_appliedImpulse = 0.f;
}
}
}
}
}
{
int i;
btPersistentManifold* manifold = 0;
btCollisionObject* colObj0=0,*colObj1=0;
for (i=0;i<numManifolds;i++)
{
manifold = manifoldPtr[i];
convertContact(manifold,infoGlobal);
} }
} }
} }

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

@@ -30,6 +30,7 @@ class btIDebugDraw;
///Applies impulses for combined restitution and penetration recovery and to simulate friction ///Applies impulses for combined restitution and penetration recovery and to simulate friction
class btSequentialImpulseConstraintSolver : public btConstraintSolver class btSequentialImpulseConstraintSolver : public btConstraintSolver
{ {
protected:
btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool; btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool;
btConstraintArray m_tmpSolverContactConstraintPool; btConstraintArray m_tmpSolverContactConstraintPool;
@@ -38,7 +39,6 @@ class btSequentialImpulseConstraintSolver : public btConstraintSolver
btAlignedObjectArray<int> m_orderTmpConstraintPool; btAlignedObjectArray<int> m_orderTmpConstraintPool;
btAlignedObjectArray<int> m_orderFrictionConstraintPool; btAlignedObjectArray<int> m_orderFrictionConstraintPool;
protected:
btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation); btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation);
///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
@@ -47,6 +47,8 @@ protected:
void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject); void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution); btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
void resolveSplitPenetrationImpulseCacheFriendly( void resolveSplitPenetrationImpulseCacheFriendly(
btSolverBody& body1, btSolverBody& body1,
btSolverBody& body2, btSolverBody& body2,

8
src/BulletDynamics/ConstraintSolver/btSolverConstraint.h
View File

@@ -44,10 +44,12 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint
btScalar m_friction; btScalar m_friction;
btScalar m_restitution;
btScalar m_jacDiagABInv; btScalar m_jacDiagABInv;
btScalar m_penetration; union
{
int m_numConsecutiveRowsPerKernel;
btScalar m_unusedPadding0;
};
union union
{ {

3
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
View File

@@ -664,9 +664,12 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
virtual void ProcessIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId) virtual void ProcessIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId)
{ {
if (islandId<0) if (islandId<0)
{
if (numManifolds + m_numConstraints)
{ {
///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
}
} else } else
{ {
//also add all non-contact constraints/joints for this island //also add all non-contact constraints/joints for this island

16
src/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.cpp
View File

@@ -1238,7 +1238,7 @@ void processSolverTask(void* userPtr, void* lsMemory)
vel = vel1 - vel2; vel = vel1 - vel2;
rel_vel = cp.m_normalWorldOnB.dot(vel); rel_vel = cp.m_normalWorldOnB.dot(vel);
constraint.m_penetration = cp.getDistance();///btScalar(infoGlobal.m_numIterations); btScalar penetration = cp.getDistance();///btScalar(infoGlobal.m_numIterations);
constraint.m_friction = cp.m_combinedFriction; constraint.m_friction = cp.m_combinedFriction;
float rest = - rel_vel * cp.m_combinedRestitution; float rest = - rel_vel * cp.m_combinedRestitution;
if (rest <= btScalar(0.)) if (rest <= btScalar(0.))
@@ -1251,7 +1251,7 @@ void processSolverTask(void* userPtr, void* lsMemory)
btScalar erp = taskDesc.m_commandData.m_manifoldSetup.m_solverInfo.m_erp; btScalar erp = taskDesc.m_commandData.m_manifoldSetup.m_solverInfo.m_erp;
btScalar timeStep = taskDesc.m_commandData.m_manifoldSetup.m_solverInfo.m_timeStep; btScalar timeStep = taskDesc.m_commandData.m_manifoldSetup.m_solverInfo.m_timeStep;
constraint.m_restitution = rest; btScalar restitution = rest;
constraint.m_appliedImpulse = cp.m_appliedImpulse*taskDesc.m_commandData.m_manifoldSetup.m_solverInfo.m_warmstartingFactor; constraint.m_appliedImpulse = cp.m_appliedImpulse*taskDesc.m_commandData.m_manifoldSetup.m_solverInfo.m_warmstartingFactor;
if (constraint.m_appliedImpulse!= 0.f) if (constraint.m_appliedImpulse!= 0.f)
{ {
@@ -1271,8 +1271,8 @@ void processSolverTask(void* userPtr, void* lsMemory)
rel_vel = vel1Dotn-vel2Dotn; rel_vel = vel1Dotn-vel2Dotn;
btScalar positionalError = 0.f; btScalar positionalError = 0.f;
positionalError = -constraint.m_penetration * erp/timeStep; positionalError = -penetration * erp/timeStep;
btScalar velocityError = constraint.m_restitution - rel_vel;// * damping; btScalar velocityError = restitution - rel_vel;// * damping;
btScalar penetrationImpulse = positionalError*constraint.m_jacDiagABInv; btScalar penetrationImpulse = positionalError*constraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *constraint.m_jacDiagABInv; btScalar velocityImpulse = velocityError *constraint.m_jacDiagABInv;
constraint.m_rhs = penetrationImpulse+velocityImpulse; constraint.m_rhs = penetrationImpulse+velocityImpulse;
@@ -1339,9 +1339,9 @@ void processSolverTask(void* userPtr, void* lsMemory)
btScalar positionalError = 0.f; btScalar positionalError = 0.f;
positionalError = 0; positionalError = 0;
constraint.m_restitution=0.f; btScalar restitution=0.f;
btSimdScalar velocityError = constraint.m_restitution - rel_vel; btSimdScalar velocityError = restitution - rel_vel;
btSimdScalar velocityImpulse = velocityError * btSimdScalar(constraint.m_jacDiagABInv); btSimdScalar velocityImpulse = velocityError * btSimdScalar(constraint.m_jacDiagABInv);
constraint.m_rhs = velocityImpulse; constraint.m_rhs = velocityImpulse;
constraint.m_cfm = 0.f; constraint.m_cfm = 0.f;
@@ -1386,9 +1386,9 @@ void processSolverTask(void* userPtr, void* lsMemory)
btScalar positionalError = 0.f; btScalar positionalError = 0.f;
positionalError = 0; positionalError = 0;
constraint.m_restitution=0.f; btScalar restitution=0.f;
btSimdScalar velocityError = constraint.m_restitution - rel_vel; btSimdScalar velocityError = restitution - rel_vel;
btSimdScalar velocityImpulse = velocityError * btSimdScalar(constraint.m_jacDiagABInv); btSimdScalar velocityImpulse = velocityError * btSimdScalar(constraint.m_jacDiagABInv);
constraint.m_rhs = velocityImpulse; constraint.m_rhs = velocityImpulse;
constraint.m_cfm = 0.f; constraint.m_cfm = 0.f;