Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/SuperOrxoBros_HS18/src/external/bullet/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp @ 12177

Last change on this file since 12177 was 12177, checked in by siramesh, 5 years ago

Super Orxo Bros Final (Sidharth Ramesh, Nisa Balta, Jeff Ren)

File size: 16.6 KB
Line 
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#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;
25extern int gOverlappingPairs;
26
27/*
28class btMultiSapSortedOverlappingPairCache : public btSortedOverlappingPairCache
29{
30public:
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
40btMultiSapBroadphase::btMultiSapBroadphase(int /*maxProxies*/,btOverlappingPairCache* pairCache)
41:m_overlappingPairs(pairCache),
42m_optimizedAabbTree(0),
43m_ownsPairCache(false),
44m_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
78btMultiSapBroadphase::~btMultiSapBroadphase()
79{
80        if (m_ownsPairCache)
81        {
82                m_overlappingPairs->~btOverlappingPairCache();
83                btAlignedFree(m_overlappingPairs);
84        }
85}
86
87
88void    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
107btBroadphaseProxy*      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
120void    btMultiSapBroadphase::destroyProxy(btBroadphaseProxy* /*proxy*/,btDispatcher* /*dispatcher*/)
121{
122        ///not yet
123        btAssert(0);
124
125}
126
127
128void    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
138bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax);
139bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax)
140{
141return
142amin.getX() >= bmin.getX() && amax.getX() <= bmax.getX() &&
143amin.getY() >= bmin.getY() && amax.getY() <= bmax.getY() &&
144amin.getZ() >= bmin.getZ() && amax.getZ() <= bmax.getZ();
145}
146
147
148
149
150
151
152void    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
159void    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
170void    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}
336bool stopUpdating=false;
337
338
339
340class 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
359void    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
456bool    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
467void    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
486void btMultiSapBroadphase::resetPool(btDispatcher* dispatcher)
487{
488        // not yet
489}
Note: See TracBrowser for help on using the repository browser.