AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-04 17:40:06 +00:00
Code Issues Releases Wiki Activity

btMultiBodyConstraintSolver writes back the applied impulse for contact points

Browse Source 
(added some debugging output for this in the demos, commented-out by default)
This commit is contained in:
erwin coumans 2014-02-24 16:55:54 -08:00
parent dfa738c13a
commit d485f2b272
6 changed files with 122 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
Demos3/AllBullet2Demos/main.cpp
View File

@ -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;
}

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

@ -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;i<m_dynamicsWorld->getDispatcher()->getNumManifolds();i++)
@ -165,7 +166,7 @@ void BasicDemo::stepSimulation(float dt)
}
}
}
*/
*/
}

21
Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp
View File

@ -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;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");
}
}
}
*/
}

19
Demos3/bullet2/FeatherstoneMultiBodyDemo/MultiDofDemo.cpp
View File

@ -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;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");
}
}
}
*/
}

78
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
View File

@ -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;j<numPoolConstraints;j++)
{
const btMultiBodySolverConstraint& solveManifold = m_multiBodyNormalContactConstraints[j];
btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
btAssert(pt);
pt->m_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;i<numPoolConstraints;i++)
{
const btMultiBodySolverConstraint& c = m_multiBodyNonContactConstraints[i];
btTypedConstraint* constr = (btTypedConstraint*)c.m_originalContactPoint;
btJointFeedback* fb = constr->getJointFeedback();
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)
{

3
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h
View File

@ -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);
};