void	btDiscreteDynamicsWorld::debugDrawWorld()
{
	BT_PROFILE("debugDrawWorld");

	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
	{
		int numManifolds = getDispatcher()->getNumManifolds();
		btVector3 color(0,0,0);
		for (int i=0;i<numManifolds;i++)
		{
			btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
			//btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
			//btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());

			int numContacts = contactManifold->getNumContacts();
			for (int j=0;j<numContacts;j++)
			{
				btManifoldPoint& cp = contactManifold->getContactPoint(j);
				getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
			}
		}
	}
	bool drawConstraints = false;
	if (getDebugDrawer())
	{
		int mode = getDebugDrawer()->getDebugMode();
		if(mode  & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
		{
			drawConstraints = true;
		}
	}
	if(drawConstraints)
	{
		for(int i = getNumConstraints()-1; i>=0 ;i--)
		{
			btTypedConstraint* constraint = getConstraint(i);
			debugDrawConstraint(constraint);
		}
	}



	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb))
	{
		int i;

		for (  i=0;i<m_collisionObjects.size();i++)
		{
			btCollisionObject* colObj = m_collisionObjects[i];
			if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
			{
				btVector3 color(btScalar(255.),btScalar(255.),btScalar(255.));
				switch(colObj->getActivationState())
				{
				case  ACTIVE_TAG:
					color = btVector3(btScalar(255.),btScalar(255.),btScalar(255.)); break;
				case ISLAND_SLEEPING:
					color =  btVector3(btScalar(0.),btScalar(255.),btScalar(0.));break;
				case WANTS_DEACTIVATION:
					color = btVector3(btScalar(0.),btScalar(255.),btScalar(255.));break;
				case DISABLE_DEACTIVATION:
					color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));break;
				case DISABLE_SIMULATION:
					color = btVector3(btScalar(255.),btScalar(255.),btScalar(0.));break;
				default:
					{
						color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));
					}
				};

				debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
			}
			if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
			{
				btVector3 minAabb,maxAabb;
				btVector3 colorvec(1,0,0);
				colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
				m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
			}

		}
	
		if (getDebugDrawer() && getDebugDrawer()->getDebugMode())
		{
			for (i=0;i<m_actions.size();i++)
			{
				m_actions[i]->debugDraw(m_debugDrawer);
			}
		}
	}
}
void	btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
{
	BT_PROFILE("createPredictiveContacts");
	
	{
		BT_PROFILE("release predictive contact manifolds");

		for (int i=0;i<m_predictiveManifolds.size();i++)
		{
			btPersistentManifold* manifold = m_predictiveManifolds[i];
			this->m_dispatcher1->releaseManifold(manifold);
		}
		m_predictiveManifolds.clear();
	}

	btTransform predictedTrans;
	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
	{
		btRigidBody* body = m_nonStaticRigidBodies[i];
		body->setHitFraction(1.f);

		if (body->isActive() && (!body->isStaticOrKinematicObject()))
		{

			body->predictIntegratedTransform(timeStep, predictedTrans);
			
			btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();

			if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
			{
				BT_PROFILE("predictive convexSweepTest");
				if (body->getCollisionShape()->isConvex())
				{
					gNumClampedCcdMotions++;
#ifdef PREDICTIVE_CONTACT_USE_STATIC_ONLY
					class StaticOnlyCallback : public btClosestNotMeConvexResultCallback
					{
					public:

						StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : 
						  btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher)
						{
						}

					  	virtual bool needsCollision(btBroadphaseProxy* proxy0) const
						{
							btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
							if (!otherObj->isStaticOrKinematicObject())
								return false;
							return btClosestNotMeConvexResultCallback::needsCollision(proxy0);
						}
					};

					StaticOnlyCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
#else
					btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
#endif
					//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
					btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
					sweepResults.m_allowedPenetration=getDispatchInfo().m_allowedCcdPenetration;

					sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
					sweepResults.m_collisionFilterMask  = body->getBroadphaseProxy()->m_collisionFilterMask;
					btTransform modifiedPredictedTrans = predictedTrans;
					modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis());

					convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);
					if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
					{
					
						btVector3 distVec = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin())*sweepResults.m_closestHitFraction;
						btScalar distance = distVec.dot(-sweepResults.m_hitNormalWorld);

						
						btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body,sweepResults.m_hitCollisionObject);
						m_predictiveManifolds.push_back(manifold);
						
						btVector3 worldPointB = body->getWorldTransform().getOrigin()+distVec;
						btVector3 localPointB = sweepResults.m_hitCollisionObject->getWorldTransform().inverse()*worldPointB;

						btManifoldPoint newPoint(btVector3(0,0,0), localPointB,sweepResults.m_hitNormalWorld,distance);

						bool isPredictive = true;
						int index = manifold->addManifoldPoint(newPoint, isPredictive);
						btManifoldPoint& pt = manifold->getContactPoint(index);
						pt.m_combinedRestitution = 0;
						pt.m_combinedFriction = btManifoldResult::calculateCombinedFriction(body,sweepResults.m_hitCollisionObject);
						pt.m_positionWorldOnA = body->getWorldTransform().getOrigin();
						pt.m_positionWorldOnB = worldPointB;

					}
				}
			}
		}
	}
}
void OpenGLExampleBrowser::update(float deltaTime)
{

		assert(glGetError()==GL_NO_ERROR);
		s_instancingRenderer->init();
        DrawGridData dg;
        dg.upAxis = s_app->getUpAxis();

        {
            BT_PROFILE("Update Camera and Light");

	

            s_instancingRenderer->updateCamera(dg.upAxis);
        }

		if (renderGrid)
        {
            BT_PROFILE("Draw Grid");
			glPolygonOffset(3.0, 3);
			glEnable(GL_POLYGON_OFFSET_FILL);
            s_app->drawGrid(dg);
			
        }
		static int frameCount = 0;
		frameCount++;

		if (0)
		{
            BT_PROFILE("Draw frame counter");
            char bla[1024];
            sprintf(bla,"Frame %d", frameCount);
            s_app->drawText(bla,10,10);
		}

		
		if (sCurrentDemo)
		{
			if (!pauseSimulation)
			{
				//printf("---------------------------------------------------\n");
				//printf("Framecount = %d\n",frameCount);

				if (gPngFileName)
				{
					
					static int skip = 0;
					skip++;
					if (skip>4)
					{
						skip=0;
						//printf("gPngFileName=%s\n",gPngFileName);
						static int s_frameCount = 100;
						
						sprintf(staticPngFileName,"%s%d.png",gPngFileName,s_frameCount++);
						//b3Printf("Made screenshot %s",staticPngFileName);
						s_app->dumpNextFrameToPng(staticPngFileName);
						 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
					}
				}
				

				if (gFixedTimeStep>0)
				{
					sCurrentDemo->stepSimulation(gFixedTimeStep);
				} else
				{
					sCurrentDemo->stepSimulation(deltaTime);//1./60.f);
				}
			}
			
			if (renderVisualGeometry && ((gDebugDrawFlags&btIDebugDraw::DBG_DrawWireframe)==0))
            {
				if (visualWireframe)
				{
					glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
				}
                BT_PROFILE("Render Scene");
                sCurrentDemo->renderScene();
            }
            {
				
				glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
                sCurrentDemo->physicsDebugDraw(gDebugDrawFlags);
            }
		}

		{
			
			if (s_guiHelper && s_guiHelper->getRenderInterface() && s_guiHelper->getRenderInterface()->getActiveCamera())
			{
				char msg[1024];
				float camDist = s_guiHelper->getRenderInterface()->getActiveCamera()->getCameraDistance();
				float pitch = s_guiHelper->getRenderInterface()->getActiveCamera()->getCameraPitch();
				float yaw = s_guiHelper->getRenderInterface()->getActiveCamera()->getCameraYaw();
				float camTarget[3];
				s_guiHelper->getRenderInterface()->getActiveCamera()->getCameraTargetPosition(camTarget);
				sprintf(msg,"dist=%f, pitch=%f, yaw=%f,target=%f,%f,%f", camDist,pitch,yaw,camTarget[0],camTarget[1],camTarget[2]);
				gui->setStatusBarMessage(msg, true);	
			}
			
		}

		static int toggle = 1;
		if (renderGui)
		{
            if (!pauseSimulation)
                processProfileData(s_profWindow,false);

            if (sUseOpenGL2)
			{
					
				saveOpenGLState(s_instancingRenderer->getScreenWidth(),s_instancingRenderer->getScreenHeight());
			}
            BT_PROFILE("Draw Gwen GUI");
            gui->draw(s_instancingRenderer->getScreenWidth(),s_instancingRenderer->getScreenHeight());
            if (sUseOpenGL2)
            {
                restoreOpenGLState();
            }

		}
	
	
	
				
		toggle=1-toggle;
        {
            BT_PROFILE("Sync Parameters");
            s_parameterInterface->syncParameters();
        }
        {
            BT_PROFILE("Swap Buffers");
            s_app->swapBuffer();
        }
	
		gui->forceUpdateScrollBars();

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

}
void	btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
{
	btDiscreteDynamicsWorld::integrateTransforms(timeStep);

	{
		BT_PROFILE("btMultiBody stepPositions");
		//integrate and update the Featherstone hierarchies
		btAlignedObjectArray<btQuaternion> world_to_local;
		btAlignedObjectArray<btVector3> local_origin;

		for (int b=0;b<m_multiBodies.size();b++)
		{
			btMultiBody* bod = m_multiBodies[b];
			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)
			{
				int nLinks = bod->getNumLinks();

				///base + num links
				world_to_local.resize(nLinks+1);
				local_origin.resize(nLinks+1);

				bod->stepPositions(timeStep);

	 

				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]));

					bod->getBaseCollider()->setWorldTransform(tr);

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


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

						int index = link+1;

						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]));

						col->setWorldTransform(tr);
					}
				}
			} else
			{
				bod->clearVelocities();
			}
		}
	}
}
void btGpu3DGridBroadphase::squeezeOverlappingPairBuff()
{
	BT_PROFILE("bt3DGrid_squeezeOverlappingPairBuff");
	btGpu_squeezeOverlappingPairBuff(m_hPairBuff, m_hPairBuffStartCurr, m_hPairScan, m_hPairOut, m_hAABB, m_numHandles);
	return;
}
void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld)
{

	BT_PROFILE("islandUnionFindAndQuickSort");
	
	btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();

	m_islandmanifold.resize(0);

	//we are going to sort the unionfind array, and store the element id in the size
	//afterwards, we clean unionfind, to make sure no-one uses it anymore
	
	getUnionFind().sortIslands();
	int numElem = getUnionFind().getNumElements();

	int endIslandIndex=1;
	int startIslandIndex;


	//update the sleeping state for bodies, if all are sleeping
	for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
	{
		int islandId = getUnionFind().getElement(startIslandIndex).m_id;
		for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
		{
		}

		//int numSleeping = 0;

		bool allSleeping = true;

		int idx;
		for (idx=startIslandIndex;idx<endIslandIndex;idx++)
		{
			int i = getUnionFind().getElement(idx).m_sz;

			btCollisionObject* colObj0 = collisionObjects[i];
			if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
			{
//				printf("error in island management\n");
			}

			btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
			if (colObj0->getIslandTag() == islandId)
			{
				if (colObj0->getActivationState()== ACTIVE_TAG)
				{
					allSleeping = false;
				}
				if (colObj0->getActivationState()== DISABLE_DEACTIVATION)
				{
					allSleeping = false;
				}
			}
		}
			

		if (allSleeping)
		{
			int idx;
			for (idx=startIslandIndex;idx<endIslandIndex;idx++)
			{
				int i = getUnionFind().getElement(idx).m_sz;
				btCollisionObject* colObj0 = collisionObjects[i];
				if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
				{
//					printf("error in island management\n");
				}

				btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));

				if (colObj0->getIslandTag() == islandId)
				{
					colObj0->setActivationState( ISLAND_SLEEPING );
				}
			}
		} else
		{

			int idx;
			for (idx=startIslandIndex;idx<endIslandIndex;idx++)
			{
				int i = getUnionFind().getElement(idx).m_sz;

				btCollisionObject* colObj0 = collisionObjects[i];
				if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
				{
//					printf("error in island management\n");
				}

				btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));

				if (colObj0->getIslandTag() == islandId)
				{
					if ( colObj0->getActivationState() == ISLAND_SLEEPING)
					{
						colObj0->setActivationState( WANTS_DEACTIVATION);
						colObj0->setDeactivationTime(0.f);
					}
				}
			}
		}
	}

	
	int i;
	int maxNumManifolds = dispatcher->getNumManifolds();

//#define SPLIT_ISLANDS 1
//#ifdef SPLIT_ISLANDS

	
//#endif //SPLIT_ISLANDS

	
	for (i=0;i<maxNumManifolds ;i++)
	{
		 btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
		 
		 btCollisionObject* colObj0 = static_cast<btCollisionObject*>(manifold->getBody0());
		 btCollisionObject* colObj1 = static_cast<btCollisionObject*>(manifold->getBody1());
		
		 ///@todo: check sleeping conditions!
		 if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
			((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
		{
		
			//kinematic objects don't merge islands, but wake up all connected objects
			if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
			{
				colObj1->activate();
			}
			if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
			{
				colObj0->activate();
			}
			if(m_splitIslands)
			{ 
				//filtering for response
				if (dispatcher->needsResponse(colObj0,colObj1))
					m_islandmanifold.push_back(manifold);
			}
		}
	}
}
void btParticlesDynamicsWorld::runCollideParticlesKernel()
{
	btAlignedObjectArray<int>	pairs;

	float particleRad = m_simParams.m_particleRad;
	float collideDist2 = (particleRad + particleRad)*(particleRad + particleRad);
	cl_int ciErrNum;
	if(m_useCpuControls[SIMSTAGE_COLLIDE_PARTICLES]->m_active)
	{	// CPU version
		int memSize = sizeof(btVector3) * m_numParticles;
		{
			BT_PROFILE("Copy from GPU");
			ciErrNum = clEnqueueReadBuffer(m_cqCommandQue, m_dSortedPos, CL_TRUE, 0, memSize, &(m_hSortedPos[0]), 0, NULL, NULL);
			oclCHECKERROR(ciErrNum, CL_SUCCESS);
			ciErrNum = clEnqueueReadBuffer(m_cqCommandQue, m_dSortedVel, CL_TRUE, 0, memSize, &(m_hSortedVel[0]), 0, NULL, NULL);
			oclCHECKERROR(ciErrNum, CL_SUCCESS);
			memSize = sizeof(btInt2) * m_numParticles;
			ciErrNum = clEnqueueReadBuffer(m_cqCommandQue, m_dPosHash, CL_TRUE, 0, memSize, &(m_hPosHash[0]), 0, NULL, NULL);
			memSize = m_numGridCells * sizeof(int);
			ciErrNum = clEnqueueReadBuffer(m_cqCommandQue, m_dCellStart, CL_TRUE, 0, memSize, &(m_hCellStart[0]), 0, NULL, NULL);
			oclCHECKERROR(ciErrNum, CL_SUCCESS);
		}

		for(int index = 0; index < m_numParticles; index++)
		{
			btVector3 posA = m_hSortedPos[index];
			btVector3 velA = m_hSortedVel[index];
			btVector3 force = btVector3(0, 0, 0);
			float particleRad = m_simParams.m_particleRad;
			float collisionDamping = m_simParams.m_collisionDamping;
			float spring = m_simParams.m_spring;
			float shear = m_simParams.m_shear;
			float attraction = m_simParams.m_attraction;
			int unsortedIndex = m_hPosHash[index].y;
			//Get address in grid
			btInt4 gridPosA = cpu_getGridPos(posA, &m_simParams);
			//Accumulate surrounding cells
			btInt4 gridPosB; 
			for(int z = -1; z <= 1; z++)
			{
				gridPosB.z = gridPosA.z + z;
				for(int y = -1; y <= 1; y++)
				{
					gridPosB.y = gridPosA.y + y;
					for(int x = -1; x <= 1; x++)
					{
						gridPosB.x = gridPosA.x + x;
						//Get start particle index for this cell
						unsigned int hashB = cpu_getPosHash(gridPosB, &m_simParams);
						int startI = m_hCellStart[hashB];
						//Skip empty cell
						if(startI < 0)
						{
							continue;
						}
						//Iterate over particles in this cell
						int endI = startI + 32;
						if(endI > m_numParticles) 
							endI = m_numParticles;

						for(int j = startI; j < endI; j++)
						{
							unsigned int hashC = m_hPosHash[j].x;
							if(hashC != hashB)
							{
								break;
							}
							if(j == index)
							{
								continue;
							}

							btPair pair;
							pair.v0[0] = index;
							pair.v0[1] = j;
							pairs.push_back(pair.value);

//							printf("index=%d, j=%d\n",index,j);
//							printf("(index=%d, j=%d) ",index,j);
							btVector3 posB = m_hSortedPos[j];
							btVector3 velB = m_hSortedVel[j];
							//Collide two spheres
							force += cpu_collideTwoParticles(	posA, posB, velA, velB, particleRad, particleRad, 
																spring, collisionDamping, shear, attraction);
						}
					}
				}
			}     
			//Write new velocity back to original unsorted location
			m_hVel[unsortedIndex] = velA + force;
		}	

//#define BRUTE_FORCE_CHECK 1
#ifdef BRUTE_FORCE_CHECK
		for(int index = 0; index < m_numParticles; index++)
		{
			btVector3 posA = m_hSortedPos[index];
			btVector3 velA = m_hSortedVel[index];
			btVector3 force = btVector3(0, 0, 0);
			int unsortedIndex = m_hPosHash[index].y;
			
			float collisionDamping = m_simParams.m_collisionDamping;
			float spring = m_simParams.m_spring;
			float shear = m_simParams.m_shear;
			float attraction = m_simParams.m_attraction;
			for(int j = 0 ; j < m_numParticles; j++)
			{
				if (index!=j)
				{
					btVector3 posB = m_hSortedPos[j];
					btVector3 velB = m_hSortedVel[j];


					btVector3  relPos = posB - posA; relPos[3] = 0.f;
					float        dist2 = (relPos[0] * relPos[0] + relPos[1] * relPos[1] + relPos[2] * relPos[2]);
					

					
					if(dist2 < collideDist2)
					{
										//Collide two spheres
						//				force += cpu_collideTwoParticles(	posA, posB, velA, velB, particleRad, particleRad, 
						//													spring, collisionDamping, shear, attraction);

						btPair pair;
						pair.v0[0] = index;
						pair.v0[1] = j;
						if (pairs.findLinearSearch(pair.value)==pairs.size())
						{
							printf("not found index=%d, j=%d\n",index,j);
						} 

										
					}
				}
			}
			//Write new velocity back to original unsorted location
			//m_hVel[unsortedIndex] = velA + force;
		}
#endif //BRUTE_FORCE_CHECK

		memSize = sizeof(btVector3) * m_numParticles;
		ciErrNum = clEnqueueWriteBuffer(m_cqCommandQue, m_dVel, CL_TRUE, 0, memSize, &(m_hVel[0]), 0, NULL, NULL);
		oclCHECKERROR(ciErrNum, CL_SUCCESS);
	}
	else
	{
		runKernelWithWorkgroupSize(PARTICLES_KERNEL_COLLIDE_PARTICLES, m_numParticles);
		cl_int ciErrNum = clFinish(m_cqCommandQue);
		oclCHECKERROR(ciErrNum, CL_SUCCESS);
	}
}
void btKinematicCharacterController::preStep( btCollisionWorld* collisionWorld )
{
    BT_PROFILE( "preStep" );

    for( int i = 0; i < 4 && recoverFromPenetration ( collisionWorld ); i++ );
}
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;

		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)
			{
				///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++;
					}
				}

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

	};

	//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];
	}

//	assert(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()->getCollisionObjectArray(),&solverCallback);

	m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc);
}
示例#11
0
int main(int argc, char* argv[])
{
		
	CommandLineArgs args(argc,argv);

	if (args.CheckCmdLineFlag("help"))
	{
		Usage();
		return 0;
	}

	args.GetCmdLineArgument("enable_interop", useInterop);
	printf("useInterop=%d\n",useInterop);

	args.GetCmdLineArgument("enable_convexheightfield", useConvexHeightfield);
	printf("enable_convexheightfield=%d\n",useConvexHeightfield);

	args.GetCmdLineArgument("enable_gpusap", useSapGpuBroadphase);
	printf("enable_gpusap=%d\n",useSapGpuBroadphase);

	args.GetCmdLineArgument("pause_simulation", pauseSimulation);
	printf("pause_simulation=%d\n",pauseSimulation);
	args.GetCmdLineArgument("x_dim", NUM_OBJECTS_X);
	args.GetCmdLineArgument("y_dim", NUM_OBJECTS_Y);
	args.GetCmdLineArgument("z_dim", NUM_OBJECTS_Z);

	args.GetCmdLineArgument("x_gap", X_GAP);
	args.GetCmdLineArgument("y_gap", Y_GAP);
	args.GetCmdLineArgument("z_gap", Z_GAP);
	printf("x_dim=%d, y_dim=%d, z_dim=%d\n",NUM_OBJECTS_X,NUM_OBJECTS_Y,NUM_OBJECTS_Z);
	printf("x_gap=%f, y_gap=%f, z_gap=%f\n",X_GAP,Y_GAP,Z_GAP);
	
	args.GetCmdLineArgument("enable_static", keepStaticObjects);
	printf("enable_static=%d\n",keepStaticObjects);	

	
	char* tmpfile = 0;
	args.GetCmdLineArgument("load_bulletfile", tmpfile );
	if (tmpfile)
		fileName = tmpfile;

	printf("load_bulletfile=%s\n",fileName);

	
	printf("\n");
#ifdef _WIN32
	Win32OpenGLWindow* window = new Win32OpenGLWindow();
#else
    MacOpenGLWindow* window = new MacOpenGLWindow();
#endif
    
	window->init(1024,768);
#ifdef _WIN32
	GLenum err = glewInit();
#endif
    
	window->startRendering();
	window->endRendering();

	GLInstancingRenderer render;

	
		


	CLPhysicsDemo demo(window);
	
	
	demo.init(-1,-1,useInterop);

	render.InitShaders();

	if (useInterop)
		demo.setupInterop();

	createScene(render, demo, useConvexHeightfield,fileName);
		

	printf("numPhysicsInstances= %d\n", demo.m_numPhysicsInstances);
	printf("numDynamicPhysicsInstances= %d\n", demo.m_numDynamicPhysicsInstances);
	


	render.writeTransforms();


    
    window->startRendering();
    render.RenderScene();
    window->endRendering();
    
	while (!window->requestedExit())
	{
		CProfileManager::Reset();
		
		if (shootObject)
		{
			shootObject = false;
			
			btVector3 linVel;// = (m_cameraPosition-m_cameraTargetPosition).normalize()*-100;

			int x,y;
			window->getMouseCoordinates(x,y);
			render.getMouseDirection(&linVel[0],x,y);
			linVel.normalize();
			linVel*=100;

//			btVector3 startPos;
			demo.setObjectLinearVelocity(&linVel[0],0);
			float orn[4] = {0,0,0,1};
			float pos[4] = {m_cameraPosition[0],m_cameraPosition[1],m_cameraPosition[2],1};
			
//			demo.setObjectTransform(pos,orn,0);
			render.writeSingleTransform(pos,orn,0);
//			createScene(render, demo);
//			printf("numPhysicsInstances= %d\n", demo.m_numPhysicsInstances);
//			printf("numDynamicPhysicsInstances= %d\n", demo.m_numDynamicPhysicsInstances);
//			render.writeTransforms();
		}
		if (!pauseSimulation )
			demo.stepSimulation();


		window->startRendering();
		render.RenderScene();
		window->endRendering();

		{
			BT_PROFILE("glFinish");
			glFinish();
		}

		CProfileManager::Increment_Frame_Counter();

		

		
		
		 if (printStats && !pauseSimulation)
		 {
			static int count = 3;
			count--;
			if (count<0)
			{
				count = 100;
				CProfileManager::dumpAll();
				printf("total broadphase pairs= %d\n", numPairsTotal);
				printf("numPairsOut (culled)  = %d\n", numPairsOut);
				//printStats  = false;
			} else
			{
//				printf(".");
			}
		 }
		

	}

	
	demo.cleanup();

	render.CleanupShaders();
	window->exit();
	delete window;
	
	
	
	return 0;
}
示例#12
0
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)
		{
			BT_PROFILE("btMultiBody debugDrawWorld");
			

			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(m_scratch_world_to_local1,m_scratch_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);
					}
					
				}
			}
		}
	}

	btDiscreteDynamicsWorld::debugDrawWorld();
}
void	btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
					  btCollisionObject* collisionObject,
					  const btCollisionShape* collisionShape,
					  const btTransform& colObjWorldTransform,
					  ConvexResultCallback& resultCallback, btScalar allowedPenetration)
{
	if (collisionShape->isConvex())
	{
		//BT_PROFILE("convexSweepConvex");
		btConvexCast::CastResult castResult;
		castResult.m_allowedPenetration = allowedPenetration;
		castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar(1.);//??

		btConvexShape* convexShape = (btConvexShape*) collisionShape;
		btVoronoiSimplexSolver	simplexSolver;
		btGjkEpaPenetrationDepthSolver	gjkEpaPenetrationSolver;
		
		btContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver);
		//btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
		//btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);

		btConvexCast* castPtr = &convexCaster1;
	
	
		
		if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
		{
			//add hit
			if (castResult.m_normal.length2() > btScalar(0.0001))
			{
				if (castResult.m_fraction < resultCallback.m_closestHitFraction)
				{
					castResult.m_normal.normalize();
					btCollisionWorld::LocalConvexResult localConvexResult
								(
									collisionObject,
									0,
									castResult.m_normal,
									castResult.m_hitPoint,
									castResult.m_fraction
								);

					bool normalInWorldSpace = true;
					resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);

				}
			}
		}
	} else {
		if (collisionShape->isConcave())
		{
			if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
			{
				//BT_PROFILE("convexSweepbtBvhTriangleMesh");
				btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
				btTransform worldTocollisionObject = colObjWorldTransform.inverse();
				btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
				btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
				// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
				btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());

				//ConvexCast::CastResult
				struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
				{
					btCollisionWorld::ConvexResultCallback* m_resultCallback;
					btCollisionObject*	m_collisionObject;
					btTriangleMeshShape*	m_triangleMesh;

					BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
						btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape*	triangleMesh, const btTransform& triangleToWorld):
						btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
							m_resultCallback(resultCallback),
							m_collisionObject(collisionObject),
							m_triangleMesh(triangleMesh)
						{
						}


					virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
					{
						btCollisionWorld::LocalShapeInfo	shapeInfo;
						shapeInfo.m_shapePart = partId;
						shapeInfo.m_triangleIndex = triangleIndex;
						if (hitFraction <= m_resultCallback->m_closestHitFraction)
						{

							btCollisionWorld::LocalConvexResult convexResult
							(m_collisionObject,
								&shapeInfo,
								hitNormalLocal,
								hitPointLocal,
								hitFraction);

							bool	normalInWorldSpace = true;


							return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
						}
						return hitFraction;
					}

				};

				BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,triangleMesh, colObjWorldTransform);
				tccb.m_hitFraction = resultCallback.m_closestHitFraction;
				btVector3 boxMinLocal, boxMaxLocal;
				castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
				triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal);
			} else
			{
				//BT_PROFILE("convexSweepConcave");
				btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
				btTransform worldTocollisionObject = colObjWorldTransform.inverse();
				btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
				btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
				// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
				btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());

				//ConvexCast::CastResult
				struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
				{
					btCollisionWorld::ConvexResultCallback* m_resultCallback;
					btCollisionObject*	m_collisionObject;
					btConcaveShape*	m_triangleMesh;

					BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
						btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape*	triangleMesh, const btTransform& triangleToWorld):
						btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
							m_resultCallback(resultCallback),
							m_collisionObject(collisionObject),
							m_triangleMesh(triangleMesh)
						{
						}


					virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
					{
						btCollisionWorld::LocalShapeInfo	shapeInfo;
						shapeInfo.m_shapePart = partId;
						shapeInfo.m_triangleIndex = triangleIndex;
						if (hitFraction <= m_resultCallback->m_closestHitFraction)
						{

							btCollisionWorld::LocalConvexResult convexResult
							(m_collisionObject,
								&shapeInfo,
								hitNormalLocal,
								hitPointLocal,
								hitFraction);

							bool	normalInWorldSpace = false;

							return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
						}
						return hitFraction;
					}

				};

				BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,concaveShape, colObjWorldTransform);
				tccb.m_hitFraction = resultCallback.m_closestHitFraction;
				btVector3 boxMinLocal, boxMaxLocal;
				castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);

				btVector3 rayAabbMinLocal = convexFromLocal;
				rayAabbMinLocal.setMin(convexToLocal);
				btVector3 rayAabbMaxLocal = convexFromLocal;
				rayAabbMaxLocal.setMax(convexToLocal);
				rayAabbMinLocal += boxMinLocal;
				rayAabbMaxLocal += boxMaxLocal;
				concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal);
			}
		} else {
			///@todo : use AABB tree or other BVH acceleration structure!
			if (collisionShape->isCompound())
			{
				BT_PROFILE("convexSweepCompound");
				const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
				int i=0;
				for (i=0;i<compoundShape->getNumChildShapes();i++)
				{
					btTransform childTrans = compoundShape->getChildTransform(i);
					const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
					btTransform childWorldTrans = colObjWorldTransform * childTrans;
					// replace collision shape so that callback can determine the triangle
					btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape();
					collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
					objectQuerySingle(castShape, convexFromTrans,convexToTrans,
						collisionObject,
						childCollisionShape,
						childWorldTrans,
						resultCallback, allowedPenetration);
					// restore
					collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
				}
			}
		}
	}
}
void	SpuGatheringCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo, btDispatcher* dispatcher) 
{

	if (dispatchInfo.m_enableSPU)
	{
		m_maxNumOutstandingTasks = m_threadInterface->getNumTasks();

		{
			BT_PROFILE("processAllOverlappingPairs");

			if (!m_spuCollisionTaskProcess)
				m_spuCollisionTaskProcess = new SpuCollisionTaskProcess(m_threadInterface,m_maxNumOutstandingTasks);
		
			m_spuCollisionTaskProcess->setNumTasks(m_maxNumOutstandingTasks);
	//		printf("m_maxNumOutstandingTasks =%d\n",m_maxNumOutstandingTasks);

			m_spuCollisionTaskProcess->initialize2(dispatchInfo.m_useEpa);
			
		
			///modified version of btCollisionDispatcher::dispatchAllCollisionPairs:
			{
				btSpuCollisionPairCallback	collisionCallback(dispatchInfo,this);

				pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher);
			}
		}

		//send one big batch
		int numTotalPairs = pairCache->getNumOverlappingPairs();
		
		btBroadphasePair* pairPtr = pairCache->getOverlappingPairArrayPtr();
		int i;
		{
			int pairRange =	SPU_BATCHSIZE_BROADPHASE_PAIRS;
			if (numTotalPairs < (m_spuCollisionTaskProcess->getNumTasks()*SPU_BATCHSIZE_BROADPHASE_PAIRS))
			{
				pairRange = (numTotalPairs/m_spuCollisionTaskProcess->getNumTasks())+1;
			}

			BT_PROFILE("addWorkToTask");
			for (i=0;i<numTotalPairs;)
			{
				//Performance Hint: tweak this number during benchmarking
				
				int endIndex = (i+pairRange) < numTotalPairs ? i+pairRange : numTotalPairs;
				m_spuCollisionTaskProcess->addWorkToTask(pairPtr,i,endIndex);
				i = endIndex;
			}
		}

		{
			BT_PROFILE("PPU fallback");
			//handle PPU fallback pairs
			for (i=0;i<numTotalPairs;i++)
			{
				btBroadphasePair& collisionPair = pairPtr[i];
				if (collisionPair.m_internalTmpValue == 3)
				{
					if (collisionPair.m_algorithm)
					{
						btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
						btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;

						if (dispatcher->needsCollision(colObj0,colObj1))
						{
							btManifoldResult contactPointResult(colObj0,colObj1);
							
							if (dispatchInfo.m_dispatchFunc == 		btDispatcherInfo::DISPATCH_DISCRETE)
							{
								//discrete collision detection query
								collisionPair.m_algorithm->processCollision(colObj0,colObj1,dispatchInfo,&contactPointResult);
							} else
							{
								//continuous collision detection query, time of impact (toi)
								btScalar toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0,colObj1,dispatchInfo,&contactPointResult);
								if (dispatchInfo.m_timeOfImpact > toi)
									dispatchInfo.m_timeOfImpact = toi;

							}
						}
					}
				}
			}
		}
		{
			BT_PROFILE("flush2");
			//make sure all SPU work is done
			m_spuCollisionTaskProcess->flush2();
		}

	} else
	{
		///PPU fallback
		///!Need to make sure to clear all 'algorithms' when switching between SPU and PPU
		btCollisionDispatcher::dispatchAllCollisionPairs(pairCache,dispatchInfo,dispatcher);
	}
}
示例#15
0
void btGpu3DGridBroadphase::findPairsLarge()
{
	BT_PROFILE("bt3DGrid_findPairsLarge");
	btGpu_findPairsLarge(m_hAABB, m_hBodiesHash, m_hCellStart, m_hPairBuff, m_hPairBuffStartCurr,	m_numHandles, m_numLargeHandles);
	return;
}
void btKinematicCharacterController::playerStep( btCollisionWorld* collisionWorld, btScalar dt )
{
    BT_PROFILE( "playerStep" );

    if( !m_useWalkDirection && m_velocityTimeInterval <= btScalar( 0.0 ) )
        return;

    bool wasOnGround = onGround();

    // Handle the gravity
    //
    m_verticalVelocity -= m_gravity * dt;

    if( m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed )
        m_verticalVelocity = m_jumpSpeed;

    if( m_verticalVelocity < 0.0 && btFabs( m_verticalVelocity ) > btFabs( m_fallSpeed ) )
        m_verticalVelocity = -btFabs( m_fallSpeed );

    m_verticalOffset = m_verticalVelocity * dt;

    // This forced stepping up can cause problems when the character
    // walks (jump in fact...) under too low ceilings.
    //
    btVector3 currentPosition = externalGhostObject->getWorldTransform().getOrigin();
    btScalar currentStepOffset;

    currentPosition = stepUp( collisionWorld, currentPosition, currentStepOffset );

    // Move in the air and slide against the walls ignoring the stair steps.
    //
    if( m_useWalkDirection )
        currentPosition = stepForwardAndStrafe( collisionWorld, currentPosition, m_walkDirection );

    else
    {
        btScalar dtMoving = ( dt < m_velocityTimeInterval ) ? dt : m_velocityTimeInterval;
        m_velocityTimeInterval -= dt;

        // How far will we move while we are moving ?
        //
        btVector3 moveDirection = m_walkDirection * dtMoving;

        currentPosition = stepForwardAndStrafe( collisionWorld, currentPosition, moveDirection );
    }

    // Finally find the ground.
    //
    currentStepOffset = addFallOffset( wasOnGround, currentStepOffset, dt );

    currentPosition = stepDown( collisionWorld, currentPosition, currentStepOffset );

    // Apply the new position to the collision objects.
    //
    btTransform tranform;
    tranform = externalGhostObject->getWorldTransform();
    tranform.setOrigin( currentPosition );

    externalGhostObject->setWorldTransform( tranform );
    internalGhostObject->setWorldTransform( tranform );
}
示例#17
0
void btGpu3DGridBroadphase::computePairCacheChanges()
{
	BT_PROFILE("bt3DGrid_computePairCacheChanges");
	btGpu_computePairCacheChanges(m_hPairBuff, m_hPairBuffStartCurr, m_hPairScan, m_hAABB, m_numHandles);
	return;
}
void btFluidRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) 
{
	BT_PROFILE("FluidRigidWorld - singleStepSimulation()");
	
	//Use emitters and absorbers
	for(int i = 0; i < m_emitters.size(); ++i)
	{
		btFluidEmitter* emitter = m_emitters[i];
		emitter->emit();
	}
	//
	
	if(m_internalFluidPreTickCallback) m_internalFluidPreTickCallback(this, timeStep);
	
	//	Temporary - allow m_fluidSolver to be 0 so that btFluidHfRigidDynamicsWorld
	//	does not require a btFluidSolverSph to be specified in its constructor.
	if(!m_fluidSolver)
	{
		btDiscreteDynamicsWorld::internalSingleStepSimulation(timeStep);
		return;
	}
	
	for(int i = 0; i < m_fluids.size(); ++i) m_fluids[i]->internalClearRigidContacts();
	
	//btFluidSph-btRigidBody/btCollisionObject AABB intersections are 
	//detected here(not midphase/narrowphase), so calling removeMarkedParticles() 
	//below does not invalidate the collisions.
	btDiscreteDynamicsWorld::internalSingleStepSimulation(timeStep);
	
	for(int i = 0; i < m_fluids.size(); ++i) m_fluids[i]->removeMarkedParticles();
	
	//SPH forces
	{
		int numDefaultSolverFluids = m_tempDefaultFluids.size();
		if(numDefaultSolverFluids)
		{
			m_fluidSolver->updateGridAndCalculateSphForces(m_globalParameters, &m_tempDefaultFluids[0], numDefaultSolverFluids);
		}
		
		for(int i = 0; i < m_tempOverrideFluids.size(); ++i)
		{
			btFluidSph* fluid = m_tempOverrideFluids[i];
		
			btFluidSphSolver* overrideSolver = fluid->getOverrideSolver();
			btFluidSphParametersGlobal* overrideParameters = fluid->getOverrideParameters();
			
			btFluidSphSolver* usedSolver = (overrideSolver) ? overrideSolver : m_fluidSolver;
			btFluidSphParametersGlobal* usedGlobalParameters = (overrideParameters) ? overrideParameters : &m_globalParameters;
			
			usedSolver->updateGridAndCalculateSphForces( *usedGlobalParameters, &fluid, 1 );
		}
	}
	
	//Detect and resolve collisions, integrate
	{
		const bool USE_IMPULSE_BOUNDARY = 1; 	//Penalty force otherwise
	
		for(int i = 0; i < m_fluids.size(); ++i) 
		{
			btFluidSph* fluid = m_fluids[i];
			const btFluidSphParametersLocal& FL = fluid->getLocalParameters();
			
			if(!USE_IMPULSE_BOUNDARY) 
			{
				m_fluidRigidCollisionDetector.performNarrowphase(m_dispatcher1, m_dispatchInfo, m_globalParameters, m_fluids[i]);
				m_fluidRigidConstraintSolver.resolveCollisionsForce(m_globalParameters, m_fluids[i]);
			}
			btFluidSphSolver::applyForcesSingleFluid(m_globalParameters, fluid);
			
			if(USE_IMPULSE_BOUNDARY) 
			{
				m_fluidRigidCollisionDetector.performNarrowphase(m_dispatcher1, m_dispatchInfo, m_globalParameters, m_fluids[i]);
				m_fluidRigidConstraintSolver.resolveCollisionsImpulse(m_globalParameters, m_fluids[i]);
			}
			btFluidSphSolver::integratePositionsSingleFluid( m_globalParameters, fluid->internalGetParticles() );
		}
	}
	
	if(m_internalFluidPostTickCallback) m_internalFluidPostTickCallback(this, timeStep);
}
示例#19
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)
			{
				//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));
				}

				bool doNotUpdatePos = false;

				if(bod->isMultiDof())
				{
					if(!bod->isUsingRK4Integration())
						bod->stepVelocitiesMultiDof(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->stepVelocitiesMultiDof(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->stepVelocitiesMultiDof(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->stepVelocitiesMultiDof(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->stepVelocitiesMultiDof(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->stepVelocitiesMultiDof(0, scratch_r, scratch_v, scratch_m);
						}
						
					}
				}
				else
					bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
			}
		}
	}

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

}
void btSimulationIslandManager::buildIslands( btDispatcher* dispatcher, btCollisionWorld* collisionWorld )
{

	BT_PROFILE("islandUnionFindAndQuickSort");
	
	btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();

	//we are going to sort the unionfind array, and store the element id in the size
	//afterwards, we clean unionfind, to make sure no-one uses it anymore
	
	getUnionFind().sortIslands();
	int numElem = getUnionFind().getNumElements();

	int endIslandIndex=1;
	int startIslandIndex;

	//update the sleeping state for bodies, if all are sleeping
	for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
	{
		int islandId = getUnionFind().getElement(startIslandIndex).m_id;
		for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
		{
		}

		//int numSleeping = 0;

		bool allSleeping = true;

		int idx;
		for (idx=startIslandIndex;idx<endIslandIndex;idx++)
		{
			int i = getUnionFind().getElement(idx).m_sz;

			btCollisionObject* colObj0 = collisionObjects[i];
			if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
			{
//				printf("error in island management\n");
			}

			btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
			if (colObj0->getIslandTag() == islandId)
			{
				if (colObj0->getActivationState()== ACTIVE_TAG)
				{
					allSleeping = false;
				}
				if (colObj0->getActivationState()== DISABLE_DEACTIVATION)
				{
					allSleeping = false;
				}
			}
		}
			
		if (allSleeping)
		{
			int idx;
			for (idx=startIslandIndex;idx<endIslandIndex;idx++)
			{
				int i = getUnionFind().getElement(idx).m_sz;
				btCollisionObject* colObj0 = collisionObjects[i];
				if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
				{
//					printf("error in island management\n");
				}

				btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));

				if (colObj0->getIslandTag() == islandId)
				{
					colObj0->setActivationState( ISLAND_SLEEPING );
				}
			}
		} else
		{

			int idx;
			for (idx=startIslandIndex;idx<endIslandIndex;idx++)
			{
				int i = getUnionFind().getElement(idx).m_sz;

				btCollisionObject* colObj0 = collisionObjects[i];
				if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
				{
//					printf("error in island management\n");
				}

				btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));

				if (colObj0->getIslandTag() == islandId)
				{
					if ( colObj0->getActivationState() == ISLAND_SLEEPING)
					{
						colObj0->setActivationState( WANTS_DEACTIVATION);
						colObj0->setDeactivationTime(0.f);
					}
				}
			}
		}
	}
}
///@todo: this is random access, it can be walked 'cache friendly'!
void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback)
{
	btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();

	buildIslands(dispatcher,collisionWorld);

	int endIslandIndex=1;
	int startIslandIndex;
	int numElem = getUnionFind().getNumElements();

	BT_PROFILE("processIslands");

	if(!m_splitIslands)
	{
		btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
		int maxNumManifolds = dispatcher->getNumManifolds();
		callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
	}
	else
	{
		// Sort manifolds, based on islands
		// Sort the vector using predicate and std::sort
		//std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);

		int numManifolds = int (m_islandmanifold.size());

		//we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
		m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());

		//now process all active islands (sets of manifolds for now)

		int startManifoldIndex = 0;
		int endManifoldIndex = 1;

		//int islandId;

		

	//	printf("Start Islands\n");

		//traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
		for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
		{
			int islandId = getUnionFind().getElement(startIslandIndex).m_id;


			   bool islandSleeping = false;
	                
					for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
					{
							int i = getUnionFind().getElement(endIslandIndex).m_sz;
							btCollisionObject* colObj0 = collisionObjects[i];
							m_islandBodies.push_back(colObj0);
							if (!colObj0->isActive())
									islandSleeping = true;
					}
	                

			//find the accompanying contact manifold for this islandId
			int numIslandManifolds = 0;
			btPersistentManifold** startManifold = 0;

			if (startManifoldIndex<numManifolds)
			{
				int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]);
				if (curIslandId == islandId)
				{
					startManifold = &m_islandmanifold[startManifoldIndex];
				
					for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++)
					{

					}
					/// Process the actual simulation, only if not sleeping/deactivated
					numIslandManifolds = endManifoldIndex-startManifoldIndex;
				}

			}

			if (!islandSleeping)
			{
				callback->ProcessIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId);
	//			printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
			}
			
			if (numIslandManifolds)
			{
				startManifoldIndex = endManifoldIndex;
			}

			m_islandBodies.resize(0);
		}
	} // else if(!splitIslands) 

}
///@todo: this is random access, it can be walked 'cache friendly'!
void btSimulationIslandManager::buildAndProcessIslands( btDispatcher* dispatcher,
                                                        btCollisionWorld* collisionWorld,
                                                        btAlignedObjectArray<btTypedConstraint*>& constraints,
                                                        IslandCallback* callback
                                                        )
{
	btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();

	buildIslands(dispatcher,collisionWorld);

	BT_PROFILE("processIslands");

	if(!m_splitIslands)
	{
        btPersistentManifold** manifolds = dispatcher->getInternalManifoldPointer();
        int maxNumManifolds = dispatcher->getNumManifolds();

        for ( int i = 0; i < maxNumManifolds; i++ )
        {
            btPersistentManifold* manifold = manifolds[ i ];

            const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>( manifold->getBody0() );
            const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>( manifold->getBody1() );

            ///@todo: check sleeping conditions!
            if ( ( ( colObj0 ) && colObj0->getActivationState() != ISLAND_SLEEPING ) ||
                 ( ( colObj1 ) && colObj1->getActivationState() != ISLAND_SLEEPING ) )
            {

                //kinematic objects don't merge islands, but wake up all connected objects
                if ( colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING )
                {
                    if ( colObj0->hasContactResponse() )
                        colObj1->activate();
                }
                if ( colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING )
                {
                    if ( colObj1->hasContactResponse() )
                        colObj0->activate();
                }
            }
        }
        btTypedConstraint** constraintsPtr = constraints.size() ? &constraints[ 0 ] : NULL;
		callback->processIsland(&collisionObjects[0],
                                 collisionObjects.size(),
                                 manifolds,
                                 maxNumManifolds,
                                 constraintsPtr,
                                 constraints.size(),
                                 -1
                                 );
	}
	else
	{
        initIslandPools();

        //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
        addBodiesToIslands( collisionWorld );
        addManifoldsToIslands( dispatcher );
        addConstraintsToIslands( constraints );

        // m_activeIslands array should now contain all non-sleeping Islands, and each Island should
        // have all the necessary bodies, manifolds and constraints.

        // if we want to merge islands with small batch counts,
        if ( m_minimumSolverBatchSize > 1 )
        {
            mergeIslands();
        }
        // dispatch islands to solver
        m_islandDispatch( &m_activeIslands, callback );
	}
}
示例#23
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);

}
示例#24
0
void btGpu3DGridBroadphase::calcHashAABB()
{
	BT_PROFILE("bt3DGrid_calcHashAABB");
	btGpu_calcHashAABB(m_hAABB, m_hBodiesHash, m_numHandles);
	return;
}
int	btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
{
	startProfiling(timeStep);

	BT_PROFILE("stepSimulation");

	int numSimulationSubSteps = 0;

	if (maxSubSteps)
	{
		//fixed timestep with interpolation
		m_localTime += timeStep;
		if (m_localTime >= fixedTimeStep)
		{
			numSimulationSubSteps = int( m_localTime / fixedTimeStep);
			m_localTime -= numSimulationSubSteps * fixedTimeStep;
		}
	} else
	{
		//variable timestep
		fixedTimeStep = timeStep;
		m_localTime = timeStep;
		if (btFuzzyZero(timeStep))
		{
			numSimulationSubSteps = 0;
			maxSubSteps = 0;
		} else
		{
			numSimulationSubSteps = 1;
			maxSubSteps = 1;
		}
	}

	//process some debugging flags
	if (getDebugDrawer())
	{
		btIDebugDraw* debugDrawer = getDebugDrawer ();
		gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
	}
	if (numSimulationSubSteps)
	{

		//clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
		int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps;

		saveKinematicState(fixedTimeStep*clampedSimulationSteps);

		applyGravity();

		

		for (int i=0;i<clampedSimulationSteps;i++)
		{
			internalSingleStepSimulation(fixedTimeStep);
			synchronizeMotionStates();
		}

	} else
	{
		synchronizeMotionStates();
	}

	clearForces();

#ifndef BT_NO_PROFILE
	CProfileManager::Increment_Frame_Counter();
#endif //BT_NO_PROFILE
	
	return numSimulationSubSteps;
}
示例#26
0
void btGpu3DGridBroadphase::findCellStart()
{
	BT_PROFILE("bt3DGrid_findCellStart");
	btGpu_findCellStart(m_hBodiesHash, m_hCellStart, m_numHandles, m_params.m_numCells);
	return;
}
void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
{
	BT_PROFILE("integrateTransforms");
	btTransform predictedTrans;
	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
	{
		btRigidBody* body = m_nonStaticRigidBodies[i];
		body->setHitFraction(1.f);

		if (body->isActive() && (!body->isStaticOrKinematicObject()))
		{

			body->predictIntegratedTransform(timeStep, predictedTrans);
			
			btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();

			

			if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
			{
				BT_PROFILE("CCD motion clamping");
				if (body->getCollisionShape()->isConvex())
				{
					gNumClampedCcdMotions++;
#ifdef USE_STATIC_ONLY
					class StaticOnlyCallback : public btClosestNotMeConvexResultCallback
					{
					public:

						StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : 
						  btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher)
						{
						}

					  	virtual bool needsCollision(btBroadphaseProxy* proxy0) const
						{
							btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
							if (!otherObj->isStaticOrKinematicObject())
								return false;
							return btClosestNotMeConvexResultCallback::needsCollision(proxy0);
						}
					};

					StaticOnlyCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
#else
					btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
#endif
					//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
					btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
					sweepResults.m_allowedPenetration=getDispatchInfo().m_allowedCcdPenetration;

					sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
					sweepResults.m_collisionFilterMask  = body->getBroadphaseProxy()->m_collisionFilterMask;
					btTransform modifiedPredictedTrans = predictedTrans;
					modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis());

					convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);
					if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
					{
						
						//printf("clamped integration to hit fraction = %f\n",fraction);
						body->setHitFraction(sweepResults.m_closestHitFraction);
						body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
						body->setHitFraction(0.f);
						body->proceedToTransform( predictedTrans);

#if 0
						btVector3 linVel = body->getLinearVelocity();

						btScalar maxSpeed = body->getCcdMotionThreshold()/getSolverInfo().m_timeStep;
						btScalar maxSpeedSqr = maxSpeed*maxSpeed;
						if (linVel.length2()>maxSpeedSqr)
						{
							linVel.normalize();
							linVel*= maxSpeed;
							body->setLinearVelocity(linVel);
							btScalar ms2 = body->getLinearVelocity().length2();
							body->predictIntegratedTransform(timeStep, predictedTrans);

							btScalar sm2 = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
							btScalar smt = body->getCcdSquareMotionThreshold();
							printf("sm2=%f\n",sm2);
						}
#else
						
						//don't apply the collision response right now, it will happen next frame
						//if you really need to, you can uncomment next 3 lines. Note that is uses zero restitution.
						//btScalar appliedImpulse = 0.f;
						//btScalar depth = 0.f;
						//appliedImpulse = resolveSingleCollision(body,(btCollisionObject*)sweepResults.m_hitCollisionObject,sweepResults.m_hitPointWorld,sweepResults.m_hitNormalWorld,getSolverInfo(), depth);
						

#endif

        				continue;
					}
				}
			}
			

			body->proceedToTransform( predictedTrans);
		
		}

	}

	///this should probably be switched on by default, but it is not well tested yet
	if (m_applySpeculativeContactRestitution)
	{
		BT_PROFILE("apply speculative contact restitution");
		for (int i=0;i<m_predictiveManifolds.size();i++)
		{
			btPersistentManifold* manifold = m_predictiveManifolds[i];
			btRigidBody* body0 = btRigidBody::upcast((btCollisionObject*)manifold->getBody0());
			btRigidBody* body1 = btRigidBody::upcast((btCollisionObject*)manifold->getBody1());
			
			for (int p=0;p<manifold->getNumContacts();p++)
			{
				const btManifoldPoint& pt = manifold->getContactPoint(p);
				btScalar combinedRestitution = btManifoldResult::calculateCombinedRestitution(body0, body1);

				if (combinedRestitution>0 && pt.m_appliedImpulse != 0.f)
				//if (pt.getDistance()>0 && combinedRestitution>0 && pt.m_appliedImpulse != 0.f)
				{
					btVector3 imp = -pt.m_normalWorldOnB * pt.m_appliedImpulse* combinedRestitution;
				
					const btVector3& pos1 = pt.getPositionWorldOnA();
					const btVector3& pos2 = pt.getPositionWorldOnB();

					btVector3 rel_pos0 = pos1 - body0->getWorldTransform().getOrigin(); 
					btVector3 rel_pos1 = pos2 - body1->getWorldTransform().getOrigin();

					if (body0)
						body0->applyImpulse(imp,rel_pos0);
					if (body1)
						body1->applyImpulse(-imp,rel_pos1);
				}
			}
		}
	}
	
}
示例#28
0
void btGpu3DGridBroadphase::findOverlappingPairs()
{
	BT_PROFILE("bt3DGrid_findOverlappingPairs");
	btGpu_findOverlappingPairs(m_hAABB, m_hBodiesHash, m_hCellStart, m_hPairBuff, m_hPairBuffStartCurr, m_numHandles);
	return;
}
示例#29
0
void Solver::solveContactConstraint(  const btOpenCLArray<RigidBodyBase::Body>* bodyBuf, const btOpenCLArray<RigidBodyBase::Inertia>* shapeBuf, 
			btOpenCLArray<Constraint4>* constraint, void* additionalData, int n ,int maxNumBatches)
{
	
	
	btInt4 cdata = btMakeInt4( n, 0, 0, 0 );
	{
		
		const int nn = N_SPLIT*N_SPLIT;

		cdata.x = 0;
		cdata.y = maxNumBatches;//250;


		int numWorkItems = 64*nn/N_BATCHES;
#ifdef DEBUG_ME
		SolverDebugInfo* debugInfo = new  SolverDebugInfo[numWorkItems];
		adl::btOpenCLArray<SolverDebugInfo> gpuDebugInfo(data->m_device,numWorkItems);
#endif



		{

			BT_PROFILE("m_batchSolveKernel iterations");
			for(int iter=0; iter<m_nIterations; iter++)
			{
				for(int ib=0; ib<N_BATCHES; ib++)
				{
#ifdef DEBUG_ME
					memset(debugInfo,0,sizeof(SolverDebugInfo)*numWorkItems);
					gpuDebugInfo.write(debugInfo,numWorkItems);
#endif


					cdata.z = ib;
					cdata.w = N_SPLIT;

				btLauncherCL launcher( m_queue, m_solveContactKernel );
#if 1
                    
					btBufferInfoCL bInfo[] = { 

						btBufferInfoCL( bodyBuf->getBufferCL() ), 
						btBufferInfoCL( shapeBuf->getBufferCL() ), 
						btBufferInfoCL( constraint->getBufferCL() ),
						btBufferInfoCL( m_numConstraints->getBufferCL() ), 
						btBufferInfoCL( m_offsets->getBufferCL() ) 
#ifdef DEBUG_ME
						,	btBufferInfoCL(&gpuDebugInfo)
#endif
						};

					

                    launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(btBufferInfoCL) );
					//launcher.setConst(  cdata.x );
                    launcher.setConst(  cdata.y );
                    launcher.setConst(  cdata.z );
                    launcher.setConst(  cdata.w );
                    launcher.launch1D( numWorkItems, 64 );

                    
#else
                    const char* fileName = "m_batchSolveKernel.bin";
                    FILE* f = fopen(fileName,"rb");
                    if (f)
                    {
                        int sizeInBytes=0;
                        if (fseek(f, 0, SEEK_END) || (sizeInBytes = ftell(f)) == EOF || fseek(f, 0, SEEK_SET))
                        {
                            printf("error, cannot get file size\n");
                            exit(0);
                        }
                        
                        unsigned char* buf = (unsigned char*) malloc(sizeInBytes);
                        fread(buf,sizeInBytes,1,f);
                        int serializedBytes = launcher.deserializeArgs(buf, sizeInBytes,m_context);
                        int num = *(int*)&buf[serializedBytes];
                        
                        launcher.launch1D( num);

                        //this clFinish is for testing on errors
                        clFinish(m_queue);
                    }

#endif
					

#ifdef DEBUG_ME
					clFinish(m_queue);
					gpuDebugInfo.read(debugInfo,numWorkItems);
					clFinish(m_queue);
					for (int i=0;i<numWorkItems;i++)
					{
						if (debugInfo[i].m_valInt2>0)
						{
							printf("debugInfo[i].m_valInt2 = %d\n",i,debugInfo[i].m_valInt2);
						}

						if (debugInfo[i].m_valInt3>0)
						{
							printf("debugInfo[i].m_valInt3 = %d\n",i,debugInfo[i].m_valInt3);
						}
					}
#endif //DEBUG_ME


				}
			}
		
			clFinish(m_queue);


		}

		cdata.x = 1;
		bool applyFriction=true;
		if (applyFriction)
    	{
			BT_PROFILE("m_batchSolveKernel iterations2");
			for(int iter=0; iter<m_nIterations; iter++)
			{
				for(int ib=0; ib<N_BATCHES; ib++)
				{
					cdata.z = ib;
					cdata.w = N_SPLIT;

					btBufferInfoCL bInfo[] = { 
						btBufferInfoCL( bodyBuf->getBufferCL() ), 
						btBufferInfoCL( shapeBuf->getBufferCL() ), 
						btBufferInfoCL( constraint->getBufferCL() ),
						btBufferInfoCL( m_numConstraints->getBufferCL() ), 
						btBufferInfoCL( m_offsets->getBufferCL() )
#ifdef DEBUG_ME
						,btBufferInfoCL(&gpuDebugInfo)
#endif //DEBUG_ME
					};
					btLauncherCL launcher( m_queue, m_solveFrictionKernel );
					launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(btBufferInfoCL) );
					//launcher.setConst(  cdata.x );
                    launcher.setConst(  cdata.y );
                    launcher.setConst(  cdata.z );
                    launcher.setConst(  cdata.w );
                    
					launcher.launch1D( 64*nn/N_BATCHES, 64 );
				}
			}
			clFinish(m_queue);
			
		}
#ifdef DEBUG_ME
		delete[] debugInfo;
#endif //DEBUG_ME
	}

	
}
示例#30
0
void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
{
	BT_PROFILE("integrateTransforms");
	btTransform predictedTrans;
	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
	{
		btRigidBody* body = m_nonStaticRigidBodies[i];
		body->setHitFraction(1.f);

		if (body->isActive() && (!body->isStaticOrKinematicObject()))
		{

			body->predictIntegratedTransform(timeStep, predictedTrans);
			
			btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();

			

			if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
			{
				BT_PROFILE("CCD motion clamping");
				
				// Urho3D: Rigid bodies always use a compound shape to allow offset positions, so do not check for being convex here
				//if (body->getCollisionShape()->isConvex())
				{
					gNumClampedCcdMotions++;
#ifdef USE_STATIC_ONLY
					class StaticOnlyCallback : public btClosestNotMeConvexResultCallback
					{
					public:

						StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : 
						  btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher)
						{
						}

					  	virtual bool needsCollision(btBroadphaseProxy* proxy0) const
						{
							btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
							if (!otherObj->isStaticOrKinematicObject())
								return false;
							return btClosestNotMeConvexResultCallback::needsCollision(proxy0);
						}
					};

					StaticOnlyCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
#else
					btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
#endif
					//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
					btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
					sweepResults.m_allowedPenetration=getDispatchInfo().m_allowedCcdPenetration;

					sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
					sweepResults.m_collisionFilterMask  = body->getBroadphaseProxy()->m_collisionFilterMask;
					btTransform modifiedPredictedTrans = predictedTrans;
					modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis());

					convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);
					if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
					{
						
						//printf("clamped integration to hit fraction = %f\n",fraction);
						body->setHitFraction(sweepResults.m_closestHitFraction);
						body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
						body->setHitFraction(0.f);
						body->proceedToTransform( predictedTrans);

#if 0
						btVector3 linVel = body->getLinearVelocity();

						btScalar maxSpeed = body->getCcdMotionThreshold()/getSolverInfo().m_timeStep;
						btScalar maxSpeedSqr = maxSpeed*maxSpeed;
						if (linVel.length2()>maxSpeedSqr)
						{
							linVel.normalize();
							linVel*= maxSpeed;
							body->setLinearVelocity(linVel);
							btScalar ms2 = body->getLinearVelocity().length2();
							body->predictIntegratedTransform(timeStep, predictedTrans);

							btScalar sm2 = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
							btScalar smt = body->getCcdSquareMotionThreshold();
							printf("sm2=%f\n",sm2);
						}
#else
						//response  between two dynamic objects without friction, assuming 0 penetration depth
						btScalar appliedImpulse = 0.f;
						btScalar depth = 0.f;
						appliedImpulse = resolveSingleCollision(body,sweepResults.m_hitCollisionObject,sweepResults.m_hitPointWorld,sweepResults.m_hitNormalWorld,getSolverInfo(), depth);
						

#endif

        				continue;
					}
				}
			}
			

			body->proceedToTransform( predictedTrans);
		}
	}
}