Welcome to mirror list, hosted at ThFree Co, Russian Federation.

git.blender.org/blender.git - Unnamed repository; edit this file 'description' to name the repository.
summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp')
-rw-r--r--extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp534
1 files changed, 477 insertions, 57 deletions
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
index 0910f8f6abb..a9c0b33b3a3 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
@@ -20,8 +20,8 @@ subject to the following restrictions:
#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
#include "LinearMath/btQuickprof.h"
#include "btMultiBodyConstraint.h"
-
-
+#include "LinearMath/btIDebugDraw.h"
+#include "LinearMath/btSerializer.h"
void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, short group, short mask)
@@ -364,7 +364,9 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;
btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
- btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;
+ btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;
+
+ //printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
m_solver->solveMultiBodyGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo,m_debugDrawer,m_dispatcher);
m_bodies.resize(0);
@@ -392,11 +394,20 @@ btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld ()
delete m_solverMultiBodyIslandCallback;
}
+void btMultiBodyDynamicsWorld::forwardKinematics()
+{
+ btAlignedObjectArray<btQuaternion> world_to_local;
+ btAlignedObjectArray<btVector3> local_origin;
-
-
+ for (int b=0;b<m_multiBodies.size();b++)
+ {
+ btMultiBody* bod = m_multiBodies[b];
+ bod->forwardKinematics(world_to_local,local_origin);
+ }
+}
void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
+ forwardKinematics();
btAlignedObjectArray<btScalar> scratch_r;
btAlignedObjectArray<btVector3> scratch_v;
@@ -430,9 +441,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
/// solve all the constraints for this island
m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback);
-
+#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
{
- BT_PROFILE("btMultiBody addForce and stepVelocities");
+ BT_PROFILE("btMultiBody addForce");
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
@@ -451,27 +462,267 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
if (!isSleeping)
{
- scratch_r.resize(bod->getNumLinks()+1);
+ //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
+ scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
scratch_v.resize(bod->getNumLinks()+1);
scratch_m.resize(bod->getNumLinks()+1);
- bod->clearForcesAndTorques();
bod->addBaseForce(m_gravity * bod->getBaseMass());
for (int j = 0; j < bod->getNumLinks(); ++j)
{
bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
}
+ }//if (!isSleeping)
+ }
+ }
+#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+
- bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
- }
+ {
+ BT_PROFILE("btMultiBody stepVelocities");
+ for (int i=0;i<this->m_multiBodies.size();i++)
+ {
+ btMultiBody* bod = m_multiBodies[i];
+
+ bool isSleeping = false;
+
+ if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+ {
+ isSleeping = true;
+ }
+ for (int b=0;b<bod->getNumLinks();b++)
+ {
+ if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
+ isSleeping = true;
+ }
+
+ if (!isSleeping)
+ {
+ //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
+ scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
+ scratch_v.resize(bod->getNumLinks()+1);
+ scratch_m.resize(bod->getNumLinks()+1);
+ bool doNotUpdatePos = false;
+
+ {
+ if(!bod->isUsingRK4Integration())
+ {
+ bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
+ }
+ else
+ {
+ //
+ int numDofs = bod->getNumDofs() + 6;
+ int numPosVars = bod->getNumPosVars() + 7;
+ btAlignedObjectArray<btScalar> scratch_r2; scratch_r2.resize(2*numPosVars + 8*numDofs);
+ //convenience
+ btScalar *pMem = &scratch_r2[0];
+ btScalar *scratch_q0 = pMem; pMem += numPosVars;
+ btScalar *scratch_qx = pMem; pMem += numPosVars;
+ btScalar *scratch_qd0 = pMem; pMem += numDofs;
+ btScalar *scratch_qd1 = pMem; pMem += numDofs;
+ btScalar *scratch_qd2 = pMem; pMem += numDofs;
+ btScalar *scratch_qd3 = pMem; pMem += numDofs;
+ btScalar *scratch_qdd0 = pMem; pMem += numDofs;
+ btScalar *scratch_qdd1 = pMem; pMem += numDofs;
+ btScalar *scratch_qdd2 = pMem; pMem += numDofs;
+ btScalar *scratch_qdd3 = pMem; pMem += numDofs;
+ btAssert((pMem - (2*numPosVars + 8*numDofs)) == &scratch_r2[0]);
+
+ /////
+ //copy q0 to scratch_q0 and qd0 to scratch_qd0
+ scratch_q0[0] = bod->getWorldToBaseRot().x();
+ scratch_q0[1] = bod->getWorldToBaseRot().y();
+ scratch_q0[2] = bod->getWorldToBaseRot().z();
+ scratch_q0[3] = bod->getWorldToBaseRot().w();
+ scratch_q0[4] = bod->getBasePos().x();
+ scratch_q0[5] = bod->getBasePos().y();
+ scratch_q0[6] = bod->getBasePos().z();
+ //
+ for(int link = 0; link < bod->getNumLinks(); ++link)
+ {
+ for(int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
+ scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];
+ }
+ //
+ for(int dof = 0; dof < numDofs; ++dof)
+ scratch_qd0[dof] = bod->getVelocityVector()[dof];
+ ////
+ struct
+ {
+ btMultiBody *bod;
+ btScalar *scratch_qx, *scratch_q0;
+
+ void operator()()
+ {
+ for(int dof = 0; dof < bod->getNumPosVars() + 7; ++dof)
+ scratch_qx[dof] = scratch_q0[dof];
+ }
+ } pResetQx = {bod, scratch_qx, scratch_q0};
+ //
+ struct
+ {
+ void operator()(btScalar dt, const btScalar *pDer, const btScalar *pCurVal, btScalar *pVal, int size)
+ {
+ for(int i = 0; i < size; ++i)
+ pVal[i] = pCurVal[i] + dt * pDer[i];
+ }
+
+ } pEulerIntegrate;
+ //
+ struct
+ {
+ void operator()(btMultiBody *pBody, const btScalar *pData)
+ {
+ btScalar *pVel = const_cast<btScalar*>(pBody->getVelocityVector());
+
+ for(int i = 0; i < pBody->getNumDofs() + 6; ++i)
+ pVel[i] = pData[i];
+
+ }
+ } pCopyToVelocityVector;
+ //
+ struct
+ {
+ void operator()(const btScalar *pSrc, btScalar *pDst, int start, int size)
+ {
+ for(int i = 0; i < size; ++i)
+ pDst[i] = pSrc[start + i];
+ }
+ } pCopy;
+ //
+
+ btScalar h = solverInfo.m_timeStep;
+ #define output &scratch_r[bod->getNumDofs()]
+ //calc qdd0 from: q0 & qd0
+ bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m);
+ pCopy(output, scratch_qdd0, 0, numDofs);
+ //calc q1 = q0 + h/2 * qd0
+ pResetQx();
+ bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd0);
+ //calc qd1 = qd0 + h/2 * qdd0
+ pEulerIntegrate(btScalar(.5)*h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
+ //
+ //calc qdd1 from: q1 & qd1
+ pCopyToVelocityVector(bod, scratch_qd1);
+ bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m);
+ pCopy(output, scratch_qdd1, 0, numDofs);
+ //calc q2 = q0 + h/2 * qd1
+ pResetQx();
+ bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd1);
+ //calc qd2 = qd0 + h/2 * qdd1
+ pEulerIntegrate(btScalar(.5)*h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
+ //
+ //calc qdd2 from: q2 & qd2
+ pCopyToVelocityVector(bod, scratch_qd2);
+ bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m);
+ pCopy(output, scratch_qdd2, 0, numDofs);
+ //calc q3 = q0 + h * qd2
+ pResetQx();
+ bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
+ //calc qd3 = qd0 + h * qdd2
+ pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
+ //
+ //calc qdd3 from: q3 & qd3
+ pCopyToVelocityVector(bod, scratch_qd3);
+ bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m);
+ pCopy(output, scratch_qdd3, 0, numDofs);
+
+ //
+ //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
+ //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)
+ btAlignedObjectArray<btScalar> delta_q; delta_q.resize(numDofs);
+ btAlignedObjectArray<btScalar> delta_qd; delta_qd.resize(numDofs);
+ for(int i = 0; i < numDofs; ++i)
+ {
+ delta_q[i] = h/btScalar(6.)*(scratch_qd0[i] + 2*scratch_qd1[i] + 2*scratch_qd2[i] + scratch_qd3[i]);
+ delta_qd[i] = h/btScalar(6.)*(scratch_qdd0[i] + 2*scratch_qdd1[i] + 2*scratch_qdd2[i] + scratch_qdd3[i]);
+ //delta_q[i] = h*scratch_qd0[i];
+ //delta_qd[i] = h*scratch_qdd0[i];
+ }
+ //
+ pCopyToVelocityVector(bod, scratch_qd0);
+ bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);
+ //
+ if(!doNotUpdatePos)
+ {
+ btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
+ pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
+
+ for(int i = 0; i < numDofs; ++i)
+ pRealBuf[i] = delta_q[i];
+
+ //bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
+ bod->setPosUpdated(true);
+ }
+
+ //ugly hack which resets the cached data to t0 (needed for constraint solver)
+ {
+ for(int link = 0; link < bod->getNumLinks(); ++link)
+ bod->getLink(link).updateCacheMultiDof();
+ bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, scratch_r, scratch_v, scratch_m);
+ }
+
+ }
+ }
+
+#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+ bod->clearForcesAndTorques();
+#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+ }//if (!isSleeping)
}
}
+ clearMultiBodyConstraintForces();
+
m_solverMultiBodyIslandCallback->processConstraints();
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
+ {
+ BT_PROFILE("btMultiBody stepVelocities");
+ for (int i=0;i<this->m_multiBodies.size();i++)
+ {
+ btMultiBody* bod = m_multiBodies[i];
+
+ bool isSleeping = false;
+
+ if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+ {
+ isSleeping = true;
+ }
+ for (int b=0;b<bod->getNumLinks();b++)
+ {
+ if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
+ isSleeping = true;
+ }
+
+ if (!isSleeping)
+ {
+ //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
+ scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
+ scratch_v.resize(bod->getNumLinks()+1);
+ scratch_m.resize(bod->getNumLinks()+1);
+
+
+ {
+ if(!bod->isUsingRK4Integration())
+ {
+ bool isConstraintPass = true;
+ bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m, isConstraintPass);
+ }
+ }
+ }
+ }
+ }
+
+ for (int i=0;i<this->m_multiBodies.size();i++)
+ {
+ btMultiBody* bod = m_multiBodies[i];
+ bod->processDeltaVeeMultiDof2();
+ }
+
}
void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
@@ -503,76 +754,245 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
{
int nLinks = bod->getNumLinks();
- ///base + num links
+ ///base + num m_links
+
+
+ {
+ if(!bod->isPosUpdated())
+ 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->setPosUpdated(false);
+ }
+ }
+
world_to_local.resize(nLinks+1);
local_origin.resize(nLinks+1);
- bod->stepPositions(timeStep);
+ bod->updateCollisionObjectWorldTransforms(world_to_local,local_origin);
+
+ } else
+ {
+ bod->clearVelocities();
+ }
+ }
+ }
+}
-
- world_to_local[0] = bod->getWorldToBaseRot();
- local_origin[0] = bod->getBasePos();
- if (bod->getBaseCollider())
- {
- btVector3 posr = local_origin[0];
- float pos[4]={posr.x(),posr.y(),posr.z(),1};
- float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
- btTransform tr;
- tr.setIdentity();
- tr.setOrigin(posr);
- tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
+void btMultiBodyDynamicsWorld::addMultiBodyConstraint( btMultiBodyConstraint* constraint)
+{
+ m_multiBodyConstraints.push_back(constraint);
+}
- bod->getBaseCollider()->setWorldTransform(tr);
+void btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* constraint)
+{
+ m_multiBodyConstraints.remove(constraint);
+}
- }
-
- for (int k=0;k<bod->getNumLinks();k++)
- {
- const int parent = bod->getParent(k);
- world_to_local[k+1] = bod->getParentToLocalRot(k) * world_to_local[parent+1];
- local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , bod->getRVector(k)));
- }
+void btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint)
+{
+ constraint->debugDraw(getDebugDrawer());
+}
- for (int m=0;m<bod->getNumLinks();m++)
- {
- btMultiBodyLinkCollider* col = bod->getLink(m).m_collider;
- if (col)
- {
- int link = col->m_link;
- btAssert(link == m);
+void btMultiBodyDynamicsWorld::debugDrawWorld()
+{
+ BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld");
- int index = link+1;
+ bool drawConstraints = false;
+ if (getDebugDrawer())
+ {
+ int mode = getDebugDrawer()->getDebugMode();
+ if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
+ {
+ drawConstraints = true;
+ }
- btVector3 posr = local_origin[index];
- float pos[4]={posr.x(),posr.y(),posr.z(),1};
- float quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
- btTransform tr;
- tr.setIdentity();
- tr.setOrigin(posr);
- tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
+ if (drawConstraints)
+ {
+ BT_PROFILE("btMultiBody debugDrawWorld");
+
+ btAlignedObjectArray<btQuaternion> world_to_local1;
+ btAlignedObjectArray<btVector3> local_origin1;
- col->setWorldTransform(tr);
+ for (int c=0;c<m_multiBodyConstraints.size();c++)
+ {
+ btMultiBodyConstraint* constraint = m_multiBodyConstraints[c];
+ debugDrawMultiBodyConstraint(constraint);
+ }
+
+ for (int b = 0; b<m_multiBodies.size(); b++)
+ {
+ btMultiBody* bod = m_multiBodies[b];
+ bod->forwardKinematics(world_to_local1,local_origin1);
+
+ getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
+
+
+ for (int m = 0; m<bod->getNumLinks(); m++)
+ {
+
+ const btTransform& tr = bod->getLink(m).m_cachedWorldTransform;
+
+ getDebugDrawer()->drawTransform(tr, 0.1);
+
+ //draw the joint axis
+ if (bod->getLink(m).m_jointType==btMultibodyLink::eRevolute)
+ {
+ btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_topVec);
+
+ btVector4 color(0,0,0,1);//1,1,1);
+ btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
+ btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
+ getDebugDrawer()->drawLine(from,to,color);
+ }
+ if (bod->getLink(m).m_jointType==btMultibodyLink::eFixed)
+ {
+ btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
+
+ btVector4 color(0,0,0,1);//1,1,1);
+ btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
+ btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
+ getDebugDrawer()->drawLine(from,to,color);
+ }
+ if (bod->getLink(m).m_jointType==btMultibodyLink::ePrismatic)
+ {
+ btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
+
+ btVector4 color(0,0,0,1);//1,1,1);
+ btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
+ btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
+ getDebugDrawer()->drawLine(from,to,color);
}
+
}
- } else
- {
- bod->clearVelocities();
}
}
}
+
+ btDiscreteDynamicsWorld::debugDrawWorld();
}
-void btMultiBodyDynamicsWorld::addMultiBodyConstraint( btMultiBodyConstraint* constraint)
+void btMultiBodyDynamicsWorld::applyGravity()
{
- m_multiBodyConstraints.push_back(constraint);
+ btDiscreteDynamicsWorld::applyGravity();
+#ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+ BT_PROFILE("btMultiBody addGravity");
+ for (int i=0;i<this->m_multiBodies.size();i++)
+ {
+ btMultiBody* bod = m_multiBodies[i];
+
+ bool isSleeping = false;
+
+ if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+ {
+ isSleeping = true;
+ }
+ for (int b=0;b<bod->getNumLinks();b++)
+ {
+ if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
+ isSleeping = true;
+ }
+
+ if (!isSleeping)
+ {
+ bod->addBaseForce(m_gravity * bod->getBaseMass());
+
+ for (int j = 0; j < bod->getNumLinks(); ++j)
+ {
+ bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
+ }
+ }//if (!isSleeping)
+ }
+#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
}
-void btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* constraint)
+void btMultiBodyDynamicsWorld::clearMultiBodyConstraintForces()
+{
+ for (int i=0;i<this->m_multiBodies.size();i++)
+ {
+ btMultiBody* bod = m_multiBodies[i];
+ bod->clearConstraintForces();
+ }
+}
+void btMultiBodyDynamicsWorld::clearMultiBodyForces()
{
- m_multiBodyConstraints.remove(constraint);
+ {
+ BT_PROFILE("clearMultiBodyForces");
+ for (int i=0;i<this->m_multiBodies.size();i++)
+ {
+ btMultiBody* bod = m_multiBodies[i];
+
+ bool isSleeping = false;
+
+ if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+ {
+ isSleeping = true;
+ }
+ for (int b=0;b<bod->getNumLinks();b++)
+ {
+ if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
+ isSleeping = true;
+ }
+
+ if (!isSleeping)
+ {
+ btMultiBody* bod = m_multiBodies[i];
+ bod->clearForcesAndTorques();
+ }
+ }
+ }
+
}
+void btMultiBodyDynamicsWorld::clearForces()
+{
+ btDiscreteDynamicsWorld::clearForces();
+
+#ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+ clearMultiBodyForces();
+#endif
+}
+
+
+
+
+void btMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
+{
+
+ serializer->startSerialization();
+
+ serializeDynamicsWorldInfo( serializer);
+
+ serializeMultiBodies(serializer);
+
+ serializeRigidBodies(serializer);
+
+ serializeCollisionObjects(serializer);
+
+ serializer->finishSerialization();
+}
+
+void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer)
+{
+ int i;
+ //serialize all collision objects
+ for (i=0;i<m_multiBodies.size();i++)
+ {
+ btMultiBody* mb = m_multiBodies[i];
+ {
+ int len = mb->calculateSerializeBufferSize();
+ btChunk* chunk = serializer->allocate(len,1);
+ const char* structType = mb->serialize(chunk->m_oldPtr, serializer);
+ serializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb);
+ }
+ }
+
+} \ No newline at end of file