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