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

a bit of rk4 clean-up

Browse Source
This commit is contained in:
kubas
2014-01-09 00:59:48 +01:00
parent 4eac9a11f3
commit c5594a5826
3 changed files with 23 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -306,8 +306,8 @@ void btMultiBody::finalizeMultiDof()
{
btAssert(m_isMultiDof);
m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices
m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices)
m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount + m_posVarCnt); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector
m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices)
updateLinksDofOffsets();
}

21
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
View File

@@ -381,7 +381,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
:btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration),
m_multiBodyConstraintSolver(constraintSolver)
m_multiBodyConstraintSolver(constraintSolver), m_useRK4ForMultiBodies(false)
{
//split impulse is not yet supported for Featherstone hierarchies
getSolverInfo().m_splitImpulse = false;
@@ -463,12 +463,11 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
}
bool rk = false;
bool doNotUpdatePos = false;
if(bod->isMultiDof())
{
if(!rk)
if(!m_useRK4ForMultiBodies)
bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
else
{
@@ -577,7 +576,13 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
//
if(!doNotUpdatePos)
{
bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
for(int i = 0; i < bod->getNumPosVars(); ++i)
pRealBuf[i] = delta_q[i];
//bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
bod->__posUpdated = true;
}
//
@@ -636,6 +641,14 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
{
if(!bod->__posUpdated)
bod->stepPositionsMultiDof(timeStep);
else
{
btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
bod->stepPositionsMultiDof(1, 0, pRealBuf);
bod->__posUpdated = false;
}
}
else
bod->stepPositions(timeStep);

4
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
View File

@@ -34,6 +34,7 @@ protected:
btAlignedObjectArray<btMultiBodyConstraint*> m_sortedMultiBodyConstraints;
btMultiBodyConstraintSolver* m_multiBodyConstraintSolver;
MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback;
bool m_useRK4ForMultiBodies;
virtual void calculateSimulationIslands();
virtual void updateActivationState(btScalar timeStep);
@@ -52,5 +53,8 @@ public:
virtual void addMultiBodyConstraint( btMultiBodyConstraint* constraint);
virtual void removeMultiBodyConstraint( btMultiBodyConstraint* constraint);
void useRK4IntegrationForMultiBodies(bool use) { m_useRK4ForMultiBodies = use; }
bool isUsingRK4IntegrationForMultiBodies() const { return m_useRK4ForMultiBodies; }
};
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H