From dfa738c13a0da79a0543e4cedcd59b35cf2f27a6 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Mon, 24 Feb 2014 13:24:49 -0800 Subject: [PATCH] Properly propagate the applied impulse for the MLCP solvers, so it will be available for contact and non-contact constraints. Use real-time clock in AllBullet2Demos, rather than hard-coded 1./60. --- Demos3/AllBullet2Demos/BulletDemoEntries.h | 5 +- Demos3/AllBullet2Demos/main.cpp | 14 +- Demos3/bullet2/BasicDemo/BasicDemo.cpp | 21 +++ Demos3/bullet2/BasicDemo/HingeDemo.cpp | 9 +- Demos3/bullet2/BasicDemo/HingeDemo.h | 8 + .../MLCPSolvers/btMLCPSolver.cpp | 144 +++++++++--------- src/BulletDynamics/MLCPSolvers/btMLCPSolver.h | 4 +- 7 files changed, 130 insertions(+), 75 deletions(-) diff --git a/Demos3/AllBullet2Demos/BulletDemoEntries.h b/Demos3/AllBullet2Demos/BulletDemoEntries.h index 832ccb710..1466b8156 100644 --- a/Demos3/AllBullet2Demos/BulletDemoEntries.h +++ b/Demos3/AllBullet2Demos/BulletDemoEntries.h @@ -25,14 +25,15 @@ static BulletDemoEntry allDemos[]= //{"emptydemo",EmptyBulletDemo::MyCreateFunc}, {"BasicDemo",BasicDemo::MyCreateFunc}, - /* + {"ChainDemo",ChainDemo::MyCreateFunc}, + {"SIHingeDemo",HingeDemo::SICreateFunc}, {"PGSHingeDemo",HingeDemo::PGSCreateFunc}, {"DantzigHingeDemo",HingeDemo::DantzigCreateFunc}, {"LemkeHingeDemo",HingeDemo::LemkeCreateFunc}, {"InertiaHingeDemo",HingeDemo::InertiaCreateFunc}, {"ABMHingeDemo",HingeDemo::FeatherstoneCreateFunc}, - */ + {"Ragdoll",RagDollDemo::MyCreateFunc}, {"MultiBody1",FeatherstoneDemo1::MyCreateFunc}, diff --git a/Demos3/AllBullet2Demos/main.cpp b/Demos3/AllBullet2Demos/main.cpp index 9f1f22a40..e8885e33c 100644 --- a/Demos3/AllBullet2Demos/main.cpp +++ b/Demos3/AllBullet2Demos/main.cpp @@ -5,7 +5,7 @@ #include "../GpuDemos/gwenUserInterface.h" #include "BulletDemoEntries.h" - +#include "../../btgui/Timing/b3Clock.h" #define DEMO_SELECTION_COMBOBOX 13 const char* startFileName = "bulletDemo.txt"; static SimpleOpenGL3App* app=0; @@ -132,7 +132,8 @@ void MyComboBoxCallback(int comboId, const char* item) int main(int argc, char* argv[]) { - + b3Clock clock; + float dt = 1./120.f; int width = 1024; int height=768; @@ -167,6 +168,7 @@ int main(int argc, char* argv[]) gui->setComboBoxCallback(MyComboBoxCallback); + unsigned long int prevTimeInMicroseconds = clock.getTimeMicroseconds(); do { @@ -191,7 +193,13 @@ int main(int argc, char* argv[]) if (sCurrentDemo) { 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(); } diff --git a/Demos3/bullet2/BasicDemo/BasicDemo.cpp b/Demos3/bullet2/BasicDemo/BasicDemo.cpp index 235536000..b8eb65c9d 100644 --- a/Demos3/bullet2/BasicDemo/BasicDemo.cpp +++ b/Demos3/bullet2/BasicDemo/BasicDemo.cpp @@ -145,6 +145,27 @@ void BasicDemo::renderScene() void BasicDemo::stepSimulation(float dt) { m_dynamicsWorld->stepSimulation(dt); +/* + //print applied force + //contact points + for (int i=0;igetDispatcher()->getNumManifolds();i++) + { + btPersistentManifold* contact = m_dynamicsWorld->getDispatcher()->getManifoldByIndexInternal(i); + for (int c=0;cgetNumContacts();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"); + } + } + } +*/ } diff --git a/Demos3/bullet2/BasicDemo/HingeDemo.cpp b/Demos3/bullet2/BasicDemo/HingeDemo.cpp index b79eeb161..57f2950d3 100644 --- a/Demos3/bullet2/BasicDemo/HingeDemo.cpp +++ b/Demos3/bullet2/BasicDemo/HingeDemo.cpp @@ -304,12 +304,18 @@ void HingeDemo::initPhysics() break; } + case PGS_HINGE: { btSolveProjectedGaussSeidel* mlcp = new btSolveProjectedGaussSeidel; m_solver = new btMLCPSolver(mlcp); break; } + case SI_HINGE: + { + m_solver = new btSequentialImpulseConstraintSolver(); + break; + } case INERTIA_HINGE: { 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); btBoxShape* box = new btBoxShape(halfExtents); - float mass = 1000.f; + float mass = 1.f; btVector3 localInertia; box->calculateLocalInertia(mass,localInertia); btRigidBody* body = new btRigidBody(mass,0,box,localInertia); @@ -423,6 +429,7 @@ void HingeDemo::initPhysics() body->setWorldTransform(startTransform); body->setUserIndex(index); body->setAngularVelocity(btVector3(0,1,0)); + body->setActivationState(DISABLE_DEACTIVATION); m_dynamicsWorld->addRigidBody(body); } diff --git a/Demos3/bullet2/BasicDemo/HingeDemo.h b/Demos3/bullet2/BasicDemo/HingeDemo.h index 3b5691e4a..65fec22ea 100644 --- a/Demos3/bullet2/BasicDemo/HingeDemo.h +++ b/Demos3/bullet2/BasicDemo/HingeDemo.h @@ -10,6 +10,7 @@ enum HINGE_CREATION_METHOD DANTZIG_HINGE, LEMKE_HINGE, PGS_HINGE, + SI_HINGE, INERTIA_HINGE }; @@ -34,6 +35,13 @@ public: { return new HingeDemo(app, PGS_HINGE); } + + static BulletDemoInterface* SICreateFunc(SimpleOpenGL3App* app) + { + return new HingeDemo(app, SI_HINGE); + } + + static BulletDemoInterface* InertiaCreateFunc(SimpleOpenGL3App* app) { return new HingeDemo(app, INERTIA_HINGE); diff --git a/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp b/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp index 2892f192f..60e4cc66b 100644 --- a/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp +++ b/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp @@ -45,7 +45,7 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, 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()); btAssert(m_limitDependencies.size() == m_tmpSolverNonContactConstraintPool.size()+m_tmpSolverContactConstraintPool.size()+m_tmpSolverContactFrictionConstraintPool.size()); // printf("m_limitDependencies.size() = %d\n",m_limitDependencies.size()); @@ -53,7 +53,7 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int dindex = 0; for (int i=0;im_jacDiagABInv; if (!btFuzzyZero(jacDiag)) { - btScalar rhs = m_allConstraintArray[i].m_rhs; - btScalar rhsPenetration = m_allConstraintArray[i].m_rhsPenetration; + btScalar rhs = m_allConstraintPtrArray[i]->m_rhs; + btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration; m_b[i]=rhs/jacDiag; m_bSplit[i] = rhsPenetration/jacDiag; } @@ -195,14 +195,14 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) m_hi[i] = BT_INFINITY; } else { - m_lo[i] = m_allConstraintArray[i].m_lowerLimit; - m_hi[i] = m_allConstraintArray[i].m_upperLimit; + m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit; + m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit; } } } // - int m=m_allConstraintArray.size(); + int m=m_allConstraintPtrArray.size(); int numBodies = m_tmpSolverBodyPool.size(); btAlignedObjectArray bodyJointNodeArray; @@ -213,7 +213,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) btAlignedObjectArray jointNodeArray; { BT_PROFILE("jointNodeArray.reserve"); - jointNodeArray.reserve(2*m_allConstraintArray.size()); + jointNodeArray.reserve(2*m_allConstraintPtrArray.size()); } static btMatrixXu J3; @@ -235,7 +235,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) { BT_PROFILE("ofs resize"); ofs.resize(0); - ofs.resizeNoInitialize(m_allConstraintArray.size()); + ofs.resizeNoInitialize(m_allConstraintPtrArray.size()); } { BT_PROFILE("Compute J and JinvM"); @@ -243,11 +243,11 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) int numRows = 0; - for (int i=0;im_solverBodyIdA; + int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB; btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; @@ -268,13 +268,13 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) } for (int row=0;rowgetInvMass(); - btVector3 relPosCrossNormalInvInertia = m_allConstraintArray[i+row].m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld(); + btVector3 normalInvMass = m_allConstraintPtrArray[i+row]->m_contactNormal1 * orgBodyA->getInvMass(); + btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld(); for (int r=0;r<3;r++) { - J3.setElem(cur,r,m_allConstraintArray[i+row].m_contactNormal1[r]); - J3.setElem(cur,r+4,m_allConstraintArray[i+row].m_relpos1CrossNormal[r]); + J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal1[r]); + J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal[r]); JinvM3.setElem(cur,r,normalInvMass[r]); JinvM3.setElem(cur,r+4,relPosCrossNormalInvInertia[r]); } @@ -305,13 +305,13 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) for (int row=0;rowgetInvMass(); - btVector3 relPosInvInertiaB = m_allConstraintArray[i+row].m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld(); + btVector3 normalInvMassB = m_allConstraintPtrArray[i+row]->m_contactNormal2*orgBodyB->getInvMass(); + btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld(); for (int r=0;r<3;r++) { - J3.setElem(cur,r,m_allConstraintArray[i+row].m_contactNormal2[r]); - J3.setElem(cur,r+4,m_allConstraintArray[i+row].m_relpos2CrossNormal[r]); + J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal2[r]); + J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal[r]); JinvM3.setElem(cur,r,normalInvMassB[r]); JinvM3.setElem(cur,r+4,relPosInvInertiaB[r]); } @@ -349,11 +349,11 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) { int numRows = 0; BT_PROFILE("Compute A"); - for (int i=0;im_solverBodyIdA; + int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB; btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].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; - 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); m_A.multiplyAdd2_p8r ( JinvMrow, Jptr + 2*8*(size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__,ofs[j0]); @@ -390,7 +390,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) if (j1m_solverBodyIdB == sbB) ? 8*numRowsOther : 0; m_A.multiplyAdd2_p8r ( JinvMrow + 8*(size_t)numRows, 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 int row__ = 0; - int numJointRows = m_allConstraintArray.size(); + int numJointRows = m_allConstraintPtrArray.size(); int jj=0; for (;row__m_solverBodyIdA; + int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB; btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].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) { - for (int i=0;im_tmpSolverBodyPool.size(); - int numConstraintRows = m_allConstraintArray.size(); + int numConstraintRows = m_allConstraintPtrArray.size(); m_b.resize(numConstraintRows); if (infoGlobal.m_splitImpulse) @@ -482,11 +482,11 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) for (int i=0;im_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) - 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;im_lowerLimit; + m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit; - int bodyIndex0 = m_allConstraintArray[i].m_solverBodyIdA; - int bodyIndex1 = m_allConstraintArray[i].m_solverBodyIdB; + int bodyIndex0 = m_allConstraintPtrArray[i]->m_solverBodyIdA; + int bodyIndex1 = m_allConstraintPtrArray[i]->m_solverBodyIdB; if (m_tmpSolverBodyPool[bodyIndex0].m_originalBody) { - setElem(J,i,6*bodyIndex0+0,m_allConstraintArray[i].m_contactNormal1[0]); - setElem(J,i,6*bodyIndex0+1,m_allConstraintArray[i].m_contactNormal1[1]); - setElem(J,i,6*bodyIndex0+2,m_allConstraintArray[i].m_contactNormal1[2]); - setElem(J,i,6*bodyIndex0+3,m_allConstraintArray[i].m_relpos1CrossNormal[0]); - setElem(J,i,6*bodyIndex0+4,m_allConstraintArray[i].m_relpos1CrossNormal[1]); - setElem(J,i,6*bodyIndex0+5,m_allConstraintArray[i].m_relpos1CrossNormal[2]); + setElem(J,i,6*bodyIndex0+0,m_allConstraintPtrArray[i]->m_contactNormal1[0]); + setElem(J,i,6*bodyIndex0+1,m_allConstraintPtrArray[i]->m_contactNormal1[1]); + setElem(J,i,6*bodyIndex0+2,m_allConstraintPtrArray[i]->m_contactNormal1[2]); + setElem(J,i,6*bodyIndex0+3,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[0]); + setElem(J,i,6*bodyIndex0+4,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[1]); + setElem(J,i,6*bodyIndex0+5,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[2]); } if (m_tmpSolverBodyPool[bodyIndex1].m_originalBody) { - setElem(J,i,6*bodyIndex1+0,m_allConstraintArray[i].m_contactNormal2[0]); - setElem(J,i,6*bodyIndex1+1,m_allConstraintArray[i].m_contactNormal2[1]); - setElem(J,i,6*bodyIndex1+2,m_allConstraintArray[i].m_contactNormal2[2]); - setElem(J,i,6*bodyIndex1+3,m_allConstraintArray[i].m_relpos2CrossNormal[0]); - setElem(J,i,6*bodyIndex1+4,m_allConstraintArray[i].m_relpos2CrossNormal[1]); - setElem(J,i,6*bodyIndex1+5,m_allConstraintArray[i].m_relpos2CrossNormal[2]); + setElem(J,i,6*bodyIndex1+0,m_allConstraintPtrArray[i]->m_contactNormal2[0]); + setElem(J,i,6*bodyIndex1+1,m_allConstraintPtrArray[i]->m_contactNormal2[1]); + setElem(J,i,6*bodyIndex1+2,m_allConstraintPtrArray[i]->m_contactNormal2[2]); + setElem(J,i,6*bodyIndex1+3,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[0]); + setElem(J,i,6*bodyIndex1+4,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[1]); + 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_x.setZero(); - for (int i=0;i m_limitDependencies; - btConstraintArray m_allConstraintArray; + btAlignedObjectArray m_allConstraintPtrArray; btMLCPSolverInterface* m_solver; int m_fallback; 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 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 createMLCPFast(const btContactSolverInfo& infoGlobal);