diff --git a/Demos3/AllBullet2Demos/main.cpp b/Demos3/AllBullet2Demos/main.cpp index e8885e33c..3bae9da6f 100644 --- a/Demos3/AllBullet2Demos/main.cpp +++ b/Demos3/AllBullet2Demos/main.cpp @@ -180,11 +180,13 @@ int main(int argc, char* argv[]) app->drawGrid(); + static int frameCount = 0; + frameCount++; + if (0) { char bla[1024]; - static int frameCount = 0; - frameCount++; + sprintf(bla,"Simple test frame %d", frameCount); app->drawText(bla,10,10); @@ -197,6 +199,9 @@ int main(int argc, char* argv[]) unsigned long int curTimeInMicroseconds = clock.getTimeMicroseconds(); unsigned long int diff = curTimeInMicroseconds-prevTimeInMicroseconds; float deltaTimeInSeconds = (diff)*1.e-6; + //printf("---------------------------------------------------\n"); + //printf("Framecount = %d\n",frameCount); + sCurrentDemo->stepSimulation(deltaTimeInSeconds);//1./60.f); prevTimeInMicroseconds = curTimeInMicroseconds; } diff --git a/Demos3/bullet2/BasicDemo/BasicDemo.cpp b/Demos3/bullet2/BasicDemo/BasicDemo.cpp index b8eb65c9d..92147c16d 100644 --- a/Demos3/bullet2/BasicDemo/BasicDemo.cpp +++ b/Demos3/bullet2/BasicDemo/BasicDemo.cpp @@ -145,7 +145,8 @@ void BasicDemo::renderScene() void BasicDemo::stepSimulation(float dt) { m_dynamicsWorld->stepSimulation(dt); -/* + + /* //print applied force //contact points for (int i=0;igetDispatcher()->getNumManifolds();i++) @@ -165,7 +166,7 @@ void BasicDemo::stepSimulation(float dt) } } } -*/ + */ } diff --git a/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp b/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp index 8158787f3..a9f1a4765 100644 --- a/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp +++ b/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp @@ -610,8 +610,27 @@ void FeatherstoneDemo1::renderScene() void FeatherstoneDemo1::stepSimulation(float deltaTime) { - m_dynamicsWorld->stepSimulation(deltaTime,0); + m_dynamicsWorld->stepSimulation(deltaTime);//,0); // CProfileManager::dumpAll(); + /* + 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/FeatherstoneMultiBodyDemo/MultiDofDemo.cpp b/Demos3/bullet2/FeatherstoneMultiBodyDemo/MultiDofDemo.cpp index 7b5fab35d..f31130428 100644 --- a/Demos3/bullet2/FeatherstoneMultiBodyDemo/MultiDofDemo.cpp +++ b/Demos3/bullet2/FeatherstoneMultiBodyDemo/MultiDofDemo.cpp @@ -46,6 +46,25 @@ void MultiDofDemo::stepSimulation(float deltaTime) float internalTimeStep = 1./240.f; m_dynamicsWorld->stepSimulation(deltaTime,10,internalTimeStep); // CProfileManager::dumpAll(); + /* + 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/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 81a30f448..90a0e25a5 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -36,8 +36,10 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl //if (iteration < constraint.m_overrideNumSolverIterations) //resolveSingleConstraintRowGenericMultiBody(constraint); resolveSingleConstraintRowGeneric(constraint); - if(constraint.m_multiBodyA) constraint.m_multiBodyA->__posUpdated = false; - if(constraint.m_multiBodyB) constraint.m_multiBodyB->__posUpdated = false; + if(constraint.m_multiBodyA) + constraint.m_multiBodyA->__posUpdated = false; + if(constraint.m_multiBodyB) + constraint.m_multiBodyB->__posUpdated = false; } //solve featherstone normal contact @@ -47,8 +49,10 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl if (iteration < infoGlobal.m_numIterations) resolveSingleConstraintRowGeneric(constraint); - if(constraint.m_multiBodyA) constraint.m_multiBodyA->__posUpdated = false; - if(constraint.m_multiBodyB) constraint.m_multiBodyB->__posUpdated = false; + if(constraint.m_multiBodyA) + constraint.m_multiBodyA->__posUpdated = false; + if(constraint.m_multiBodyB) + constraint.m_multiBodyB->__posUpdated = false; } //solve featherstone frictional contact @@ -66,8 +70,10 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; resolveSingleConstraintRowGeneric(frictionConstraint); - if(frictionConstraint.m_multiBodyA) frictionConstraint.m_multiBodyA->__posUpdated = false; - if(frictionConstraint.m_multiBodyB) frictionConstraint.m_multiBodyB->__posUpdated = false; + if(frictionConstraint.m_multiBodyA) + frictionConstraint.m_multiBodyA->__posUpdated = false; + if(frictionConstraint.m_multiBodyB) + frictionConstraint.m_multiBodyB->__posUpdated = false; } } } @@ -474,7 +480,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol ///warm starting (or zero if disabled) - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + //disable warmstarting for btMultiBody, it has issues gaining energy (==explosion) + if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) { solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; @@ -826,6 +833,63 @@ btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher); } +btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal) +{ + int numPoolConstraints = m_multiBodyNormalContactConstraints.size(); + int i,j; + + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + for (j=0;jm_appliedImpulse = solveManifold.m_appliedImpulse; + + pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solveManifold.m_frictionIndex].m_appliedImpulse; + //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1); + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + { + pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solveManifold.m_frictionIndex+1].m_appliedImpulse; + } + //do a callback here? + } + } + + + numPoolConstraints = m_multiBodyNonContactConstraints.size(); + +#if 0 + //@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint + for (int i=0;igetJointFeedback(); + if (fb) + { + fb->m_appliedForceBodyA += c.m_contactNormal1*c.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep; + fb->m_appliedForceBodyB += c.m_contactNormal2*c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep; + fb->m_appliedTorqueBodyA += c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; + fb->m_appliedTorqueBodyB += c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */ + + } + + constr->internalSetAppliedImpulse(c.m_appliedImpulse); + if (btFabs(c.m_appliedImpulse)>=constr->getBreakingImpulseThreshold()) + { + constr->setEnabled(false); + } + + } +#endif + + + return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies,numBodies,infoGlobal); +} + void btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher) { diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h index 28485cf92..800941c64 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h @@ -73,7 +73,8 @@ public: ///this method should not be called, it was just used during porting/integration of Featherstone btMultiBody, providing backwards compatibility but no support for btMultiBodyConstraint (only contact constraints) virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher); - + virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal); + virtual void solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher); };