void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
	BT_PROFILE("solveConstraints");

	m_sortedConstraints.resize( m_constraints.size());
	int i;
	for (i=0;i<getNumConstraints();i++)
	{
		m_sortedConstraints[i] = m_constraints[i];
	}

//	btAssert(0);



	m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate());

	btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;

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

	/// solve all the constraints for this island
	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverIslandCallback);

	m_solverIslandCallback->processConstraints();

	m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
}
void	btDiscreteDynamicsWorld::calculateSimulationIslands()
{
	BT_PROFILE("calculateSimulationIslands");

	getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());

	{
		int i;
		int numConstraints = int(m_constraints.size());
		for (i=0;i< numConstraints ; i++ )
		{
			btTypedConstraint* constraint = m_constraints[i];

			const btRigidBody* colObj0 = &constraint->getRigidBodyA();
			const btRigidBody* colObj1 = &constraint->getRigidBodyB();

			if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
				((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
			{
				if (colObj0->isActive() || colObj1->isActive())
				{

					getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),
						(colObj1)->getIslandTag());
				}
			}
		}
	}

	//Store the island id in each body
	getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());

	
}
void	btDiscreteDynamicsWorld::calculateSimulationIslands()
{
	BT_PROFILE("calculateSimulationIslands");

	getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());

    {
        //merge islands based on speculative contact manifolds too
        for (int i=0;i<this->m_predictiveManifolds.size();i++)
        {
            btPersistentManifold* manifold = m_predictiveManifolds[i];
            
            const btCollisionObject* colObj0 = manifold->getBody0();
            const btCollisionObject* colObj1 = manifold->getBody1();
            
            if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
                ((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
            {
                if (colObj0->isActive() || colObj1->isActive())
                {
                    
                    getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),
                                                                       (colObj1)->getIslandTag());
                }
            }
        }
    }
    
	{
		int i;
		int numConstraints = int(m_constraints.size());
		for (i=0;i< numConstraints ; i++ )
		{
			btTypedConstraint* constraint = m_constraints[i];
			if (constraint->isEnabled())
			{
				const btRigidBody* colObj0 = &constraint->getRigidBodyA();
				const btRigidBody* colObj1 = &constraint->getRigidBodyB();

				if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
					((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
				{
					if (colObj0->isActive() || colObj1->isActive())
					{

						getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),
							(colObj1)->getIslandTag());
					}
				}
			}
		}
	}

	//Store the island id in each body
	getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());

	
}
Esempio n. 4
0
void	btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo)
{
	BT_PROFILE("solveConstraints");

	m_solverIslandCallbackMt->setup(&solverInfo, getDebugDrawer());
	m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());

	/// solve all the constraints for this island
    btSimulationIslandManagerMt* im = static_cast<btSimulationIslandManagerMt*>(m_islandManager);
    im->buildAndProcessIslands( getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_constraints, m_solverIslandCallbackMt );

	m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
}
    // -------------------------------------------------------------------------
    Object::~Object()
    {
        if (mRootNode)
        {
            showDebugShape(false);
            mShapeNode->detachObject (this);
            mRootNode->removeAndDestroyChild (mShapeNode->getName ());
            //mRootNode->getParentSceneNode ()->removeAndDestroyChild (mRootNode->getName ());
        }

        getBulletCollisionWorld()->removeCollisionObject( mObject );
		getCollisionWorld()->removeObject(this);

        delete mObject;        
        //delete mShape;
        delete mState;
        delete mDebugShape;
    }
void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
	BT_PROFILE("solveConstraints");
	
	struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
	{

		btContactSolverInfo&	m_solverInfo;
		btConstraintSolver*		m_solver;
		btTypedConstraint**		m_sortedConstraints;
		int						m_numConstraints;
		btIDebugDraw*			m_debugDrawer;
		btStackAlloc*			m_stackAlloc;
		btDispatcher*			m_dispatcher;
		
		btAlignedObjectArray<btCollisionObject*> m_bodies;
		btAlignedObjectArray<btPersistentManifold*> m_manifolds;
		btAlignedObjectArray<btTypedConstraint*> m_constraints;


		InplaceSolverIslandCallback(
			btContactSolverInfo& solverInfo,
			btConstraintSolver*	solver,
			btTypedConstraint** sortedConstraints,
			int	numConstraints,
			btIDebugDraw*	debugDrawer,
			btStackAlloc*			stackAlloc,
			btDispatcher* dispatcher)
			:m_solverInfo(solverInfo),
			m_solver(solver),
			m_sortedConstraints(sortedConstraints),
			m_numConstraints(numConstraints),
			m_debugDrawer(debugDrawer),
			m_stackAlloc(stackAlloc),
			m_dispatcher(dispatcher)
		{

		}


		InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other)
		{
			btAssert(0);
			(void)other;
			return *this;
		}
		virtual	void	ProcessIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold**	manifolds,int numManifolds, int islandId)
		{
			if (islandId<0)
			{
				if (numManifolds + m_numConstraints)
				{
					///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
					m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
				}
			} else
			{
					//also add all non-contact constraints/joints for this island
				btTypedConstraint** startConstraint = 0;
				int numCurConstraints = 0;
				int i;
				
				//find the first constraint for this island
				for (i=0;i<m_numConstraints;i++)
				{
					if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
					{
						startConstraint = &m_sortedConstraints[i];
						break;
					}
				}
				//count the number of constraints in this island
				for (;i<m_numConstraints;i++)
				{
					if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
					{
						numCurConstraints++;
					}
				}

				if (m_solverInfo.m_minimumSolverBatchSize<=1)
				{
					///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive
					if (numManifolds + numCurConstraints)
					{
						m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
					}
				} else
				{
					
					for (i=0;i<numBodies;i++)
						m_bodies.push_back(bodies[i]);
					for (i=0;i<numManifolds;i++)
						m_manifolds.push_back(manifolds[i]);
					for (i=0;i<numCurConstraints;i++)
						m_constraints.push_back(startConstraint[i]);
					if ((m_constraints.size()+m_manifolds.size())>m_solverInfo.m_minimumSolverBatchSize)
					{
						processConstraints();
					} else
					{
						//printf("deferred\n");
					}
				}
			}
		}
		void	processConstraints()
		{
			if (m_manifolds.size() + m_constraints.size()>0)
			{
				m_solver->solveGroup( &m_bodies[0],m_bodies.size(), &m_manifolds[0], m_manifolds.size(), &m_constraints[0], m_constraints.size() ,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
			}
			m_bodies.resize(0);
			m_manifolds.resize(0);
			m_constraints.resize(0);

		}

	};

	

	//sorted version of all btTypedConstraint, based on islandId
	btAlignedObjectArray<btTypedConstraint*>	sortedConstraints;
	sortedConstraints.resize( m_constraints.size());
	int i; 
	for (i=0;i<getNumConstraints();i++)
	{
		sortedConstraints[i] = m_constraints[i];
	}

//	btAssert(0);
		
	

	sortedConstraints.quickSort(btSortConstraintOnIslandPredicate());
	
	btTypedConstraint** constraintsPtr = getNumConstraints() ? &sortedConstraints[0] : 0;
	
	InplaceSolverIslandCallback	solverCallback(	solverInfo,	m_constraintSolver, constraintsPtr,sortedConstraints.size(),	m_debugDrawer,m_stackAlloc,m_dispatcher1);
	
	m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
	
	/// solve all the constraints for this island
	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),&solverCallback);

	solverCallback.processConstraints();

	m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc);
}
void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
	forwardKinematics();



	BT_PROFILE("solveConstraints");
	
	m_sortedConstraints.resize( m_constraints.size());
	int i; 
	for (i=0;i<getNumConstraints();i++)
	{
		m_sortedConstraints[i] = m_constraints[i];
	}
	m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
	btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;

	m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
	for (i=0;i<m_multiBodyConstraints.size();i++)
	{
		m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
	}
	m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());

	btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ?  &m_sortedMultiBodyConstraints[0] : 0;
	

	m_solverMultiBodyIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer());
	m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
	
	/// 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");
		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)
				m_scratch_r.resize(bod->getNumLinks()+1);			//multidof? ("Y"s use it and it is used to store qdd)
				m_scratch_v.resize(bod->getNumLinks()+1);
				m_scratch_m.resize(bod->getNumLinks()+1);

				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
	

	{
		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)
				m_scratch_r.resize(bod->getNumLinks()+1);			//multidof? ("Y"s use it and it is used to store qdd)
				m_scratch_v.resize(bod->getNumLinks()+1);
				m_scratch_m.resize(bod->getNumLinks()+1);
				bool doNotUpdatePos = false;

				{
					if(!bod->isUsingRK4Integration())
					{
						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_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 &m_scratch_r[bod->getNumDofs()]
						//calc qdd0 from: q0 & qd0	
						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_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., m_scratch_r, m_scratch_v, m_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., m_scratch_r, m_scratch_v, m_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., m_scratch_r, m_scratch_v, m_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, m_scratch_r, m_scratch_v, m_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)
                                m_scratch_r.resize(bod->getNumLinks()+1);                 //multidof? ("Y"s use it and it is used to store qdd)
                                m_scratch_v.resize(bod->getNumLinks()+1);
                                m_scratch_m.resize(bod->getNumLinks()+1);

                                
                            {
                                if(!bod->isUsingRK4Integration())
                                {
									bool isConstraintPass = true;
                                    bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass);
                                }
				}
			}
		}
	}

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

}
void	btMultiBodyDynamicsWorld::calculateSimulationIslands()
{
	BT_PROFILE("calculateSimulationIslands");

	getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());

    {
        //merge islands based on speculative contact manifolds too
        for (int i=0;i<this->m_predictiveManifolds.size();i++)
        {
            btPersistentManifold* manifold = m_predictiveManifolds[i];
            
            const btCollisionObject* colObj0 = manifold->getBody0();
            const btCollisionObject* colObj1 = manifold->getBody1();
            
            if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
                ((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
            {
				getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
            }
        }
    }
    
	{
		int i;
		int numConstraints = int(m_constraints.size());
		for (i=0;i< numConstraints ; i++ )
		{
			btTypedConstraint* constraint = m_constraints[i];
			if (constraint->isEnabled())
			{
				const btRigidBody* colObj0 = &constraint->getRigidBodyA();
				const btRigidBody* colObj1 = &constraint->getRigidBodyB();

				if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
					((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
				{
					getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
				}
			}
		}
	}

	//merge islands linked by Featherstone link colliders
	for (int i=0;i<m_multiBodies.size();i++)
	{
		btMultiBody* body = m_multiBodies[i];
		{
			btMultiBodyLinkCollider* prev = body->getBaseCollider();

			for (int b=0;b<body->getNumLinks();b++)
			{
				btMultiBodyLinkCollider* cur = body->getLink(b).m_collider;
				
				if (((cur) && (!(cur)->isStaticOrKinematicObject())) &&
					((prev) && (!(prev)->isStaticOrKinematicObject())))
				{
					int tagPrev = prev->getIslandTag();
					int tagCur = cur->getIslandTag();
					getSimulationIslandManager()->getUnionFind().unite(tagPrev, tagCur);
				}
				if (cur && !cur->isStaticOrKinematicObject())
					prev = cur;
				
			}
		}
	}

	//merge islands linked by multibody constraints
	{
		for (int i=0;i<this->m_multiBodyConstraints.size();i++)
		{
			btMultiBodyConstraint* c = m_multiBodyConstraints[i];
			int tagA = c->getIslandIdA();
			int tagB = c->getIslandIdB();
			if (tagA>=0 && tagB>=0)
				getSimulationIslandManager()->getUnionFind().unite(tagA, tagB);
		}
	}

	//Store the island id in each body
	getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());

}
Esempio n. 9
0
void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{

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


	BT_PROFILE("solveConstraints");
	
	m_sortedConstraints.resize( m_constraints.size());
	int i; 
	for (i=0;i<getNumConstraints();i++)
	{
		m_sortedConstraints[i] = m_constraints[i];
	}
	m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
	btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;

	m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
	for (i=0;i<m_multiBodyConstraints.size();i++)
	{
		m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
	}
	m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());

	btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ?  &m_sortedMultiBodyConstraints[0] : 0;
	

	m_solverMultiBodyIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer());
	m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
	
	/// solve all the constraints for this island
	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback);


	{
		BT_PROFILE("btMultiBody addForce and 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)
			{
				scratch_r.resize(bod->getNumLinks()+1);
				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));
				}

				bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
			}
		}
	}

	m_solverMultiBodyIslandCallback->processConstraints();
	
	m_constraintSolver->allSolved(solverInfo, m_debugDrawer);

}