Esempio n. 1
0
bool	SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,TypedConstraint** constraintsBaseAddress,BroadphasePair*	overlappingPairBaseAddress, Dispatcher* dispatcher,BroadphaseInterface* broadphase,class ConstraintSolver*	solver,float timeStep)
{


#ifdef USE_QUICKPROF

	Profiler::beginBlock("predictIntegratedTransform");
#endif //USE_QUICKPROF

	{
		//		std::vector<CcdPhysicsController*>::iterator i;



		int k;
		for (k=0;k<GetNumControllers();k++)
		{
			CcdPhysicsController* ctrl = m_controllers[k];
			//		SimdTransform predictedTrans;
			RigidBody* body = ctrl->GetRigidBody();
			//todo: only do this when necessary, it's used for contact points
			body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse();

			if (body->IsActive())
			{
				if (!body->IsStatic())
				{
					body->applyForces( timeStep);
					body->integrateVelocities( timeStep);
					body->predictIntegratedTransform(timeStep,body->m_interpolationWorldTransform);
				}
			}

		}
	}

#ifdef USE_QUICKPROF
	Profiler::endBlock("predictIntegratedTransform");
#endif //USE_QUICKPROF
	
	//BroadphaseInterface*	scene = GetBroadphase();


	//
	// collision detection (?)
	//


	#ifdef USE_QUICKPROF
	Profiler::beginBlock("DispatchAllCollisionPairs");
	#endif //USE_QUICKPROF


//	int numsubstep = m_numIterations;


	DispatcherInfo dispatchInfo;
	dispatchInfo.m_timeStep = timeStep;
	dispatchInfo.m_stepCount = 0;
	dispatchInfo.m_enableSatConvex = false;//m_enableSatCollisionDetection;
	dispatchInfo.m_debugDraw = debugDrawer;
	
	std::vector<BroadphasePair>	overlappingPairs;
	overlappingPairs.resize(this->m_overlappingPairIndices.size());

	//gather overlapping pair info
	int i;
	for (i=0;i<m_overlappingPairIndices.size();i++)
	{
		overlappingPairs[i] = overlappingPairBaseAddress[m_overlappingPairIndices[i]];
	}

	
	//pairCache->RefreshOverlappingPairs();
	if (overlappingPairs.size())
	{
		dispatcher->DispatchAllCollisionPairs(&overlappingPairs[0],overlappingPairs.size(),dispatchInfo);///numsubstep,g);
	}

	//scatter overlapping pair info, mainly the created algorithms/contact caches
	
	for (i=0;i<m_overlappingPairIndices.size();i++)
	{
		overlappingPairBaseAddress[m_overlappingPairIndices[i]] = overlappingPairs[i];
	}


	#ifdef USE_QUICKPROF
	Profiler::endBlock("DispatchAllCollisionPairs");
	#endif //USE_QUICKPROF




	int numRigidBodies = m_controllers.size();




	//contacts
	#ifdef USE_QUICKPROF
	Profiler::beginBlock("SolveConstraint");
	#endif //USE_QUICKPROF


	//solve the regular constraints (point 2 point, hinge, etc)

	for (int g=0;g<numSolverIterations;g++)
	{
		//
		// constraint solving
		//

		int i;
		int numConstraints = m_constraintIndices.size();

		//point to point constraints
		for (i=0;i< numConstraints ; i++ )
		{
			TypedConstraint* constraint = constraintsBaseAddress[m_constraintIndices[i]];
			constraint->BuildJacobian();
			constraint->SolveConstraint( timeStep );

		}


	}

	#ifdef USE_QUICKPROF
	Profiler::endBlock("SolveConstraint");
	#endif //USE_QUICKPROF

	/*

	//solve the vehicles

	#ifdef NEW_BULLET_VEHICLE_SUPPORT
	//vehicles
	int numVehicles = m_wrapperVehicles.size();
	for (int i=0;i<numVehicles;i++)
	{
	WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
	RaycastVehicle* vehicle = wrapperVehicle->GetVehicle();
	vehicle->UpdateVehicle( timeStep);
	}
	#endif //NEW_BULLET_VEHICLE_SUPPORT
*/

	/*
	
	Profiler::beginBlock("CallbackTriggers");
	#endif //USE_QUICKPROF

	CallbackTriggers();

	#ifdef USE_QUICKPROF
	Profiler::endBlock("CallbackTriggers");

	}
	*/

	//OverlappingPairCache*	scene = GetCollisionWorld()->GetPairCache();
	
	ContactSolverInfo	solverInfo;

	solverInfo.m_friction = 0.9f;
	solverInfo.m_numIterations = numSolverIterations;
	solverInfo.m_timeStep = timeStep;
	solverInfo.m_restitution = 0.f;//m_restitution;


	if (m_manifolds.size())
	{
		solver->SolveGroup( &m_manifolds[0],m_manifolds.size(),solverInfo,0);
	}


#ifdef USE_QUICKPROF
	Profiler::beginBlock("proceedToTransform");
#endif //USE_QUICKPROF
	{



		{


			UpdateAabbs(debugDrawer,broadphase,timeStep);


			float toi = 1.f;

			//experimental continuous collision detection

			/*		if (m_ccdMode == 3)
			{
				DispatcherInfo dispatchInfo;
				dispatchInfo.m_timeStep = timeStep;
				dispatchInfo.m_stepCount = 0;
				dispatchInfo.m_dispatchFunc = DispatcherInfo::DISPATCH_CONTINUOUS;

				//			GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(scene,dispatchInfo);
				toi = dispatchInfo.m_timeOfImpact;

			}
			*/



			//
			// integrating solution
			//

			{

				std::vector<CcdPhysicsController*>::iterator i;

				for (i=m_controllers.begin();
					!(i==m_controllers.end()); i++)
				{

					CcdPhysicsController* ctrl = *i;

					SimdTransform predictedTrans;
					RigidBody* body = ctrl->GetRigidBody();

					if (body->IsActive())
					{

						if (!body->IsStatic())
						{
							body->predictIntegratedTransform(timeStep*	toi, predictedTrans);
							body->proceedToTransform( predictedTrans);
						}

					}
				}

			}





			//
			// disable sleeping physics objects
			//

			std::vector<CcdPhysicsController*> m_sleepingControllers;

			std::vector<CcdPhysicsController*>::iterator i;

			for (i=m_controllers.begin();
				!(i==m_controllers.end()); i++)
			{
				CcdPhysicsController* ctrl = (*i);
				RigidBody* body = ctrl->GetRigidBody();

				ctrl->UpdateDeactivation(timeStep);


				if (ctrl->wantsSleeping())
				{
					if (body->GetActivationState() == ACTIVE_TAG)
						body->SetActivationState( WANTS_DEACTIVATION );
				} else
				{
					if (body->GetActivationState() != DISABLE_DEACTIVATION)
						body->SetActivationState( ACTIVE_TAG );
				}

				if (true)
				{
					if (body->GetActivationState() == ISLAND_SLEEPING)
					{
						m_sleepingControllers.push_back(ctrl);
					}
				} else
				{
					if (ctrl->wantsSleeping())
					{
						m_sleepingControllers.push_back(ctrl);
					}
				}
			}




		}


#ifdef USE_QUICKPROF
		Profiler::endBlock("proceedToTransform");

		Profiler::beginBlock("SyncMotionStates");
#endif //USE_QUICKPROF

		SyncMotionStates(timeStep);

#ifdef USE_QUICKPROF
		Profiler::endBlock("SyncMotionStates");

#endif //USE_QUICKPROF


#ifdef NEW_BULLET_VEHICLE_SUPPORT
		//sync wheels for vehicles
		int numVehicles = m_wrapperVehicles.size();
		for (int i=0;i<numVehicles;i++)
		{
			WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];

			wrapperVehicle->SyncWheels();
		}
#endif //NEW_BULLET_VEHICLE_SUPPORT

		return true;
	}
}
Esempio n. 2
0
/// Perform an integration step of duration 'timeStep'.
bool	CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
{


	//printf("CcdPhysicsEnvironment::proceedDeltaTime\n");

	if (SimdFuzzyZero(timeStep))
		return true;

	if (m_debugDrawer)
	{
		gDisableDeactivation = (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_NoDeactivation);
	}


#ifdef USE_QUICKPROF
	Profiler::beginBlock("SyncMotionStates");
#endif //USE_QUICKPROF


	//this is needed because scaling is not known in advance, and scaling has to propagate to the shape
	if (!m_scalingPropagated)
	{
		SyncMotionStates(timeStep);
		m_scalingPropagated = true;
	}


#ifdef USE_QUICKPROF
	Profiler::endBlock("SyncMotionStates");

	Profiler::beginBlock("predictIntegratedTransform");
#endif //USE_QUICKPROF

	{
		//		std::vector<CcdPhysicsController*>::iterator i;



		int k;
		for (k=0;k<GetNumControllers();k++)
		{
			CcdPhysicsController* ctrl = m_controllers[k];
			//		SimdTransform predictedTrans;
			RigidBody* body = ctrl->GetRigidBody();
			
			body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse();

			if (body->IsActive())
			{
				if (!body->IsStatic())
				{
					body->applyForces( timeStep);
					body->integrateVelocities( timeStep);
					body->predictIntegratedTransform(timeStep,body->m_interpolationWorldTransform);
				}
			}

		}
	}

#ifdef USE_QUICKPROF
	Profiler::endBlock("predictIntegratedTransform");
#endif //USE_QUICKPROF

	OverlappingPairCache*	scene = m_collisionWorld->GetPairCache();


	//
	// collision detection (?)
	//


#ifdef USE_QUICKPROF
	Profiler::beginBlock("DispatchAllCollisionPairs");
#endif //USE_QUICKPROF


	int numsubstep = m_numIterations;


	DispatcherInfo dispatchInfo;
	dispatchInfo.m_timeStep = timeStep;
	dispatchInfo.m_stepCount = 0;
	dispatchInfo.m_enableSatConvex = m_enableSatCollisionDetection;
	dispatchInfo.m_debugDraw = this->m_debugDrawer;

	scene->RefreshOverlappingPairs();
	GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(&scene->GetOverlappingPair(0),scene->GetNumOverlappingPairs(),dispatchInfo);


#ifdef USE_QUICKPROF
	Profiler::endBlock("DispatchAllCollisionPairs");
#endif //USE_QUICKPROF


	int numRigidBodies = m_controllers.size();

	
	m_islandManager->UpdateActivationState(GetCollisionWorld(),GetCollisionWorld()->GetDispatcher());

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

			const RigidBody* colObj0 = &constraint->GetRigidBodyA();
			const RigidBody* colObj1 = &constraint->GetRigidBodyB();
			
			if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
						((colObj1) && ((colObj1)->mergesSimulationIslands())))
			{
				if (colObj0->IsActive() || colObj1->IsActive())
				{

					m_islandManager->GetUnionFind().unite((colObj0)->m_islandTag1,
						(colObj1)->m_islandTag1);
				}
			}
		}
	}

	m_islandManager->StoreIslandActivationState(GetCollisionWorld());


	//contacts
#ifdef USE_QUICKPROF
	Profiler::beginBlock("SolveConstraint");
#endif //USE_QUICKPROF


	//solve the regular constraints (point 2 point, hinge, etc)

	for (int g=0;g<numsubstep;g++)
	{
		//
		// constraint solving
		//


		int i;
		int numConstraints = m_constraints.size();

		//point to point constraints
		for (i=0;i< numConstraints ; i++ )
		{
			TypedConstraint* constraint = m_constraints[i];

			constraint->BuildJacobian();
			constraint->SolveConstraint( timeStep );

		}


	}

#ifdef USE_QUICKPROF
	Profiler::endBlock("SolveConstraint");
#endif //USE_QUICKPROF

	//solve the vehicles

#ifdef NEW_BULLET_VEHICLE_SUPPORT
	//vehicles
	int numVehicles = m_wrapperVehicles.size();
	for (int i=0;i<numVehicles;i++)
	{
		WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
		RaycastVehicle* vehicle = wrapperVehicle->GetVehicle();
		vehicle->UpdateVehicle( timeStep);
	}
#endif //NEW_BULLET_VEHICLE_SUPPORT


	struct InplaceSolverIslandCallback : public SimulationIslandManager::IslandCallback
	{

		ContactSolverInfo& m_solverInfo;
		ConstraintSolver*	m_solver;
		IDebugDraw*	m_debugDrawer;

		InplaceSolverIslandCallback(
			ContactSolverInfo& solverInfo,
			ConstraintSolver*	solver,
			IDebugDraw*	debugDrawer)
			:m_solverInfo(solverInfo),
			m_solver(solver),
			m_debugDrawer(debugDrawer)
		{

		}

		virtual	void	ProcessIsland(PersistentManifold**	manifolds,int numManifolds)
		{
			m_solver->SolveGroup( manifolds, numManifolds,m_solverInfo,m_debugDrawer);
		}

	};


	m_solverInfo.m_friction = 0.9f;
	m_solverInfo.m_numIterations = m_numIterations;
	m_solverInfo.m_timeStep = timeStep;
	m_solverInfo.m_restitution = 0.f;//m_restitution;

	InplaceSolverIslandCallback	solverCallback(
		m_solverInfo,
		m_solver,
		m_debugDrawer);

#ifdef USE_QUICKPROF
	Profiler::beginBlock("BuildAndProcessIslands");
#endif //USE_QUICKPROF

	/// solve all the contact points and contact friction
	m_islandManager->BuildAndProcessIslands(GetCollisionWorld()->GetDispatcher(),m_collisionWorld->GetCollisionObjectArray(),&solverCallback);

#ifdef USE_QUICKPROF
	Profiler::endBlock("BuildAndProcessIslands");

	Profiler::beginBlock("CallbackTriggers");
#endif //USE_QUICKPROF

	CallbackTriggers();

#ifdef USE_QUICKPROF
	Profiler::endBlock("CallbackTriggers");


	Profiler::beginBlock("proceedToTransform");

#endif //USE_QUICKPROF
	{



		{

			
			
			UpdateAabbs(timeStep);


			float toi = 1.f;



			if (m_ccdMode == 3)
			{
				DispatcherInfo dispatchInfo;
				dispatchInfo.m_timeStep = timeStep;
				dispatchInfo.m_stepCount = 0;
				dispatchInfo.m_dispatchFunc = DispatcherInfo::DISPATCH_CONTINUOUS;

				//pairCache->RefreshOverlappingPairs();//??
				GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(&scene->GetOverlappingPair(0),scene->GetNumOverlappingPairs(),dispatchInfo);
				
				toi = dispatchInfo.m_timeOfImpact;

			}

			

			//
			// integrating solution
			//

			{
				
				std::vector<CcdPhysicsController*>::iterator i;

				for (i=m_controllers.begin();
					!(i==m_controllers.end()); i++)
				{

					CcdPhysicsController* ctrl = *i;

					SimdTransform predictedTrans;
					RigidBody* body = ctrl->GetRigidBody();
					
					if (body->IsActive())
					{

						if (!body->IsStatic())
						{
							body->predictIntegratedTransform(timeStep*	toi, predictedTrans);
							body->proceedToTransform( predictedTrans);
						}

					}
				}

			}





			//
			// disable sleeping physics objects
			//

			std::vector<CcdPhysicsController*> m_sleepingControllers;

			std::vector<CcdPhysicsController*>::iterator i;

			for (i=m_controllers.begin();
				!(i==m_controllers.end()); i++)
			{
				CcdPhysicsController* ctrl = (*i);
				RigidBody* body = ctrl->GetRigidBody();

				ctrl->UpdateDeactivation(timeStep);


				if (ctrl->wantsSleeping())
				{
					if (body->GetActivationState() == ACTIVE_TAG)
						body->SetActivationState( WANTS_DEACTIVATION );
				} else
				{
					if (body->GetActivationState() != DISABLE_DEACTIVATION)
						body->SetActivationState( ACTIVE_TAG );
				}

				if (useIslands)
				{
					if (body->GetActivationState() == ISLAND_SLEEPING)
					{
						m_sleepingControllers.push_back(ctrl);
					}
				} else
				{
					if (ctrl->wantsSleeping())
					{
						m_sleepingControllers.push_back(ctrl);
					}
				}
			}




		}


#ifdef USE_QUICKPROF
		Profiler::endBlock("proceedToTransform");

		Profiler::beginBlock("SyncMotionStates");
#endif //USE_QUICKPROF

		SyncMotionStates(timeStep);

#ifdef USE_QUICKPROF
		Profiler::endBlock("SyncMotionStates");

		Profiler::endProfilingCycle();
#endif //USE_QUICKPROF


#ifdef NEW_BULLET_VEHICLE_SUPPORT
		//sync wheels for vehicles
		int numVehicles = m_wrapperVehicles.size();
		for (int i=0;i<numVehicles;i++)
		{
			WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];

			wrapperVehicle->SyncWheels();
		}
#endif //NEW_BULLET_VEHICLE_SUPPORT
	}

	return true;
}