[dali_2.3.21] Merge branch 'devel/master'
[platform/core/uifw/dali-toolkit.git] / dali-physics / third-party / bullet3 / src / BulletCollision / BroadphaseCollision / btOverlappingPairCache.cpp
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
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 "btOverlappingPairCache.h"
17
18 #include "btDispatcher.h"
19 #include "btCollisionAlgorithm.h"
20 #include "LinearMath/btAabbUtil2.h"
21
22 #include <stdio.h>
23
24 btHashedOverlappingPairCache::btHashedOverlappingPairCache() : m_overlapFilterCallback(0),
25                                                                                                                            m_ghostPairCallback(0)
26 {
27         int initialAllocatedSize = 2;
28         m_overlappingPairArray.reserve(initialAllocatedSize);
29         growTables();
30 }
31
32 btHashedOverlappingPairCache::~btHashedOverlappingPairCache()
33 {
34 }
35
36 void btHashedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair, btDispatcher* dispatcher)
37 {
38         if (pair.m_algorithm && dispatcher)
39         {
40                 {
41                         pair.m_algorithm->~btCollisionAlgorithm();
42                         dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
43                         pair.m_algorithm = 0;
44                 }
45         }
46 }
47
48 void btHashedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy, btDispatcher* dispatcher)
49 {
50         class CleanPairCallback : public btOverlapCallback
51         {
52                 btBroadphaseProxy* m_cleanProxy;
53                 btOverlappingPairCache* m_pairCache;
54                 btDispatcher* m_dispatcher;
55
56         public:
57                 CleanPairCallback(btBroadphaseProxy* cleanProxy, btOverlappingPairCache* pairCache, btDispatcher* dispatcher)
58                         : m_cleanProxy(cleanProxy),
59                           m_pairCache(pairCache),
60                           m_dispatcher(dispatcher)
61                 {
62                 }
63                 virtual bool processOverlap(btBroadphasePair& pair)
64                 {
65                         if ((pair.m_pProxy0 == m_cleanProxy) ||
66                                 (pair.m_pProxy1 == m_cleanProxy))
67                         {
68                                 m_pairCache->cleanOverlappingPair(pair, m_dispatcher);
69                         }
70                         return false;
71                 }
72         };
73
74         CleanPairCallback cleanPairs(proxy, this, dispatcher);
75
76         processAllOverlappingPairs(&cleanPairs, dispatcher);
77 }
78
79 void btHashedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher)
80 {
81         class RemovePairCallback : public btOverlapCallback
82         {
83                 btBroadphaseProxy* m_obsoleteProxy;
84
85         public:
86                 RemovePairCallback(btBroadphaseProxy* obsoleteProxy)
87                         : m_obsoleteProxy(obsoleteProxy)
88                 {
89                 }
90                 virtual bool processOverlap(btBroadphasePair& pair)
91                 {
92                         return ((pair.m_pProxy0 == m_obsoleteProxy) ||
93                                         (pair.m_pProxy1 == m_obsoleteProxy));
94                 }
95         };
96
97         RemovePairCallback removeCallback(proxy);
98
99         processAllOverlappingPairs(&removeCallback, dispatcher);
100 }
101
102 btBroadphasePair* btHashedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
103 {
104         if (proxy0->m_uniqueId > proxy1->m_uniqueId)
105                 btSwap(proxy0, proxy1);
106         int proxyId1 = proxy0->getUid();
107         int proxyId2 = proxy1->getUid();
108
109         /*if (proxyId1 > proxyId2)
110                 btSwap(proxyId1, proxyId2);*/
111
112         int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));
113
114         if (hash >= m_hashTable.size())
115         {
116                 return NULL;
117         }
118
119         int index = m_hashTable[hash];
120         while (index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
121         {
122                 index = m_next[index];
123         }
124
125         if (index == BT_NULL_PAIR)
126         {
127                 return NULL;
128         }
129
130         btAssert(index < m_overlappingPairArray.size());
131
132         return &m_overlappingPairArray[index];
133 }
134
135 //#include <stdio.h>
136
137 void btHashedOverlappingPairCache::growTables()
138 {
139         int newCapacity = m_overlappingPairArray.capacity();
140
141         if (m_hashTable.size() < newCapacity)
142         {
143                 //grow hashtable and next table
144                 int curHashtableSize = m_hashTable.size();
145
146                 m_hashTable.resize(newCapacity);
147                 m_next.resize(newCapacity);
148
149                 int i;
150
151                 for (i = 0; i < newCapacity; ++i)
152                 {
153                         m_hashTable[i] = BT_NULL_PAIR;
154                 }
155                 for (i = 0; i < newCapacity; ++i)
156                 {
157                         m_next[i] = BT_NULL_PAIR;
158                 }
159
160                 for (i = 0; i < curHashtableSize; i++)
161                 {
162                         const btBroadphasePair& pair = m_overlappingPairArray[i];
163                         int proxyId1 = pair.m_pProxy0->getUid();
164                         int proxyId2 = pair.m_pProxy1->getUid();
165                         /*if (proxyId1 > proxyId2) 
166                                 btSwap(proxyId1, proxyId2);*/
167                         int hashValue = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));  // New hash value with new mask
168                         m_next[i] = m_hashTable[hashValue];
169                         m_hashTable[hashValue] = i;
170                 }
171         }
172 }
173
174 btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
175 {
176         if (proxy0->m_uniqueId > proxy1->m_uniqueId)
177                 btSwap(proxy0, proxy1);
178         int proxyId1 = proxy0->getUid();
179         int proxyId2 = proxy1->getUid();
180
181         /*if (proxyId1 > proxyId2) 
182                 btSwap(proxyId1, proxyId2);*/
183
184         int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));  // New hash value with new mask
185
186         btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
187         if (pair != NULL)
188         {
189                 return pair;
190         }
191         /*for(int i=0;i<m_overlappingPairArray.size();++i)
192                 {
193                 if(     (m_overlappingPairArray[i].m_pProxy0==proxy0)&&
194                         (m_overlappingPairArray[i].m_pProxy1==proxy1))
195                         {
196                         printf("Adding duplicated %u<>%u\r\n",proxyId1,proxyId2);
197                         internalFindPair(proxy0, proxy1, hash);
198                         }
199                 }*/
200         int count = m_overlappingPairArray.size();
201         int oldCapacity = m_overlappingPairArray.capacity();
202         void* mem = &m_overlappingPairArray.expandNonInitializing();
203
204         //this is where we add an actual pair, so also call the 'ghost'
205         if (m_ghostPairCallback)
206                 m_ghostPairCallback->addOverlappingPair(proxy0, proxy1);
207
208         int newCapacity = m_overlappingPairArray.capacity();
209
210         if (oldCapacity < newCapacity)
211         {
212                 growTables();
213                 //hash with new capacity
214                 hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));
215         }
216
217         pair = new (mem) btBroadphasePair(*proxy0, *proxy1);
218         //      pair->m_pProxy0 = proxy0;
219         //      pair->m_pProxy1 = proxy1;
220         pair->m_algorithm = 0;
221         pair->m_internalTmpValue = 0;
222
223         m_next[count] = m_hashTable[hash];
224         m_hashTable[hash] = count;
225
226         return pair;
227 }
228
229 void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, btDispatcher* dispatcher)
230 {
231         if (proxy0->m_uniqueId > proxy1->m_uniqueId)
232                 btSwap(proxy0, proxy1);
233         int proxyId1 = proxy0->getUid();
234         int proxyId2 = proxy1->getUid();
235
236         /*if (proxyId1 > proxyId2)
237                 btSwap(proxyId1, proxyId2);*/
238
239         int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));
240
241         btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
242         if (pair == NULL)
243         {
244                 return 0;
245         }
246
247         cleanOverlappingPair(*pair, dispatcher);
248
249         void* userData = pair->m_internalInfo1;
250
251         btAssert(pair->m_pProxy0->getUid() == proxyId1);
252         btAssert(pair->m_pProxy1->getUid() == proxyId2);
253
254         int pairIndex = int(pair - &m_overlappingPairArray[0]);
255         btAssert(pairIndex < m_overlappingPairArray.size());
256
257         // Remove the pair from the hash table.
258         int index = m_hashTable[hash];
259         btAssert(index != BT_NULL_PAIR);
260
261         int previous = BT_NULL_PAIR;
262         while (index != pairIndex)
263         {
264                 previous = index;
265                 index = m_next[index];
266         }
267
268         if (previous != BT_NULL_PAIR)
269         {
270                 btAssert(m_next[previous] == pairIndex);
271                 m_next[previous] = m_next[pairIndex];
272         }
273         else
274         {
275                 m_hashTable[hash] = m_next[pairIndex];
276         }
277
278         // We now move the last pair into spot of the
279         // pair being removed. We need to fix the hash
280         // table indices to support the move.
281
282         int lastPairIndex = m_overlappingPairArray.size() - 1;
283
284         if (m_ghostPairCallback)
285                 m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1, dispatcher);
286
287         // If the removed pair is the last pair, we are done.
288         if (lastPairIndex == pairIndex)
289         {
290                 m_overlappingPairArray.pop_back();
291                 return userData;
292         }
293
294         // Remove the last pair from the hash table.
295         const btBroadphasePair* last = &m_overlappingPairArray[lastPairIndex];
296         /* missing swap here too, Nat. */
297         int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->m_pProxy0->getUid()), static_cast<unsigned int>(last->m_pProxy1->getUid())) & (m_overlappingPairArray.capacity() - 1));
298
299         index = m_hashTable[lastHash];
300         btAssert(index != BT_NULL_PAIR);
301
302         previous = BT_NULL_PAIR;
303         while (index != lastPairIndex)
304         {
305                 previous = index;
306                 index = m_next[index];
307         }
308
309         if (previous != BT_NULL_PAIR)
310         {
311                 btAssert(m_next[previous] == lastPairIndex);
312                 m_next[previous] = m_next[lastPairIndex];
313         }
314         else
315         {
316                 m_hashTable[lastHash] = m_next[lastPairIndex];
317         }
318
319         // Copy the last pair into the remove pair's spot.
320         m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex];
321
322         // Insert the last pair into the hash table
323         m_next[pairIndex] = m_hashTable[lastHash];
324         m_hashTable[lastHash] = pairIndex;
325
326         m_overlappingPairArray.pop_back();
327
328         return userData;
329 }
330 //#include <stdio.h>
331 #include "LinearMath/btQuickprof.h"
332 void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback, btDispatcher* dispatcher)
333 {
334         BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs");
335         int i;
336
337         //      printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size());
338         for (i = 0; i < m_overlappingPairArray.size();)
339         {
340                 btBroadphasePair* pair = &m_overlappingPairArray[i];
341                 if (callback->processOverlap(*pair))
342                 {
343                         removeOverlappingPair(pair->m_pProxy0, pair->m_pProxy1, dispatcher);
344                 }
345                 else
346                 {
347                         i++;
348                 }
349         }
350 }
351
352 struct MyPairIndex
353 {
354         int m_orgIndex;
355         int m_uidA0;
356         int m_uidA1;
357 };
358
359 class MyPairIndeSortPredicate
360 {
361 public:
362         bool operator()(const MyPairIndex& a, const MyPairIndex& b) const
363         {
364                 const int uidA0 = a.m_uidA0;
365                 const int uidB0 = b.m_uidA0;
366                 const int uidA1 = a.m_uidA1;
367                 const int uidB1 = b.m_uidA1;
368                 return uidA0 > uidB0 || (uidA0 == uidB0 && uidA1 > uidB1);
369         }
370 };
371
372 void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback, btDispatcher* dispatcher, const struct btDispatcherInfo& dispatchInfo)
373 {
374         if (dispatchInfo.m_deterministicOverlappingPairs)
375         {
376                 btBroadphasePairArray& pa = getOverlappingPairArray();
377                 btAlignedObjectArray<MyPairIndex> indices;
378                 {
379                         BT_PROFILE("sortOverlappingPairs");
380                         indices.resize(pa.size());
381                         for (int i = 0; i < indices.size(); i++)
382                         {
383                                 const btBroadphasePair& p = pa[i];
384                                 const int uidA0 = p.m_pProxy0 ? p.m_pProxy0->m_uniqueId : -1;
385                                 const int uidA1 = p.m_pProxy1 ? p.m_pProxy1->m_uniqueId : -1;
386
387                                 indices[i].m_uidA0 = uidA0;
388                                 indices[i].m_uidA1 = uidA1;
389                                 indices[i].m_orgIndex = i;
390                         }
391                         indices.quickSort(MyPairIndeSortPredicate());
392                 }
393                 {
394                         BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs");
395                         int i;
396                         for (i = 0; i < indices.size();)
397                         {
398                                 btBroadphasePair* pair = &pa[indices[i].m_orgIndex];
399                                 if (callback->processOverlap(*pair))
400                                 {
401                                         removeOverlappingPair(pair->m_pProxy0, pair->m_pProxy1, dispatcher);
402                                 }
403                                 else
404                                 {
405                                         i++;
406                                 }
407                         }
408                 }
409         }
410         else
411         {
412                 processAllOverlappingPairs(callback, dispatcher);
413         }
414 }
415
416 void btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
417 {
418         ///need to keep hashmap in sync with pair address, so rebuild all
419         btBroadphasePairArray tmpPairs;
420         int i;
421         for (i = 0; i < m_overlappingPairArray.size(); i++)
422         {
423                 tmpPairs.push_back(m_overlappingPairArray[i]);
424         }
425
426         for (i = 0; i < tmpPairs.size(); i++)
427         {
428                 removeOverlappingPair(tmpPairs[i].m_pProxy0, tmpPairs[i].m_pProxy1, dispatcher);
429         }
430
431         for (i = 0; i < m_next.size(); i++)
432         {
433                 m_next[i] = BT_NULL_PAIR;
434         }
435
436         tmpPairs.quickSort(btBroadphasePairSortPredicate());
437
438         for (i = 0; i < tmpPairs.size(); i++)
439         {
440                 addOverlappingPair(tmpPairs[i].m_pProxy0, tmpPairs[i].m_pProxy1);
441         }
442 }
443
444 void* btSortedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, btDispatcher* dispatcher)
445 {
446         if (!hasDeferredRemoval())
447         {
448                 btBroadphasePair findPair(*proxy0, *proxy1);
449
450                 int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
451                 if (findIndex < m_overlappingPairArray.size())
452                 {
453                         btBroadphasePair& pair = m_overlappingPairArray[findIndex];
454                         void* userData = pair.m_internalInfo1;
455                         cleanOverlappingPair(pair, dispatcher);
456                         if (m_ghostPairCallback)
457                                 m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1, dispatcher);
458
459                         m_overlappingPairArray.swap(findIndex, m_overlappingPairArray.capacity() - 1);
460                         m_overlappingPairArray.pop_back();
461                         return userData;
462                 }
463         }
464
465         return 0;
466 }
467
468 btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
469 {
470         //don't add overlap with own
471         btAssert(proxy0 != proxy1);
472
473         if (!needsBroadphaseCollision(proxy0, proxy1))
474                 return 0;
475
476         void* mem = &m_overlappingPairArray.expandNonInitializing();
477         btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0, *proxy1);
478
479         if (m_ghostPairCallback)
480                 m_ghostPairCallback->addOverlappingPair(proxy0, proxy1);
481         return pair;
482 }
483
484 ///this findPair becomes really slow. Either sort the list to speedup the query, or
485 ///use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed.
486 ///we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address)
487 ///Also we can use a 2D bitmap, which can be useful for a future GPU implementation
488 btBroadphasePair* btSortedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
489 {
490         if (!needsBroadphaseCollision(proxy0, proxy1))
491                 return 0;
492
493         btBroadphasePair tmpPair(*proxy0, *proxy1);
494         int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair);
495
496         if (findIndex < m_overlappingPairArray.size())
497         {
498                 //btAssert(it != m_overlappingPairSet.end());
499                 btBroadphasePair* pair = &m_overlappingPairArray[findIndex];
500                 return pair;
501         }
502         return 0;
503 }
504
505 //#include <stdio.h>
506
507 void btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback, btDispatcher* dispatcher)
508 {
509         int i;
510
511         for (i = 0; i < m_overlappingPairArray.size();)
512         {
513                 btBroadphasePair* pair = &m_overlappingPairArray[i];
514                 if (callback->processOverlap(*pair))
515                 {
516                         cleanOverlappingPair(*pair, dispatcher);
517                         pair->m_pProxy0 = 0;
518                         pair->m_pProxy1 = 0;
519                         m_overlappingPairArray.swap(i, m_overlappingPairArray.size() - 1);
520                         m_overlappingPairArray.pop_back();
521                 }
522                 else
523                 {
524                         i++;
525                 }
526         }
527 }
528
529 btSortedOverlappingPairCache::btSortedOverlappingPairCache() : m_blockedForChanges(false),
530                                                                                                                            m_hasDeferredRemoval(true),
531                                                                                                                            m_overlapFilterCallback(0),
532                                                                                                                            m_ghostPairCallback(0)
533 {
534         int initialAllocatedSize = 2;
535         m_overlappingPairArray.reserve(initialAllocatedSize);
536 }
537
538 btSortedOverlappingPairCache::~btSortedOverlappingPairCache()
539 {
540 }
541
542 void btSortedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair, btDispatcher* dispatcher)
543 {
544         if (pair.m_algorithm)
545         {
546                 {
547                         pair.m_algorithm->~btCollisionAlgorithm();
548                         dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
549                         pair.m_algorithm = 0;
550                 }
551         }
552 }
553
554 void btSortedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy, btDispatcher* dispatcher)
555 {
556         class CleanPairCallback : public btOverlapCallback
557         {
558                 btBroadphaseProxy* m_cleanProxy;
559                 btOverlappingPairCache* m_pairCache;
560                 btDispatcher* m_dispatcher;
561
562         public:
563                 CleanPairCallback(btBroadphaseProxy* cleanProxy, btOverlappingPairCache* pairCache, btDispatcher* dispatcher)
564                         : m_cleanProxy(cleanProxy),
565                           m_pairCache(pairCache),
566                           m_dispatcher(dispatcher)
567                 {
568                 }
569                 virtual bool processOverlap(btBroadphasePair& pair)
570                 {
571                         if ((pair.m_pProxy0 == m_cleanProxy) ||
572                                 (pair.m_pProxy1 == m_cleanProxy))
573                         {
574                                 m_pairCache->cleanOverlappingPair(pair, m_dispatcher);
575                         }
576                         return false;
577                 }
578         };
579
580         CleanPairCallback cleanPairs(proxy, this, dispatcher);
581
582         processAllOverlappingPairs(&cleanPairs, dispatcher);
583 }
584
585 void btSortedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher)
586 {
587         class RemovePairCallback : public btOverlapCallback
588         {
589                 btBroadphaseProxy* m_obsoleteProxy;
590
591         public:
592                 RemovePairCallback(btBroadphaseProxy* obsoleteProxy)
593                         : m_obsoleteProxy(obsoleteProxy)
594                 {
595                 }
596                 virtual bool processOverlap(btBroadphasePair& pair)
597                 {
598                         return ((pair.m_pProxy0 == m_obsoleteProxy) ||
599                                         (pair.m_pProxy1 == m_obsoleteProxy));
600                 }
601         };
602
603         RemovePairCallback removeCallback(proxy);
604
605         processAllOverlappingPairs(&removeCallback, dispatcher);
606 }
607
608 void btSortedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
609 {
610         //should already be sorted
611 }