Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/physics/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @ 2274

Last change on this file since 2274 was 2192, checked in by rgrieder, 16 years ago
Reverted all changes of attempt to update physics branch.
Property svn:eol-style set to `native`
File size: 34.8 KB

Rev	Line
[1963]	1	/*
	2	Bullet Continuous Collision Detection and Physics Library
	3	Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
	4
	5	This software is provided 'as-is', without any express or implied warranty.
	6	In no event will the authors be held liable for any damages arising from the use of this software.
	7	Permission is granted to anyone to use this software for any purpose,
	8	including commercial applications, and to alter it and redistribute it freely,
	9	subject to the following restrictions:
	10
	11	1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
	12	2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
	13	3. This notice may not be removed or altered from any source distribution.
	14	*/
	15
	16
	17	#include "btDiscreteDynamicsWorld.h"
	18
	19	//collision detection
	20	#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
	21	#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
	22	#include "BulletCollision/CollisionShapes/btCollisionShape.h"
	23	#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
	24	#include "LinearMath/btTransformUtil.h"
	25	#include "LinearMath/btQuickprof.h"
	26
	27	//rigidbody & constraints
	28	#include "BulletDynamics/Dynamics/btRigidBody.h"
	29	#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
	30	#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
	31	#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
	32
	33	//for debug rendering
	34	#include "BulletCollision/CollisionShapes/btBoxShape.h"
	35	#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
	36	#include "BulletCollision/CollisionShapes/btCompoundShape.h"
	37	#include "BulletCollision/CollisionShapes/btConeShape.h"
	38	#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
	39	#include "BulletCollision/CollisionShapes/btCylinderShape.h"
	40	#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
	41	#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
	42	#include "BulletCollision/CollisionShapes/btSphereShape.h"
	43	#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
	44	#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
	45	#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
	46	#include "LinearMath/btIDebugDraw.h"
	47
	48
	49
	50	//vehicle
	51	#include "BulletDynamics/Vehicle/btRaycastVehicle.h"
	52	#include "BulletDynamics/Vehicle/btVehicleRaycaster.h"
	53	#include "BulletDynamics/Vehicle/btWheelInfo.h"
	54	#include "LinearMath/btIDebugDraw.h"
	55	#include "LinearMath/btQuickprof.h"
	56	#include "LinearMath/btMotionState.h"
	57
	58
	59
	60
	61
	62	btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
	63	:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration),
	64	m_constraintSolver(constraintSolver),
	65	m_gravity(0,-10,0),
	66	m_localTime(btScalar(1.)/btScalar(60.)),
	67	m_profileTimings(0)
	68	{
	69	if (!m_constraintSolver)
	70	{
	71	void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16);
	72	m_constraintSolver = new (mem) btSequentialImpulseConstraintSolver;
	73	m_ownsConstraintSolver = true;
	74	} else
	75	{
	76	m_ownsConstraintSolver = false;
	77	}
	78
	79	{
	80	void* mem = btAlignedAlloc(sizeof(btSimulationIslandManager),16);
	81	m_islandManager = new (mem) btSimulationIslandManager();
	82	}
	83
	84	m_ownsIslandManager = true;
	85	}
	86
	87
	88	btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld()
	89	{
	90	//only delete it when we created it
	91	if (m_ownsIslandManager)
	92	{
	93	m_islandManager->~btSimulationIslandManager();
	94	btAlignedFree( m_islandManager);
	95	}
	96	if (m_ownsConstraintSolver)
	97	{
	98
	99	m_constraintSolver->~btConstraintSolver();
	100	btAlignedFree(m_constraintSolver);
	101	}
	102	}
	103
	104	void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
	105	{
	106
	107	for (int i=0;i<m_collisionObjects.size();i++)
	108	{
	109	btCollisionObject* colObj = m_collisionObjects[i];
	110	btRigidBody* body = btRigidBody::upcast(colObj);
	111	if (body)
	112	{
	113	btTransform predictedTrans;
	114	if (body->getActivationState() != ISLAND_SLEEPING)
	115	{
	116	if (body->isKinematicObject())
	117	{
	118	//to calculate velocities next frame
	119	body->saveKinematicState(timeStep);
	120	}
	121	}
	122	}
	123	}
	124	}
	125
	126	void btDiscreteDynamicsWorld::debugDrawWorld()
	127	{
	128
	129	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
	130	{
	131	int numManifolds = getDispatcher()->getNumManifolds();
	132	btVector3 color(0,0,0);
	133	for (int i=0;i<numManifolds;i++)
	134	{
	135	btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
	136	//btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
	137	//btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
	138
	139	int numContacts = contactManifold->getNumContacts();
	140	for (int j=0;j<numContacts;j++)
	141	{
	142	btManifoldPoint& cp = contactManifold->getContactPoint(j);
	143	getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
	144	}
	145	}
	146	}
	147
	148
	149	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe \| btIDebugDraw::DBG_DrawAabb))
	150	{
	151	int i;
	152
	153	//todo: iterate over awake simulation islands!
	154	for ( i=0;i<m_collisionObjects.size();i++)
	155	{
	156	btCollisionObject* colObj = m_collisionObjects[i];
	157	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
	158	{
	159	btVector3 color(btScalar(255.),btScalar(255.),btScalar(255.));
	160	switch(colObj->getActivationState())
	161	{
	162	case ACTIVE_TAG:
	163	color = btVector3(btScalar(255.),btScalar(255.),btScalar(255.)); break;
	164	case ISLAND_SLEEPING:
	165	color = btVector3(btScalar(0.),btScalar(255.),btScalar(0.));break;
	166	case WANTS_DEACTIVATION:
	167	color = btVector3(btScalar(0.),btScalar(255.),btScalar(255.));break;
	168	case DISABLE_DEACTIVATION:
	169	color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));break;
	170	case DISABLE_SIMULATION:
	171	color = btVector3(btScalar(255.),btScalar(255.),btScalar(0.));break;
	172	default:
	173	{
	174	color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));
	175	}
	176	};
	177
	178	debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
	179	}
	180	if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
	181	{
	182	btPoint3 minAabb,maxAabb;
	183	btVector3 colorvec(1,0,0);
	184	colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
	185	m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
	186	}
	187
	188	}
	189
	190	for ( i=0;i<this->m_vehicles.size();i++)
	191	{
	192	for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
	193	{
	194	btVector3 wheelColor(0,255,255);
	195	if (m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_isInContact)
	196	{
	197	wheelColor.setValue(0,0,255);
	198	} else
	199	{
	200	wheelColor.setValue(255,0,255);
	201	}
	202
	203	btVector3 wheelPosWS = m_vehicles[i]->getWheelInfo(v).m_worldTransform.getOrigin();
	204
	205	btVector3 axle = btVector3(
	206	m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[0][m_vehicles[i]->getRightAxis()],
	207	m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[1][m_vehicles[i]->getRightAxis()],
	208	m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[2][m_vehicles[i]->getRightAxis()]);
	209
	210
	211	//m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS
	212	//debug wheels (cylinders)
	213	m_debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
	214	m_debugDrawer->drawLine(wheelPosWS,m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);
	215
	216	}
	217	}
	218	}
	219	}
	220
	221	void btDiscreteDynamicsWorld::clearForces()
	222	{
	223	//todo: iterate over awake simulation islands!
	224	for ( int i=0;i<m_collisionObjects.size();i++)
	225	{
	226	btCollisionObject* colObj = m_collisionObjects[i];
	227
	228	btRigidBody* body = btRigidBody::upcast(colObj);
	229	if (body)
	230	{
	231	body->clearForces();
	232	}
	233	}
	234	}
	235
	236	///apply gravity, call this once per timestep
	237	void btDiscreteDynamicsWorld::applyGravity()
	238	{
	239	//todo: iterate over awake simulation islands!
	240	for ( int i=0;i<m_collisionObjects.size();i++)
	241	{
	242	btCollisionObject* colObj = m_collisionObjects[i];
	243
	244	btRigidBody* body = btRigidBody::upcast(colObj);
	245	if (body && body->isActive())
	246	{
	247	body->applyGravity();
	248	}
	249	}
	250	}
	251
	252
	253
	254	void btDiscreteDynamicsWorld::synchronizeMotionStates()
	255	{
	256	{
	257	//todo: iterate over awake simulation islands!
	258	for ( int i=0;i<m_collisionObjects.size();i++)
	259	{
	260	btCollisionObject* colObj = m_collisionObjects[i];
	261
	262	btRigidBody* body = btRigidBody::upcast(colObj);
	263	if (body && body->getMotionState() && !body->isStaticOrKinematicObject())
	264	{
	265	//we need to call the update at least once, even for sleeping objects
	266	//otherwise the 'graphics' transform never updates properly
	267	//so todo: add 'dirty' flag
	268	//if (body->getActivationState() != ISLAND_SLEEPING)
	269	{
	270	btTransform interpolatedTransform;
	271	btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
	272	body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime*body->getHitFraction(),interpolatedTransform);
	273	body->getMotionState()->setWorldTransform(interpolatedTransform);
	274	}
	275	}
	276	}
	277	}
	278
	279	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
	280	{
	281	for ( int i=0;i<this->m_vehicles.size();i++)
	282	{
	283	for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
	284	{
	285	//synchronize the wheels with the (interpolated) chassis worldtransform
	286	m_vehicles[i]->updateWheelTransform(v,true);
	287	}
	288	}
	289	}
	290
	291	}
	292
	293
	294	int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
	295	{
	296	startProfiling(timeStep);
	297
	298	BT_PROFILE("stepSimulation");
	299
	300	int numSimulationSubSteps = 0;
	301
	302	if (maxSubSteps)
	303	{
	304	//fixed timestep with interpolation
	305	m_localTime += timeStep;
	306	if (m_localTime >= fixedTimeStep)
	307	{
	308	numSimulationSubSteps = int( m_localTime / fixedTimeStep);
	309	m_localTime -= numSimulationSubSteps * fixedTimeStep;
	310	}
	311	} else
	312	{
	313	//variable timestep
	314	fixedTimeStep = timeStep;
	315	m_localTime = timeStep;
	316	if (btFuzzyZero(timeStep))
	317	{
	318	numSimulationSubSteps = 0;
	319	maxSubSteps = 0;
	320	} else
	321	{
	322	numSimulationSubSteps = 1;
	323	maxSubSteps = 1;
	324	}
	325	}
	326
	327	//process some debugging flags
	328	if (getDebugDrawer())
	329	{
	330	gDisableDeactivation = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
	331	}
	332	if (numSimulationSubSteps)
	333	{
	334
	335	saveKinematicState(fixedTimeStep);
	336
	337	applyGravity();
	338
	339	//clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
	340	int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps;
	341
	342	for (int i=0;i<clampedSimulationSteps;i++)
	343	{
	344	internalSingleStepSimulation(fixedTimeStep);
	345	synchronizeMotionStates();
	346	}
	347
	348	}
	349
	350	synchronizeMotionStates();
	351
	352	clearForces();
	353
	354	#ifndef BT_NO_PROFILE
	355	CProfileManager::Increment_Frame_Counter();
	356	#endif //BT_NO_PROFILE
	357
	358	return numSimulationSubSteps;
	359	}
	360
	361	void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
	362	{
	363
	364	BT_PROFILE("internalSingleStepSimulation");
	365
	366	///apply gravity, predict motion
	367	predictUnconstraintMotion(timeStep);
	368
	369	btDispatcherInfo& dispatchInfo = getDispatchInfo();
	370
	371	dispatchInfo.m_timeStep = timeStep;
	372	dispatchInfo.m_stepCount = 0;
	373	dispatchInfo.m_debugDraw = getDebugDrawer();
	374
	375	///perform collision detection
	376	performDiscreteCollisionDetection();
	377
	378	calculateSimulationIslands();
	379
	380
	381	getSolverInfo().m_timeStep = timeStep;
	382
	383
	384
	385	///solve contact and other joint constraints
	386	solveConstraints(getSolverInfo());
	387
	388	///CallbackTriggers();
	389
	390	///integrate transforms
	391	integrateTransforms(timeStep);
	392
	393	///update vehicle simulation
	394	updateVehicles(timeStep);
	395
	396
	397	updateActivationState( timeStep );
	398
	399	if(0 != m_internalTickCallback) {
	400	(*m_internalTickCallback)(this, timeStep);
	401	}
	402	}
	403
	404	void btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)
	405	{
	406	m_gravity = gravity;
	407	for ( int i=0;i<m_collisionObjects.size();i++)
	408	{
	409	btCollisionObject* colObj = m_collisionObjects[i];
	410	btRigidBody* body = btRigidBody::upcast(colObj);
	411	if (body)
	412	{
	413	body->setGravity(gravity);
	414	}
	415	}
	416	}
	417
	418	btVector3 btDiscreteDynamicsWorld::getGravity () const
	419	{
	420	return m_gravity;
	421	}
	422
	423
	424	void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body)
	425	{
	426	removeCollisionObject(body);
	427	}
	428
	429	void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)
	430	{
	431	if (!body->isStaticOrKinematicObject())
	432	{
	433	body->setGravity(m_gravity);
	434	}
	435
	436	if (body->getCollisionShape())
	437	{
	438	bool isDynamic = !(body->isStaticObject() \|\| body->isKinematicObject());
	439	short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
	440	short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
	441
	442	addCollisionObject(body,collisionFilterGroup,collisionFilterMask);
	443	}
	444	}
	445
	446	void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask)
	447	{
	448	if (!body->isStaticOrKinematicObject())
	449	{
	450	body->setGravity(m_gravity);
	451	}
	452
	453	if (body->getCollisionShape())
	454	{
	455	addCollisionObject(body,group,mask);
	456	}
	457	}
	458
	459
	460	void btDiscreteDynamicsWorld::updateVehicles(btScalar timeStep)
	461	{
	462	BT_PROFILE("updateVehicles");
	463
	464	for ( int i=0;i<m_vehicles.size();i++)
	465	{
	466	btRaycastVehicle* vehicle = m_vehicles[i];
	467	vehicle->updateVehicle( timeStep);
	468	}
	469	}
	470
	471	void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
	472	{
	473	BT_PROFILE("updateActivationState");
	474
	475	for ( int i=0;i<m_collisionObjects.size();i++)
	476	{
	477	btCollisionObject* colObj = m_collisionObjects[i];
	478	btRigidBody* body = btRigidBody::upcast(colObj);
	479	if (body)
	480	{
	481	body->updateDeactivation(timeStep);
	482
	483	if (body->wantsSleeping())
	484	{
	485	if (body->isStaticOrKinematicObject())
	486	{
	487	body->setActivationState(ISLAND_SLEEPING);
	488	} else
	489	{
	490	if (body->getActivationState() == ACTIVE_TAG)
	491	body->setActivationState( WANTS_DEACTIVATION );
	492	if (body->getActivationState() == ISLAND_SLEEPING)
	493	{
	494	body->setAngularVelocity(btVector3(0,0,0));
	495	body->setLinearVelocity(btVector3(0,0,0));
	496	}
	497
	498	}
	499	} else
	500	{
	501	if (body->getActivationState() != DISABLE_DEACTIVATION)
	502	body->setActivationState( ACTIVE_TAG );
	503	}
	504	}
	505	}
	506	}
	507
	508	void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies)
	509	{
	510	m_constraints.push_back(constraint);
	511	if (disableCollisionsBetweenLinkedBodies)
	512	{
	513	constraint->getRigidBodyA().addConstraintRef(constraint);
	514	constraint->getRigidBodyB().addConstraintRef(constraint);
	515	}
	516	}
	517
	518	void btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint)
	519	{
	520	m_constraints.remove(constraint);
	521	constraint->getRigidBodyA().removeConstraintRef(constraint);
	522	constraint->getRigidBodyB().removeConstraintRef(constraint);
	523	}
	524
	525	void btDiscreteDynamicsWorld::addVehicle(btRaycastVehicle* vehicle)
	526	{
	527	m_vehicles.push_back(vehicle);
	528	}
	529
	530	void btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle)
	531	{
	532	m_vehicles.remove(vehicle);
	533	}
	534
	535	SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs)
	536	{
	537	int islandId;
	538
	539	const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
	540	const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
	541	islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag();
	542	return islandId;
	543
	544	}
	545
	546
	547	class btSortConstraintOnIslandPredicate
	548	{
	549	public:
	550
	551	bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs )
	552	{
	553	int rIslandId0,lIslandId0;
	554	rIslandId0 = btGetConstraintIslandId(rhs);
	555	lIslandId0 = btGetConstraintIslandId(lhs);
	556	return lIslandId0 < rIslandId0;
	557	}
	558	};
	559
	560
	561
	562
	563	void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
	564	{
	565	BT_PROFILE("solveConstraints");
	566
	567	struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
	568	{
	569
	570	btContactSolverInfo& m_solverInfo;
	571	btConstraintSolver* m_solver;
	572	btTypedConstraint** m_sortedConstraints;
	573	int m_numConstraints;
	574	btIDebugDraw* m_debugDrawer;
	575	btStackAlloc* m_stackAlloc;
	576	btDispatcher* m_dispatcher;
	577
	578	InplaceSolverIslandCallback(
	579	btContactSolverInfo& solverInfo,
	580	btConstraintSolver* solver,
	581	btTypedConstraint** sortedConstraints,
	582	int numConstraints,
	583	btIDebugDraw* debugDrawer,
	584	btStackAlloc* stackAlloc,
	585	btDispatcher* dispatcher)
	586	:m_solverInfo(solverInfo),
	587	m_solver(solver),
	588	m_sortedConstraints(sortedConstraints),
	589	m_numConstraints(numConstraints),
	590	m_debugDrawer(debugDrawer),
	591	m_stackAlloc(stackAlloc),
	592	m_dispatcher(dispatcher)
	593	{
	594
	595	}
	596
	597	InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other)
	598	{
	599	btAssert(0);
	600	(void)other;
	601	return *this;
	602	}
	603	virtual void ProcessIsland(btCollisionObject bodies,int numBodies,btPersistentManifold manifolds,int numManifolds, int islandId)
	604	{
	605	if (islandId<0)
	606	{
	607	///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
	608	m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
	609	} else
	610	{
	611	//also add all non-contact constraints/joints for this island
	612	btTypedConstraint** startConstraint = 0;
	613	int numCurConstraints = 0;
	614	int i;
	615
	616	//find the first constraint for this island
	617	for (i=0;i<m_numConstraints;i++)
	618	{
	619	if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
	620	{
	621	startConstraint = &m_sortedConstraints[i];
	622	break;
	623	}
	624	}
	625	//count the number of constraints in this island
	626	for (;i<m_numConstraints;i++)
	627	{
	628	if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
	629	{
	630	numCurConstraints++;
	631	}
	632	}
	633
	634	///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive
	635	if (numManifolds + numCurConstraints)
	636	{
	637	m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
	638	}
	639
	640	}
	641	}
	642
	643	};
	644
	645	//sorted version of all btTypedConstraint, based on islandId
	646	btAlignedObjectArray<btTypedConstraint*> sortedConstraints;
	647	sortedConstraints.resize( m_constraints.size());
	648	int i;
	649	for (i=0;i<getNumConstraints();i++)
	650	{
	651	sortedConstraints[i] = m_constraints[i];
	652	}
	653
	654	// assert(0);
	655
	656
	657
	658	sortedConstraints.quickSort(btSortConstraintOnIslandPredicate());
	659
	660	btTypedConstraint** constraintsPtr = getNumConstraints() ? &sortedConstraints[0] : 0;
	661
	662	InplaceSolverIslandCallback solverCallback( solverInfo, m_constraintSolver, constraintsPtr,sortedConstraints.size(), m_debugDrawer,m_stackAlloc,m_dispatcher1);
	663
	664	m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
	665
	666	/// solve all the constraints for this island
	667	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld()->getCollisionObjectArray(),&solverCallback);
	668
	669	m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc);
	670	}
	671
	672
	673
	674
	675	void btDiscreteDynamicsWorld::calculateSimulationIslands()
	676	{
	677	BT_PROFILE("calculateSimulationIslands");
	678
	679	getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());
	680
	681	{
	682	int i;
	683	int numConstraints = int(m_constraints.size());
	684	for (i=0;i< numConstraints ; i++ )
	685	{
	686	btTypedConstraint* constraint = m_constraints[i];
	687
	688	const btRigidBody* colObj0 = &constraint->getRigidBodyA();
	689	const btRigidBody* colObj1 = &constraint->getRigidBodyB();
	690
	691	if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
	692	((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
	693	{
	694	if (colObj0->isActive() \|\| colObj1->isActive())
	695	{
	696
	697	getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),
	698	(colObj1)->getIslandTag());
	699	}
	700	}
	701	}
	702	}
	703
	704	//Store the island id in each body
	705	getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
	706
	707
	708	}
	709
	710
	711	#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
	712
	713	class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
	714	{
	715	btCollisionObject* m_me;
	716	btScalar m_allowedPenetration;
	717	btOverlappingPairCache* m_pairCache;
	718
	719
	720	public:
	721	btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache) :
	722	btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
	723	m_allowedPenetration(0.0f),
	724	m_me(me),
	725	m_pairCache(pairCache)
	726	{
	727	}
	728
	729	virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace)
	730	{
	731	if (convexResult.m_hitCollisionObject == m_me)
	732	return 1.0;
	733
	734	btVector3 linVelA,linVelB;
	735	linVelA = m_convexToWorld-m_convexFromWorld;
	736	linVelB = btVector3(0,0,0);//toB.getOrigin()-fromB.getOrigin();
	737
	738	btVector3 relativeVelocity = (linVelA-linVelB);
	739	//don't report time of impact for motion away from the contact normal (or causes minor penetration)
	740	if (convexResult.m_hitNormalLocal.dot(relativeVelocity)>=-m_allowedPenetration)
	741	return 1.f;
	742
	743	return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace);
	744	}
	745
	746	virtual bool needsCollision(btBroadphaseProxy* proxy0) const
	747	{
	748	//don't collide with itself
	749	if (proxy0->m_clientObject == m_me)
	750	return false;
	751
	752	///don't do CCD when the collision filters are not matching
	753	if (!btCollisionWorld::ClosestConvexResultCallback::needsCollision(proxy0))
	754	return false;
	755
	756	///don't do CCD when there are already contact points (touching contact/penetration)
	757	btAlignedObjectArray<btPersistentManifold*> manifoldArray;
	758	btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0);
	759	if (collisionPair)
	760	{
	761	if (collisionPair->m_algorithm)
	762	{
	763	manifoldArray.resize(0);
	764	collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);
	765	for (int j=0;j<manifoldArray.size();j++)
	766	{
	767	btPersistentManifold* manifold = manifoldArray[j];
	768	if (manifold->getNumContacts()>0)
	769	return false;
	770	}
	771	}
	772	}
	773	return true;
	774	}
	775
	776
	777	};
	778
	779	///internal debugging variable. this value shouldn't be too high
	780	int gNumClampedCcdMotions=0;
	781
	782	//#include "stdio.h"
	783	void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
	784	{
	785	BT_PROFILE("integrateTransforms");
	786	btTransform predictedTrans;
	787	for ( int i=0;i<m_collisionObjects.size();i++)
	788	{
	789	btCollisionObject* colObj = m_collisionObjects[i];
	790	btRigidBody* body = btRigidBody::upcast(colObj);
	791	if (body)
	792	{
	793	body->setHitFraction(1.f);
	794
	795	if (body->isActive() && (!body->isStaticOrKinematicObject()))
	796	{
	797	body->predictIntegratedTransform(timeStep, predictedTrans);
	798	btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
	799
	800	if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
	801	{
	802	BT_PROFILE("CCD motion clamping");
	803	if (body->getCollisionShape()->isConvex())
	804	{
	805	gNumClampedCcdMotions++;
	806
	807	btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache());
	808	btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
	809	btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
	810	convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults);
	811	if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
	812	{
	813	body->setHitFraction(sweepResults.m_closestHitFraction);
	814	body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
	815	body->setHitFraction(0.f);
	816	// printf("clamped integration to hit fraction = %f\n",fraction);
	817	}
	818	}
	819	}
	820
	821	body->proceedToTransform( predictedTrans);
	822	}
	823	}
	824	}
	825	}
	826
	827
	828
	829	void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
	830	{
	831	BT_PROFILE("predictUnconstraintMotion");
	832	for ( int i=0;i<m_collisionObjects.size();i++)
	833	{
	834	btCollisionObject* colObj = m_collisionObjects[i];
	835	btRigidBody* body = btRigidBody::upcast(colObj);
	836	if (body)
	837	{
	838	if (!body->isStaticOrKinematicObject())
	839	{
	840	if (body->isActive())
	841	{
	842	body->integrateVelocities( timeStep);
	843	//damping
	844	body->applyDamping(timeStep);
	845
	846	body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
	847	}
	848	}
	849	}
	850	}
	851	}
	852
	853
	854	void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep)
	855	{
	856	(void)timeStep;
	857
	858	#ifndef BT_NO_PROFILE
	859	CProfileManager::Reset();
	860	#endif //BT_NO_PROFILE
	861
	862	}
	863
	864
	865
	866
	867
	868
	869	class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
	870	{
	871	btIDebugDraw* m_debugDrawer;
	872	btVector3 m_color;
	873	btTransform m_worldTrans;
	874
	875	public:
	876
	877	DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) :
	878	m_debugDrawer(debugDrawer),
	879	m_color(color),
	880	m_worldTrans(worldTrans)
	881	{
	882	}
	883
	884	virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex)
	885	{
	886	processTriangle(triangle,partId,triangleIndex);
	887	}
	888
	889	virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
	890	{
	891	(void)partId;
	892	(void)triangleIndex;
	893
	894	btVector3 wv0,wv1,wv2;
	895	wv0 = m_worldTrans*triangle[0];
	896	wv1 = m_worldTrans*triangle[1];
	897	wv2 = m_worldTrans*triangle[2];
	898	m_debugDrawer->drawLine(wv0,wv1,m_color);
	899	m_debugDrawer->drawLine(wv1,wv2,m_color);
	900	m_debugDrawer->drawLine(wv2,wv0,m_color);
	901	}
	902	};
	903
	904	void btDiscreteDynamicsWorld::debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color)
	905	{
	906	btVector3 start = transform.getOrigin();
	907
	908	const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0);
	909	const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0);
	910	const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius);
	911
	912	// XY
	913	getDebugDrawer()->drawLine(start-xoffs, start+yoffs, color);
	914	getDebugDrawer()->drawLine(start+yoffs, start+xoffs, color);
	915	getDebugDrawer()->drawLine(start+xoffs, start-yoffs, color);
	916	getDebugDrawer()->drawLine(start-yoffs, start-xoffs, color);
	917
	918	// XZ
	919	getDebugDrawer()->drawLine(start-xoffs, start+zoffs, color);
	920	getDebugDrawer()->drawLine(start+zoffs, start+xoffs, color);
	921	getDebugDrawer()->drawLine(start+xoffs, start-zoffs, color);
	922	getDebugDrawer()->drawLine(start-zoffs, start-xoffs, color);
	923
	924	// YZ
	925	getDebugDrawer()->drawLine(start-yoffs, start+zoffs, color);
	926	getDebugDrawer()->drawLine(start+zoffs, start+yoffs, color);
	927	getDebugDrawer()->drawLine(start+yoffs, start-zoffs, color);
	928	getDebugDrawer()->drawLine(start-zoffs, start-yoffs, color);
	929	}
	930
	931	void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
	932	{
	933	// Draw a small simplex at the center of the object
	934	{
	935	btVector3 start = worldTransform.getOrigin();
	936	getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(1,0,0), btVector3(1,0,0));
	937	getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,1,0), btVector3(0,1,0));
	938	getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,0,1), btVector3(0,0,1));
	939	}
	940
	941	if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
	942	{
	943	const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
	944	for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
	945	{
	946	btTransform childTrans = compoundShape->getChildTransform(i);
	947	const btCollisionShape* colShape = compoundShape->getChildShape(i);
	948	debugDrawObject(worldTransform*childTrans,colShape,color);
	949	}
	950
	951	} else
	952	{
	953	switch (shape->getShapeType())
	954	{
	955
	956	case SPHERE_SHAPE_PROXYTYPE:
	957	{
	958	const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
	959	btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
	960
	961	debugDrawSphere(radius, worldTransform, color);
	962	break;
	963	}
	964	case MULTI_SPHERE_SHAPE_PROXYTYPE:
	965	{
	966	const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
	967
	968	for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
	969	{
	970	btTransform childTransform = worldTransform;
	971	childTransform.getOrigin() += multiSphereShape->getSpherePosition(i);
	972	debugDrawSphere(multiSphereShape->getSphereRadius(i), childTransform, color);
	973	}
	974
	975	break;
	976	}
	977	case CAPSULE_SHAPE_PROXYTYPE:
	978	{
	979	const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
	980
	981	btScalar radius = capsuleShape->getRadius();
	982	btScalar halfHeight = capsuleShape->getHalfHeight();
	983
	984	int upAxis = capsuleShape->getUpAxis();
	985
	986
	987	btVector3 capStart(0.f,0.f,0.f);
	988	capStart[upAxis] = -halfHeight;
	989
	990	btVector3 capEnd(0.f,0.f,0.f);
	991	capEnd[upAxis] = halfHeight;
	992
	993	// Draw the ends
	994	{
	995
	996	btTransform childTransform = worldTransform;
	997	childTransform.getOrigin() = worldTransform * capStart;
	998	debugDrawSphere(radius, childTransform, color);
	999	}
	1000
	1001	{
	1002	btTransform childTransform = worldTransform;
	1003	childTransform.getOrigin() = worldTransform * capEnd;
	1004	debugDrawSphere(radius, childTransform, color);
	1005	}
	1006
	1007	// Draw some additional lines
	1008	btVector3 start = worldTransform.getOrigin();
	1009
	1010
	1011	capStart[(upAxis+1)%3] = radius;
	1012	capEnd[(upAxis+1)%3] = radius;
	1013	getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
	1014	capStart[(upAxis+1)%3] = -radius;
	1015	capEnd[(upAxis+1)%3] = -radius;
	1016	getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
	1017
	1018	capStart[(upAxis+1)%3] = 0.f;
	1019	capEnd[(upAxis+1)%3] = 0.f;
	1020
	1021	capStart[(upAxis+2)%3] = radius;
	1022	capEnd[(upAxis+2)%3] = radius;
	1023	getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
	1024	capStart[(upAxis+2)%3] = -radius;
	1025	capEnd[(upAxis+2)%3] = -radius;
	1026	getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
	1027
	1028
	1029	break;
	1030	}
	1031	case CONE_SHAPE_PROXYTYPE:
	1032	{
	1033	const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
	1034	btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
	1035	btScalar height = coneShape->getHeight();//+coneShape->getMargin();
	1036	btVector3 start = worldTransform.getOrigin();
	1037
	1038	int upAxis= coneShape->getConeUpIndex();
	1039
	1040
	1041	btVector3 offsetHeight(0,0,0);
	1042	offsetHeight[upAxis] = height * btScalar(0.5);
	1043	btVector3 offsetRadius(0,0,0);
	1044	offsetRadius[(upAxis+1)%3] = radius;
	1045	btVector3 offset2Radius(0,0,0);
	1046	offset2Radius[(upAxis+2)%3] = radius;
	1047
	1048	getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
	1049	getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
	1050	getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color);
	1051	getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color);
	1052
	1053
	1054
	1055	break;
	1056
	1057	}
	1058	case CYLINDER_SHAPE_PROXYTYPE:
	1059	{
	1060	const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
	1061	int upAxis = cylinder->getUpAxis();
	1062	btScalar radius = cylinder->getRadius();
	1063	btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
	1064	btVector3 start = worldTransform.getOrigin();
	1065	btVector3 offsetHeight(0,0,0);
	1066	offsetHeight[upAxis] = halfHeight;
	1067	btVector3 offsetRadius(0,0,0);
	1068	offsetRadius[(upAxis+1)%3] = radius;
	1069	getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
	1070	getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
	1071	break;
	1072	}
	1073
	1074	case STATIC_PLANE_PROXYTYPE:
	1075	{
	1076	const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
	1077	btScalar planeConst = staticPlaneShape->getPlaneConstant();
	1078	const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
	1079	btVector3 planeOrigin = planeNormal * planeConst;
	1080	btVector3 vec0,vec1;
	1081	btPlaneSpace1(planeNormal,vec0,vec1);
	1082	btScalar vecLen = 100.f;
	1083	btVector3 pt0 = planeOrigin + vec0*vecLen;
	1084	btVector3 pt1 = planeOrigin - vec0*vecLen;
	1085	btVector3 pt2 = planeOrigin + vec1*vecLen;
	1086	btVector3 pt3 = planeOrigin - vec1*vecLen;
	1087	getDebugDrawer()->drawLine(worldTransformpt0,worldTransformpt1,color);
	1088	getDebugDrawer()->drawLine(worldTransformpt2,worldTransformpt3,color);
	1089	break;
	1090
	1091	}
	1092	default:
	1093	{
	1094
	1095	if (shape->isConcave())
	1096	{
	1097	btConcaveShape* concaveMesh = (btConcaveShape*) shape;
	1098
	1099	//todo pass camera, for some culling
	1100	btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
	1101	btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
	1102
	1103	DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
	1104	concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
	1105
	1106	}
	1107
	1108	if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
	1109	{
	1110	btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
	1111	//todo: pass camera for some culling
	1112	btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
	1113	btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
	1114	//DebugDrawcallback drawCallback;
	1115	DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
	1116	convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
	1117	}
	1118
	1119
	1120	/// for polyhedral shapes
	1121	if (shape->isPolyhedral())
	1122	{
	1123	btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
	1124
	1125	int i;
	1126	for (i=0;i<polyshape->getNumEdges();i++)
	1127	{
	1128	btPoint3 a,b;
	1129	polyshape->getEdge(i,a,b);
	1130	btVector3 wa = worldTransform * a;
	1131	btVector3 wb = worldTransform * b;
	1132	getDebugDrawer()->drawLine(wa,wb,color);
	1133
	1134	}
	1135
	1136
	1137	}
	1138	}
	1139	}
	1140	}
	1141	}
	1142
	1143
	1144	void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
	1145	{
	1146	if (m_ownsConstraintSolver)
	1147	{
	1148	btAlignedFree( m_constraintSolver);
	1149	}
	1150	m_ownsConstraintSolver = false;
	1151	m_constraintSolver = solver;
	1152	}
	1153
	1154	btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver()
	1155	{
	1156	return m_constraintSolver;
	1157	}
	1158
	1159
	1160	int btDiscreteDynamicsWorld::getNumConstraints() const
	1161	{
	1162	return int(m_constraints.size());
	1163	}
	1164	btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index)
	1165	{
	1166	return m_constraints[index];
	1167	}
	1168	const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const
	1169	{
	1170	return m_constraints[index];
	1171	}

Note: See TracBrowser for help on using the repository browser.

Download in other formats: