← Back to branch summary

~ubuntu-branches/ubuntu/saucy/emscripten/saucy-proposed

« back to all changes in this revision

Viewing changes to tests/bullet/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.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/btSimpleBroadphase.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 "btSimpleBroadphase.h"
 
17
#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
 
18
#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
 
19
 
 
20
#include "LinearMath/btVector3.h"
 
21
#include "LinearMath/btTransform.h"
 
22
#include "LinearMath/btMatrix3x3.h"
 
23
#include "LinearMath/btAabbUtil2.h"
 
24
 
 
25
#include <new>
 
26
 
 
27
extern int gOverlappingPairs;
 
28
 
 
29
void    btSimpleBroadphase::validate()
 
30
{
 
31
        for (int i=0;i<m_numHandles;i++)
 
32
        {
 
33
                for (int j=i+1;j<m_numHandles;j++)
 
34
                {
 
35
                        btAssert(&m_pHandles[i] != &m_pHandles[j]);
 
36
                }
 
37
        }
 
38
        
 
39
}
 
40
 
 
41
btSimpleBroadphase::btSimpleBroadphase(int maxProxies, btOverlappingPairCache* overlappingPairCache)
 
42
        :m_pairCache(overlappingPairCache),
 
43
        m_ownsPairCache(false),
 
44
        m_invalidPair(0)
 
45
{
 
46
 
 
47
        if (!overlappingPairCache)
 
48
        {
 
49
                void* mem = btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16);
 
50
                m_pairCache = new (mem)btHashedOverlappingPairCache();
 
51
                m_ownsPairCache = true;
 
52
        }
 
53
 
 
54
        // allocate handles buffer and put all handles on free list
 
55
        m_pHandlesRawPtr = btAlignedAlloc(sizeof(btSimpleBroadphaseProxy)*maxProxies,16);
 
56
        m_pHandles = new(m_pHandlesRawPtr) btSimpleBroadphaseProxy[maxProxies];
 
57
        m_maxHandles = maxProxies;
 
58
        m_numHandles = 0;
 
59
        m_firstFreeHandle = 0;
 
60
        m_LastHandleIndex = -1;
 
61
        
 
62
 
 
63
        {
 
64
                for (int i = m_firstFreeHandle; i < maxProxies; i++)
 
65
                {
 
66
                        m_pHandles[i].SetNextFree(i + 1);
 
67
                        m_pHandles[i].m_uniqueId = i+2;//any UID will do, we just avoid too trivial values (0,1) for debugging purposes
 
68
                }
 
69
                m_pHandles[maxProxies - 1].SetNextFree(0);
 
70
        
 
71
        }
 
72
 
 
73
}
 
74
 
 
75
btSimpleBroadphase::~btSimpleBroadphase()
 
76
{
 
77
        btAlignedFree(m_pHandlesRawPtr);
 
78
 
 
79
        if (m_ownsPairCache)
 
80
        {
 
81
                m_pairCache->~btOverlappingPairCache();
 
82
                btAlignedFree(m_pairCache);
 
83
        }
 
84
}
 
85
 
 
86
 
 
87
btBroadphaseProxy*      btSimpleBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* /*dispatcher*/,void* multiSapProxy)
 
88
{
 
89
        if (m_numHandles >= m_maxHandles)
 
90
        {
 
91
                btAssert(0);
 
92
                return 0; //should never happen, but don't let the game crash ;-)
 
93
        }
 
94
        btAssert(aabbMin[0]<= aabbMax[0] && aabbMin[1]<= aabbMax[1] && aabbMin[2]<= aabbMax[2]);
 
95
 
 
96
        int newHandleIndex = allocHandle();
 
97
        btSimpleBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex])btSimpleBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy);
 
98
 
 
99
        return proxy;
 
100
}
 
101
 
 
102
class   RemovingOverlapCallback : public btOverlapCallback
 
103
{
 
104
protected:
 
105
        virtual bool    processOverlap(btBroadphasePair& pair)
 
106
        {
 
107
                (void)pair;
 
108
                btAssert(0);
 
109
                return false;
 
110
        }
 
111
};
 
112
 
 
113
class RemovePairContainingProxy
 
114
{
 
115
 
 
116
        btBroadphaseProxy*      m_targetProxy;
 
117
        public:
 
118
        virtual ~RemovePairContainingProxy()
 
119
        {
 
120
        }
 
121
protected:
 
122
        virtual bool processOverlap(btBroadphasePair& pair)
 
123
        {
 
124
                btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy0);
 
125
                btSimpleBroadphaseProxy* proxy1 = static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy1);
 
126
 
 
127
                return ((m_targetProxy == proxy0 || m_targetProxy == proxy1));
 
128
        };
 
129
};
 
130
 
 
131
void    btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg,btDispatcher* dispatcher)
 
132
{
 
133
                
 
134
                btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxyOrg);
 
135
                freeHandle(proxy0);
 
136
 
 
137
                m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg,dispatcher);
 
138
 
 
139
                //validate();
 
140
                
 
141
}
 
142
 
 
143
void    btSimpleBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
 
144
{
 
145
        const btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy);
 
146
        aabbMin = sbp->m_aabbMin;
 
147
        aabbMax = sbp->m_aabbMax;
 
148
}
 
149
 
 
150
void    btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* /*dispatcher*/)
 
151
{
 
152
        btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy);
 
153
        sbp->m_aabbMin = aabbMin;
 
154
        sbp->m_aabbMax = aabbMax;
 
155
}
 
156
 
 
157
void    btSimpleBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin,const btVector3& aabbMax)
 
158
{
 
159
        for (int i=0; i <= m_LastHandleIndex; i++)
 
160
        {
 
161
                btSimpleBroadphaseProxy* proxy = &m_pHandles[i];
 
162
                if(!proxy->m_clientObject)
 
163
                {
 
164
                        continue;
 
165
                }
 
166
                rayCallback.process(proxy);
 
167
        }
 
168
}
 
169
 
 
170
 
 
171
void    btSimpleBroadphase::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback)
 
172
{
 
173
        for (int i=0; i <= m_LastHandleIndex; i++)
 
174
        {
 
175
                btSimpleBroadphaseProxy* proxy = &m_pHandles[i];
 
176
                if(!proxy->m_clientObject)
 
177
                {
 
178
                        continue;
 
179
                }
 
180
                if (TestAabbAgainstAabb2(aabbMin,aabbMax,proxy->m_aabbMin,proxy->m_aabbMax))
 
181
                {
 
182
                        callback.process(proxy);
 
183
                }
 
184
        }
 
185
}
 
186
 
 
187
 
 
188
 
 
189
        
 
190
 
 
191
 
 
192
 
 
193
bool    btSimpleBroadphase::aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1)
 
194
{
 
195
        return proxy0->m_aabbMin[0] <= proxy1->m_aabbMax[0] && proxy1->m_aabbMin[0] <= proxy0->m_aabbMax[0] && 
 
196
                   proxy0->m_aabbMin[1] <= proxy1->m_aabbMax[1] && proxy1->m_aabbMin[1] <= proxy0->m_aabbMax[1] &&
 
197
                   proxy0->m_aabbMin[2] <= proxy1->m_aabbMax[2] && proxy1->m_aabbMin[2] <= proxy0->m_aabbMax[2];
 
198
 
 
199
}
 
200
 
 
201
 
 
202
 
 
203
//then remove non-overlapping ones
 
204
class CheckOverlapCallback : public btOverlapCallback
 
205
{
 
206
public:
 
207
        virtual bool processOverlap(btBroadphasePair& pair)
 
208
        {
 
209
                return (!btSimpleBroadphase::aabbOverlap(static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy0),static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy1)));
 
210
        }
 
211
};
 
212
 
 
213
void    btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 
214
{
 
215
        //first check for new overlapping pairs
 
216
        int i,j;
 
217
        if (m_numHandles >= 0)
 
218
        {
 
219
                int new_largest_index = -1;
 
220
                for (i=0; i <= m_LastHandleIndex; i++)
 
221
                {
 
222
                        btSimpleBroadphaseProxy* proxy0 = &m_pHandles[i];
 
223
                        if(!proxy0->m_clientObject)
 
224
                        {
 
225
                                continue;
 
226
                        }
 
227
                        new_largest_index = i;
 
228
                        for (j=i+1; j <= m_LastHandleIndex; j++)
 
229
                        {
 
230
                                btSimpleBroadphaseProxy* proxy1 = &m_pHandles[j];
 
231
                                btAssert(proxy0 != proxy1);
 
232
                                if(!proxy1->m_clientObject)
 
233
                                {
 
234
                                        continue;
 
235
                                }
 
236
 
 
237
                                btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
 
238
                                btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
 
239
 
 
240
                                if (aabbOverlap(p0,p1))
 
241
                                {
 
242
                                        if ( !m_pairCache->findPair(proxy0,proxy1))
 
243
                                        {
 
244
                                                m_pairCache->addOverlappingPair(proxy0,proxy1);
 
245
                                        }
 
246
                                } else
 
247
                                {
 
248
                                        if (!m_pairCache->hasDeferredRemoval())
 
249
                                        {
 
250
                                                if ( m_pairCache->findPair(proxy0,proxy1))
 
251
                                                {
 
252
                                                        m_pairCache->removeOverlappingPair(proxy0,proxy1,dispatcher);
 
253
                                                }
 
254
                                        }
 
255
                                }
 
256
                        }
 
257
                }
 
258
 
 
259
                m_LastHandleIndex = new_largest_index;
 
260
 
 
261
                if (m_ownsPairCache && m_pairCache->hasDeferredRemoval())
 
262
                {
 
263
 
 
264
                        btBroadphasePairArray&  overlappingPairArray = m_pairCache->getOverlappingPairArray();
 
265
 
 
266
                        //perform a sort, to find duplicates and to sort 'invalid' pairs to the end
 
267
                        overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
 
268
 
 
269
                        overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
 
270
                        m_invalidPair = 0;
 
271
 
 
272
 
 
273
                        btBroadphasePair previousPair;
 
274
                        previousPair.m_pProxy0 = 0;
 
275
                        previousPair.m_pProxy1 = 0;
 
276
                        previousPair.m_algorithm = 0;
 
277
 
 
278
 
 
279
                        for (i=0;i<overlappingPairArray.size();i++)
 
280
                        {
 
281
 
 
282
                                btBroadphasePair& pair = overlappingPairArray[i];
 
283
 
 
284
                                bool isDuplicate = (pair == previousPair);
 
285
 
 
286
                                previousPair = pair;
 
287
 
 
288
                                bool needsRemoval = false;
 
289
 
 
290
                                if (!isDuplicate)
 
291
                                {
 
292
                                        bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
 
293
 
 
294
                                        if (hasOverlap)
 
295
                                        {
 
296
                                                needsRemoval = false;//callback->processOverlap(pair);
 
297
                                        } else
 
298
                                        {
 
299
                                                needsRemoval = true;
 
300
                                        }
 
301
                                } else
 
302
                                {
 
303
                                        //remove duplicate
 
304
                                        needsRemoval = true;
 
305
                                        //should have no algorithm
 
306
                                        btAssert(!pair.m_algorithm);
 
307
                                }
 
308
 
 
309
                                if (needsRemoval)
 
310
                                {
 
311
                                        m_pairCache->cleanOverlappingPair(pair,dispatcher);
 
312
 
 
313
                                        //              m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
 
314
                                        //              m_overlappingPairArray.pop_back();
 
315
                                        pair.m_pProxy0 = 0;
 
316
                                        pair.m_pProxy1 = 0;
 
317
                                        m_invalidPair++;
 
318
                                        gOverlappingPairs--;
 
319
                                } 
 
320
 
 
321
                        }
 
322
 
 
323
                        ///if you don't like to skip the invalid pairs in the array, execute following code:
 
324
#define CLEAN_INVALID_PAIRS 1
 
325
#ifdef CLEAN_INVALID_PAIRS
 
326
 
 
327
                        //perform a sort, to sort 'invalid' pairs to the end
 
328
                        overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
 
329
 
 
330
                        overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
 
331
                        m_invalidPair = 0;
 
332
#endif//CLEAN_INVALID_PAIRS
 
333
 
 
334
                }
 
335
        }
 
336
}
 
337
 
 
338
 
 
339
bool btSimpleBroadphase::testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
 
340
{
 
341
        btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
 
342
        btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
 
343
        return aabbOverlap(p0,p1);
 
344
}
 
345
 
 
346
void    btSimpleBroadphase::resetPool(btDispatcher* dispatcher)
 
347
{
 
348
        //not yet
 
349
}