Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

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

Last change on this file since 5738 was 5738, checked in by landauf, 15 years ago
merged libraries2 back to trunk
Property svn:eol-style set to `native`
File size: 42.8 KB

Line
1	/*
2	Bullet Continuous Collision Detection and Physics Library
3	Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
4
5	This software is provided 'as-is', without any express or implied warranty.
6	In no event will the authors be held liable for any damages arising from the use of this software.
7	Permission is granted to anyone to use this software for any purpose,
8	including commercial applications, and to alter it and redistribute it freely,
9	subject to the following restrictions:
10
11	1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12	2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13	3. This notice may not be removed or altered from any source distribution.
14	*/
15
16
17	#include "btDiscreteDynamicsWorld.h"
18
19	//collision detection
20	#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
21	#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
22	#include "BulletCollision/CollisionShapes/btCollisionShape.h"
23	#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
24	#include "LinearMath/btTransformUtil.h"
25	#include "LinearMath/btQuickprof.h"
26
27	//rigidbody & constraints
28	#include "BulletDynamics/Dynamics/btRigidBody.h"
29	#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
30	#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
31	#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
32	#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"
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
54	#include "BulletDynamics/Dynamics/btActionInterface.h"
55	#include "LinearMath/btQuickprof.h"
56	#include "LinearMath/btMotionState.h"
57
58
59
60
61
62	btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
63	:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration),
64	m_constraintSolver(constraintSolver),
65	m_gravity(0,-10,0),
66	m_localTime(btScalar(1.)/btScalar(60.)),
67	m_profileTimings(0)
68	{
69	if (!m_constraintSolver)
70	{
71	void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16);
72	m_constraintSolver = new (mem) btSequentialImpulseConstraintSolver;
73	m_ownsConstraintSolver = true;
74	} else
75	{
76	m_ownsConstraintSolver = false;
77	}
78
79	{
80	void* mem = btAlignedAlloc(sizeof(btSimulationIslandManager),16);
81	m_islandManager = new (mem) btSimulationIslandManager();
82	}
83
84	m_ownsIslandManager = true;
85	}
86
87
88	btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld()
89	{
90	//only delete it when we created it
91	if (m_ownsIslandManager)
92	{
93	m_islandManager->~btSimulationIslandManager();
94	btAlignedFree( m_islandManager);
95	}
96	if (m_ownsConstraintSolver)
97	{
98
99	m_constraintSolver->~btConstraintSolver();
100	btAlignedFree(m_constraintSolver);
101	}
102	}
103
104	void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
105	{
106
107	for (int i=0;i<m_collisionObjects.size();i++)
108	{
109	btCollisionObject* colObj = m_collisionObjects[i];
110	btRigidBody* body = btRigidBody::upcast(colObj);
111	if (body)
112	{
113	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	{
127	BT_PROFILE("debugDrawWorld");
128
129	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
130	{
131	int numManifolds = getDispatcher()->getNumManifolds();
132	btVector3 color(0,0,0);
133	for (int i=0;i<numManifolds;i++)
134	{
135	btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
136	//btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
137	//btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
138
139	int numContacts = contactManifold->getNumContacts();
140	for (int j=0;j<numContacts;j++)
141	{
142	btManifoldPoint& cp = contactManifold->getContactPoint(j);
143	getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
144	}
145	}
146	}
147	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	}
164
165
166
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	{
199	btVector3 minAabb,maxAabb;
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
207	if (getDebugDrawer() && getDebugDrawer()->getDebugMode())
208	{
209	for (i=0;i<m_actions.size();i++)
210	{
211	m_actions[i]->debugDraw(m_debugDrawer);
212	}
213	}
214	}
215	}
216
217	void btDiscreteDynamicsWorld::clearForces()
218	{
219	///@todo: iterate over awake simulation islands!
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	{
235	///@todo: iterate over awake simulation islands!
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
249	void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body)
250	{
251	btAssert(body);
252
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
269	void btDiscreteDynamicsWorld::synchronizeMotionStates()
270	{
271	BT_PROFILE("synchronizeMotionStates");
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);
279	if (body)
280	synchronizeSingleMotionState(body);
281	}
282	}
283	/*
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	}
295	*/
296
297
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	{
337	btIDebugDraw* debugDrawer = getDebugDrawer ();
338	gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
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
402	updateActions(timeStep);
403
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
467	void btDiscreteDynamicsWorld::updateActions(btScalar timeStep)
468	{
469	BT_PROFILE("updateActions");
470
471	for ( int i=0;i<m_actions.size();i++)
472	{
473	m_actions[i]->updateAction( this, timeStep);
474	}
475	}
476
477
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
532	void btDiscreteDynamicsWorld::addAction(btActionInterface* action)
533	{
534	m_actions.push_back(action);
535	}
536
537	void btDiscreteDynamicsWorld::removeAction(btActionInterface* action)
538	{
539	m_actions.remove(action);
540	}
541
542
543	void btDiscreteDynamicsWorld::addVehicle(btActionInterface* vehicle)
544	{
545	addAction(vehicle);
546	}
547
548	void btDiscreteDynamicsWorld::removeVehicle(btActionInterface* vehicle)
549	{
550	removeAction(vehicle);
551	}
552
553	void btDiscreteDynamicsWorld::addCharacter(btActionInterface* character)
554	{
555	addAction(character);
556	}
557
558	void btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character)
559	{
560	removeAction(character);
561	}
562
563
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	{
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	}
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
686	// btAssert(0);
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
699	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),&solverCallback);
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;
750	btDispatcher* m_dispatcher;
751
752
753	public:
754	btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
755	btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
756	m_allowedPenetration(0.0f),
757	m_me(me),
758	m_pairCache(pairCache),
759	m_dispatcher(dispatcher)
760	{
761	}
762
763	virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace)
764	{
765	if (convexResult.m_hitCollisionObject == m_me)
766	return 1.0f;
767
768	//ignore result if there is no contact response
769	if(!convexResult.m_hitCollisionObject->hasContactResponse())
770	return 1.0f;
771
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
791	if (!ClosestConvexResultCallback::needsCollision(proxy0))
792	return false;
793
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))
798	{
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)
803	{
804	if (collisionPair->m_algorithm)
805	{
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	}
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
851	btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
852	btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
853	btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
854
855	sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
856	sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask;
857
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
877
878
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	{
890
891	body->integrateVelocities( timeStep);
892	//damping
893	body->applyDamping(timeStep);
894
895	body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
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
1147	///@todo pass camera, for some culling? no -> we are not a graphics lib
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	{
1176	btVector3 a,b;
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
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
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	}
1403
1404

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

Download in other formats: