Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

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

Last change on this file since 11772 was 8393, checked in by rgrieder, 14 years ago

Updated Bullet from v2.77 to v2.78.
(I'm not going to make a branch for that since the update from 2.74 to 2.77 hasn't been tested that much either).

You will HAVE to do a complete RECOMPILE! I tested with MSVC and MinGW and they both threw linker errors at me.

Property svn:eol-style set to native

File size: 39.8 KB

Rev	Line
[1963]	1	/*
	2	Bullet Continuous Collision Detection and Physics Library
[8351]	3	Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
[1963]	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"
[8351]	22	#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
[1963]	23	#include "BulletCollision/CollisionShapes/btCollisionShape.h"
	24	#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
	25	#include "LinearMath/btTransformUtil.h"
	26	#include "LinearMath/btQuickprof.h"
	27
	28	//rigidbody & constraints
	29	#include "BulletDynamics/Dynamics/btRigidBody.h"
	30	#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
	31	#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
	32	#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
[2882]	33	#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
	34	#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
	35	#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
	36	#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
	37	#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
[8393]	38	#include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
[1963]	39
[8393]	40
[8351]	41	#include "LinearMath/btIDebugDraw.h"
[1963]	42	#include "BulletCollision/CollisionShapes/btSphereShape.h"
	43
	44
[2882]	45	#include "BulletDynamics/Dynamics/btActionInterface.h"
[1963]	46	#include "LinearMath/btQuickprof.h"
	47	#include "LinearMath/btMotionState.h"
	48
[8351]	49	#include "LinearMath/btSerializer.h"
[1963]	50
[8393]	51	#if 0
	52	btAlignedObjectArray<btVector3> debugContacts;
	53	btAlignedObjectArray<btVector3> debugNormals;
	54	int startHit=2;
	55	int firstHit=startHit;
	56	#endif
[1963]	57
	58
[8393]	59
[1963]	60	btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
	61	:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration),
	62	m_constraintSolver(constraintSolver),
	63	m_gravity(0,-10,0),
[8351]	64	m_localTime(0),
	65	m_synchronizeAllMotionStates(false),
[1963]	66	m_profileTimings(0)
	67	{
	68	if (!m_constraintSolver)
	69	{
	70	void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16);
	71	m_constraintSolver = new (mem) btSequentialImpulseConstraintSolver;
	72	m_ownsConstraintSolver = true;
	73	} else
	74	{
	75	m_ownsConstraintSolver = false;
	76	}
	77
	78	{
	79	void* mem = btAlignedAlloc(sizeof(btSimulationIslandManager),16);
	80	m_islandManager = new (mem) btSimulationIslandManager();
	81	}
	82
	83	m_ownsIslandManager = true;
	84	}
	85
	86
	87	btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld()
	88	{
	89	//only delete it when we created it
	90	if (m_ownsIslandManager)
	91	{
	92	m_islandManager->~btSimulationIslandManager();
	93	btAlignedFree( m_islandManager);
	94	}
	95	if (m_ownsConstraintSolver)
	96	{
	97
	98	m_constraintSolver->~btConstraintSolver();
	99	btAlignedFree(m_constraintSolver);
	100	}
	101	}
	102
	103	void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
	104	{
[8351]	105	///would like to iterate over m_nonStaticRigidBodies, but unfortunately old API allows
	106	///to switch status _after_ adding kinematic objects to the world
	107	///fix it for Bullet 3.x release
[1963]	108	for (int i=0;i<m_collisionObjects.size();i++)
	109	{
	110	btCollisionObject* colObj = m_collisionObjects[i];
	111	btRigidBody* body = btRigidBody::upcast(colObj);
[8351]	112	if (body && body->getActivationState() != ISLAND_SLEEPING)
[1963]	113	{
[8351]	114	if (body->isKinematicObject())
	115	{
	116	//to calculate velocities next frame
	117	body->saveKinematicState(timeStep);
	118	}
[1963]	119	}
	120	}
[8351]	121
[1963]	122	}
	123
	124	void btDiscreteDynamicsWorld::debugDrawWorld()
	125	{
[2430]	126	BT_PROFILE("debugDrawWorld");
[1963]	127
[8351]	128	btCollisionWorld::debugDrawWorld();
[1963]	129
[2882]	130	bool drawConstraints = false;
	131	if (getDebugDrawer())
	132	{
	133	int mode = getDebugDrawer()->getDebugMode();
	134	if(mode & (btIDebugDraw::DBG_DrawConstraints \| btIDebugDraw::DBG_DrawConstraintLimits))
	135	{
	136	drawConstraints = true;
	137	}
	138	}
	139	if(drawConstraints)
	140	{
	141	for(int i = getNumConstraints()-1; i>=0 ;i--)
	142	{
	143	btTypedConstraint* constraint = getConstraint(i);
	144	debugDrawConstraint(constraint);
	145	}
	146	}
[1963]	147
	148
[2882]	149
[1963]	150	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe \| btIDebugDraw::DBG_DrawAabb))
	151	{
	152	int i;
	153
[2882]	154	if (getDebugDrawer() && getDebugDrawer()->getDebugMode())
[1963]	155	{
[2882]	156	for (i=0;i<m_actions.size();i++)
[1963]	157	{
[2882]	158	m_actions[i]->debugDraw(m_debugDrawer);
[1963]	159	}
	160	}
	161	}
	162	}
	163
	164	void btDiscreteDynamicsWorld::clearForces()
	165	{
[2430]	166	///@todo: iterate over awake simulation islands!
[8351]	167	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
[1963]	168	{
[8351]	169	btRigidBody* body = m_nonStaticRigidBodies[i];
	170	//need to check if next line is ok
	171	//it might break backward compatibility (people applying forces on sleeping objects get never cleared and accumulate on wake-up
	172	body->clearForces();
[1963]	173	}
	174	}
	175
	176	///apply gravity, call this once per timestep
	177	void btDiscreteDynamicsWorld::applyGravity()
	178	{
[2430]	179	///@todo: iterate over awake simulation islands!
[8351]	180	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
[1963]	181	{
[8351]	182	btRigidBody* body = m_nonStaticRigidBodies[i];
	183	if (body->isActive())
[1963]	184	{
	185	body->applyGravity();
	186	}
	187	}
	188	}
	189
	190
[2430]	191	void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body)
	192	{
	193	btAssert(body);
[1963]	194
[2430]	195	if (body->getMotionState() && !body->isStaticOrKinematicObject())
	196	{
	197	//we need to call the update at least once, even for sleeping objects
	198	//otherwise the 'graphics' transform never updates properly
	199	///@todo: add 'dirty' flag
	200	//if (body->getActivationState() != ISLAND_SLEEPING)
	201	{
	202	btTransform interpolatedTransform;
	203	btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
	204	body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime*body->getHitFraction(),interpolatedTransform);
	205	body->getMotionState()->setWorldTransform(interpolatedTransform);
	206	}
	207	}
	208	}
	209
	210
[1963]	211	void btDiscreteDynamicsWorld::synchronizeMotionStates()
	212	{
[2430]	213	BT_PROFILE("synchronizeMotionStates");
[8351]	214	if (m_synchronizeAllMotionStates)
[1963]	215	{
[8351]	216	//iterate over all collision objects
[1963]	217	for ( int i=0;i<m_collisionObjects.size();i++)
	218	{
	219	btCollisionObject* colObj = m_collisionObjects[i];
	220	btRigidBody* body = btRigidBody::upcast(colObj);
[2430]	221	if (body)
	222	synchronizeSingleMotionState(body);
[1963]	223	}
[8351]	224	} else
[1963]	225	{
[8351]	226	//iterate over all active rigid bodies
	227	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
[1963]	228	{
[8351]	229	btRigidBody* body = m_nonStaticRigidBodies[i];
	230	if (body->isActive())
	231	synchronizeSingleMotionState(body);
[1963]	232	}
	233	}
	234	}
	235
	236
	237	int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
	238	{
	239	startProfiling(timeStep);
	240
	241	BT_PROFILE("stepSimulation");
	242
	243	int numSimulationSubSteps = 0;
	244
	245	if (maxSubSteps)
	246	{
	247	//fixed timestep with interpolation
	248	m_localTime += timeStep;
	249	if (m_localTime >= fixedTimeStep)
	250	{
	251	numSimulationSubSteps = int( m_localTime / fixedTimeStep);
	252	m_localTime -= numSimulationSubSteps * fixedTimeStep;
	253	}
	254	} else
	255	{
	256	//variable timestep
	257	fixedTimeStep = timeStep;
	258	m_localTime = timeStep;
	259	if (btFuzzyZero(timeStep))
	260	{
	261	numSimulationSubSteps = 0;
	262	maxSubSteps = 0;
	263	} else
	264	{
	265	numSimulationSubSteps = 1;
	266	maxSubSteps = 1;
	267	}
	268	}
	269
	270	//process some debugging flags
	271	if (getDebugDrawer())
	272	{
[2882]	273	btIDebugDraw* debugDrawer = getDebugDrawer ();
	274	gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
[1963]	275	}
	276	if (numSimulationSubSteps)
	277	{
	278
[8351]	279	//clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
	280	int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps;
[1963]	281
[8351]	282	saveKinematicState(fixedTimeStep*clampedSimulationSteps);
	283
[1963]	284	applyGravity();
	285
[8351]	286
[1963]	287
	288	for (int i=0;i<clampedSimulationSteps;i++)
	289	{
	290	internalSingleStepSimulation(fixedTimeStep);
	291	synchronizeMotionStates();
	292	}
	293
[8351]	294	} else
	295	{
	296	synchronizeMotionStates();
	297	}
[1963]	298
	299	clearForces();
	300
	301	#ifndef BT_NO_PROFILE
	302	CProfileManager::Increment_Frame_Counter();
	303	#endif //BT_NO_PROFILE
	304
	305	return numSimulationSubSteps;
	306	}
	307
	308	void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
	309	{
	310
	311	BT_PROFILE("internalSingleStepSimulation");
	312
[8351]	313	if(0 != m_internalPreTickCallback) {
	314	(*m_internalPreTickCallback)(this, timeStep);
	315	}
	316
[1963]	317	///apply gravity, predict motion
	318	predictUnconstraintMotion(timeStep);
	319
	320	btDispatcherInfo& dispatchInfo = getDispatchInfo();
	321
	322	dispatchInfo.m_timeStep = timeStep;
	323	dispatchInfo.m_stepCount = 0;
	324	dispatchInfo.m_debugDraw = getDebugDrawer();
	325
[8393]	326
[1963]	327	///perform collision detection
	328	performDiscreteCollisionDetection();
	329
[8393]	330	if (getDispatchInfo().m_useContinuous)
	331	addSpeculativeContacts(timeStep);
	332
	333
[1963]	334	calculateSimulationIslands();
	335
	336
	337	getSolverInfo().m_timeStep = timeStep;
	338
	339
	340
	341	///solve contact and other joint constraints
	342	solveConstraints(getSolverInfo());
	343
	344	///CallbackTriggers();
	345
	346	///integrate transforms
	347	integrateTransforms(timeStep);
	348
	349	///update vehicle simulation
[2882]	350	updateActions(timeStep);
[2430]	351
[1963]	352	updateActivationState( timeStep );
	353
	354	if(0 != m_internalTickCallback) {
	355	(*m_internalTickCallback)(this, timeStep);
	356	}
	357	}
	358
	359	void btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)
	360	{
	361	m_gravity = gravity;
[8351]	362	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
[1963]	363	{
[8351]	364	btRigidBody* body = m_nonStaticRigidBodies[i];
	365	if (body->isActive() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY))
[1963]	366	{
	367	body->setGravity(gravity);
	368	}
	369	}
	370	}
	371
	372	btVector3 btDiscreteDynamicsWorld::getGravity () const
	373	{
	374	return m_gravity;
	375	}
	376
[8351]	377	void btDiscreteDynamicsWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
	378	{
	379	btCollisionWorld::addCollisionObject(collisionObject,collisionFilterGroup,collisionFilterMask);
	380	}
[1963]	381
[8351]	382	void btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
	383	{
	384	btRigidBody* body = btRigidBody::upcast(collisionObject);
	385	if (body)
	386	removeRigidBody(body);
	387	else
	388	btCollisionWorld::removeCollisionObject(collisionObject);
	389	}
	390
[1963]	391	void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body)
	392	{
[8351]	393	m_nonStaticRigidBodies.remove(body);
	394	btCollisionWorld::removeCollisionObject(body);
[1963]	395	}
	396
[8351]	397
[1963]	398	void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)
	399	{
[8351]	400	if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY))
[1963]	401	{
	402	body->setGravity(m_gravity);
	403	}
	404
	405	if (body->getCollisionShape())
	406	{
[8351]	407	if (!body->isStaticObject())
	408	{
	409	m_nonStaticRigidBodies.push_back(body);
	410	} else
	411	{
	412	body->setActivationState(ISLAND_SLEEPING);
	413	}
	414
[1963]	415	bool isDynamic = !(body->isStaticObject() \|\| body->isKinematicObject());
	416	short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
	417	short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
	418
	419	addCollisionObject(body,collisionFilterGroup,collisionFilterMask);
	420	}
	421	}
	422
	423	void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask)
	424	{
[8351]	425	if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY))
[1963]	426	{
	427	body->setGravity(m_gravity);
	428	}
	429
	430	if (body->getCollisionShape())
	431	{
[8351]	432	if (!body->isStaticObject())
	433	{
	434	m_nonStaticRigidBodies.push_back(body);
	435	}
	436	else
	437	{
	438	body->setActivationState(ISLAND_SLEEPING);
	439	}
[1963]	440	addCollisionObject(body,group,mask);
	441	}
	442	}
	443
	444
[2882]	445	void btDiscreteDynamicsWorld::updateActions(btScalar timeStep)
[1963]	446	{
[2882]	447	BT_PROFILE("updateActions");
[1963]	448
[2882]	449	for ( int i=0;i<m_actions.size();i++)
[1963]	450	{
[2882]	451	m_actions[i]->updateAction( this, timeStep);
[1963]	452	}
	453	}
[2430]	454
	455
[1963]	456	void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
	457	{
	458	BT_PROFILE("updateActivationState");
	459
[8351]	460	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
[1963]	461	{
[8351]	462	btRigidBody* body = m_nonStaticRigidBodies[i];
[1963]	463	if (body)
	464	{
	465	body->updateDeactivation(timeStep);
	466
	467	if (body->wantsSleeping())
	468	{
	469	if (body->isStaticOrKinematicObject())
	470	{
	471	body->setActivationState(ISLAND_SLEEPING);
	472	} else
	473	{
	474	if (body->getActivationState() == ACTIVE_TAG)
	475	body->setActivationState( WANTS_DEACTIVATION );
	476	if (body->getActivationState() == ISLAND_SLEEPING)
	477	{
	478	body->setAngularVelocity(btVector3(0,0,0));
	479	body->setLinearVelocity(btVector3(0,0,0));
	480	}
	481
	482	}
	483	} else
	484	{
	485	if (body->getActivationState() != DISABLE_DEACTIVATION)
	486	body->setActivationState( ACTIVE_TAG );
	487	}
	488	}
	489	}
	490	}
	491
	492	void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies)
	493	{
	494	m_constraints.push_back(constraint);
	495	if (disableCollisionsBetweenLinkedBodies)
	496	{
	497	constraint->getRigidBodyA().addConstraintRef(constraint);
	498	constraint->getRigidBodyB().addConstraintRef(constraint);
	499	}
	500	}
	501
	502	void btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint)
	503	{
	504	m_constraints.remove(constraint);
	505	constraint->getRigidBodyA().removeConstraintRef(constraint);
	506	constraint->getRigidBodyB().removeConstraintRef(constraint);
	507	}
	508
[2882]	509	void btDiscreteDynamicsWorld::addAction(btActionInterface* action)
[1963]	510	{
[2882]	511	m_actions.push_back(action);
[1963]	512	}
	513
[2882]	514	void btDiscreteDynamicsWorld::removeAction(btActionInterface* action)
[1963]	515	{
[2882]	516	m_actions.remove(action);
[1963]	517	}
	518
[2882]	519
	520	void btDiscreteDynamicsWorld::addVehicle(btActionInterface* vehicle)
[2430]	521	{
[2882]	522	addAction(vehicle);
[2430]	523	}
	524
[2882]	525	void btDiscreteDynamicsWorld::removeVehicle(btActionInterface* vehicle)
[2430]	526	{
[2882]	527	removeAction(vehicle);
[2430]	528	}
	529
[2882]	530	void btDiscreteDynamicsWorld::addCharacter(btActionInterface* character)
	531	{
	532	addAction(character);
	533	}
[2430]	534
[2882]	535	void btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character)
	536	{
	537	removeAction(character);
	538	}
	539
	540
[1963]	541	SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs)
	542	{
	543	int islandId;
	544
	545	const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
	546	const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
	547	islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag();
	548	return islandId;
	549
	550	}
	551
	552
	553	class btSortConstraintOnIslandPredicate
	554	{
	555	public:
	556
	557	bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs )
	558	{
	559	int rIslandId0,lIslandId0;
	560	rIslandId0 = btGetConstraintIslandId(rhs);
	561	lIslandId0 = btGetConstraintIslandId(lhs);
	562	return lIslandId0 < rIslandId0;
	563	}
	564	};
	565
	566
	567
	568	void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
	569	{
	570	BT_PROFILE("solveConstraints");
	571
	572	struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
	573	{
	574
	575	btContactSolverInfo& m_solverInfo;
	576	btConstraintSolver* m_solver;
	577	btTypedConstraint** m_sortedConstraints;
	578	int m_numConstraints;
	579	btIDebugDraw* m_debugDrawer;
	580	btStackAlloc* m_stackAlloc;
	581	btDispatcher* m_dispatcher;
[8351]	582
	583	btAlignedObjectArray<btCollisionObject*> m_bodies;
	584	btAlignedObjectArray<btPersistentManifold*> m_manifolds;
	585	btAlignedObjectArray<btTypedConstraint*> m_constraints;
[1963]	586
[8351]	587
[1963]	588	InplaceSolverIslandCallback(
	589	btContactSolverInfo& solverInfo,
	590	btConstraintSolver* solver,
	591	btTypedConstraint** sortedConstraints,
	592	int numConstraints,
	593	btIDebugDraw* debugDrawer,
	594	btStackAlloc* stackAlloc,
	595	btDispatcher* dispatcher)
	596	:m_solverInfo(solverInfo),
	597	m_solver(solver),
	598	m_sortedConstraints(sortedConstraints),
	599	m_numConstraints(numConstraints),
	600	m_debugDrawer(debugDrawer),
	601	m_stackAlloc(stackAlloc),
	602	m_dispatcher(dispatcher)
	603	{
	604
	605	}
	606
[8351]	607
[1963]	608	InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other)
	609	{
	610	btAssert(0);
	611	(void)other;
	612	return *this;
	613	}
	614	virtual void ProcessIsland(btCollisionObject bodies,int numBodies,btPersistentManifold manifolds,int numManifolds, int islandId)
	615	{
	616	if (islandId<0)
	617	{
[2882]	618	if (numManifolds + m_numConstraints)
	619	{
	620	///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
	621	m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
	622	}
[1963]	623	} else
	624	{
	625	//also add all non-contact constraints/joints for this island
	626	btTypedConstraint** startConstraint = 0;
	627	int numCurConstraints = 0;
	628	int i;
	629
	630	//find the first constraint for this island
	631	for (i=0;i<m_numConstraints;i++)
	632	{
	633	if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
	634	{
	635	startConstraint = &m_sortedConstraints[i];
	636	break;
	637	}
	638	}
	639	//count the number of constraints in this island
	640	for (;i<m_numConstraints;i++)
	641	{
	642	if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
	643	{
	644	numCurConstraints++;
	645	}
	646	}
	647
[8351]	648	if (m_solverInfo.m_minimumSolverBatchSize<=1)
[1963]	649	{
[8351]	650	///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive
	651	if (numManifolds + numCurConstraints)
	652	{
	653	m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
	654	}
	655	} else
	656	{
	657
	658	for (i=0;i<numBodies;i++)
	659	m_bodies.push_back(bodies[i]);
	660	for (i=0;i<numManifolds;i++)
	661	m_manifolds.push_back(manifolds[i]);
	662	for (i=0;i<numCurConstraints;i++)
	663	m_constraints.push_back(startConstraint[i]);
	664	if ((m_constraints.size()+m_manifolds.size())>m_solverInfo.m_minimumSolverBatchSize)
	665	{
	666	processConstraints();
	667	} else
	668	{
	669	//printf("deferred\n");
	670	}
[1963]	671	}
	672	}
	673	}
[8351]	674	void processConstraints()
	675	{
	676	if (m_manifolds.size() + m_constraints.size()>0)
	677	{
	678	m_solver->solveGroup( &m_bodies[0],m_bodies.size(), &m_manifolds[0], m_manifolds.size(), &m_constraints[0], m_constraints.size() ,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
	679	}
	680	m_bodies.resize(0);
	681	m_manifolds.resize(0);
	682	m_constraints.resize(0);
[1963]	683
[8351]	684	}
	685
[1963]	686	};
	687
[8351]	688
	689
[1963]	690	//sorted version of all btTypedConstraint, based on islandId
	691	btAlignedObjectArray<btTypedConstraint*> sortedConstraints;
	692	sortedConstraints.resize( m_constraints.size());
	693	int i;
	694	for (i=0;i<getNumConstraints();i++)
	695	{
	696	sortedConstraints[i] = m_constraints[i];
	697	}
	698
[2882]	699	// btAssert(0);
[1963]	700
	701
	702
	703	sortedConstraints.quickSort(btSortConstraintOnIslandPredicate());
	704
	705	btTypedConstraint** constraintsPtr = getNumConstraints() ? &sortedConstraints[0] : 0;
	706
	707	InplaceSolverIslandCallback solverCallback( solverInfo, m_constraintSolver, constraintsPtr,sortedConstraints.size(), m_debugDrawer,m_stackAlloc,m_dispatcher1);
	708
	709	m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
	710
	711	/// solve all the constraints for this island
[2430]	712	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),&solverCallback);
[1963]	713
[8351]	714	solverCallback.processConstraints();
	715
[1963]	716	m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc);
	717	}
	718
	719
	720
	721
	722	void btDiscreteDynamicsWorld::calculateSimulationIslands()
	723	{
	724	BT_PROFILE("calculateSimulationIslands");
	725
	726	getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());
	727
	728	{
	729	int i;
	730	int numConstraints = int(m_constraints.size());
	731	for (i=0;i< numConstraints ; i++ )
	732	{
	733	btTypedConstraint* constraint = m_constraints[i];
	734
	735	const btRigidBody* colObj0 = &constraint->getRigidBodyA();
	736	const btRigidBody* colObj1 = &constraint->getRigidBodyB();
	737
	738	if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
	739	((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
	740	{
	741	if (colObj0->isActive() \|\| colObj1->isActive())
	742	{
	743
	744	getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),
	745	(colObj1)->getIslandTag());
	746	}
	747	}
	748	}
	749	}
	750
	751	//Store the island id in each body
	752	getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
	753
	754
	755	}
	756
	757
	758
[8351]	759
[1963]	760	class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
	761	{
[8393]	762	public:
	763
[1963]	764	btCollisionObject* m_me;
	765	btScalar m_allowedPenetration;
	766	btOverlappingPairCache* m_pairCache;
[2882]	767	btDispatcher* m_dispatcher;
[1963]	768
	769	public:
[2882]	770	btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
[1963]	771	btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
[8351]	772	m_me(me),
[1963]	773	m_allowedPenetration(0.0f),
[2882]	774	m_pairCache(pairCache),
	775	m_dispatcher(dispatcher)
[1963]	776	{
	777	}
	778
	779	virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace)
	780	{
	781	if (convexResult.m_hitCollisionObject == m_me)
[2430]	782	return 1.0f;
[1963]	783
[2430]	784	//ignore result if there is no contact response
	785	if(!convexResult.m_hitCollisionObject->hasContactResponse())
	786	return 1.0f;
	787
[1963]	788	btVector3 linVelA,linVelB;
	789	linVelA = m_convexToWorld-m_convexFromWorld;
	790	linVelB = btVector3(0,0,0);//toB.getOrigin()-fromB.getOrigin();
	791
	792	btVector3 relativeVelocity = (linVelA-linVelB);
	793	//don't report time of impact for motion away from the contact normal (or causes minor penetration)
	794	if (convexResult.m_hitNormalLocal.dot(relativeVelocity)>=-m_allowedPenetration)
	795	return 1.f;
	796
	797	return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace);
	798	}
	799
	800	virtual bool needsCollision(btBroadphaseProxy* proxy0) const
	801	{
	802	//don't collide with itself
	803	if (proxy0->m_clientObject == m_me)
	804	return false;
	805
	806	///don't do CCD when the collision filters are not matching
[2430]	807	if (!ClosestConvexResultCallback::needsCollision(proxy0))
[1963]	808	return false;
	809
[2882]	810	btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
	811
	812	//call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179
	813	if (m_dispatcher->needsResponse(m_me,otherObj))
[1963]	814	{
[8393]	815	#if 0
[2882]	816	///don't do CCD when there are already contact points (touching contact/penetration)
	817	btAlignedObjectArray<btPersistentManifold*> manifoldArray;
	818	btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0);
	819	if (collisionPair)
[1963]	820	{
[2882]	821	if (collisionPair->m_algorithm)
[1963]	822	{
[2882]	823	manifoldArray.resize(0);
	824	collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);
	825	for (int j=0;j<manifoldArray.size();j++)
	826	{
	827	btPersistentManifold* manifold = manifoldArray[j];
	828	if (manifold->getNumContacts()>0)
	829	return false;
	830	}
[1963]	831	}
	832	}
[8393]	833	#endif
	834	return true;
[1963]	835	}
[8393]	836
	837	return false;
[1963]	838	}
	839
	840
	841	};
	842
	843	///internal debugging variable. this value shouldn't be too high
	844	int gNumClampedCcdMotions=0;
	845
	846	void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
	847	{
	848	BT_PROFILE("integrateTransforms");
	849	btTransform predictedTrans;
[8351]	850	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
[1963]	851	{
[8351]	852	btRigidBody* body = m_nonStaticRigidBodies[i];
	853	body->setHitFraction(1.f);
	854
	855	if (body->isActive() && (!body->isStaticOrKinematicObject()))
[1963]	856	{
[8393]	857
[8351]	858	body->predictIntegratedTransform(timeStep, predictedTrans);
[8393]	859
[8351]	860	btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
[1963]	861
[8393]	862
	863
	864	if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
[1963]	865	{
[8351]	866	BT_PROFILE("CCD motion clamping");
	867	if (body->getCollisionShape()->isConvex())
[1963]	868	{
[8351]	869	gNumClampedCcdMotions++;
[8393]	870	#ifdef USE_STATIC_ONLY
	871	class StaticOnlyCallback : public btClosestNotMeConvexResultCallback
	872	{
	873	public:
	874
	875	StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
	876	btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher)
	877	{
	878	}
	879
	880	virtual bool needsCollision(btBroadphaseProxy* proxy0) const
	881	{
	882	btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
	883	if (!otherObj->isStaticOrKinematicObject())
	884	return false;
	885	return btClosestNotMeConvexResultCallback::needsCollision(proxy0);
	886	}
	887	};
	888
	889	StaticOnlyCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
	890	#else
[8351]	891	btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
[8393]	892	#endif
[8351]	893	//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
	894	btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
[8393]	895	sweepResults.m_allowedPenetration=getDispatchInfo().m_allowedCcdPenetration;
[2430]	896
[8351]	897	sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
	898	sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask;
[8393]	899	btTransform modifiedPredictedTrans = predictedTrans;
	900	modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis());
[2430]	901
[8393]	902	convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);
[8351]	903	if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
	904	{
[8393]	905
	906	//printf("clamped integration to hit fraction = %f\n",fraction);
[8351]	907	body->setHitFraction(sweepResults.m_closestHitFraction);
	908	body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
	909	body->setHitFraction(0.f);
[8393]	910	body->proceedToTransform( predictedTrans);
	911
	912	#if 0
	913	btVector3 linVel = body->getLinearVelocity();
	914
	915	btScalar maxSpeed = body->getCcdMotionThreshold()/getSolverInfo().m_timeStep;
	916	btScalar maxSpeedSqr = maxSpeed*maxSpeed;
	917	if (linVel.length2()>maxSpeedSqr)
	918	{
	919	linVel.normalize();
	920	linVel*= maxSpeed;
	921	body->setLinearVelocity(linVel);
	922	btScalar ms2 = body->getLinearVelocity().length2();
	923	body->predictIntegratedTransform(timeStep, predictedTrans);
	924
	925	btScalar sm2 = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
	926	btScalar smt = body->getCcdSquareMotionThreshold();
	927	printf("sm2=%f\n",sm2);
	928	}
	929	#else
	930	//response between two dynamic objects without friction, assuming 0 penetration depth
	931	btScalar appliedImpulse = 0.f;
	932	btScalar depth = 0.f;
	933	appliedImpulse = resolveSingleCollision(body,sweepResults.m_hitCollisionObject,sweepResults.m_hitPointWorld,sweepResults.m_hitNormalWorld,getSolverInfo(), depth);
	934
	935
	936	#endif
	937
	938	continue;
[1963]	939	}
	940	}
	941	}
[8351]	942
[8393]	943
[8351]	944	body->proceedToTransform( predictedTrans);
[1963]	945	}
	946	}
	947	}
	948
[8393]	949	void btDiscreteDynamicsWorld::addSpeculativeContacts(btScalar timeStep)
	950	{
	951	BT_PROFILE("addSpeculativeContacts");
	952	btTransform predictedTrans;
	953	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
	954	{
	955	btRigidBody* body = m_nonStaticRigidBodies[i];
	956	body->setHitFraction(1.f);
[1963]	957
[8393]	958	if (body->isActive() && (!body->isStaticOrKinematicObject()))
	959	{
	960	body->predictIntegratedTransform(timeStep, predictedTrans);
	961	btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
[1963]	962
[8393]	963	if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
	964	{
	965	BT_PROFILE("search speculative contacts");
	966	if (body->getCollisionShape()->isConvex())
	967	{
	968	gNumClampedCcdMotions++;
	969
	970	btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
	971	//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
	972	btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
[2430]	973
[8393]	974	sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
	975	sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask;
	976	btTransform modifiedPredictedTrans;
	977	modifiedPredictedTrans = predictedTrans;
	978	modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis());
[2430]	979
[8393]	980	convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);
	981	if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
	982	{
	983	btBroadphaseProxy* proxy0 = body->getBroadphaseHandle();
	984	btBroadphaseProxy* proxy1 = sweepResults.m_hitCollisionObject->getBroadphaseHandle();
	985	btBroadphasePair* pair = sweepResults.m_pairCache->findPair(proxy0,proxy1);
	986	if (pair)
	987	{
	988	if (pair->m_algorithm)
	989	{
	990	btManifoldArray contacts;
	991	pair->m_algorithm->getAllContactManifolds(contacts);
	992	if (contacts.size())
	993	{
	994	btManifoldResult result(body,sweepResults.m_hitCollisionObject);
	995	result.setPersistentManifold(contacts[0]);
	996
	997	btVector3 vec = (modifiedPredictedTrans.getOrigin()-body->getWorldTransform().getOrigin());
	998	vec*=sweepResults.m_closestHitFraction;
	999
	1000	btScalar lenSqr = vec.length2();
	1001	btScalar depth = 0.f;
	1002	btVector3 pointWorld = sweepResults.m_hitPointWorld;
	1003	if (lenSqr>SIMD_EPSILON)
	1004	{
	1005	depth = btSqrt(lenSqr);
	1006	pointWorld -= vec;
	1007	vec /= depth;
	1008	}
	1009
	1010	if (contacts[0]->getBody0()==body)
	1011	{
	1012	result.addContactPoint(sweepResults.m_hitNormalWorld,pointWorld,depth);
	1013	#if 0
	1014	debugContacts.push_back(sweepResults.m_hitPointWorld);//sweepResults.m_hitPointWorld);
	1015	debugNormals.push_back(sweepResults.m_hitNormalWorld);
	1016	#endif
	1017	} else
	1018	{
	1019	//swapped
	1020	result.addContactPoint(-sweepResults.m_hitNormalWorld,pointWorld,depth);
	1021	//sweepResults.m_hitPointWorld,depth);
	1022
	1023	#if 0
	1024	if (1)//firstHit==1)
	1025	{
	1026	firstHit=0;
	1027	debugNormals.push_back(sweepResults.m_hitNormalWorld);
	1028	debugContacts.push_back(pointWorld);//sweepResults.m_hitPointWorld);
	1029	debugNormals.push_back(sweepResults.m_hitNormalWorld);
	1030	debugContacts.push_back(sweepResults.m_hitPointWorld);
	1031	}
	1032	firstHit--;
	1033	#endif
	1034	}
	1035	}
	1036
	1037	} else
	1038	{
	1039	//no algorithm, use dispatcher to create one
	1040
	1041	}
	1042
	1043
	1044	} else
	1045	{
	1046	//add an overlapping pair
	1047	//printf("pair missing\n");
	1048
	1049	}
	1050	}
	1051	}
	1052	}
	1053
	1054	}
	1055	}
	1056	}
	1057
	1058
	1059
	1060
	1061
[1963]	1062	void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
	1063	{
	1064	BT_PROFILE("predictUnconstraintMotion");
[8351]	1065	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
[1963]	1066	{
[8351]	1067	btRigidBody* body = m_nonStaticRigidBodies[i];
	1068	if (!body->isStaticOrKinematicObject())
[1963]	1069	{
[8351]	1070	body->integrateVelocities( timeStep);
	1071	//damping
	1072	body->applyDamping(timeStep);
[1963]	1073
[8351]	1074	body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
[1963]	1075	}
	1076	}
	1077	}
	1078
	1079
	1080	void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep)
	1081	{
	1082	(void)timeStep;
	1083
	1084	#ifndef BT_NO_PROFILE
	1085	CProfileManager::Reset();
	1086	#endif //BT_NO_PROFILE
	1087
	1088	}
	1089
	1090
	1091
	1092
	1093
	1094
[2882]	1095	void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
	1096	{
	1097	bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0;
	1098	bool drawLimits = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraintLimits) != 0;
	1099	btScalar dbgDrawSize = constraint->getDbgDrawSize();
	1100	if(dbgDrawSize <= btScalar(0.f))
	1101	{
	1102	return;
	1103	}
	1104
	1105	switch(constraint->getConstraintType())
	1106	{
	1107	case POINT2POINT_CONSTRAINT_TYPE:
	1108	{
	1109	btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint;
	1110	btTransform tr;
	1111	tr.setIdentity();
	1112	btVector3 pivot = p2pC->getPivotInA();
	1113	pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot;
	1114	tr.setOrigin(pivot);
	1115	getDebugDrawer()->drawTransform(tr, dbgDrawSize);
	1116	// that ideally should draw the same frame
	1117	pivot = p2pC->getPivotInB();
	1118	pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot;
	1119	tr.setOrigin(pivot);
	1120	if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
	1121	}
	1122	break;
	1123	case HINGE_CONSTRAINT_TYPE:
	1124	{
	1125	btHingeConstraint* pHinge = (btHingeConstraint*)constraint;
	1126	btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame();
	1127	if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
	1128	tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame();
	1129	if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
	1130	btScalar minAng = pHinge->getLowerLimit();
	1131	btScalar maxAng = pHinge->getUpperLimit();
	1132	if(minAng == maxAng)
	1133	{
	1134	break;
	1135	}
	1136	bool drawSect = true;
	1137	if(minAng > maxAng)
	1138	{
	1139	minAng = btScalar(0.f);
	1140	maxAng = SIMD_2_PI;
	1141	drawSect = false;
	1142	}
	1143	if(drawLimits)
	1144	{
	1145	btVector3& center = tr.getOrigin();
	1146	btVector3 normal = tr.getBasis().getColumn(2);
	1147	btVector3 axis = tr.getBasis().getColumn(0);
	1148	getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0,0,0), drawSect);
	1149	}
	1150	}
	1151	break;
	1152	case CONETWIST_CONSTRAINT_TYPE:
	1153	{
	1154	btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint;
	1155	btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
	1156	if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
	1157	tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
	1158	if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
	1159	if(drawLimits)
	1160	{
	1161	//const btScalar length = btScalar(5);
	1162	const btScalar length = dbgDrawSize;
	1163	static int nSegments = 8*4;
	1164	btScalar fAngleInRadians = btScalar(2.3.1415926) (btScalar)(nSegments-1)/btScalar(nSegments);
	1165	btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length);
	1166	pPrev = tr * pPrev;
	1167	for (int i=0; i<nSegments; i++)
	1168	{
	1169	fAngleInRadians = btScalar(2.3.1415926) (btScalar)i/btScalar(nSegments);
	1170	btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length);
	1171	pCur = tr * pCur;
	1172	getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0,0,0));
	1173
	1174	if (i%(nSegments/8) == 0)
	1175	getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0));
	1176
	1177	pPrev = pCur;
	1178	}
	1179	btScalar tws = pCT->getTwistSpan();
	1180	btScalar twa = pCT->getTwistAngle();
	1181	bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f));
	1182	if(useFrameB)
	1183	{
	1184	tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
	1185	}
	1186	else
	1187	{
	1188	tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
	1189	}
	1190	btVector3 pivot = tr.getOrigin();
	1191	btVector3 normal = tr.getBasis().getColumn(0);
	1192	btVector3 axis1 = tr.getBasis().getColumn(1);
	1193	getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa-tws, -twa+tws, btVector3(0,0,0), true);
	1194
	1195	}
	1196	}
	1197	break;
[8393]	1198	case D6_SPRING_CONSTRAINT_TYPE:
[2882]	1199	case D6_CONSTRAINT_TYPE:
	1200	{
	1201	btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint;
	1202	btTransform tr = p6DOF->getCalculatedTransformA();
	1203	if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
	1204	tr = p6DOF->getCalculatedTransformB();
	1205	if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
	1206	if(drawLimits)
	1207	{
	1208	tr = p6DOF->getCalculatedTransformA();
	1209	const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
	1210	btVector3 up = tr.getBasis().getColumn(2);
	1211	btVector3 axis = tr.getBasis().getColumn(0);
	1212	btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
	1213	btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
	1214	btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
	1215	btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
	1216	getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0,0,0));
	1217	axis = tr.getBasis().getColumn(1);
	1218	btScalar ay = p6DOF->getAngle(1);
	1219	btScalar az = p6DOF->getAngle(2);
	1220	btScalar cy = btCos(ay);
	1221	btScalar sy = btSin(ay);
	1222	btScalar cz = btCos(az);
	1223	btScalar sz = btSin(az);
	1224	btVector3 ref;
	1225	ref[0] = cyczaxis[0] + cyszaxis[1] - sy*axis[2];
	1226	ref[1] = -szaxis[0] + czaxis[1];
	1227	ref[2] = czsyaxis[0] + szsyaxis[1] + cy*axis[2];
	1228	tr = p6DOF->getCalculatedTransformB();
	1229	btVector3 normal = -tr.getBasis().getColumn(0);
	1230	btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;
	1231	btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit;
	1232	if(minFi > maxFi)
	1233	{
	1234	getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0,0,0), false);
	1235	}
	1236	else if(minFi < maxFi)
	1237	{
	1238	getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0,0,0), true);
	1239	}
	1240	tr = p6DOF->getCalculatedTransformA();
	1241	btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit;
	1242	btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit;
	1243	getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0,0,0));
	1244	}
	1245	}
	1246	break;
	1247	case SLIDER_CONSTRAINT_TYPE:
	1248	{
	1249	btSliderConstraint* pSlider = (btSliderConstraint*)constraint;
	1250	btTransform tr = pSlider->getCalculatedTransformA();
	1251	if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
	1252	tr = pSlider->getCalculatedTransformB();
	1253	if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
	1254	if(drawLimits)
	1255	{
[8351]	1256	btTransform tr = pSlider->getUseLinearReferenceFrameA() ? pSlider->getCalculatedTransformA() : pSlider->getCalculatedTransformB();
[2882]	1257	btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f);
	1258	btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f);
	1259	getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0));
	1260	btVector3 normal = tr.getBasis().getColumn(0);
	1261	btVector3 axis = tr.getBasis().getColumn(1);
	1262	btScalar a_min = pSlider->getLowerAngLimit();
	1263	btScalar a_max = pSlider->getUpperAngLimit();
	1264	const btVector3& center = pSlider->getCalculatedTransformB().getOrigin();
	1265	getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0,0,0), true);
	1266	}
	1267	}
	1268	break;
	1269	default :
	1270	break;
	1271	}
	1272	return;
[8351]	1273	}
[2882]	1274
	1275
	1276
	1277
	1278
[1963]	1279	void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
	1280	{
	1281	if (m_ownsConstraintSolver)
	1282	{
	1283	btAlignedFree( m_constraintSolver);
	1284	}
	1285	m_ownsConstraintSolver = false;
	1286	m_constraintSolver = solver;
	1287	}
	1288
	1289	btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver()
	1290	{
	1291	return m_constraintSolver;
	1292	}
	1293
	1294
	1295	int btDiscreteDynamicsWorld::getNumConstraints() const
	1296	{
	1297	return int(m_constraints.size());
	1298	}
	1299	btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index)
	1300	{
	1301	return m_constraints[index];
	1302	}
	1303	const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const
	1304	{
	1305	return m_constraints[index];
	1306	}
[2882]	1307
	1308
[8351]	1309
	1310	void btDiscreteDynamicsWorld::serializeRigidBodies(btSerializer* serializer)
	1311	{
	1312	int i;
	1313	//serialize all collision objects
	1314	for (i=0;i<m_collisionObjects.size();i++)
	1315	{
	1316	btCollisionObject* colObj = m_collisionObjects[i];
	1317	if (colObj->getInternalType() & btCollisionObject::CO_RIGID_BODY)
	1318	{
	1319	int len = colObj->calculateSerializeBufferSize();
	1320	btChunk* chunk = serializer->allocate(len,1);
	1321	const char* structType = colObj->serialize(chunk->m_oldPtr, serializer);
	1322	serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,colObj);
	1323	}
	1324	}
	1325
	1326	for (i=0;i<m_constraints.size();i++)
	1327	{
	1328	btTypedConstraint* constraint = m_constraints[i];
	1329	int size = constraint->calculateSerializeBufferSize();
	1330	btChunk* chunk = serializer->allocate(size,1);
	1331	const char* structType = constraint->serialize(chunk->m_oldPtr,serializer);
	1332	serializer->finalizeChunk(chunk,structType,BT_CONSTRAINT_CODE,constraint);
	1333	}
	1334	}
	1335
	1336
	1337	void btDiscreteDynamicsWorld::serialize(btSerializer* serializer)
	1338	{
	1339
	1340	serializer->startSerialization();
	1341
	1342	serializeRigidBodies(serializer);
	1343
	1344	serializeCollisionObjects(serializer);
	1345
	1346	serializer->finishSerialization();
	1347	}
	1348

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

Download in other formats: