== SoC Bullet - Bullet Upgrade to 2.76 ==
[blender.git] / extern / bullet2 / 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 }