Пример #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;
	}
}
Пример #2
0
bool	CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
{
	//don't simulate without timesubsteps
	if (m_numTimeSubSteps<1)
		return true;

	//printf("proceedDeltaTime\n");
	

#ifdef USE_QUICKPROF
	//toggle Profiler
	if ( m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_ProfileTimings)
	{
		if (!m_profileTimings)
		{
			m_profileTimings = 1;
			// To disable profiling, simply comment out the following line.
			static int counter = 0;

			char filename[128];
			sprintf(filename,"quickprof_bullet_timings%i.csv",counter++);
			Profiler::init(filename, Profiler::BLOCK_CYCLE_SECONDS);//BLOCK_TOTAL_MICROSECONDS

		}
	} else
	{
		if (m_profileTimings)
		{
			m_profileTimings = 0;
			Profiler::destroy();
		}
	}
#endif //USE_QUICKPROF



	if (!SimdFuzzyZero(timeStep))
	{
		
		{
			//do the kinematic calculation here, over the full timestep
			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->GetActivationState() != ISLAND_SLEEPING)
						{

							if (body->IsStatic())
							{
								//to calculate velocities next frame
								body->saveKinematicState(timeStep);
							}
						}
			}
		}


		int i;
		float subTimeStep = timeStep / float(m_numTimeSubSteps);

		for (i=0;i<this->m_numTimeSubSteps;i++)
		{
			proceedDeltaTimeOneStep(subTimeStep);
		}
	} else
	{
		//todo: interpolate
	}

	return true;
}
Пример #3
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;
}
Пример #4
0
void	CcdPhysicsEnvironment::UpdateAabbs(float	timeStep)
{
	std::vector<CcdPhysicsController*>::iterator i;
	BroadphaseInterface* scene =  GetBroadphase();

	//
			// update aabbs, only for moving objects (!)
			//
			for (i=m_controllers.begin();
				!(i==m_controllers.end()); i++)
			{
				CcdPhysicsController* ctrl = (*i);
				RigidBody* body = ctrl->GetRigidBody();


				SimdPoint3 minAabb,maxAabb;
				CollisionShape* shapeinterface = ctrl->GetCollisionShape();



				shapeinterface->CalculateTemporalAabb(body->getCenterOfMassTransform(),
					body->getLinearVelocity(),
					//body->getAngularVelocity(),
					SimdVector3(0.f,0.f,0.f),//no angular effect for now //body->getAngularVelocity(),
					timeStep,minAabb,maxAabb);


				SimdVector3 manifoldExtraExtents(gContactBreakingTreshold,gContactBreakingTreshold,gContactBreakingTreshold);
				minAabb -= manifoldExtraExtents;
				maxAabb += manifoldExtraExtents;

				BroadphaseProxy* bp = body->m_broadphaseHandle;
				if (bp)
				{

					SimdVector3 color (1,1,0);

					if (m_debugDrawer)
					{	
						//draw aabb
						switch (body->GetActivationState())
						{
						case ISLAND_SLEEPING:
							{
								color.setValue(1,1,1);
								break;
							}
						case WANTS_DEACTIVATION:
							{
								color.setValue(0,0,1);
								break;
							}
						case ACTIVE_TAG:
							{
								break;
							}
						case DISABLE_DEACTIVATION:
							{
								color.setValue(1,0,1);
							};

						};

						if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb)
						{
							DrawAabb(m_debugDrawer,minAabb,maxAabb,color);
						}
					}

			
					if ( (maxAabb-minAabb).length2() < 1e12f)
					{
						scene->SetAabb(bp,minAabb,maxAabb);
					} else
					{
						//something went wrong, investigate
						//removeCcdPhysicsController(ctrl);
						body->SetActivationState(DISABLE_SIMULATION);

						static bool reportMe = true;
						if (reportMe)
						{
							reportMe = false;
							printf("Overflow in AABB, object removed from simulation \n");
							printf("If you can reproduce this, please email [email protected]\n");
							printf("Please include above information, your Platform, version of OS.\n");
							printf("Thanks.\n");
						}
						
					}

				}
			}
}