Differential D1739 Diff 5899 extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

Changeset View

Standalone View

extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

Show All 14 Lines

#include "btMultiBodyDynamicsWorld.h"		#include "btMultiBodyDynamicsWorld.h"
#include "btMultiBodyConstraintSolver.h"		#include "btMultiBodyConstraintSolver.h"
#include "btMultiBody.h"		#include "btMultiBody.h"
#include "btMultiBodyLinkCollider.h"		#include "btMultiBodyLinkCollider.h"
#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"		#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
#include "LinearMath/btQuickprof.h"		#include "LinearMath/btQuickprof.h"
#include "btMultiBodyConstraint.h"		#include "btMultiBodyConstraint.h"
		#include "LinearMath/btIDebugDraw.h"
		#include "LinearMath/btSerializer.h"


void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, short group, short mask)		void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, short group, short mask)
{		{
m_multiBodies.push_back(body);		m_multiBodies.push_back(body);

}		}

▲ Show 20 Lines • Show All 326 Lines • ▼ Show 20 Lines	virtual void processIsland(btCollisionObject bodies,int numBodies,btPersistentManifold manifolds,int numManifolds, int islandId)
}		}
}		}
void processConstraints()		void processConstraints()
{		{

btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;		btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;
btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;		btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
btTypedConstraint** constraints = m_constraints.size()?&m_constraints[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_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);		m_bodies.resize(0);
m_manifolds.resize(0);		m_manifolds.resize(0);
m_constraints.resize(0);		m_constraints.resize(0);
m_multiBodyConstraints.resize(0);		m_multiBodyConstraints.resize(0);
}		}

};		};
Show All 10 Lines	btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver,dispatcher);		m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver,dispatcher);
}		}

btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld ()		btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld ()
{		{
delete m_solverMultiBodyIslandCallback;		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)		void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{		{
		forwardKinematics();

btAlignedObjectArray<btScalar> scratch_r;		btAlignedObjectArray<btScalar> scratch_r;
btAlignedObjectArray<btVector3> scratch_v;		btAlignedObjectArray<btVector3> scratch_v;
btAlignedObjectArray<btMatrix3x3> scratch_m;		btAlignedObjectArray<btMatrix3x3> scratch_m;


BT_PROFILE("solveConstraints");		BT_PROFILE("solveConstraints");

Show All 17 Lines	void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)


m_solverMultiBodyIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer());		m_solverMultiBodyIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer());
m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());		m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());

/// solve all the constraints for this island		/// solve all the constraints for this island
m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback);		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++)		for (int i=0;i<this->m_multiBodies.size();i++)
{		{
btMultiBody* bod = m_multiBodies[i];		btMultiBody* bod = m_multiBodies[i];

bool isSleeping = false;		bool isSleeping = false;

if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)		if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{		{
isSleeping = true;		isSleeping = true;
}		}
for (int b=0;b<bod->getNumLinks();b++)		for (int b=0;b<bod->getNumLinks();b++)
{		{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)		if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
isSleeping = true;		isSleeping = true;
}		}

if (!isSleeping)		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_v.resize(bod->getNumLinks()+1);
scratch_m.resize(bod->getNumLinks()+1);		scratch_m.resize(bod->getNumLinks()+1);

bod->clearForcesAndTorques();
bod->addBaseForce(m_gravity * bod->getBaseMass());		bod->addBaseForce(m_gravity * bod->getBaseMass());

for (int j = 0; j < bod->getNumLinks(); ++j)		for (int j = 0; j < bod->getNumLinks(); ++j)
{		{
bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));		bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
}		}
		}//if (!isSleeping)
		}
		}
		#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY


		{
		BT_PROFILE("btMultiBody stepVelocities");
		for (int i=0;i<this->m_multiBodies.size();i++)
		{
		btMultiBody* bod = m_multiBodies[i];

		bool isSleeping = false;

bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);		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(2numPosVars + 8numDofs);
		//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 - (2numPosVars + 8numDofs)) == &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] + 2scratch_qd1[i] + 2*scratch_qd2[i] + scratch_qd3[i]);
		delta_qd[i] = h/btScalar(6.)(scratch_qdd0[i] + 2scratch_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_solverMultiBodyIslandCallback->processConstraints();

m_constraintSolver->allSolved(solverInfo, m_debugDrawer);		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)		void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
{		{
btDiscreteDynamicsWorld::integrateTransforms(timeStep);		btDiscreteDynamicsWorld::integrateTransforms(timeStep);

{		{
BT_PROFILE("btMultiBody stepPositions");		BT_PROFILE("btMultiBody stepPositions");
Show All 15 Lines	for (int b=0;b<m_multiBodies.size();b++)
isSleeping = true;		isSleeping = true;
}		}


if (!isSleeping)		if (!isSleeping)
{		{
int nLinks = bod->getNumLinks();		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);		world_to_local.resize(nLinks+1);
local_origin.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())		void btMultiBodyDynamicsWorld::addMultiBodyConstraint( btMultiBodyConstraint* constraint)
{		{
btVector3 posr = local_origin[0];		m_multiBodyConstraints.push_back(constraint);
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]));

bod->getBaseCollider()->setWorldTransform(tr);		void btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* constraint)
		{
		m_multiBodyConstraints.remove(constraint);
		}

		void btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint)
		{
		constraint->debugDraw(getDebugDrawer());
}		}

for (int k=0;k<bod->getNumLinks();k++)
		void btMultiBodyDynamicsWorld::debugDrawWorld()
		{
		BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld");

		bool drawConstraints = false;
		if (getDebugDrawer())
		{
		int mode = getDebugDrawer()->getDebugMode();
		if (mode & (btIDebugDraw::DBG_DrawConstraints \| btIDebugDraw::DBG_DrawConstraintLimits))
		{
		drawConstraints = true;
		}

		if (drawConstraints)
{		{
const int parent = bod->getParent(k);		BT_PROFILE("btMultiBody debugDrawWorld");
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)));		btAlignedObjectArray<btQuaternion> world_to_local1;
		btAlignedObjectArray<btVector3> local_origin1;

		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++)		for (int m = 0; m<bod->getNumLinks(); m++)
{		{
btMultiBodyLinkCollider* col = bod->getLink(m).m_collider;
if (col)		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)
{		{
int link = col->m_link;		btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_topVec);
btAssert(link == m);

int index = link+1;		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);

btVector3 posr = local_origin[index];		btVector4 color(0,0,0,1);//1,1,1);
float pos[4]={posr.x(),posr.y(),posr.z(),1};		btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
float quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};		btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
btTransform tr;		getDebugDrawer()->drawLine(from,to,color);
tr.setIdentity();		}
tr.setOrigin(posr);		if (bod->getLink(m).m_jointType==btMultibodyLink::ePrismatic)
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));		{
		btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);

col->setWorldTransform(tr);		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		}
		}
		}

		btDiscreteDynamicsWorld::debugDrawWorld();
		}



		void btMultiBodyDynamicsWorld::applyGravity()
{		{
bod->clearVelocities();		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::clearMultiBodyConstraintForces()
		{
		for (int i=0;i<this->m_multiBodies.size();i++)
		{
		btMultiBody* bod = m_multiBodies[i];
		bod->clearConstraintForces();
}		}
}		}
		void btMultiBodyDynamicsWorld::clearMultiBodyForces()
		{
		{
		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;
		}

void btMultiBodyDynamicsWorld::addMultiBodyConstraint( btMultiBodyConstraint* constraint)		if (!isSleeping)
{		{
m_multiBodyConstraints.push_back(constraint);		btMultiBody* bod = m_multiBodies[i];
		bod->clearForcesAndTorques();
		}
		}
}		}

void btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* constraint)		}
		void btMultiBodyDynamicsWorld::clearForces()
{		{
m_multiBodyConstraints.remove(constraint);		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