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

Properly propagate the applied impulse for the MLCP solvers, so it will be available for contact and non-contact constraints.

Browse Source 
Use real-time clock in AllBullet2Demos, rather than hard-coded 1./60.
This commit is contained in:
erwin coumans
2014-02-24 13:24:49 -08:00
parent fabdf8b4a9
commit dfa738c13a
7 changed files with 130 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
Demos3/AllBullet2Demos/BulletDemoEntries.h
View File

@@ -25,14 +25,15 @@ static BulletDemoEntry allDemos[]=
//{"emptydemo",EmptyBulletDemo::MyCreateFunc}, //{"emptydemo",EmptyBulletDemo::MyCreateFunc},
{"BasicDemo",BasicDemo::MyCreateFunc}, {"BasicDemo",BasicDemo::MyCreateFunc},
/*
{"ChainDemo",ChainDemo::MyCreateFunc}, {"ChainDemo",ChainDemo::MyCreateFunc},
{"SIHingeDemo",HingeDemo::SICreateFunc},
{"PGSHingeDemo",HingeDemo::PGSCreateFunc}, {"PGSHingeDemo",HingeDemo::PGSCreateFunc},
{"DantzigHingeDemo",HingeDemo::DantzigCreateFunc}, {"DantzigHingeDemo",HingeDemo::DantzigCreateFunc},
{"LemkeHingeDemo",HingeDemo::LemkeCreateFunc}, {"LemkeHingeDemo",HingeDemo::LemkeCreateFunc},
{"InertiaHingeDemo",HingeDemo::InertiaCreateFunc}, {"InertiaHingeDemo",HingeDemo::InertiaCreateFunc},
{"ABMHingeDemo",HingeDemo::FeatherstoneCreateFunc}, {"ABMHingeDemo",HingeDemo::FeatherstoneCreateFunc},
*/
{"Ragdoll",RagDollDemo::MyCreateFunc}, {"Ragdoll",RagDollDemo::MyCreateFunc},
{"MultiBody1",FeatherstoneDemo1::MyCreateFunc}, {"MultiBody1",FeatherstoneDemo1::MyCreateFunc},

12
Demos3/AllBullet2Demos/main.cpp
View File

@@ -5,7 +5,7 @@
#include "../GpuDemos/gwenUserInterface.h" #include "../GpuDemos/gwenUserInterface.h"
#include "BulletDemoEntries.h" #include "BulletDemoEntries.h"
#include "../../btgui/Timing/b3Clock.h"
#define DEMO_SELECTION_COMBOBOX 13 #define DEMO_SELECTION_COMBOBOX 13
const char* startFileName = "bulletDemo.txt"; const char* startFileName = "bulletDemo.txt";
static SimpleOpenGL3App* app=0; static SimpleOpenGL3App* app=0;
@@ -132,6 +132,7 @@ void MyComboBoxCallback(int comboId, const char* item)
int main(int argc, char* argv[]) int main(int argc, char* argv[])
{ {
b3Clock clock;
float dt = 1./120.f; float dt = 1./120.f;
int width = 1024; int width = 1024;
@@ -167,6 +168,7 @@ int main(int argc, char* argv[])
gui->setComboBoxCallback(MyComboBoxCallback); gui->setComboBoxCallback(MyComboBoxCallback);
unsigned long int prevTimeInMicroseconds = clock.getTimeMicroseconds();
do do
{ {
@@ -191,7 +193,13 @@ int main(int argc, char* argv[])
if (sCurrentDemo) if (sCurrentDemo)
{ {
if (!pauseSimulation) if (!pauseSimulation)
sCurrentDemo->stepSimulation(1./60.f); {
unsigned long int curTimeInMicroseconds = clock.getTimeMicroseconds();
unsigned long int diff = curTimeInMicroseconds-prevTimeInMicroseconds;
float deltaTimeInSeconds = (diff)*1.e-6;
sCurrentDemo->stepSimulation(deltaTimeInSeconds);//1./60.f);
prevTimeInMicroseconds = curTimeInMicroseconds;
}
sCurrentDemo->renderScene(); sCurrentDemo->renderScene();
} }

21
Demos3/bullet2/BasicDemo/BasicDemo.cpp
View File

@@ -145,6 +145,27 @@ void BasicDemo::renderScene()
void BasicDemo::stepSimulation(float dt) void BasicDemo::stepSimulation(float dt)
{ {
m_dynamicsWorld->stepSimulation(dt); m_dynamicsWorld->stepSimulation(dt);
/*
//print applied force
//contact points
for (int i=0;i<m_dynamicsWorld->getDispatcher()->getNumManifolds();i++)
{
btPersistentManifold* contact = m_dynamicsWorld->getDispatcher()->getManifoldByIndexInternal(i);
for (int c=0;c<contact->getNumContacts();c++)
{
btManifoldPoint& pt = contact->getContactPoint(c);
btScalar dist = pt.getDistance();
if (dist< contact->getContactProcessingThreshold())
{
printf("normalImpulse[%d.%d] = %f\n",i,c,pt.m_appliedImpulse);
} else
{
printf("?\n");
}
}
}
*/
} }

9
Demos3/bullet2/BasicDemo/HingeDemo.cpp
View File

@@ -304,12 +304,18 @@ void HingeDemo::initPhysics()
break; break;
} }
case PGS_HINGE: case PGS_HINGE:
{ {
btSolveProjectedGaussSeidel* mlcp = new btSolveProjectedGaussSeidel; btSolveProjectedGaussSeidel* mlcp = new btSolveProjectedGaussSeidel;
m_solver = new btMLCPSolver(mlcp); m_solver = new btMLCPSolver(mlcp);
break; break;
} }
case SI_HINGE:
{
m_solver = new btSequentialImpulseConstraintSolver();
break;
}
case INERTIA_HINGE: case INERTIA_HINGE:
{ {
m_solver = new btSequentialImpulseConstraintSolver(); m_solver = new btSequentialImpulseConstraintSolver();
@@ -415,7 +421,7 @@ void HingeDemo::initPhysics()
int index = m_glApp->m_instancingRenderer->registerGraphicsInstance(cubeShapeId,startTransform.getOrigin(),startTransform.getRotation(),color,halfExtents); int index = m_glApp->m_instancingRenderer->registerGraphicsInstance(cubeShapeId,startTransform.getOrigin(),startTransform.getRotation(),color,halfExtents);
btBoxShape* box = new btBoxShape(halfExtents); btBoxShape* box = new btBoxShape(halfExtents);
float mass = 1000.f; float mass = 1.f;
btVector3 localInertia; btVector3 localInertia;
box->calculateLocalInertia(mass,localInertia); box->calculateLocalInertia(mass,localInertia);
btRigidBody* body = new btRigidBody(mass,0,box,localInertia); btRigidBody* body = new btRigidBody(mass,0,box,localInertia);
@@ -423,6 +429,7 @@ void HingeDemo::initPhysics()
body->setWorldTransform(startTransform); body->setWorldTransform(startTransform);
body->setUserIndex(index); body->setUserIndex(index);
body->setAngularVelocity(btVector3(0,1,0)); body->setAngularVelocity(btVector3(0,1,0));
body->setActivationState(DISABLE_DEACTIVATION);
m_dynamicsWorld->addRigidBody(body); m_dynamicsWorld->addRigidBody(body);
} }

8
Demos3/bullet2/BasicDemo/HingeDemo.h
View File

@@ -10,6 +10,7 @@ enum HINGE_CREATION_METHOD
DANTZIG_HINGE, DANTZIG_HINGE,
LEMKE_HINGE, LEMKE_HINGE,
PGS_HINGE, PGS_HINGE,
SI_HINGE,
INERTIA_HINGE INERTIA_HINGE
}; };
@@ -34,6 +35,13 @@ public:
{ {
return new HingeDemo(app, PGS_HINGE); return new HingeDemo(app, PGS_HINGE);
} }
static BulletDemoInterface* SICreateFunc(SimpleOpenGL3App* app)
{
return new HingeDemo(app, SI_HINGE);
}
static BulletDemoInterface* InertiaCreateFunc(SimpleOpenGL3App* app) static BulletDemoInterface* InertiaCreateFunc(SimpleOpenGL3App* app)
{ {
return new HingeDemo(app, INERTIA_HINGE); return new HingeDemo(app, INERTIA_HINGE);

142
src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
View File

@@ -45,7 +45,7 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,
int numBodies = m_tmpSolverBodyPool.size(); int numBodies = m_tmpSolverBodyPool.size();
m_allConstraintArray.resize(0); m_allConstraintPtrArray.resize(0);
m_limitDependencies.resize(m_tmpSolverNonContactConstraintPool.size()+m_tmpSolverContactConstraintPool.size()+m_tmpSolverContactFrictionConstraintPool.size()); m_limitDependencies.resize(m_tmpSolverNonContactConstraintPool.size()+m_tmpSolverContactConstraintPool.size()+m_tmpSolverContactFrictionConstraintPool.size());
btAssert(m_limitDependencies.size() == m_tmpSolverNonContactConstraintPool.size()+m_tmpSolverContactConstraintPool.size()+m_tmpSolverContactFrictionConstraintPool.size()); btAssert(m_limitDependencies.size() == m_tmpSolverNonContactConstraintPool.size()+m_tmpSolverContactConstraintPool.size()+m_tmpSolverContactFrictionConstraintPool.size());
// printf("m_limitDependencies.size() = %d\n",m_limitDependencies.size()); // printf("m_limitDependencies.size() = %d\n",m_limitDependencies.size());
@@ -53,7 +53,7 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,
int dindex = 0; int dindex = 0;
for (int i=0;i<m_tmpSolverNonContactConstraintPool.size();i++) for (int i=0;i<m_tmpSolverNonContactConstraintPool.size();i++)
{ {
m_allConstraintArray.push_back(m_tmpSolverNonContactConstraintPool[i]); m_allConstraintPtrArray.push_back(&m_tmpSolverNonContactConstraintPool[i]);
m_limitDependencies[dindex++] = -1; m_limitDependencies[dindex++] = -1;
} }
@@ -65,14 +65,14 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,
{ {
for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++) for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
{ {
m_allConstraintArray.push_back(m_tmpSolverContactConstraintPool[i]); m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
m_limitDependencies[dindex++] = -1; m_limitDependencies[dindex++] = -1;
m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact]); m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact]);
int findex = (m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact].m_frictionIndex*(1+numFrictionPerContact)); int findex = (m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact].m_frictionIndex*(1+numFrictionPerContact));
m_limitDependencies[dindex++] = findex +firstContactConstraintOffset; m_limitDependencies[dindex++] = findex +firstContactConstraintOffset;
if (numFrictionPerContact==2) if (numFrictionPerContact==2)
{ {
m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact+1]); m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact+1]);
m_limitDependencies[dindex++] = findex+firstContactConstraintOffset; m_limitDependencies[dindex++] = findex+firstContactConstraintOffset;
} }
} }
@@ -80,19 +80,19 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,
{ {
for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++) for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
{ {
m_allConstraintArray.push_back(m_tmpSolverContactConstraintPool[i]); m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
m_limitDependencies[dindex++] = -1; m_limitDependencies[dindex++] = -1;
} }
for (int i=0;i<m_tmpSolverContactFrictionConstraintPool.size();i++) for (int i=0;i<m_tmpSolverContactFrictionConstraintPool.size();i++)
{ {
m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i]); m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i]);
m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex+firstContactConstraintOffset; m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex+firstContactConstraintOffset;
} }
} }
if (!m_allConstraintArray.size()) if (!m_allConstraintPtrArray.size())
{ {
m_A.resize(0,0); m_A.resize(0,0);
m_b.resize(0); m_b.resize(0);
@@ -156,7 +156,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
{ {
int numContactRows = interleaveContactAndFriction ? 3 : 1; int numContactRows = interleaveContactAndFriction ? 3 : 1;
int numConstraintRows = m_allConstraintArray.size(); int numConstraintRows = m_allConstraintPtrArray.size();
int n = numConstraintRows; int n = numConstraintRows;
{ {
BT_PROFILE("init b (rhs)"); BT_PROFILE("init b (rhs)");
@@ -166,11 +166,11 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
m_bSplit.setZero(); m_bSplit.setZero();
for (int i=0;i<numConstraintRows ;i++) for (int i=0;i<numConstraintRows ;i++)
{ {
btScalar jacDiag = m_allConstraintArray[i].m_jacDiagABInv; btScalar jacDiag = m_allConstraintPtrArray[i]->m_jacDiagABInv;
if (!btFuzzyZero(jacDiag)) if (!btFuzzyZero(jacDiag))
{ {
btScalar rhs = m_allConstraintArray[i].m_rhs; btScalar rhs = m_allConstraintPtrArray[i]->m_rhs;
btScalar rhsPenetration = m_allConstraintArray[i].m_rhsPenetration; btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration;
m_b[i]=rhs/jacDiag; m_b[i]=rhs/jacDiag;
m_bSplit[i] = rhsPenetration/jacDiag; m_bSplit[i] = rhsPenetration/jacDiag;
} }
@@ -195,14 +195,14 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
m_hi[i] = BT_INFINITY; m_hi[i] = BT_INFINITY;
} else } else
{ {
m_lo[i] = m_allConstraintArray[i].m_lowerLimit; m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
m_hi[i] = m_allConstraintArray[i].m_upperLimit; m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
} }
} }
} }
// //
int m=m_allConstraintArray.size(); int m=m_allConstraintPtrArray.size();
int numBodies = m_tmpSolverBodyPool.size(); int numBodies = m_tmpSolverBodyPool.size();
btAlignedObjectArray<int> bodyJointNodeArray; btAlignedObjectArray<int> bodyJointNodeArray;
@@ -213,7 +213,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
btAlignedObjectArray<btJointNode> jointNodeArray; btAlignedObjectArray<btJointNode> jointNodeArray;
{ {
BT_PROFILE("jointNodeArray.reserve"); BT_PROFILE("jointNodeArray.reserve");
jointNodeArray.reserve(2*m_allConstraintArray.size()); jointNodeArray.reserve(2*m_allConstraintPtrArray.size());
} }
static btMatrixXu J3; static btMatrixXu J3;
@@ -235,7 +235,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
{ {
BT_PROFILE("ofs resize"); BT_PROFILE("ofs resize");
ofs.resize(0); ofs.resize(0);
ofs.resizeNoInitialize(m_allConstraintArray.size()); ofs.resizeNoInitialize(m_allConstraintPtrArray.size());
} }
{ {
BT_PROFILE("Compute J and JinvM"); BT_PROFILE("Compute J and JinvM");
@@ -243,11 +243,11 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
int numRows = 0; int numRows = 0;
for (int i=0;i<m_allConstraintArray.size();i+=numRows,c++) for (int i=0;i<m_allConstraintPtrArray.size();i+=numRows,c++)
{ {
ofs[c] = rowOffset; ofs[c] = rowOffset;
int sbA = m_allConstraintArray[i].m_solverBodyIdA; int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
int sbB = m_allConstraintArray[i].m_solverBodyIdB; int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
@@ -268,13 +268,13 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
} }
for (int row=0;row<numRows;row++,cur++) for (int row=0;row<numRows;row++,cur++)
{ {
btVector3 normalInvMass = m_allConstraintArray[i+row].m_contactNormal1 * orgBodyA->getInvMass(); btVector3 normalInvMass = m_allConstraintPtrArray[i+row]->m_contactNormal1 * orgBodyA->getInvMass();
btVector3 relPosCrossNormalInvInertia = m_allConstraintArray[i+row].m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld(); btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld();
for (int r=0;r<3;r++) for (int r=0;r<3;r++)
{ {
J3.setElem(cur,r,m_allConstraintArray[i+row].m_contactNormal1[r]); J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal1[r]);
J3.setElem(cur,r+4,m_allConstraintArray[i+row].m_relpos1CrossNormal[r]); J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal[r]);
JinvM3.setElem(cur,r,normalInvMass[r]); JinvM3.setElem(cur,r,normalInvMass[r]);
JinvM3.setElem(cur,r+4,relPosCrossNormalInvInertia[r]); JinvM3.setElem(cur,r+4,relPosCrossNormalInvInertia[r]);
} }
@@ -305,13 +305,13 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
for (int row=0;row<numRows;row++,cur++) for (int row=0;row<numRows;row++,cur++)
{ {
btVector3 normalInvMassB = m_allConstraintArray[i+row].m_contactNormal2*orgBodyB->getInvMass(); btVector3 normalInvMassB = m_allConstraintPtrArray[i+row]->m_contactNormal2*orgBodyB->getInvMass();
btVector3 relPosInvInertiaB = m_allConstraintArray[i+row].m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld(); btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld();
for (int r=0;r<3;r++) for (int r=0;r<3;r++)
{ {
J3.setElem(cur,r,m_allConstraintArray[i+row].m_contactNormal2[r]); J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal2[r]);
J3.setElem(cur,r+4,m_allConstraintArray[i+row].m_relpos2CrossNormal[r]); J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal[r]);
JinvM3.setElem(cur,r,normalInvMassB[r]); JinvM3.setElem(cur,r,normalInvMassB[r]);
JinvM3.setElem(cur,r+4,relPosInvInertiaB[r]); JinvM3.setElem(cur,r+4,relPosInvInertiaB[r]);
} }
@@ -349,11 +349,11 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
{ {
int numRows = 0; int numRows = 0;
BT_PROFILE("Compute A"); BT_PROFILE("Compute A");
for (int i=0;i<m_allConstraintArray.size();i+= numRows,c++) for (int i=0;i<m_allConstraintPtrArray.size();i+= numRows,c++)
{ {
int row__ = ofs[c]; int row__ = ofs[c];
int sbA = m_allConstraintArray[i].m_solverBodyIdA; int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
int sbB = m_allConstraintArray[i].m_solverBodyIdB; int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
@@ -371,7 +371,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
{ {
int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows; int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows;
size_t ofsother = (m_allConstraintArray[cr0].m_solverBodyIdB == sbA) ? 8*numRowsOther : 0; size_t ofsother = (m_allConstraintPtrArray[cr0]->m_solverBodyIdB == sbA) ? 8*numRowsOther : 0;
//printf("%d joint i %d and j0: %d: ",count++,i,j0); //printf("%d joint i %d and j0: %d: ",count++,i,j0);
m_A.multiplyAdd2_p8r ( JinvMrow, m_A.multiplyAdd2_p8r ( JinvMrow,
Jptr + 2*8*(size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__,ofs[j0]); Jptr + 2*8*(size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__,ofs[j0]);
@@ -390,7 +390,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
if (j1<c) if (j1<c)
{ {
int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows; int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows;
size_t ofsother = (m_allConstraintArray[cj1].m_solverBodyIdB == sbB) ? 8*numRowsOther : 0; size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8*numRowsOther : 0;
m_A.multiplyAdd2_p8r ( JinvMrow + 8*(size_t)numRows, m_A.multiplyAdd2_p8r ( JinvMrow + 8*(size_t)numRows,
Jptr + 2*8*(size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__,ofs[j1]); Jptr + 2*8*(size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__,ofs[j1]);
} }
@@ -404,14 +404,14 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
// compute diagonal blocks of m_A // compute diagonal blocks of m_A
int row__ = 0; int row__ = 0;
int numJointRows = m_allConstraintArray.size(); int numJointRows = m_allConstraintPtrArray.size();
int jj=0; int jj=0;
for (;row__<numJointRows;) for (;row__<numJointRows;)
{ {
int sbA = m_allConstraintArray[row__].m_solverBodyIdA; int sbA = m_allConstraintPtrArray[row__]->m_solverBodyIdA;
int sbB = m_allConstraintArray[row__].m_solverBodyIdB; int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB;
btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
@@ -453,9 +453,9 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
if (infoGlobal.m_solverMode&SOLVER_USE_WARMSTARTING) if (infoGlobal.m_solverMode&SOLVER_USE_WARMSTARTING)
{ {
for (int i=0;i<m_allConstraintArray.size();i++) for (int i=0;i<m_allConstraintPtrArray.size();i++)
{ {
const btSolverConstraint& c = m_allConstraintArray[i]; const btSolverConstraint& c = *m_allConstraintPtrArray[i];
m_x[i]=c.m_appliedImpulse; m_x[i]=c.m_appliedImpulse;
m_xSplit[i] = c.m_appliedPushImpulse; m_xSplit[i] = c.m_appliedPushImpulse;
} }
@@ -471,7 +471,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
{ {
int numBodies = this->m_tmpSolverBodyPool.size(); int numBodies = this->m_tmpSolverBodyPool.size();
int numConstraintRows = m_allConstraintArray.size(); int numConstraintRows = m_allConstraintPtrArray.size();
m_b.resize(numConstraintRows); m_b.resize(numConstraintRows);
if (infoGlobal.m_splitImpulse) if (infoGlobal.m_splitImpulse)
@@ -482,11 +482,11 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
for (int i=0;i<numConstraintRows ;i++) for (int i=0;i<numConstraintRows ;i++)
{ {
if (m_allConstraintArray[i].m_jacDiagABInv) if (m_allConstraintPtrArray[i]->m_jacDiagABInv)
{ {
m_b[i]=m_allConstraintArray[i].m_rhs/m_allConstraintArray[i].m_jacDiagABInv; m_b[i]=m_allConstraintPtrArray[i]->m_rhs/m_allConstraintPtrArray[i]->m_jacDiagABInv;
if (infoGlobal.m_splitImpulse) if (infoGlobal.m_splitImpulse)
m_bSplit[i] = m_allConstraintArray[i].m_rhsPenetration/m_allConstraintArray[i].m_jacDiagABInv; m_bSplit[i] = m_allConstraintPtrArray[i]->m_rhsPenetration/m_allConstraintPtrArray[i]->m_jacDiagABInv;
} }
} }
@@ -517,28 +517,28 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
for (int i=0;i<numConstraintRows;i++) for (int i=0;i<numConstraintRows;i++)
{ {
m_lo[i] = m_allConstraintArray[i].m_lowerLimit; m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
m_hi[i] = m_allConstraintArray[i].m_upperLimit; m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
int bodyIndex0 = m_allConstraintArray[i].m_solverBodyIdA; int bodyIndex0 = m_allConstraintPtrArray[i]->m_solverBodyIdA;
int bodyIndex1 = m_allConstraintArray[i].m_solverBodyIdB; int bodyIndex1 = m_allConstraintPtrArray[i]->m_solverBodyIdB;
if (m_tmpSolverBodyPool[bodyIndex0].m_originalBody) if (m_tmpSolverBodyPool[bodyIndex0].m_originalBody)
{ {
setElem(J,i,6*bodyIndex0+0,m_allConstraintArray[i].m_contactNormal1[0]); setElem(J,i,6*bodyIndex0+0,m_allConstraintPtrArray[i]->m_contactNormal1[0]);
setElem(J,i,6*bodyIndex0+1,m_allConstraintArray[i].m_contactNormal1[1]); setElem(J,i,6*bodyIndex0+1,m_allConstraintPtrArray[i]->m_contactNormal1[1]);
setElem(J,i,6*bodyIndex0+2,m_allConstraintArray[i].m_contactNormal1[2]); setElem(J,i,6*bodyIndex0+2,m_allConstraintPtrArray[i]->m_contactNormal1[2]);
setElem(J,i,6*bodyIndex0+3,m_allConstraintArray[i].m_relpos1CrossNormal[0]); setElem(J,i,6*bodyIndex0+3,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[0]);
setElem(J,i,6*bodyIndex0+4,m_allConstraintArray[i].m_relpos1CrossNormal[1]); setElem(J,i,6*bodyIndex0+4,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[1]);
setElem(J,i,6*bodyIndex0+5,m_allConstraintArray[i].m_relpos1CrossNormal[2]); setElem(J,i,6*bodyIndex0+5,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[2]);
} }
if (m_tmpSolverBodyPool[bodyIndex1].m_originalBody) if (m_tmpSolverBodyPool[bodyIndex1].m_originalBody)
{ {
setElem(J,i,6*bodyIndex1+0,m_allConstraintArray[i].m_contactNormal2[0]); setElem(J,i,6*bodyIndex1+0,m_allConstraintPtrArray[i]->m_contactNormal2[0]);
setElem(J,i,6*bodyIndex1+1,m_allConstraintArray[i].m_contactNormal2[1]); setElem(J,i,6*bodyIndex1+1,m_allConstraintPtrArray[i]->m_contactNormal2[1]);
setElem(J,i,6*bodyIndex1+2,m_allConstraintArray[i].m_contactNormal2[2]); setElem(J,i,6*bodyIndex1+2,m_allConstraintPtrArray[i]->m_contactNormal2[2]);
setElem(J,i,6*bodyIndex1+3,m_allConstraintArray[i].m_relpos2CrossNormal[0]); setElem(J,i,6*bodyIndex1+3,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[0]);
setElem(J,i,6*bodyIndex1+4,m_allConstraintArray[i].m_relpos2CrossNormal[1]); setElem(J,i,6*bodyIndex1+4,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[1]);
setElem(J,i,6*bodyIndex1+5,m_allConstraintArray[i].m_relpos2CrossNormal[2]); setElem(J,i,6*bodyIndex1+5,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[2]);
} }
} }
@@ -573,9 +573,9 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
m_xSplit.resize(numConstraintRows); m_xSplit.resize(numConstraintRows);
// m_x.setZero(); // m_x.setZero();
for (int i=0;i<m_allConstraintArray.size();i++) for (int i=0;i<m_allConstraintPtrArray.size();i++)
{ {
const btSolverConstraint& c = m_allConstraintArray[i]; const btSolverConstraint& c = *m_allConstraintPtrArray[i];
m_x[i]=c.m_appliedImpulse; m_x[i]=c.m_appliedImpulse;
if (infoGlobal.m_splitImpulse) if (infoGlobal.m_splitImpulse)
m_xSplit[i] = c.m_appliedPushImpulse; m_xSplit[i] = c.m_appliedPushImpulse;
@@ -597,10 +597,10 @@ btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bod
if (result) if (result)
{ {
BT_PROFILE("process MLCP results"); BT_PROFILE("process MLCP results");
for (int i=0;i<m_allConstraintArray.size();i++) for (int i=0;i<m_allConstraintPtrArray.size();i++)
{ {
{ {
btSolverConstraint& c = m_allConstraintArray[i]; btSolverConstraint& c = *m_allConstraintPtrArray[i];
int sbA = c.m_solverBodyIdA; int sbA = c.m_solverBodyIdA;
int sbB = c.m_solverBodyIdB; int sbB = c.m_solverBodyIdB;
btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
@@ -609,15 +609,21 @@ btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bod
btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA]; btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA];
btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB]; btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB];
solverBodyA.internalApplyImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,m_x[i]); {
solverBodyB.internalApplyImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,m_x[i]); btScalar deltaImpulse = m_x[i]-c.m_appliedImpulse;
c.m_appliedImpulse = m_x[i];
solverBodyA.internalApplyImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
solverBodyB.internalApplyImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
}
if (infoGlobal.m_splitImpulse) if (infoGlobal.m_splitImpulse)
{ {
solverBodyA.internalApplyPushImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,m_xSplit[i]); btScalar deltaImpulse = m_xSplit[i] - c.m_appliedPushImpulse;
solverBodyB.internalApplyPushImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,m_xSplit[i]); solverBodyA.internalApplyPushImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
solverBodyB.internalApplyPushImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
c.m_appliedPushImpulse = m_xSplit[i]; c.m_appliedPushImpulse = m_xSplit[i];
} }
c.m_appliedImpulse = m_x[i];
} }
} }
} }
@@ -630,3 +636,5 @@ btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bod
return 0.f; return 0.f;
} }

4
src/BulletDynamics/MLCPSolvers/btMLCPSolver.h
View File

@@ -39,13 +39,15 @@ protected:
btVectorXu m_xSplit2; btVectorXu m_xSplit2;
btAlignedObjectArray<int> m_limitDependencies; btAlignedObjectArray<int> m_limitDependencies;
btConstraintArray m_allConstraintArray; btAlignedObjectArray<btSolverConstraint*> m_allConstraintPtrArray;
btMLCPSolverInterface* m_solver; btMLCPSolverInterface* m_solver;
int m_fallback; int m_fallback;
btScalar m_cfm; btScalar m_cfm;
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
virtual void createMLCP(const btContactSolverInfo& infoGlobal); virtual void createMLCP(const btContactSolverInfo& infoGlobal);
virtual void createMLCPFast(const btContactSolverInfo& infoGlobal); virtual void createMLCPFast(const btContactSolverInfo& infoGlobal);