← Back to branch summary

~ubuntu-branches/ubuntu/vivid/emscripten/vivid

« back to all changes in this revision

Viewing changes to tests/bullet/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp

  • Committer: Package Import Robot
  • Author(s): Sylvestre Ledru
  • Date: 2013-05-02 13:11:51 UTC
  • Revision ID: package-import@ubuntu.com-20130502131151-q8dvteqr1ef2x7xz
Tags: upstream-1.4.1~20130504~adb56cb
Import upstream version 1.4.1~20130504~adb56cb

Show diffs side-by-side

added added

removed removed

Lines of Context:
tests/bullet/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp
 
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
#include "btMultiSapBroadphase.h"
 
17
 
 
18
#include "btSimpleBroadphase.h"
 
19
#include "LinearMath/btAabbUtil2.h"
 
20
#include "btQuantizedBvh.h"
 
21
 
 
22
///     btSapBroadphaseArray    m_sapBroadphases;
 
23
 
 
24
///     btOverlappingPairCache* m_overlappingPairs;
 
25
extern int gOverlappingPairs;
 
26
 
 
27
/*
 
28
class btMultiSapSortedOverlappingPairCache : public btSortedOverlappingPairCache
 
29
{
 
30
public:
 
31
 
 
32
        virtual btBroadphasePair*       addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
 
33
        {
 
34
                return btSortedOverlappingPairCache::addOverlappingPair((btBroadphaseProxy*)proxy0->m_multiSapParentProxy,(btBroadphaseProxy*)proxy1->m_multiSapParentProxy);
 
35
        }
 
36
};
 
37
 
 
38
*/
 
39
 
 
40
btMultiSapBroadphase::btMultiSapBroadphase(int /*maxProxies*/,btOverlappingPairCache* pairCache)
 
41
:m_overlappingPairs(pairCache),
 
42
m_optimizedAabbTree(0),
 
43
m_ownsPairCache(false),
 
44
m_invalidPair(0)
 
45
{
 
46
        if (!m_overlappingPairs)
 
47
        {
 
48
                m_ownsPairCache = true;
 
49
                void* mem = btAlignedAlloc(sizeof(btSortedOverlappingPairCache),16);
 
50
                m_overlappingPairs = new (mem)btSortedOverlappingPairCache();
 
51
        }
 
52
 
 
53
        struct btMultiSapOverlapFilterCallback : public btOverlapFilterCallback
 
54
        {
 
55
                virtual ~btMultiSapOverlapFilterCallback()
 
56
                {}
 
57
                // return true when pairs need collision
 
58
                virtual bool    needBroadphaseCollision(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1) const
 
59
                {
 
60
                        btBroadphaseProxy* multiProxy0 = (btBroadphaseProxy*)childProxy0->m_multiSapParentProxy;
 
61
                        btBroadphaseProxy* multiProxy1 = (btBroadphaseProxy*)childProxy1->m_multiSapParentProxy;
 
62
                        
 
63
                        bool collides = (multiProxy0->m_collisionFilterGroup & multiProxy1->m_collisionFilterMask) != 0;
 
64
                        collides = collides && (multiProxy1->m_collisionFilterGroup & multiProxy0->m_collisionFilterMask);
 
65
        
 
66
                        return collides;
 
67
                }
 
68
        };
 
69
 
 
70
        void* mem = btAlignedAlloc(sizeof(btMultiSapOverlapFilterCallback),16);
 
71
        m_filterCallback = new (mem)btMultiSapOverlapFilterCallback();
 
72
 
 
73
        m_overlappingPairs->setOverlapFilterCallback(m_filterCallback);
 
74
//      mem = btAlignedAlloc(sizeof(btSimpleBroadphase),16);
 
75
//      m_simpleBroadphase = new (mem) btSimpleBroadphase(maxProxies,m_overlappingPairs);
 
76
}
 
77
 
 
78
btMultiSapBroadphase::~btMultiSapBroadphase()
 
79
{
 
80
        if (m_ownsPairCache)
 
81
        {
 
82
                m_overlappingPairs->~btOverlappingPairCache();
 
83
                btAlignedFree(m_overlappingPairs);
 
84
        }
 
85
}
 
86
 
 
87
 
 
88
void    btMultiSapBroadphase::buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax)
 
89
{
 
90
        m_optimizedAabbTree = new btQuantizedBvh();
 
91
        m_optimizedAabbTree->setQuantizationValues(bvhAabbMin,bvhAabbMax);
 
92
        QuantizedNodeArray&     nodes = m_optimizedAabbTree->getLeafNodeArray();
 
93
        for (int i=0;i<m_sapBroadphases.size();i++)
 
94
        {
 
95
                btQuantizedBvhNode node;
 
96
                btVector3 aabbMin,aabbMax;
 
97
                m_sapBroadphases[i]->getBroadphaseAabb(aabbMin,aabbMax);
 
98
                m_optimizedAabbTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0);
 
99
                m_optimizedAabbTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1);
 
100
                int partId = 0;
 
101
                node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | i;
 
102
                nodes.push_back(node);
 
103
        }
 
104
        m_optimizedAabbTree->buildInternal();
 
105
}
 
106
 
 
107
btBroadphaseProxy*      btMultiSapBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* /*ignoreMe*/)
 
108
{
 
109
        //void* ignoreMe -> we could think of recursive multi-sap, if someone is interested
 
110
 
 
111
        void* mem = btAlignedAlloc(sizeof(btMultiSapProxy),16);
 
112
        btMultiSapProxy* proxy = new (mem)btMultiSapProxy(aabbMin,  aabbMax,shapeType,userPtr, collisionFilterGroup,collisionFilterMask);
 
113
        m_multiSapProxies.push_back(proxy);
 
114
 
 
115
        ///this should deal with inserting/removal into child broadphases
 
116
        setAabb(proxy,aabbMin,aabbMax,dispatcher);
 
117
        return proxy;
 
118
}
 
119
 
 
120
void    btMultiSapBroadphase::destroyProxy(btBroadphaseProxy* /*proxy*/,btDispatcher* /*dispatcher*/)
 
121
{
 
122
        ///not yet
 
123
        btAssert(0);
 
124
 
 
125
}
 
126
 
 
127
 
 
128
void    btMultiSapBroadphase::addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface*  childBroadphase)
 
129
{
 
130
        void* mem = btAlignedAlloc(sizeof(btBridgeProxy),16);
 
131
        btBridgeProxy* bridgeProxyRef = new(mem) btBridgeProxy;
 
132
        bridgeProxyRef->m_childProxy = childProxy;
 
133
        bridgeProxyRef->m_childBroadphase = childBroadphase;
 
134
        parentMultiSapProxy->m_bridgeProxies.push_back(bridgeProxyRef);
 
135
}
 
136
 
 
137
 
 
138
bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax);
 
139
bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax)
 
140
{
 
141
return
 
142
amin.getX() >= bmin.getX() && amax.getX() <= bmax.getX() &&
 
143
amin.getY() >= bmin.getY() && amax.getY() <= bmax.getY() &&
 
144
amin.getZ() >= bmin.getZ() && amax.getZ() <= bmax.getZ();
 
145
}
 
146
 
 
147
 
 
148
 
 
149
 
 
150
 
 
151
 
 
152
void    btMultiSapBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
 
153
{
 
154
        btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy);
 
155
        aabbMin = multiProxy->m_aabbMin;
 
156
        aabbMax = multiProxy->m_aabbMax;
 
157
}
 
158
 
 
159
void    btMultiSapBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin,const btVector3& aabbMax)
 
160
{
 
161
        for (int i=0;i<m_multiSapProxies.size();i++)
 
162
        {
 
163
                rayCallback.process(m_multiSapProxies[i]);
 
164
        }
 
165
}
 
166
 
 
167
 
 
168
//#include <stdio.h>
 
169
 
 
170
void    btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)
 
171
{
 
172
        btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy);
 
173
        multiProxy->m_aabbMin = aabbMin;
 
174
        multiProxy->m_aabbMax = aabbMax;
 
175
        
 
176
        
 
177
//      bool fullyContained = false;
 
178
//      bool alreadyInSimple = false;
 
179
        
 
180
 
 
181
 
 
182
        
 
183
        struct MyNodeOverlapCallback : public btNodeOverlapCallback
 
184
        {
 
185
                btMultiSapBroadphase*   m_multiSap;
 
186
                btMultiSapProxy*                m_multiProxy;
 
187
                btDispatcher*                   m_dispatcher;
 
188
 
 
189
                MyNodeOverlapCallback(btMultiSapBroadphase* multiSap,btMultiSapProxy* multiProxy,btDispatcher* dispatcher)
 
190
                        :m_multiSap(multiSap),
 
191
                        m_multiProxy(multiProxy),
 
192
                        m_dispatcher(dispatcher)
 
193
                {
 
194
 
 
195
                }
 
196
 
 
197
                virtual void processNode(int /*nodeSubPart*/, int broadphaseIndex)
 
198
                {
 
199
                        btBroadphaseInterface* childBroadphase = m_multiSap->getBroadphaseArray()[broadphaseIndex];
 
200
 
 
201
                        int containingBroadphaseIndex = -1;
 
202
                        //already found?
 
203
                        for (int i=0;i<m_multiProxy->m_bridgeProxies.size();i++)
 
204
                        {
 
205
 
 
206
                                if (m_multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase)
 
207
                                {
 
208
                                        containingBroadphaseIndex = i;
 
209
                                        break;
 
210
                                }
 
211
                        }
 
212
                        if (containingBroadphaseIndex<0)
 
213
                        {
 
214
                                //add it
 
215
                                btBroadphaseProxy* childProxy = childBroadphase->createProxy(m_multiProxy->m_aabbMin,m_multiProxy->m_aabbMax,m_multiProxy->m_shapeType,m_multiProxy->m_clientObject,m_multiProxy->m_collisionFilterGroup,m_multiProxy->m_collisionFilterMask, m_dispatcher,m_multiProxy);
 
216
                                m_multiSap->addToChildBroadphase(m_multiProxy,childProxy,childBroadphase);
 
217
 
 
218
                        }
 
219
                }
 
220
        };
 
221
 
 
222
        MyNodeOverlapCallback   myNodeCallback(this,multiProxy,dispatcher);
 
223
 
 
224
 
 
225
 
 
226
        
 
227
        if (m_optimizedAabbTree)
 
228
                m_optimizedAabbTree->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax);
 
229
 
 
230
        int i;
 
231
 
 
232
        for ( i=0;i<multiProxy->m_bridgeProxies.size();i++)
 
233
        {
 
234
                btVector3 worldAabbMin,worldAabbMax;
 
235
                multiProxy->m_bridgeProxies[i]->m_childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax);
 
236
                bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
 
237
                if (!overlapsBroadphase)
 
238
                {
 
239
                        //remove it now
 
240
                        btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[i];
 
241
 
 
242
                        btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy;
 
243
                        bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher);
 
244
                        
 
245
                        multiProxy->m_bridgeProxies.swap( i,multiProxy->m_bridgeProxies.size()-1);
 
246
                        multiProxy->m_bridgeProxies.pop_back();
 
247
 
 
248
                }
 
249
        }
 
250
 
 
251
 
 
252
        /*
 
253
 
 
254
        if (1)
 
255
        {
 
256
 
 
257
                //find broadphase that contain this multiProxy
 
258
                int numChildBroadphases = getBroadphaseArray().size();
 
259
                for (int i=0;i<numChildBroadphases;i++)
 
260
                {
 
261
                        btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i];
 
262
                        btVector3 worldAabbMin,worldAabbMax;
 
263
                        childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax);
 
264
                        bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
 
265
                        
 
266
                //      fullyContained = fullyContained || boxIsContainedWithinBox(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
 
267
                        int containingBroadphaseIndex = -1;
 
268
                        
 
269
                        //if already contains this
 
270
                        
 
271
                        for (int i=0;i<multiProxy->m_bridgeProxies.size();i++)
 
272
                        {
 
273
                                if (multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase)
 
274
                                {
 
275
                                        containingBroadphaseIndex = i;
 
276
                                }
 
277
                                alreadyInSimple = alreadyInSimple || (multiProxy->m_bridgeProxies[i]->m_childBroadphase == m_simpleBroadphase);
 
278
                        }
 
279
 
 
280
                        if (overlapsBroadphase)
 
281
                        {
 
282
                                if (containingBroadphaseIndex<0)
 
283
                                {
 
284
                                        btBroadphaseProxy* childProxy = childBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
 
285
                                        childProxy->m_multiSapParentProxy = multiProxy;
 
286
                                        addToChildBroadphase(multiProxy,childProxy,childBroadphase);
 
287
                                }
 
288
                        } else
 
289
                        {
 
290
                                if (containingBroadphaseIndex>=0)
 
291
                                {
 
292
                                        //remove
 
293
                                        btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[containingBroadphaseIndex];
 
294
 
 
295
                                        btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy;
 
296
                                        bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher);
 
297
                                        
 
298
                                        multiProxy->m_bridgeProxies.swap( containingBroadphaseIndex,multiProxy->m_bridgeProxies.size()-1);
 
299
                                        multiProxy->m_bridgeProxies.pop_back();
 
300
                                }
 
301
                        }
 
302
                }
 
303
 
 
304
 
 
305
                ///If we are in no other child broadphase, stick the proxy in the global 'simple' broadphase (brute force)
 
306
                ///hopefully we don't end up with many entries here (can assert/provide feedback on stats)
 
307
                if (0)//!multiProxy->m_bridgeProxies.size())
 
308
                {
 
309
                        ///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision
 
310
                        ///this is needed to be able to calculate the aabb overlap
 
311
                        btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
 
312
                        childProxy->m_multiSapParentProxy = multiProxy;
 
313
                        addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase);
 
314
                }
 
315
        }
 
316
 
 
317
        if (!multiProxy->m_bridgeProxies.size())
 
318
        {
 
319
                ///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision
 
320
                ///this is needed to be able to calculate the aabb overlap
 
321
                btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
 
322
                childProxy->m_multiSapParentProxy = multiProxy;
 
323
                addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase);
 
324
        }
 
325
*/
 
326
 
 
327
 
 
328
        //update
 
329
        for ( i=0;i<multiProxy->m_bridgeProxies.size();i++)
 
330
        {
 
331
                btBridgeProxy* bridgeProxyRef = multiProxy->m_bridgeProxies[i];
 
332
                bridgeProxyRef->m_childBroadphase->setAabb(bridgeProxyRef->m_childProxy,aabbMin,aabbMax,dispatcher);
 
333
        }
 
334
 
 
335
}
 
336
bool stopUpdating=false;
 
337
 
 
338
 
 
339
 
 
340
class btMultiSapBroadphasePairSortPredicate
 
341
{
 
342
        public:
 
343
 
 
344
                bool operator() ( const btBroadphasePair& a1, const btBroadphasePair& b1 )
 
345
                {
 
346
                                btMultiSapBroadphase::btMultiSapProxy* aProxy0 = a1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy0->m_multiSapParentProxy : 0;
 
347
                                btMultiSapBroadphase::btMultiSapProxy* aProxy1 = a1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy1->m_multiSapParentProxy : 0;
 
348
                                btMultiSapBroadphase::btMultiSapProxy* bProxy0 = b1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy0->m_multiSapParentProxy : 0;
 
349
                                btMultiSapBroadphase::btMultiSapProxy* bProxy1 = b1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy1->m_multiSapParentProxy : 0;
 
350
 
 
351
                                 return aProxy0 > bProxy0 || 
 
352
                                        (aProxy0 == bProxy0 && aProxy1 > bProxy1) ||
 
353
                                        (aProxy0 == bProxy0 && aProxy1 == bProxy1 && a1.m_algorithm > b1.m_algorithm); 
 
354
                }
 
355
};
 
356
 
 
357
 
 
358
        ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
 
359
void    btMultiSapBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 
360
{
 
361
 
 
362
//      m_simpleBroadphase->calculateOverlappingPairs(dispatcher);
 
363
 
 
364
        if (!stopUpdating && getOverlappingPairCache()->hasDeferredRemoval())
 
365
        {
 
366
        
 
367
                btBroadphasePairArray&  overlappingPairArray = getOverlappingPairCache()->getOverlappingPairArray();
 
368
 
 
369
        //      quicksort(overlappingPairArray,0,overlappingPairArray.size());
 
370
 
 
371
                overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate());
 
372
 
 
373
                //perform a sort, to find duplicates and to sort 'invalid' pairs to the end
 
374
        //      overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate());
 
375
 
 
376
                overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
 
377
                m_invalidPair = 0;
 
378
 
 
379
                
 
380
                int i;
 
381
 
 
382
                btBroadphasePair previousPair;
 
383
                previousPair.m_pProxy0 = 0;
 
384
                previousPair.m_pProxy1 = 0;
 
385
                previousPair.m_algorithm = 0;
 
386
                
 
387
                
 
388
                for (i=0;i<overlappingPairArray.size();i++)
 
389
                {
 
390
                
 
391
                        btBroadphasePair& pair = overlappingPairArray[i];
 
392
 
 
393
                        btMultiSapProxy* aProxy0 = pair.m_pProxy0 ? (btMultiSapProxy*)pair.m_pProxy0->m_multiSapParentProxy : 0;
 
394
                        btMultiSapProxy* aProxy1 = pair.m_pProxy1 ? (btMultiSapProxy*)pair.m_pProxy1->m_multiSapParentProxy : 0;
 
395
                        btMultiSapProxy* bProxy0 = previousPair.m_pProxy0 ? (btMultiSapProxy*)previousPair.m_pProxy0->m_multiSapParentProxy : 0;
 
396
                        btMultiSapProxy* bProxy1 = previousPair.m_pProxy1 ? (btMultiSapProxy*)previousPair.m_pProxy1->m_multiSapParentProxy : 0;
 
397
 
 
398
                        bool isDuplicate = (aProxy0 == bProxy0) && (aProxy1 == bProxy1);
 
399
                        
 
400
                        previousPair = pair;
 
401
 
 
402
                        bool needsRemoval = false;
 
403
 
 
404
                        if (!isDuplicate)
 
405
                        {
 
406
                                bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
 
407
 
 
408
                                if (hasOverlap)
 
409
                                {
 
410
                                        needsRemoval = false;//callback->processOverlap(pair);
 
411
                                } else
 
412
                                {
 
413
                                        needsRemoval = true;
 
414
                                }
 
415
                        } else
 
416
                        {
 
417
                                //remove duplicate
 
418
                                needsRemoval = true;
 
419
                                //should have no algorithm
 
420
                                btAssert(!pair.m_algorithm);
 
421
                        }
 
422
                        
 
423
                        if (needsRemoval)
 
424
                        {
 
425
                                getOverlappingPairCache()->cleanOverlappingPair(pair,dispatcher);
 
426
 
 
427
                //              m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
 
428
                //              m_overlappingPairArray.pop_back();
 
429
                                pair.m_pProxy0 = 0;
 
430
                                pair.m_pProxy1 = 0;
 
431
                                m_invalidPair++;
 
432
                                gOverlappingPairs--;
 
433
                        } 
 
434
                        
 
435
                }
 
436
 
 
437
        ///if you don't like to skip the invalid pairs in the array, execute following code:
 
438
        #define CLEAN_INVALID_PAIRS 1
 
439
        #ifdef CLEAN_INVALID_PAIRS
 
440
 
 
441
                //perform a sort, to sort 'invalid' pairs to the end
 
442
                //overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate());
 
443
                overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate());
 
444
 
 
445
                overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
 
446
                m_invalidPair = 0;
 
447
        #endif//CLEAN_INVALID_PAIRS
 
448
                
 
449
                //printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size());
 
450
        }
 
451
 
 
452
 
 
453
}
 
454
 
 
455
 
 
456
bool    btMultiSapBroadphase::testAabbOverlap(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1)
 
457
{
 
458
        btMultiSapProxy* multiSapProxy0 = (btMultiSapProxy*)childProxy0->m_multiSapParentProxy;
 
459
                btMultiSapProxy* multiSapProxy1 = (btMultiSapProxy*)childProxy1->m_multiSapParentProxy;
 
460
 
 
461
                return  TestAabbAgainstAabb2(multiSapProxy0->m_aabbMin,multiSapProxy0->m_aabbMax,
 
462
                        multiSapProxy1->m_aabbMin,multiSapProxy1->m_aabbMax);
 
463
                
 
464
}
 
465
 
 
466
 
 
467
void    btMultiSapBroadphase::printStats()
 
468
{
 
469
/*      printf("---------------------------------\n");
 
470
        
 
471
                printf("btMultiSapBroadphase.h\n");
 
472
                printf("numHandles = %d\n",m_multiSapProxies.size());
 
473
                        //find broadphase that contain this multiProxy
 
474
                int numChildBroadphases = getBroadphaseArray().size();
 
475
                for (int i=0;i<numChildBroadphases;i++)
 
476
                {
 
477
 
 
478
                        btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i];
 
479
                        childBroadphase->printStats();
 
480
 
 
481
                }
 
482
                */
 
483
 
 
484
}
 
485
 
 
486
void btMultiSapBroadphase::resetPool(btDispatcher* dispatcher)
 
487
{
 
488
        // not yet
 
489
}