Upgrade Bullet to version 2.83.
[blender.git] / extern / bullet2 / src / BulletCollision / CollisionDispatch / btCompoundCompoundCollisionAlgorithm.cpp
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2013 Erwin Coumans  http://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
17 #include "btCompoundCompoundCollisionAlgorithm.h"
18 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
19 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
20 #include "BulletCollision/BroadphaseCollision/btDbvt.h"
21 #include "LinearMath/btIDebugDraw.h"
22 #include "LinearMath/btAabbUtil2.h"
23 #include "BulletCollision/CollisionDispatch/btManifoldResult.h"
24 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
25
26
27 btShapePairCallback gCompoundCompoundChildShapePairCallback = 0;
28
29 btCompoundCompoundCollisionAlgorithm::btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
30 :btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,isSwapped)
31 {
32
33         void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache),16);
34         m_childCollisionAlgorithmCache= new(ptr) btHashedSimplePairCache();
35
36         const btCollisionObjectWrapper* col0ObjWrap = body0Wrap;
37         btAssert (col0ObjWrap->getCollisionShape()->isCompound());
38
39         const btCollisionObjectWrapper* col1ObjWrap = body1Wrap;
40         btAssert (col1ObjWrap->getCollisionShape()->isCompound());
41         
42         const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape());
43         m_compoundShapeRevision0 = compoundShape0->getUpdateRevision();
44
45         const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape());
46         m_compoundShapeRevision1 = compoundShape1->getUpdateRevision();
47         
48         
49 }
50
51
52 btCompoundCompoundCollisionAlgorithm::~btCompoundCompoundCollisionAlgorithm()
53 {
54         removeChildAlgorithms();
55         m_childCollisionAlgorithmCache->~btHashedSimplePairCache();
56         btAlignedFree(m_childCollisionAlgorithmCache);
57 }
58
59 void    btCompoundCompoundCollisionAlgorithm::getAllContactManifolds(btManifoldArray&   manifoldArray)
60 {
61         int i;
62         btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
63         for (i=0;i<pairs.size();i++)
64         {
65                 if (pairs[i].m_userPointer)
66                 {
67                         
68                         ((btCollisionAlgorithm*)pairs[i].m_userPointer)->getAllContactManifolds(manifoldArray);
69                 }
70         }
71 }
72
73
74 void    btCompoundCompoundCollisionAlgorithm::removeChildAlgorithms()
75 {
76         btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
77
78         int numChildren = pairs.size();
79         int i;
80         for (i=0;i<numChildren;i++)
81         {
82                 if (pairs[i].m_userPointer)
83                 {
84                         btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer;
85                         algo->~btCollisionAlgorithm();
86                         m_dispatcher->freeCollisionAlgorithm(algo);
87                 }
88         }
89         m_childCollisionAlgorithmCache->removeAllPairs();
90 }
91
92 struct  btCompoundCompoundLeafCallback : btDbvt::ICollide
93 {
94         int m_numOverlapPairs;
95
96
97         const btCollisionObjectWrapper* m_compound0ColObjWrap;
98         const btCollisionObjectWrapper* m_compound1ColObjWrap;
99         btDispatcher* m_dispatcher;
100         const btDispatcherInfo& m_dispatchInfo;
101         btManifoldResult*       m_resultOut;
102         
103         
104         class btHashedSimplePairCache*  m_childCollisionAlgorithmCache;
105         
106         btPersistentManifold*   m_sharedManifold;
107         
108         btCompoundCompoundLeafCallback (const btCollisionObjectWrapper* compound1ObjWrap,
109                                                                         const btCollisionObjectWrapper* compound0ObjWrap,
110                                                                         btDispatcher* dispatcher,
111                                                                         const btDispatcherInfo& dispatchInfo,
112                                                                         btManifoldResult*       resultOut,
113                                                                         btHashedSimplePairCache* childAlgorithmsCache,
114                                                                         btPersistentManifold*   sharedManifold)
115                 :m_numOverlapPairs(0),m_compound0ColObjWrap(compound1ObjWrap),m_compound1ColObjWrap(compound0ObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut),
116                 m_childCollisionAlgorithmCache(childAlgorithmsCache),
117                 m_sharedManifold(sharedManifold)
118         {
119
120         }
121
122
123
124         
125         void            Process(const btDbvtNode* leaf0,const btDbvtNode* leaf1)
126         {
127                 m_numOverlapPairs++;
128
129
130                 int childIndex0 = leaf0->dataAsInt;
131                 int childIndex1 = leaf1->dataAsInt;
132                 
133
134                 btAssert(childIndex0>=0);
135                 btAssert(childIndex1>=0);
136
137
138                 const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(m_compound0ColObjWrap->getCollisionShape());
139                 btAssert(childIndex0<compoundShape0->getNumChildShapes());
140
141                 const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(m_compound1ColObjWrap->getCollisionShape());
142                 btAssert(childIndex1<compoundShape1->getNumChildShapes());
143
144                 const btCollisionShape* childShape0 = compoundShape0->getChildShape(childIndex0);
145                 const btCollisionShape* childShape1 = compoundShape1->getChildShape(childIndex1);
146
147                 //backup
148                 btTransform     orgTrans0 = m_compound0ColObjWrap->getWorldTransform();
149                 const btTransform& childTrans0 = compoundShape0->getChildTransform(childIndex0);
150                 btTransform     newChildWorldTrans0 = orgTrans0*childTrans0 ;
151                 
152                 btTransform     orgTrans1 = m_compound1ColObjWrap->getWorldTransform();
153                 const btTransform& childTrans1 = compoundShape1->getChildTransform(childIndex1);
154                 btTransform     newChildWorldTrans1 = orgTrans1*childTrans1 ;
155                 
156
157                 //perform an AABB check first
158                 btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
159                 childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
160                 childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
161                 
162                 if (gCompoundCompoundChildShapePairCallback)
163                 {
164                         if (!gCompoundCompoundChildShapePairCallback(childShape0,childShape1))
165                                 return;
166                 }
167
168                 if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
169                 {
170                         btCollisionObjectWrapper compoundWrap0(this->m_compound0ColObjWrap,childShape0, m_compound0ColObjWrap->getCollisionObject(),newChildWorldTrans0,-1,childIndex0);
171                         btCollisionObjectWrapper compoundWrap1(this->m_compound1ColObjWrap,childShape1,m_compound1ColObjWrap->getCollisionObject(),newChildWorldTrans1,-1,childIndex1);
172                         
173
174                         btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0,childIndex1);
175
176                         btCollisionAlgorithm* colAlgo = 0;
177
178                         if (pair)
179                         {
180                                 colAlgo = (btCollisionAlgorithm*)pair->m_userPointer;
181                                 
182                         } else
183                         {
184                                 colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0,&compoundWrap1,m_sharedManifold);
185                                 pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0,childIndex1);
186                                 btAssert(pair);
187                                 pair->m_userPointer = colAlgo;
188                         }
189
190                         btAssert(colAlgo);
191                                                 
192                         const btCollisionObjectWrapper* tmpWrap0 = 0;
193                         const btCollisionObjectWrapper* tmpWrap1 = 0;
194
195                         tmpWrap0 = m_resultOut->getBody0Wrap();
196                         tmpWrap1 = m_resultOut->getBody1Wrap();
197
198                         m_resultOut->setBody0Wrap(&compoundWrap0);
199                         m_resultOut->setBody1Wrap(&compoundWrap1);
200
201                         m_resultOut->setShapeIdentifiersA(-1,childIndex0);
202                         m_resultOut->setShapeIdentifiersB(-1,childIndex1);
203
204
205                         colAlgo->processCollision(&compoundWrap0,&compoundWrap1,m_dispatchInfo,m_resultOut);
206                         
207                         m_resultOut->setBody0Wrap(tmpWrap0);
208                         m_resultOut->setBody1Wrap(tmpWrap1);
209                         
210
211
212                 }
213         }
214 };
215
216
217 static DBVT_INLINE bool         MyIntersect(    const btDbvtAabbMm& a,
218                                                                   const btDbvtAabbMm& b, const btTransform& xform)
219 {
220         btVector3 newmin,newmax;
221         btTransformAabb(b.Mins(),b.Maxs(),0.f,xform,newmin,newmax);
222         btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin,newmax);
223         return Intersect(a,newb);
224 }
225
226
227 static inline void              MycollideTT(    const btDbvtNode* root0,
228                                                                   const btDbvtNode* root1,
229                                                                   const btTransform& xform,
230                                                                   btCompoundCompoundLeafCallback* callback)
231 {
232
233                 if(root0&&root1)
234                 {
235                         int                                                             depth=1;
236                         int                                                             treshold=btDbvt::DOUBLE_STACKSIZE-4;
237                         btAlignedObjectArray<btDbvt::sStkNN>    stkStack;
238                         stkStack.resize(btDbvt::DOUBLE_STACKSIZE);
239                         stkStack[0]=btDbvt::sStkNN(root0,root1);
240                         do      {
241                                 btDbvt::sStkNN  p=stkStack[--depth];
242                                 if(MyIntersect(p.a->volume,p.b->volume,xform))
243                                 {
244                                         if(depth>treshold)
245                                         {
246                                                 stkStack.resize(stkStack.size()*2);
247                                                 treshold=stkStack.size()-4;
248                                         }
249                                         if(p.a->isinternal())
250                                         {
251                                                 if(p.b->isinternal())
252                                                 {                                       
253                                                         stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[0]);
254                                                         stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[0]);
255                                                         stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[1]);
256                                                         stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[1]);
257                                                 }
258                                                 else
259                                                 {
260                                                         stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b);
261                                                         stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b);
262                                                 }
263                                         }
264                                         else
265                                         {
266                                                 if(p.b->isinternal())
267                                                 {
268                                                         stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[0]);
269                                                         stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[1]);
270                                                 }
271                                                 else
272                                                 {
273                                                         callback->Process(p.a,p.b);
274                                                 }
275                                         }
276                                 }
277                         } while(depth);
278                 }
279 }
280
281 void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
282 {
283
284         const btCollisionObjectWrapper* col0ObjWrap = body0Wrap;
285         const btCollisionObjectWrapper* col1ObjWrap= body1Wrap;
286
287         btAssert (col0ObjWrap->getCollisionShape()->isCompound());
288         btAssert (col1ObjWrap->getCollisionShape()->isCompound());
289         const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape());
290         const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape());
291
292         const btDbvt* tree0 = compoundShape0->getDynamicAabbTree();
293         const btDbvt* tree1 = compoundShape1->getDynamicAabbTree();
294         if (!tree0 || !tree1)
295         {
296                 return btCompoundCollisionAlgorithm::processCollision(body0Wrap,body1Wrap,dispatchInfo,resultOut);
297         }
298         ///btCompoundShape might have changed:
299         ////make sure the internal child collision algorithm caches are still valid
300         if ((compoundShape0->getUpdateRevision() != m_compoundShapeRevision0) || (compoundShape1->getUpdateRevision() != m_compoundShapeRevision1))
301         {
302                 ///clear all
303                 removeChildAlgorithms();
304                 m_compoundShapeRevision0 = compoundShape0->getUpdateRevision();
305                 m_compoundShapeRevision1 = compoundShape1->getUpdateRevision();
306
307         }
308
309
310         ///we need to refresh all contact manifolds
311         ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
312         ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
313         {
314                 int i;
315                 btManifoldArray manifoldArray;
316                 btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
317                 for (i=0;i<pairs.size();i++)
318                 {
319                         if (pairs[i].m_userPointer)
320                         {
321                                 btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer;
322                                 algo->getAllContactManifolds(manifoldArray);
323                                 for (int m=0;m<manifoldArray.size();m++)
324                                 {
325                                         if (manifoldArray[m]->getNumContacts())
326                                         {
327                                                 resultOut->setPersistentManifold(manifoldArray[m]);
328                                                 resultOut->refreshContactPoints();
329                                                 resultOut->setPersistentManifold(0);
330                                         }
331                                 }
332                                 manifoldArray.resize(0);
333                         }
334                 }
335         }
336
337
338         
339
340         btCompoundCompoundLeafCallback callback(col0ObjWrap,col1ObjWrap,this->m_dispatcher,dispatchInfo,resultOut,this->m_childCollisionAlgorithmCache,m_sharedManifold);
341
342
343         const btTransform       xform=col0ObjWrap->getWorldTransform().inverse()*col1ObjWrap->getWorldTransform();
344         MycollideTT(tree0->m_root,tree1->m_root,xform,&callback);
345
346         //printf("#compound-compound child/leaf overlap =%d                      \r",callback.m_numOverlapPairs);
347
348         //remove non-overlapping child pairs
349
350         {
351                 btAssert(m_removePairs.size()==0);
352
353                 //iterate over all children, perform an AABB check inside ProcessChildShape
354                 btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
355                 
356                 int i;
357                 btManifoldArray manifoldArray;
358         
359                 
360
361         
362         
363         btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;        
364         
365                 for (i=0;i<pairs.size();i++)
366                 {
367                         if (pairs[i].m_userPointer)
368                         {
369                                 btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer;
370
371                                 {
372                                         btTransform     orgTrans0;
373                                         const btCollisionShape* childShape0 = 0;
374                                         
375                                         btTransform     newChildWorldTrans0;
376                                         btTransform     orgInterpolationTrans0;
377                                         childShape0 = compoundShape0->getChildShape(pairs[i].m_indexA);
378                                         orgTrans0 = col0ObjWrap->getWorldTransform();
379                                         orgInterpolationTrans0 = col0ObjWrap->getWorldTransform();
380                                         const btTransform& childTrans0 = compoundShape0->getChildTransform(pairs[i].m_indexA);
381                                         newChildWorldTrans0 = orgTrans0*childTrans0 ;
382                                         childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
383                                 }
384
385                                 {
386                                         btTransform     orgInterpolationTrans1;
387                                         const btCollisionShape* childShape1 = 0;
388                                         btTransform     orgTrans1;
389                                         btTransform     newChildWorldTrans1;
390
391                                         childShape1 = compoundShape1->getChildShape(pairs[i].m_indexB);
392                                         orgTrans1 = col1ObjWrap->getWorldTransform();
393                                         orgInterpolationTrans1 = col1ObjWrap->getWorldTransform();
394                                         const btTransform& childTrans1 = compoundShape1->getChildTransform(pairs[i].m_indexB);
395                                         newChildWorldTrans1 = orgTrans1*childTrans1 ;
396                                         childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
397                                 }
398                                 
399                                 
400
401                                 if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
402                                 {
403                                         algo->~btCollisionAlgorithm();
404                                         m_dispatcher->freeCollisionAlgorithm(algo);
405                                         m_removePairs.push_back(btSimplePair(pairs[i].m_indexA,pairs[i].m_indexB));
406                                 }
407                         }
408                 }
409                 for (int i=0;i<m_removePairs.size();i++)
410                 {
411                         m_childCollisionAlgorithmCache->removeOverlappingPair(m_removePairs[i].m_indexA,m_removePairs[i].m_indexB);
412                 }
413                 m_removePairs.clear();
414         }
415
416 }
417
418 btScalar        btCompoundCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
419 {
420         btAssert(0);
421         return 0.f;
422
423 }
424
425
426