Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/trunk/src/external/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @ 7561

Last change on this file since 7561 was 5781, checked in by rgrieder, 16 years ago
Reverted trunk again. We might want to find a way to delete these revisions again (x3n's changes are still available as diff in the commit mails).
Property svn:eol-style set to `native`
File size: 42.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"
[2882]	32	#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
	33	#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
	34	#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
	35	#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
	36	#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
[1963]	37
	38	//for debug rendering
	39	#include "BulletCollision/CollisionShapes/btBoxShape.h"
	40	#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
	41	#include "BulletCollision/CollisionShapes/btCompoundShape.h"
	42	#include "BulletCollision/CollisionShapes/btConeShape.h"
	43	#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
	44	#include "BulletCollision/CollisionShapes/btCylinderShape.h"
	45	#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
	46	#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
	47	#include "BulletCollision/CollisionShapes/btSphereShape.h"
	48	#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
	49	#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
	50	#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
	51	#include "LinearMath/btIDebugDraw.h"
	52
	53
[2882]	54	#include "BulletDynamics/Dynamics/btActionInterface.h"
[1963]	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	if (body->getActivationState() != ISLAND_SLEEPING)
	114	{
	115	if (body->isKinematicObject())
	116	{
	117	//to calculate velocities next frame
	118	body->saveKinematicState(timeStep);
	119	}
	120	}
	121	}
	122	}
	123	}
	124
	125	void btDiscreteDynamicsWorld::debugDrawWorld()
	126	{
[2430]	127	BT_PROFILE("debugDrawWorld");
[1963]	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	}
[2882]	147	bool drawConstraints = false;
	148	if (getDebugDrawer())
	149	{
	150	int mode = getDebugDrawer()->getDebugMode();
	151	if(mode & (btIDebugDraw::DBG_DrawConstraints \| btIDebugDraw::DBG_DrawConstraintLimits))
	152	{
	153	drawConstraints = true;
	154	}
	155	}
	156	if(drawConstraints)
	157	{
	158	for(int i = getNumConstraints()-1; i>=0 ;i--)
	159	{
	160	btTypedConstraint* constraint = getConstraint(i);
	161	debugDrawConstraint(constraint);
	162	}
	163	}
[1963]	164
	165
[2882]	166
[1963]	167	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe \| btIDebugDraw::DBG_DrawAabb))
	168	{
	169	int i;
	170
	171	for ( i=0;i<m_collisionObjects.size();i++)
	172	{
	173	btCollisionObject* colObj = m_collisionObjects[i];
	174	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
	175	{
	176	btVector3 color(btScalar(255.),btScalar(255.),btScalar(255.));
	177	switch(colObj->getActivationState())
	178	{
	179	case ACTIVE_TAG:
	180	color = btVector3(btScalar(255.),btScalar(255.),btScalar(255.)); break;
	181	case ISLAND_SLEEPING:
	182	color = btVector3(btScalar(0.),btScalar(255.),btScalar(0.));break;
	183	case WANTS_DEACTIVATION:
	184	color = btVector3(btScalar(0.),btScalar(255.),btScalar(255.));break;
	185	case DISABLE_DEACTIVATION:
	186	color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));break;
	187	case DISABLE_SIMULATION:
	188	color = btVector3(btScalar(255.),btScalar(255.),btScalar(0.));break;
	189	default:
	190	{
	191	color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));
	192	}
	193	};
	194
	195	debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
	196	}
	197	if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
	198	{
[2430]	199	btVector3 minAabb,maxAabb;
[1963]	200	btVector3 colorvec(1,0,0);
	201	colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
	202	m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
	203	}
	204
	205	}
	206
[2882]	207	if (getDebugDrawer() && getDebugDrawer()->getDebugMode())
[1963]	208	{
[2882]	209	for (i=0;i<m_actions.size();i++)
[1963]	210	{
[2882]	211	m_actions[i]->debugDraw(m_debugDrawer);
[1963]	212	}
	213	}
	214	}
	215	}
	216
	217	void btDiscreteDynamicsWorld::clearForces()
	218	{
[2430]	219	///@todo: iterate over awake simulation islands!
[1963]	220	for ( int i=0;i<m_collisionObjects.size();i++)
	221	{
	222	btCollisionObject* colObj = m_collisionObjects[i];
	223
	224	btRigidBody* body = btRigidBody::upcast(colObj);
	225	if (body)
	226	{
	227	body->clearForces();
	228	}
	229	}
	230	}
	231
	232	///apply gravity, call this once per timestep
	233	void btDiscreteDynamicsWorld::applyGravity()
	234	{
[2430]	235	///@todo: iterate over awake simulation islands!
[1963]	236	for ( int i=0;i<m_collisionObjects.size();i++)
	237	{
	238	btCollisionObject* colObj = m_collisionObjects[i];
	239
	240	btRigidBody* body = btRigidBody::upcast(colObj);
	241	if (body && body->isActive())
	242	{
	243	body->applyGravity();
	244	}
	245	}
	246	}
	247
	248
[2430]	249	void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body)
	250	{
	251	btAssert(body);
[1963]	252
[2430]	253	if (body->getMotionState() && !body->isStaticOrKinematicObject())
	254	{
	255	//we need to call the update at least once, even for sleeping objects
	256	//otherwise the 'graphics' transform never updates properly
	257	///@todo: add 'dirty' flag
	258	//if (body->getActivationState() != ISLAND_SLEEPING)
	259	{
	260	btTransform interpolatedTransform;
	261	btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
	262	body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime*body->getHitFraction(),interpolatedTransform);
	263	body->getMotionState()->setWorldTransform(interpolatedTransform);
	264	}
	265	}
	266	}
	267
	268
[1963]	269	void btDiscreteDynamicsWorld::synchronizeMotionStates()
	270	{
[2430]	271	BT_PROFILE("synchronizeMotionStates");
[1963]	272	{
	273	//todo: iterate over awake simulation islands!
	274	for ( int i=0;i<m_collisionObjects.size();i++)
	275	{
	276	btCollisionObject* colObj = m_collisionObjects[i];
	277
	278	btRigidBody* body = btRigidBody::upcast(colObj);
[2430]	279	if (body)
	280	synchronizeSingleMotionState(body);
[1963]	281	}
	282	}
[2882]	283	/*
[1963]	284	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
	285	{
	286	for ( int i=0;i<this->m_vehicles.size();i++)
	287	{
	288	for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
	289	{
	290	//synchronize the wheels with the (interpolated) chassis worldtransform
	291	m_vehicles[i]->updateWheelTransform(v,true);
	292	}
	293	}
	294	}
[2882]	295	*/
[1963]	296
[2882]	297
[1963]	298	}
	299
	300
	301	int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
	302	{
	303	startProfiling(timeStep);
	304
	305	BT_PROFILE("stepSimulation");
	306
	307	int numSimulationSubSteps = 0;
	308
	309	if (maxSubSteps)
	310	{
	311	//fixed timestep with interpolation
	312	m_localTime += timeStep;
	313	if (m_localTime >= fixedTimeStep)
	314	{
	315	numSimulationSubSteps = int( m_localTime / fixedTimeStep);
	316	m_localTime -= numSimulationSubSteps * fixedTimeStep;
	317	}
	318	} else
	319	{
	320	//variable timestep
	321	fixedTimeStep = timeStep;
	322	m_localTime = timeStep;
	323	if (btFuzzyZero(timeStep))
	324	{
	325	numSimulationSubSteps = 0;
	326	maxSubSteps = 0;
	327	} else
	328	{
	329	numSimulationSubSteps = 1;
	330	maxSubSteps = 1;
	331	}
	332	}
	333
	334	//process some debugging flags
	335	if (getDebugDrawer())
	336	{
[2882]	337	btIDebugDraw* debugDrawer = getDebugDrawer ();
	338	gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
[1963]	339	}
	340	if (numSimulationSubSteps)
	341	{
	342
	343	saveKinematicState(fixedTimeStep);
	344
	345	applyGravity();
	346
	347	//clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
	348	int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps;
	349
	350	for (int i=0;i<clampedSimulationSteps;i++)
	351	{
	352	internalSingleStepSimulation(fixedTimeStep);
	353	synchronizeMotionStates();
	354	}
	355
	356	}
	357
	358	synchronizeMotionStates();
	359
	360	clearForces();
	361
	362	#ifndef BT_NO_PROFILE
	363	CProfileManager::Increment_Frame_Counter();
	364	#endif //BT_NO_PROFILE
	365
	366	return numSimulationSubSteps;
	367	}
	368
	369	void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
	370	{
	371
	372	BT_PROFILE("internalSingleStepSimulation");
	373
	374	///apply gravity, predict motion
	375	predictUnconstraintMotion(timeStep);
	376
	377	btDispatcherInfo& dispatchInfo = getDispatchInfo();
	378
	379	dispatchInfo.m_timeStep = timeStep;
	380	dispatchInfo.m_stepCount = 0;
	381	dispatchInfo.m_debugDraw = getDebugDrawer();
	382
	383	///perform collision detection
	384	performDiscreteCollisionDetection();
	385
	386	calculateSimulationIslands();
	387
	388
	389	getSolverInfo().m_timeStep = timeStep;
	390
	391
	392
	393	///solve contact and other joint constraints
	394	solveConstraints(getSolverInfo());
	395
	396	///CallbackTriggers();
	397
	398	///integrate transforms
	399	integrateTransforms(timeStep);
	400
	401	///update vehicle simulation
[2882]	402	updateActions(timeStep);
[2430]	403
[1963]	404	updateActivationState( timeStep );
	405
	406	if(0 != m_internalTickCallback) {
	407	(*m_internalTickCallback)(this, timeStep);
	408	}
	409	}
	410
	411	void btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)
	412	{
	413	m_gravity = gravity;
	414	for ( int i=0;i<m_collisionObjects.size();i++)
	415	{
	416	btCollisionObject* colObj = m_collisionObjects[i];
	417	btRigidBody* body = btRigidBody::upcast(colObj);
	418	if (body)
	419	{
	420	body->setGravity(gravity);
	421	}
	422	}
	423	}
	424
	425	btVector3 btDiscreteDynamicsWorld::getGravity () const
	426	{
	427	return m_gravity;
	428	}
	429
	430
	431	void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body)
	432	{
	433	removeCollisionObject(body);
	434	}
	435
	436	void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)
	437	{
	438	if (!body->isStaticOrKinematicObject())
	439	{
	440	body->setGravity(m_gravity);
	441	}
	442
	443	if (body->getCollisionShape())
	444	{
	445	bool isDynamic = !(body->isStaticObject() \|\| body->isKinematicObject());
	446	short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
	447	short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
	448
	449	addCollisionObject(body,collisionFilterGroup,collisionFilterMask);
	450	}
	451	}
	452
	453	void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask)
	454	{
	455	if (!body->isStaticOrKinematicObject())
	456	{
	457	body->setGravity(m_gravity);
	458	}
	459
	460	if (body->getCollisionShape())
	461	{
	462	addCollisionObject(body,group,mask);
	463	}
	464	}
	465
	466
[2882]	467	void btDiscreteDynamicsWorld::updateActions(btScalar timeStep)
[1963]	468	{
[2882]	469	BT_PROFILE("updateActions");
[1963]	470
[2882]	471	for ( int i=0;i<m_actions.size();i++)
[1963]	472	{
[2882]	473	m_actions[i]->updateAction( this, timeStep);
[1963]	474	}
	475	}
[2430]	476
	477
[1963]	478	void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
	479	{
	480	BT_PROFILE("updateActivationState");
	481
	482	for ( int i=0;i<m_collisionObjects.size();i++)
	483	{
	484	btCollisionObject* colObj = m_collisionObjects[i];
	485	btRigidBody* body = btRigidBody::upcast(colObj);
	486	if (body)
	487	{
	488	body->updateDeactivation(timeStep);
	489
	490	if (body->wantsSleeping())
	491	{
	492	if (body->isStaticOrKinematicObject())
	493	{
	494	body->setActivationState(ISLAND_SLEEPING);
	495	} else
	496	{
	497	if (body->getActivationState() == ACTIVE_TAG)
	498	body->setActivationState( WANTS_DEACTIVATION );
	499	if (body->getActivationState() == ISLAND_SLEEPING)
	500	{
	501	body->setAngularVelocity(btVector3(0,0,0));
	502	body->setLinearVelocity(btVector3(0,0,0));
	503	}
	504
	505	}
	506	} else
	507	{
	508	if (body->getActivationState() != DISABLE_DEACTIVATION)
	509	body->setActivationState( ACTIVE_TAG );
	510	}
	511	}
	512	}
	513	}
	514
	515	void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies)
	516	{
	517	m_constraints.push_back(constraint);
	518	if (disableCollisionsBetweenLinkedBodies)
	519	{
	520	constraint->getRigidBodyA().addConstraintRef(constraint);
	521	constraint->getRigidBodyB().addConstraintRef(constraint);
	522	}
	523	}
	524
	525	void btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint)
	526	{
	527	m_constraints.remove(constraint);
	528	constraint->getRigidBodyA().removeConstraintRef(constraint);
	529	constraint->getRigidBodyB().removeConstraintRef(constraint);
	530	}
	531
[2882]	532	void btDiscreteDynamicsWorld::addAction(btActionInterface* action)
[1963]	533	{
[2882]	534	m_actions.push_back(action);
[1963]	535	}
	536
[2882]	537	void btDiscreteDynamicsWorld::removeAction(btActionInterface* action)
[1963]	538	{
[2882]	539	m_actions.remove(action);
[1963]	540	}
	541
[2882]	542
	543	void btDiscreteDynamicsWorld::addVehicle(btActionInterface* vehicle)
[2430]	544	{
[2882]	545	addAction(vehicle);
[2430]	546	}
	547
[2882]	548	void btDiscreteDynamicsWorld::removeVehicle(btActionInterface* vehicle)
[2430]	549	{
[2882]	550	removeAction(vehicle);
[2430]	551	}
	552
[2882]	553	void btDiscreteDynamicsWorld::addCharacter(btActionInterface* character)
	554	{
	555	addAction(character);
	556	}
[2430]	557
[2882]	558	void btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character)
	559	{
	560	removeAction(character);
	561	}
	562
	563
[1963]	564	SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs)
	565	{
	566	int islandId;
	567
	568	const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
	569	const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
	570	islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag();
	571	return islandId;
	572
	573	}
	574
	575
	576	class btSortConstraintOnIslandPredicate
	577	{
	578	public:
	579
	580	bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs )
	581	{
	582	int rIslandId0,lIslandId0;
	583	rIslandId0 = btGetConstraintIslandId(rhs);
	584	lIslandId0 = btGetConstraintIslandId(lhs);
	585	return lIslandId0 < rIslandId0;
	586	}
	587	};
	588
	589
	590
	591
	592	void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
	593	{
	594	BT_PROFILE("solveConstraints");
	595
	596	struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
	597	{
	598
	599	btContactSolverInfo& m_solverInfo;
	600	btConstraintSolver* m_solver;
	601	btTypedConstraint** m_sortedConstraints;
	602	int m_numConstraints;
	603	btIDebugDraw* m_debugDrawer;
	604	btStackAlloc* m_stackAlloc;
	605	btDispatcher* m_dispatcher;
	606
	607	InplaceSolverIslandCallback(
	608	btContactSolverInfo& solverInfo,
	609	btConstraintSolver* solver,
	610	btTypedConstraint** sortedConstraints,
	611	int numConstraints,
	612	btIDebugDraw* debugDrawer,
	613	btStackAlloc* stackAlloc,
	614	btDispatcher* dispatcher)
	615	:m_solverInfo(solverInfo),
	616	m_solver(solver),
	617	m_sortedConstraints(sortedConstraints),
	618	m_numConstraints(numConstraints),
	619	m_debugDrawer(debugDrawer),
	620	m_stackAlloc(stackAlloc),
	621	m_dispatcher(dispatcher)
	622	{
	623
	624	}
	625
	626	InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other)
	627	{
	628	btAssert(0);
	629	(void)other;
	630	return *this;
	631	}
	632	virtual void ProcessIsland(btCollisionObject bodies,int numBodies,btPersistentManifold manifolds,int numManifolds, int islandId)
	633	{
	634	if (islandId<0)
	635	{
[2882]	636	if (numManifolds + m_numConstraints)
	637	{
	638	///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
	639	m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
	640	}
[1963]	641	} else
	642	{
	643	//also add all non-contact constraints/joints for this island
	644	btTypedConstraint** startConstraint = 0;
	645	int numCurConstraints = 0;
	646	int i;
	647
	648	//find the first constraint for this island
	649	for (i=0;i<m_numConstraints;i++)
	650	{
	651	if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
	652	{
	653	startConstraint = &m_sortedConstraints[i];
	654	break;
	655	}
	656	}
	657	//count the number of constraints in this island
	658	for (;i<m_numConstraints;i++)
	659	{
	660	if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
	661	{
	662	numCurConstraints++;
	663	}
	664	}
	665
	666	///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive
	667	if (numManifolds + numCurConstraints)
	668	{
	669	m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
	670	}
	671
	672	}
	673	}
	674
	675	};
	676
	677	//sorted version of all btTypedConstraint, based on islandId
	678	btAlignedObjectArray<btTypedConstraint*> sortedConstraints;
	679	sortedConstraints.resize( m_constraints.size());
	680	int i;
	681	for (i=0;i<getNumConstraints();i++)
	682	{
	683	sortedConstraints[i] = m_constraints[i];
	684	}
	685
[2882]	686	// btAssert(0);
[1963]	687
	688
	689
	690	sortedConstraints.quickSort(btSortConstraintOnIslandPredicate());
	691
	692	btTypedConstraint** constraintsPtr = getNumConstraints() ? &sortedConstraints[0] : 0;
	693
	694	InplaceSolverIslandCallback solverCallback( solverInfo, m_constraintSolver, constraintsPtr,sortedConstraints.size(), m_debugDrawer,m_stackAlloc,m_dispatcher1);
	695
	696	m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
	697
	698	/// solve all the constraints for this island
[2430]	699	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),&solverCallback);
[1963]	700
	701	m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc);
	702	}
	703
	704
	705
	706
	707	void btDiscreteDynamicsWorld::calculateSimulationIslands()
	708	{
	709	BT_PROFILE("calculateSimulationIslands");
	710
	711	getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());
	712
	713	{
	714	int i;
	715	int numConstraints = int(m_constraints.size());
	716	for (i=0;i< numConstraints ; i++ )
	717	{
	718	btTypedConstraint* constraint = m_constraints[i];
	719
	720	const btRigidBody* colObj0 = &constraint->getRigidBodyA();
	721	const btRigidBody* colObj1 = &constraint->getRigidBodyB();
	722
	723	if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
	724	((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
	725	{
	726	if (colObj0->isActive() \|\| colObj1->isActive())
	727	{
	728
	729	getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),
	730	(colObj1)->getIslandTag());
	731	}
	732	}
	733	}
	734	}
	735
	736	//Store the island id in each body
	737	getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
	738
	739
	740	}
	741
	742
	743	#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
	744
	745	class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
	746	{
	747	btCollisionObject* m_me;
	748	btScalar m_allowedPenetration;
	749	btOverlappingPairCache* m_pairCache;
[2882]	750	btDispatcher* m_dispatcher;
[1963]	751
	752
	753	public:
[2882]	754	btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
[1963]	755	btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
	756	m_allowedPenetration(0.0f),
	757	m_me(me),
[2882]	758	m_pairCache(pairCache),
	759	m_dispatcher(dispatcher)
[1963]	760	{
	761	}
	762
	763	virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace)
	764	{
	765	if (convexResult.m_hitCollisionObject == m_me)
[2430]	766	return 1.0f;
[1963]	767
[2430]	768	//ignore result if there is no contact response
	769	if(!convexResult.m_hitCollisionObject->hasContactResponse())
	770	return 1.0f;
	771
[1963]	772	btVector3 linVelA,linVelB;
	773	linVelA = m_convexToWorld-m_convexFromWorld;
	774	linVelB = btVector3(0,0,0);//toB.getOrigin()-fromB.getOrigin();
	775
	776	btVector3 relativeVelocity = (linVelA-linVelB);
	777	//don't report time of impact for motion away from the contact normal (or causes minor penetration)
	778	if (convexResult.m_hitNormalLocal.dot(relativeVelocity)>=-m_allowedPenetration)
	779	return 1.f;
	780
	781	return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace);
	782	}
	783
	784	virtual bool needsCollision(btBroadphaseProxy* proxy0) const
	785	{
	786	//don't collide with itself
	787	if (proxy0->m_clientObject == m_me)
	788	return false;
	789
	790	///don't do CCD when the collision filters are not matching
[2430]	791	if (!ClosestConvexResultCallback::needsCollision(proxy0))
[1963]	792	return false;
	793
[2882]	794	btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
	795
	796	//call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179
	797	if (m_dispatcher->needsResponse(m_me,otherObj))
[1963]	798	{
[2882]	799	///don't do CCD when there are already contact points (touching contact/penetration)
	800	btAlignedObjectArray<btPersistentManifold*> manifoldArray;
	801	btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0);
	802	if (collisionPair)
[1963]	803	{
[2882]	804	if (collisionPair->m_algorithm)
[1963]	805	{
[2882]	806	manifoldArray.resize(0);
	807	collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);
	808	for (int j=0;j<manifoldArray.size();j++)
	809	{
	810	btPersistentManifold* manifold = manifoldArray[j];
	811	if (manifold->getNumContacts()>0)
	812	return false;
	813	}
[1963]	814	}
	815	}
	816	}
	817	return true;
	818	}
	819
	820
	821	};
	822
	823	///internal debugging variable. this value shouldn't be too high
	824	int gNumClampedCcdMotions=0;
	825
	826	//#include "stdio.h"
	827	void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
	828	{
	829	BT_PROFILE("integrateTransforms");
	830	btTransform predictedTrans;
	831	for ( int i=0;i<m_collisionObjects.size();i++)
	832	{
	833	btCollisionObject* colObj = m_collisionObjects[i];
	834	btRigidBody* body = btRigidBody::upcast(colObj);
	835	if (body)
	836	{
	837	body->setHitFraction(1.f);
	838
	839	if (body->isActive() && (!body->isStaticOrKinematicObject()))
	840	{
	841	body->predictIntegratedTransform(timeStep, predictedTrans);
	842	btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
	843
	844	if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
	845	{
	846	BT_PROFILE("CCD motion clamping");
	847	if (body->getCollisionShape()->isConvex())
	848	{
	849	gNumClampedCcdMotions++;
	850
[2882]	851	btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
[1963]	852	btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
	853	btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
[2430]	854
	855	sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
	856	sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask;
	857
[1963]	858	convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults);
	859	if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
	860	{
	861	body->setHitFraction(sweepResults.m_closestHitFraction);
	862	body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
	863	body->setHitFraction(0.f);
	864	// printf("clamped integration to hit fraction = %f\n",fraction);
	865	}
	866	}
	867	}
	868
	869	body->proceedToTransform( predictedTrans);
	870	}
	871	}
	872	}
	873	}
	874
	875
	876
[2430]	877
	878
[1963]	879	void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
	880	{
	881	BT_PROFILE("predictUnconstraintMotion");
	882	for ( int i=0;i<m_collisionObjects.size();i++)
	883	{
	884	btCollisionObject* colObj = m_collisionObjects[i];
	885	btRigidBody* body = btRigidBody::upcast(colObj);
	886	if (body)
	887	{
	888	if (!body->isStaticOrKinematicObject())
	889	{
[2430]	890
	891	body->integrateVelocities( timeStep);
	892	//damping
	893	body->applyDamping(timeStep);
[1963]	894
[2430]	895	body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
[1963]	896	}
	897	}
	898	}
	899	}
	900
	901
	902	void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep)
	903	{
	904	(void)timeStep;
	905
	906	#ifndef BT_NO_PROFILE
	907	CProfileManager::Reset();
	908	#endif //BT_NO_PROFILE
	909
	910	}
	911
	912
	913
	914
	915
	916
	917	class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
	918	{
	919	btIDebugDraw* m_debugDrawer;
	920	btVector3 m_color;
	921	btTransform m_worldTrans;
	922
	923	public:
	924
	925	DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) :
	926	m_debugDrawer(debugDrawer),
	927	m_color(color),
	928	m_worldTrans(worldTrans)
	929	{
	930	}
	931
	932	virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex)
	933	{
	934	processTriangle(triangle,partId,triangleIndex);
	935	}
	936
	937	virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
	938	{
	939	(void)partId;
	940	(void)triangleIndex;
	941
	942	btVector3 wv0,wv1,wv2;
	943	wv0 = m_worldTrans*triangle[0];
	944	wv1 = m_worldTrans*triangle[1];
	945	wv2 = m_worldTrans*triangle[2];
	946	m_debugDrawer->drawLine(wv0,wv1,m_color);
	947	m_debugDrawer->drawLine(wv1,wv2,m_color);
	948	m_debugDrawer->drawLine(wv2,wv0,m_color);
	949	}
	950	};
	951
	952	void btDiscreteDynamicsWorld::debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color)
	953	{
	954	btVector3 start = transform.getOrigin();
	955
	956	const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0);
	957	const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0);
	958	const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius);
	959
	960	// XY
	961	getDebugDrawer()->drawLine(start-xoffs, start+yoffs, color);
	962	getDebugDrawer()->drawLine(start+yoffs, start+xoffs, color);
	963	getDebugDrawer()->drawLine(start+xoffs, start-yoffs, color);
	964	getDebugDrawer()->drawLine(start-yoffs, start-xoffs, color);
	965
	966	// XZ
	967	getDebugDrawer()->drawLine(start-xoffs, start+zoffs, color);
	968	getDebugDrawer()->drawLine(start+zoffs, start+xoffs, color);
	969	getDebugDrawer()->drawLine(start+xoffs, start-zoffs, color);
	970	getDebugDrawer()->drawLine(start-zoffs, start-xoffs, color);
	971
	972	// YZ
	973	getDebugDrawer()->drawLine(start-yoffs, start+zoffs, color);
	974	getDebugDrawer()->drawLine(start+zoffs, start+yoffs, color);
	975	getDebugDrawer()->drawLine(start+yoffs, start-zoffs, color);
	976	getDebugDrawer()->drawLine(start-zoffs, start-yoffs, color);
	977	}
	978
	979	void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
	980	{
	981	// Draw a small simplex at the center of the object
	982	{
	983	btVector3 start = worldTransform.getOrigin();
	984	getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(1,0,0), btVector3(1,0,0));
	985	getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,1,0), btVector3(0,1,0));
	986	getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,0,1), btVector3(0,0,1));
	987	}
	988
	989	if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
	990	{
	991	const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
	992	for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
	993	{
	994	btTransform childTrans = compoundShape->getChildTransform(i);
	995	const btCollisionShape* colShape = compoundShape->getChildShape(i);
	996	debugDrawObject(worldTransform*childTrans,colShape,color);
	997	}
	998
	999	} else
	1000	{
	1001	switch (shape->getShapeType())
	1002	{
	1003
	1004	case SPHERE_SHAPE_PROXYTYPE:
	1005	{
	1006	const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
	1007	btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
	1008
	1009	debugDrawSphere(radius, worldTransform, color);
	1010	break;
	1011	}
	1012	case MULTI_SPHERE_SHAPE_PROXYTYPE:
	1013	{
	1014	const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
	1015
	1016	for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
	1017	{
	1018	btTransform childTransform = worldTransform;
	1019	childTransform.getOrigin() += multiSphereShape->getSpherePosition(i);
	1020	debugDrawSphere(multiSphereShape->getSphereRadius(i), childTransform, color);
	1021	}
	1022
	1023	break;
	1024	}
	1025	case CAPSULE_SHAPE_PROXYTYPE:
	1026	{
	1027	const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
	1028
	1029	btScalar radius = capsuleShape->getRadius();
	1030	btScalar halfHeight = capsuleShape->getHalfHeight();
	1031
	1032	int upAxis = capsuleShape->getUpAxis();
	1033
	1034
	1035	btVector3 capStart(0.f,0.f,0.f);
	1036	capStart[upAxis] = -halfHeight;
	1037
	1038	btVector3 capEnd(0.f,0.f,0.f);
	1039	capEnd[upAxis] = halfHeight;
	1040
	1041	// Draw the ends
	1042	{
	1043
	1044	btTransform childTransform = worldTransform;
	1045	childTransform.getOrigin() = worldTransform * capStart;
	1046	debugDrawSphere(radius, childTransform, color);
	1047	}
	1048
	1049	{
	1050	btTransform childTransform = worldTransform;
	1051	childTransform.getOrigin() = worldTransform * capEnd;
	1052	debugDrawSphere(radius, childTransform, color);
	1053	}
	1054
	1055	// Draw some additional lines
	1056	btVector3 start = worldTransform.getOrigin();
	1057
	1058
	1059	capStart[(upAxis+1)%3] = radius;
	1060	capEnd[(upAxis+1)%3] = radius;
	1061	getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
	1062	capStart[(upAxis+1)%3] = -radius;
	1063	capEnd[(upAxis+1)%3] = -radius;
	1064	getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
	1065
	1066	capStart[(upAxis+1)%3] = 0.f;
	1067	capEnd[(upAxis+1)%3] = 0.f;
	1068
	1069	capStart[(upAxis+2)%3] = radius;
	1070	capEnd[(upAxis+2)%3] = radius;
	1071	getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
	1072	capStart[(upAxis+2)%3] = -radius;
	1073	capEnd[(upAxis+2)%3] = -radius;
	1074	getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
	1075
	1076
	1077	break;
	1078	}
	1079	case CONE_SHAPE_PROXYTYPE:
	1080	{
	1081	const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
	1082	btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
	1083	btScalar height = coneShape->getHeight();//+coneShape->getMargin();
	1084	btVector3 start = worldTransform.getOrigin();
	1085
	1086	int upAxis= coneShape->getConeUpIndex();
	1087
	1088
	1089	btVector3 offsetHeight(0,0,0);
	1090	offsetHeight[upAxis] = height * btScalar(0.5);
	1091	btVector3 offsetRadius(0,0,0);
	1092	offsetRadius[(upAxis+1)%3] = radius;
	1093	btVector3 offset2Radius(0,0,0);
	1094	offset2Radius[(upAxis+2)%3] = radius;
	1095
	1096	getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
	1097	getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
	1098	getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color);
	1099	getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color);
	1100
	1101
	1102
	1103	break;
	1104
	1105	}
	1106	case CYLINDER_SHAPE_PROXYTYPE:
	1107	{
	1108	const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
	1109	int upAxis = cylinder->getUpAxis();
	1110	btScalar radius = cylinder->getRadius();
	1111	btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
	1112	btVector3 start = worldTransform.getOrigin();
	1113	btVector3 offsetHeight(0,0,0);
	1114	offsetHeight[upAxis] = halfHeight;
	1115	btVector3 offsetRadius(0,0,0);
	1116	offsetRadius[(upAxis+1)%3] = radius;
	1117	getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
	1118	getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
	1119	break;
	1120	}
	1121
	1122	case STATIC_PLANE_PROXYTYPE:
	1123	{
	1124	const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
	1125	btScalar planeConst = staticPlaneShape->getPlaneConstant();
	1126	const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
	1127	btVector3 planeOrigin = planeNormal * planeConst;
	1128	btVector3 vec0,vec1;
	1129	btPlaneSpace1(planeNormal,vec0,vec1);
	1130	btScalar vecLen = 100.f;
	1131	btVector3 pt0 = planeOrigin + vec0*vecLen;
	1132	btVector3 pt1 = planeOrigin - vec0*vecLen;
	1133	btVector3 pt2 = planeOrigin + vec1*vecLen;
	1134	btVector3 pt3 = planeOrigin - vec1*vecLen;
	1135	getDebugDrawer()->drawLine(worldTransformpt0,worldTransformpt1,color);
	1136	getDebugDrawer()->drawLine(worldTransformpt2,worldTransformpt3,color);
	1137	break;
	1138
	1139	}
	1140	default:
	1141	{
	1142
	1143	if (shape->isConcave())
	1144	{
	1145	btConcaveShape* concaveMesh = (btConcaveShape*) shape;
	1146
[2430]	1147	///@todo pass camera, for some culling? no -> we are not a graphics lib
[1963]	1148	btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
	1149	btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
	1150
	1151	DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
	1152	concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
	1153
	1154	}
	1155
	1156	if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
	1157	{
	1158	btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
	1159	//todo: pass camera for some culling
	1160	btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
	1161	btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
	1162	//DebugDrawcallback drawCallback;
	1163	DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
	1164	convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
	1165	}
	1166
	1167
	1168	/// for polyhedral shapes
	1169	if (shape->isPolyhedral())
	1170	{
	1171	btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
	1172
	1173	int i;
	1174	for (i=0;i<polyshape->getNumEdges();i++)
	1175	{
[2430]	1176	btVector3 a,b;
[1963]	1177	polyshape->getEdge(i,a,b);
	1178	btVector3 wa = worldTransform * a;
	1179	btVector3 wb = worldTransform * b;
	1180	getDebugDrawer()->drawLine(wa,wb,color);
	1181
	1182	}
	1183
	1184
	1185	}
	1186	}
	1187	}
	1188	}
	1189	}
	1190
	1191
[2882]	1192	void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
	1193	{
	1194	bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0;
	1195	bool drawLimits = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraintLimits) != 0;
	1196	btScalar dbgDrawSize = constraint->getDbgDrawSize();
	1197	if(dbgDrawSize <= btScalar(0.f))
	1198	{
	1199	return;
	1200	}
	1201
	1202	switch(constraint->getConstraintType())
	1203	{
	1204	case POINT2POINT_CONSTRAINT_TYPE:
	1205	{
	1206	btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint;
	1207	btTransform tr;
	1208	tr.setIdentity();
	1209	btVector3 pivot = p2pC->getPivotInA();
	1210	pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot;
	1211	tr.setOrigin(pivot);
	1212	getDebugDrawer()->drawTransform(tr, dbgDrawSize);
	1213	// that ideally should draw the same frame
	1214	pivot = p2pC->getPivotInB();
	1215	pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot;
	1216	tr.setOrigin(pivot);
	1217	if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
	1218	}
	1219	break;
	1220	case HINGE_CONSTRAINT_TYPE:
	1221	{
	1222	btHingeConstraint* pHinge = (btHingeConstraint*)constraint;
	1223	btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame();
	1224	if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
	1225	tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame();
	1226	if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
	1227	btScalar minAng = pHinge->getLowerLimit();
	1228	btScalar maxAng = pHinge->getUpperLimit();
	1229	if(minAng == maxAng)
	1230	{
	1231	break;
	1232	}
	1233	bool drawSect = true;
	1234	if(minAng > maxAng)
	1235	{
	1236	minAng = btScalar(0.f);
	1237	maxAng = SIMD_2_PI;
	1238	drawSect = false;
	1239	}
	1240	if(drawLimits)
	1241	{
	1242	btVector3& center = tr.getOrigin();
	1243	btVector3 normal = tr.getBasis().getColumn(2);
	1244	btVector3 axis = tr.getBasis().getColumn(0);
	1245	getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0,0,0), drawSect);
	1246	}
	1247	}
	1248	break;
	1249	case CONETWIST_CONSTRAINT_TYPE:
	1250	{
	1251	btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint;
	1252	btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
	1253	if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
	1254	tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
	1255	if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
	1256	if(drawLimits)
	1257	{
	1258	//const btScalar length = btScalar(5);
	1259	const btScalar length = dbgDrawSize;
	1260	static int nSegments = 8*4;
	1261	btScalar fAngleInRadians = btScalar(2.3.1415926) (btScalar)(nSegments-1)/btScalar(nSegments);
	1262	btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length);
	1263	pPrev = tr * pPrev;
	1264	for (int i=0; i<nSegments; i++)
	1265	{
	1266	fAngleInRadians = btScalar(2.3.1415926) (btScalar)i/btScalar(nSegments);
	1267	btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length);
	1268	pCur = tr * pCur;
	1269	getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0,0,0));
	1270
	1271	if (i%(nSegments/8) == 0)
	1272	getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0));
	1273
	1274	pPrev = pCur;
	1275	}
	1276	btScalar tws = pCT->getTwistSpan();
	1277	btScalar twa = pCT->getTwistAngle();
	1278	bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f));
	1279	if(useFrameB)
	1280	{
	1281	tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
	1282	}
	1283	else
	1284	{
	1285	tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
	1286	}
	1287	btVector3 pivot = tr.getOrigin();
	1288	btVector3 normal = tr.getBasis().getColumn(0);
	1289	btVector3 axis1 = tr.getBasis().getColumn(1);
	1290	getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa-tws, -twa+tws, btVector3(0,0,0), true);
	1291
	1292	}
	1293	}
	1294	break;
	1295	case D6_CONSTRAINT_TYPE:
	1296	{
	1297	btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint;
	1298	btTransform tr = p6DOF->getCalculatedTransformA();
	1299	if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
	1300	tr = p6DOF->getCalculatedTransformB();
	1301	if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
	1302	if(drawLimits)
	1303	{
	1304	tr = p6DOF->getCalculatedTransformA();
	1305	const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
	1306	btVector3 up = tr.getBasis().getColumn(2);
	1307	btVector3 axis = tr.getBasis().getColumn(0);
	1308	btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
	1309	btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
	1310	btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
	1311	btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
	1312	getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0,0,0));
	1313	axis = tr.getBasis().getColumn(1);
	1314	btScalar ay = p6DOF->getAngle(1);
	1315	btScalar az = p6DOF->getAngle(2);
	1316	btScalar cy = btCos(ay);
	1317	btScalar sy = btSin(ay);
	1318	btScalar cz = btCos(az);
	1319	btScalar sz = btSin(az);
	1320	btVector3 ref;
	1321	ref[0] = cyczaxis[0] + cyszaxis[1] - sy*axis[2];
	1322	ref[1] = -szaxis[0] + czaxis[1];
	1323	ref[2] = czsyaxis[0] + szsyaxis[1] + cy*axis[2];
	1324	tr = p6DOF->getCalculatedTransformB();
	1325	btVector3 normal = -tr.getBasis().getColumn(0);
	1326	btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;
	1327	btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit;
	1328	if(minFi > maxFi)
	1329	{
	1330	getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0,0,0), false);
	1331	}
	1332	else if(minFi < maxFi)
	1333	{
	1334	getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0,0,0), true);
	1335	}
	1336	tr = p6DOF->getCalculatedTransformA();
	1337	btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit;
	1338	btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit;
	1339	getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0,0,0));
	1340	}
	1341	}
	1342	break;
	1343	case SLIDER_CONSTRAINT_TYPE:
	1344	{
	1345	btSliderConstraint* pSlider = (btSliderConstraint*)constraint;
	1346	btTransform tr = pSlider->getCalculatedTransformA();
	1347	if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
	1348	tr = pSlider->getCalculatedTransformB();
	1349	if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
	1350	if(drawLimits)
	1351	{
	1352	btTransform tr = pSlider->getCalculatedTransformA();
	1353	btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f);
	1354	btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f);
	1355	getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0));
	1356	btVector3 normal = tr.getBasis().getColumn(0);
	1357	btVector3 axis = tr.getBasis().getColumn(1);
	1358	btScalar a_min = pSlider->getLowerAngLimit();
	1359	btScalar a_max = pSlider->getUpperAngLimit();
	1360	const btVector3& center = pSlider->getCalculatedTransformB().getOrigin();
	1361	getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0,0,0), true);
	1362	}
	1363	}
	1364	break;
	1365	default :
	1366	break;
	1367	}
	1368	return;
	1369	} // btDiscreteDynamicsWorld::debugDrawConstraint()
	1370
	1371
	1372
	1373
	1374
[1963]	1375	void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
	1376	{
	1377	if (m_ownsConstraintSolver)
	1378	{
	1379	btAlignedFree( m_constraintSolver);
	1380	}
	1381	m_ownsConstraintSolver = false;
	1382	m_constraintSolver = solver;
	1383	}
	1384
	1385	btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver()
	1386	{
	1387	return m_constraintSolver;
	1388	}
	1389
	1390
	1391	int btDiscreteDynamicsWorld::getNumConstraints() const
	1392	{
	1393	return int(m_constraints.size());
	1394	}
	1395	btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index)
	1396	{
	1397	return m_constraints[index];
	1398	}
	1399	const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const
	1400	{
	1401	return m_constraints[index];
	1402	}
[2882]	1403
	1404

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

Download in other formats: