Bullet Collision Detection & Physics Library
btSimulationIslandManagerMt.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15
16
17#include "LinearMath/btScalar.h"
25
26//#include <stdio.h>
28
29
30SIMD_FORCE_INLINE int calcBatchCost( int bodies, int manifolds, int constraints )
31{
32 // rough estimate of the cost of a batch, used for merging
33 int batchCost = bodies + 8 * manifolds + 4 * constraints;
34 return batchCost;
35}
36
37
39{
40 return calcBatchCost( island->bodyArray.size(), island->manifoldArray.size(), island->constraintArray.size() );
41}
42
43
45{
49 m_batchIsland = NULL;
50}
51
52
54{
55 for ( int i = 0; i < m_allocatedIslands.size(); ++i )
56 {
57 delete m_allocatedIslands[ i ];
58 }
62}
63
64
65inline int getIslandId(const btPersistentManifold* lhs)
66{
67 const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0());
68 const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1());
69 int islandId = rcolObj0->getIslandTag() >= 0 ? rcolObj0->getIslandTag() : rcolObj1->getIslandTag();
70 return islandId;
71}
72
73
75{
76 const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
77 const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
78 int islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag();
79 return islandId;
80}
81
84{
85public:
87 {
88 int lCost = calcBatchCost( lhs );
89 int rCost = calcBatchCost( rhs );
90 return lCost > rCost;
91 }
92};
93
94
96{
97public:
99 {
100 return lhs->bodyArray.capacity() > rhs->bodyArray.capacity();
101 }
102};
103
104
106{
107 // append bodies
108 for ( int i = 0; i < other.bodyArray.size(); ++i )
109 {
110 bodyArray.push_back( other.bodyArray[ i ] );
111 }
112 // append manifolds
113 for ( int i = 0; i < other.manifoldArray.size(); ++i )
114 {
116 }
117 // append constraints
118 for ( int i = 0; i < other.constraintArray.size(); ++i )
119 {
120 constraintArray.push_back( other.constraintArray[ i ] );
121 }
122}
123
124
126{
127 for ( int i = 0; i < island.bodyArray.size(); ++i )
128 {
129 if ( island.bodyArray[ i ] == obj )
130 {
131 return true;
132 }
133 }
134 return false;
135}
136
137
139{
140 // reset island pools
141 int numElem = getUnionFind().getNumElements();
142 m_lookupIslandFromId.resize( numElem );
143 for ( int i = 0; i < m_lookupIslandFromId.size(); ++i )
144 {
145 m_lookupIslandFromId[ i ] = NULL;
146 }
149 // check whether allocated islands are sorted by body capacity (largest to smallest)
150 int lastCapacity = 0;
151 bool isSorted = true;
152 for ( int i = 0; i < m_allocatedIslands.size(); ++i )
153 {
154 Island* island = m_allocatedIslands[ i ];
155 int cap = island->bodyArray.capacity();
156 if ( cap > lastCapacity )
157 {
158 isSorted = false;
159 break;
160 }
161 lastCapacity = cap;
162 }
163 if ( !isSorted )
164 {
166 }
167
168 m_batchIsland = NULL;
169 // mark all islands free (but avoid deallocation)
170 for ( int i = 0; i < m_allocatedIslands.size(); ++i )
171 {
172 Island* island = m_allocatedIslands[ i ];
173 island->bodyArray.resize( 0 );
174 island->manifoldArray.resize( 0 );
175 island->constraintArray.resize( 0 );
176 island->id = -1;
177 island->isSleeping = true;
178 m_freeIslands.push_back( island );
179 }
180}
181
182
184{
185 Island* island = m_lookupIslandFromId[ id ];
186 if ( island == NULL )
187 {
188 // search for existing island
189 for ( int i = 0; i < m_activeIslands.size(); ++i )
190 {
191 if ( m_activeIslands[ i ]->id == id )
192 {
193 island = m_activeIslands[ i ];
194 break;
195 }
196 }
197 m_lookupIslandFromId[ id ] = island;
198 }
199 return island;
200}
201
202
204{
205 Island* island = NULL;
206 int allocSize = numBodies;
207 if ( numBodies < m_batchIslandMinBodyCount )
208 {
209 if ( m_batchIsland )
210 {
211 island = m_batchIsland;
212 m_lookupIslandFromId[ id ] = island;
213 // if we've made a large enough batch,
214 if ( island->bodyArray.size() + numBodies >= m_batchIslandMinBodyCount )
215 {
216 // next time start a new batch
217 m_batchIsland = NULL;
218 }
219 return island;
220 }
221 else
222 {
223 // need to allocate a batch island
224 allocSize = m_batchIslandMinBodyCount * 2;
225 }
226 }
228
229 // search for free island
230 if ( freeIslands.size() > 0 )
231 {
232 // try to reuse a previously allocated island
233 int iFound = freeIslands.size();
234 // linear search for smallest island that can hold our bodies
235 for ( int i = freeIslands.size() - 1; i >= 0; --i )
236 {
237 if ( freeIslands[ i ]->bodyArray.capacity() >= allocSize )
238 {
239 iFound = i;
240 island = freeIslands[ i ];
241 island->id = id;
242 break;
243 }
244 }
245 // if found, shrink array while maintaining ordering
246 if ( island )
247 {
248 int iDest = iFound;
249 int iSrc = iDest + 1;
250 while ( iSrc < freeIslands.size() )
251 {
252 freeIslands[ iDest++ ] = freeIslands[ iSrc++ ];
253 }
254 freeIslands.pop_back();
255 }
256 }
257 if ( island == NULL )
258 {
259 // no free island found, allocate
260 island = new Island(); // TODO: change this to use the pool allocator
261 island->id = id;
262 island->bodyArray.reserve( allocSize );
264 }
265 m_lookupIslandFromId[ id ] = island;
266 if ( numBodies < m_batchIslandMinBodyCount )
267 {
268 m_batchIsland = island;
269 }
270 m_activeIslands.push_back( island );
271 return island;
272}
273
274
276{
277
278 BT_PROFILE("islandUnionFindAndQuickSort");
279
280 btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
281
282 //we are going to sort the unionfind array, and store the element id in the size
283 //afterwards, we clean unionfind, to make sure no-one uses it anymore
284
286 int numElem = getUnionFind().getNumElements();
287
288 int endIslandIndex=1;
289 int startIslandIndex;
290
291 //update the sleeping state for bodies, if all are sleeping
292 for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
293 {
294 int islandId = getUnionFind().getElement(startIslandIndex).m_id;
295 for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
296 {
297 }
298
299 //int numSleeping = 0;
300
301 bool allSleeping = true;
302
303 int idx;
304 for (idx=startIslandIndex;idx<endIslandIndex;idx++)
305 {
306 int i = getUnionFind().getElement(idx).m_sz;
307
308 btCollisionObject* colObj0 = collisionObjects[i];
309 if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
310 {
311// printf("error in island management\n");
312 }
313
314 btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
315 if (colObj0->getIslandTag() == islandId)
316 {
317 if (colObj0->getActivationState()== ACTIVE_TAG)
318 {
319 allSleeping = false;
320 }
322 {
323 allSleeping = false;
324 }
325 }
326 }
327
328 if (allSleeping)
329 {
330 int idx;
331 for (idx=startIslandIndex;idx<endIslandIndex;idx++)
332 {
333 int i = getUnionFind().getElement(idx).m_sz;
334 btCollisionObject* colObj0 = collisionObjects[i];
335 if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
336 {
337// printf("error in island management\n");
338 }
339
340 btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
341
342 if (colObj0->getIslandTag() == islandId)
343 {
345 }
346 }
347 } else
348 {
349
350 int idx;
351 for (idx=startIslandIndex;idx<endIslandIndex;idx++)
352 {
353 int i = getUnionFind().getElement(idx).m_sz;
354
355 btCollisionObject* colObj0 = collisionObjects[i];
356 if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
357 {
358// printf("error in island management\n");
359 }
360
361 btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
362
363 if (colObj0->getIslandTag() == islandId)
364 {
365 if ( colObj0->getActivationState() == ISLAND_SLEEPING)
366 {
368 colObj0->setDeactivationTime(0.f);
369 }
370 }
371 }
372 }
373 }
374}
375
376
378{
379 btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
380 int endIslandIndex = 1;
381 int startIslandIndex;
382 int numElem = getUnionFind().getNumElements();
383
384 // create explicit islands and add bodies to each
385 for ( startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex )
386 {
387 int islandId = getUnionFind().getElement( startIslandIndex ).m_id;
388
389 // find end index
390 for ( endIslandIndex = startIslandIndex; ( endIslandIndex < numElem ) && ( getUnionFind().getElement( endIslandIndex ).m_id == islandId ); endIslandIndex++ )
391 {
392 }
393 // check if island is sleeping
394 bool islandSleeping = true;
395 for ( int iElem = startIslandIndex; iElem < endIslandIndex; iElem++ )
396 {
397 int i = getUnionFind().getElement( iElem ).m_sz;
398 btCollisionObject* colObj = collisionObjects[ i ];
399 if ( colObj->isActive() )
400 {
401 islandSleeping = false;
402 }
403 }
404 if ( !islandSleeping )
405 {
406 // want to count the number of bodies before allocating the island to optimize memory usage of the Island structures
407 int numBodies = endIslandIndex - startIslandIndex;
408 Island* island = allocateIsland( islandId, numBodies );
409 island->isSleeping = false;
410
411 // add bodies to island
412 for ( int iElem = startIslandIndex; iElem < endIslandIndex; iElem++ )
413 {
414 int i = getUnionFind().getElement( iElem ).m_sz;
415 btCollisionObject* colObj = collisionObjects[ i ];
416 island->bodyArray.push_back( colObj );
417 }
418 }
419 }
420
421}
422
423
425{
426 // walk all the manifolds, activating bodies touched by kinematic objects, and add each manifold to its Island
427 int maxNumManifolds = dispatcher->getNumManifolds();
428 for ( int i = 0; i < maxNumManifolds; i++ )
429 {
430 btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal( i );
431
432 const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>( manifold->getBody0() );
433 const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>( manifold->getBody1() );
434
436 if ( ( ( colObj0 ) && colObj0->getActivationState() != ISLAND_SLEEPING ) ||
437 ( ( colObj1 ) && colObj1->getActivationState() != ISLAND_SLEEPING ) )
438 {
439
440 //kinematic objects don't merge islands, but wake up all connected objects
441 if ( colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING )
442 {
443 if ( colObj0->hasContactResponse() )
444 colObj1->activate();
445 }
446 if ( colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING )
447 {
448 if ( colObj1->hasContactResponse() )
449 colObj0->activate();
450 }
451 //filtering for response
452 if ( dispatcher->needsResponse( colObj0, colObj1 ) )
453 {
454 // scatter manifolds into various islands
455 int islandId = getIslandId( manifold );
456 // if island not sleeping,
457 if ( Island* island = getIsland( islandId ) )
458 {
459 island->manifoldArray.push_back( manifold );
460 }
461 }
462 }
463 }
464}
465
466
468{
469 // walk constraints
470 for ( int i = 0; i < constraints.size(); i++ )
471 {
472 // scatter constraints into various islands
473 btTypedConstraint* constraint = constraints[ i ];
474 if ( constraint->isEnabled() )
475 {
476 int islandId = btGetConstraintIslandId( constraint );
477 // if island is not sleeping,
478 if ( Island* island = getIsland( islandId ) )
479 {
480 island->constraintArray.push_back( constraint );
481 }
482 }
483 }
484}
485
486
488{
489 // sort islands in order of decreasing batch size
491
492 // merge small islands to satisfy minimum batch size
493 // find first small batch island
494 int destIslandIndex = m_activeIslands.size();
495 for ( int i = 0; i < m_activeIslands.size(); ++i )
496 {
497 Island* island = m_activeIslands[ i ];
498 int batchSize = calcBatchCost( island );
499 if ( batchSize < m_minimumSolverBatchSize )
500 {
501 destIslandIndex = i;
502 break;
503 }
504 }
505 int lastIndex = m_activeIslands.size() - 1;
506 while ( destIslandIndex < lastIndex )
507 {
508 // merge islands from the back of the list
509 Island* island = m_activeIslands[ destIslandIndex ];
510 int numBodies = island->bodyArray.size();
511 int numManifolds = island->manifoldArray.size();
512 int numConstraints = island->constraintArray.size();
513 int firstIndex = lastIndex;
514 // figure out how many islands we want to merge and find out how many bodies, manifolds and constraints we will have
515 while ( true )
516 {
517 Island* src = m_activeIslands[ firstIndex ];
518 numBodies += src->bodyArray.size();
519 numManifolds += src->manifoldArray.size();
520 numConstraints += src->constraintArray.size();
521 int batchCost = calcBatchCost( numBodies, numManifolds, numConstraints );
522 if ( batchCost >= m_minimumSolverBatchSize )
523 {
524 break;
525 }
526 if ( firstIndex - 1 == destIslandIndex )
527 {
528 break;
529 }
530 firstIndex--;
531 }
532 // reserve space for these pointers to minimize reallocation
533 island->bodyArray.reserve( numBodies );
534 island->manifoldArray.reserve( numManifolds );
535 island->constraintArray.reserve( numConstraints );
536 // merge islands
537 for ( int i = firstIndex; i <= lastIndex; ++i )
538 {
539 island->append( *m_activeIslands[ i ] );
540 }
541 // shrink array to exclude the islands that were merged from
542 m_activeIslands.resize( firstIndex );
543 lastIndex = firstIndex - 1;
544 destIslandIndex++;
545 }
546}
547
548
550{
551 BT_PROFILE( "serialIslandDispatch" );
552 // serial dispatch
553 btAlignedObjectArray<Island*>& islands = *islandsPtr;
554 for ( int i = 0; i < islands.size(); ++i )
555 {
556 Island* island = islands[ i ];
557 btPersistentManifold** manifolds = island->manifoldArray.size() ? &island->manifoldArray[ 0 ] : NULL;
558 btTypedConstraint** constraintsPtr = island->constraintArray.size() ? &island->constraintArray[ 0 ] : NULL;
559 callback->processIsland( &island->bodyArray[ 0 ],
560 island->bodyArray.size(),
561 manifolds,
562 island->manifoldArray.size(),
563 constraintsPtr,
564 island->constraintArray.size(),
565 island->id
566 );
567 }
568}
569
571{
574
575 void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
576 {
577 for ( int i = iBegin; i < iEnd; ++i )
578 {
579 btSimulationIslandManagerMt::Island* island = ( *islandsPtr )[ i ];
580 btPersistentManifold** manifolds = island->manifoldArray.size() ? &island->manifoldArray[ 0 ] : NULL;
581 btTypedConstraint** constraintsPtr = island->constraintArray.size() ? &island->constraintArray[ 0 ] : NULL;
582 callback->processIsland( &island->bodyArray[ 0 ],
583 island->bodyArray.size(),
584 manifolds,
585 island->manifoldArray.size(),
586 constraintsPtr,
587 island->constraintArray.size(),
588 island->id
589 );
590 }
591 }
592};
593
595{
596 BT_PROFILE( "parallelIslandDispatch" );
597 int grainSize = 1; // iterations per task
598 UpdateIslandDispatcher dispatcher;
599 dispatcher.islandsPtr = islandsPtr;
600 dispatcher.callback = callback;
601 btParallelFor( 0, islandsPtr->size(), grainSize, dispatcher );
602}
603
604
607 btCollisionWorld* collisionWorld,
609 IslandCallback* callback
610 )
611{
612 btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
613
614 buildIslands(dispatcher,collisionWorld);
615
616 BT_PROFILE("processIslands");
617
618 if(!getSplitIslands())
619 {
620 btPersistentManifold** manifolds = dispatcher->getInternalManifoldPointer();
621 int maxNumManifolds = dispatcher->getNumManifolds();
622
623 for ( int i = 0; i < maxNumManifolds; i++ )
624 {
625 btPersistentManifold* manifold = manifolds[ i ];
626
627 const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>( manifold->getBody0() );
628 const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>( manifold->getBody1() );
629
631 if ( ( ( colObj0 ) && colObj0->getActivationState() != ISLAND_SLEEPING ) ||
632 ( ( colObj1 ) && colObj1->getActivationState() != ISLAND_SLEEPING ) )
633 {
634
635 //kinematic objects don't merge islands, but wake up all connected objects
636 if ( colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING )
637 {
638 if ( colObj0->hasContactResponse() )
639 colObj1->activate();
640 }
641 if ( colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING )
642 {
643 if ( colObj1->hasContactResponse() )
644 colObj0->activate();
645 }
646 }
647 }
648 btTypedConstraint** constraintsPtr = constraints.size() ? &constraints[ 0 ] : NULL;
649 callback->processIsland(&collisionObjects[0],
650 collisionObjects.size(),
651 manifolds,
652 maxNumManifolds,
653 constraintsPtr,
654 constraints.size(),
655 -1
656 );
657 }
658 else
659 {
661
662 //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
663 addBodiesToIslands( collisionWorld );
664 addManifoldsToIslands( dispatcher );
665 addConstraintsToIslands( constraints );
666
667 // m_activeIslands array should now contain all non-sleeping Islands, and each Island should
668 // have all the necessary bodies, manifolds and constraints.
669
670 // if we want to merge islands with small batch counts,
671 if ( m_minimumSolverBatchSize > 1 )
672 {
673 mergeIslands();
674 }
675 // dispatch islands to solver
676 m_islandDispatch( &m_activeIslands, callback );
677 }
678}
static void getElement(int arrayLen, const char *cur, const char *old, char *oldPtr, char *curData)
Definition: bFile.cpp:868
#define ACTIVE_TAG
#define DISABLE_DEACTIVATION
#define WANTS_DEACTIVATION
#define ISLAND_SLEEPING
#define BT_PROFILE(name)
Definition: btQuickprof.h:215
#define SIMD_FORCE_INLINE
Definition: btScalar.h:81
#define btAssert(x)
Definition: btScalar.h:131
bool btIsBodyInIsland(const btSimulationIslandManagerMt::Island &island, const btCollisionObject *obj)
int btGetConstraintIslandId(const btTypedConstraint *lhs)
int getIslandId(const btPersistentManifold *lhs)
int calcBatchCost(int bodies, int manifolds, int constraints)
void btParallelFor(int iBegin, int iEnd, int grainSize, const btIParallelForBody &body)
Definition: btThreads.cpp:429
#define BT_OVERRIDE
Definition: btThreads.h:28
function object that routes calls to operator<
bool operator()(const btSimulationIslandManagerMt::Island *lhs, const btSimulationIslandManagerMt::Island *rhs) const
bool operator()(const btSimulationIslandManagerMt::Island *lhs, const btSimulationIslandManagerMt::Island *rhs) const
int size() const
return the number of elements in the array
void resize(int newsize, const T &fillData=T())
void quickSort(const L &CompareFunc)
void push_back(const T &_Val)
int capacity() const
return the pre-allocated (reserved) elements, this is at least as large as the total number of elemen...
btCollisionObject can be used to manage collision detection objects.
bool hasContactResponse() const
void activate(bool forceActivation=false) const
void setActivationState(int newState) const
bool isKinematicObject() const
int getIslandTag() const
void setDeactivationTime(btScalar time)
int getActivationState() const
CollisionWorld is interface and container for the collision detection.
btCollisionObjectArray & getCollisionObjectArray()
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:76
virtual int getNumManifolds() const =0
virtual btPersistentManifold * getManifoldByIndexInternal(int index)=0
virtual bool needsResponse(const btCollisionObject *body0, const btCollisionObject *body1)=0
virtual btPersistentManifold ** getInternalManifoldPointer()=0
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
const btCollisionObject * getBody0() const
const btCollisionObject * getBody1() const
btAlignedObjectArray< Island * > m_allocatedIslands
virtual void addBodiesToIslands(btCollisionWorld *collisionWorld)
static void parallelIslandDispatch(btAlignedObjectArray< Island * > *islandsPtr, IslandCallback *callback)
virtual Island * allocateIsland(int id, int numBodies)
btAlignedObjectArray< Island * > m_freeIslands
btAlignedObjectArray< Island * > m_lookupIslandFromId
virtual void addManifoldsToIslands(btDispatcher *dispatcher)
virtual void addConstraintsToIslands(btAlignedObjectArray< btTypedConstraint * > &constraints)
virtual void buildAndProcessIslands(btDispatcher *dispatcher, btCollisionWorld *collisionWorld, btAlignedObjectArray< btTypedConstraint * > &constraints, IslandCallback *callback)
btAlignedObjectArray< Island * > m_activeIslands
static void serialIslandDispatch(btAlignedObjectArray< Island * > *islandsPtr, IslandCallback *callback)
virtual void buildIslands(btDispatcher *dispatcher, btCollisionWorld *colWorld)
TypedConstraint is the baseclass for Bullet constraints and vehicles.
bool isEnabled() const
const btRigidBody & getRigidBodyA() const
const btRigidBody & getRigidBodyB() const
int getNumElements() const
Definition: btUnionFind.h:52
btElement & getElement(int index)
Definition: btUnionFind.h:61
void sortIslands()
this is a special operation, destroying the content of btUnionFind.
Definition: btUnionFind.cpp:64
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
btAlignedObjectArray< btSimulationIslandManagerMt::Island * > * islandsPtr
btSimulationIslandManagerMt::IslandCallback * callback
virtual void processIsland(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifolds, int numManifolds, btTypedConstraint **constraints, int numConstraints, int islandId)=0
btAlignedObjectArray< btTypedConstraint * > constraintArray
btAlignedObjectArray< btCollisionObject * > bodyArray
btAlignedObjectArray< btPersistentManifold * > manifoldArray