54889a6375d1540aea34d56425ebd2b168e3ccd2
[blender.git] / extern / bullet2 / src / BulletCollision / CollisionDispatch / btCompoundCollisionAlgorithm.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 "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h"
17 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
18 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
19 #include "BulletCollision/BroadphaseCollision/btDbvt.h"
20 #include "LinearMath/btIDebugDraw.h"
21 #include "LinearMath/btAabbUtil2.h"
22 #include "btManifoldResult.h"
23
24 btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped)
25 :btActivatingCollisionAlgorithm(ci,body0,body1),
26 m_isSwapped(isSwapped),
27 m_sharedManifold(ci.m_manifold)
28 {
29         m_ownsManifold = false;
30
31         btCollisionObject* colObj = m_isSwapped? body1 : body0;
32         btAssert (colObj->getCollisionShape()->isCompound());
33         
34         btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
35         m_compoundShapeRevision = compoundShape->getUpdateRevision();
36         
37         preallocateChildAlgorithms(body0,body1);
38 }
39
40 void    btCompoundCollisionAlgorithm::preallocateChildAlgorithms(btCollisionObject* body0,btCollisionObject* body1)
41 {
42         btCollisionObject* colObj = m_isSwapped? body1 : body0;
43         btCollisionObject* otherObj = m_isSwapped? body0 : body1;
44         btAssert (colObj->getCollisionShape()->isCompound());
45         
46         btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
47
48         int numChildren = compoundShape->getNumChildShapes();
49         int i;
50         
51         m_childCollisionAlgorithms.resize(numChildren);
52         for (i=0;i<numChildren;i++)
53         {
54                 if (compoundShape->getDynamicAabbTree())
55                 {
56                         m_childCollisionAlgorithms[i] = 0;
57                 } else
58                 {
59                         btCollisionShape* tmpShape = colObj->getCollisionShape();
60                         btCollisionShape* childShape = compoundShape->getChildShape(i);
61                         colObj->internalSetTemporaryCollisionShape( childShape );
62                         m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(colObj,otherObj,m_sharedManifold);
63                         colObj->internalSetTemporaryCollisionShape( tmpShape );
64                 }
65         }
66 }
67
68 void    btCompoundCollisionAlgorithm::removeChildAlgorithms()
69 {
70         int numChildren = m_childCollisionAlgorithms.size();
71         int i;
72         for (i=0;i<numChildren;i++)
73         {
74                 if (m_childCollisionAlgorithms[i])
75                 {
76                         m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
77                         m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
78                 }
79         }
80 }
81
82 btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm()
83 {
84         removeChildAlgorithms();
85 }
86
87
88
89
90 struct  btCompoundLeafCallback : btDbvt::ICollide
91 {
92
93 public:
94
95         btCollisionObject* m_compoundColObj;
96         btCollisionObject* m_otherObj;
97         btDispatcher* m_dispatcher;
98         const btDispatcherInfo& m_dispatchInfo;
99         btManifoldResult*       m_resultOut;
100         btCollisionAlgorithm**  m_childCollisionAlgorithms;
101         btPersistentManifold*   m_sharedManifold;
102
103
104
105
106         btCompoundLeafCallback (btCollisionObject* compoundObj,btCollisionObject* otherObj,btDispatcher* dispatcher,const btDispatcherInfo& dispatchInfo,btManifoldResult*      resultOut,btCollisionAlgorithm**        childCollisionAlgorithms,btPersistentManifold*  sharedManifold)
107                 :m_compoundColObj(compoundObj),m_otherObj(otherObj),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut),
108                 m_childCollisionAlgorithms(childCollisionAlgorithms),
109                 m_sharedManifold(sharedManifold)
110         {
111
112         }
113
114
115         void    ProcessChildShape(btCollisionShape* childShape,int index)
116         {
117                 btAssert(index>=0);
118                 btCompoundShape* compoundShape = static_cast<btCompoundShape*>(m_compoundColObj->getCollisionShape());
119                 btAssert(index<compoundShape->getNumChildShapes());
120
121
122                 //backup
123                 btTransform     orgTrans = m_compoundColObj->getWorldTransform();
124                 btTransform     orgInterpolationTrans = m_compoundColObj->getInterpolationWorldTransform();
125                 const btTransform& childTrans = compoundShape->getChildTransform(index);
126                 btTransform     newChildWorldTrans = orgTrans*childTrans ;
127
128                 //perform an AABB check first
129                 btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
130                 childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
131                 m_otherObj->getCollisionShape()->getAabb(m_otherObj->getWorldTransform(),aabbMin1,aabbMax1);
132
133                 if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
134                 {
135
136                         m_compoundColObj->setWorldTransform( newChildWorldTrans);
137                         m_compoundColObj->setInterpolationWorldTransform(newChildWorldTrans);
138
139                         //the contactpoint is still projected back using the original inverted worldtrans
140                         btCollisionShape* tmpShape = m_compoundColObj->getCollisionShape();
141                         m_compoundColObj->internalSetTemporaryCollisionShape( childShape );
142
143                         if (!m_childCollisionAlgorithms[index])
144                                 m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(m_compoundColObj,m_otherObj,m_sharedManifold);
145
146                         ///detect swapping case
147                         if (m_resultOut->getBody0Internal() == m_compoundColObj)
148                         {
149                                 m_resultOut->setShapeIdentifiersA(-1,index);
150                         } else
151                         {
152                                 m_resultOut->setShapeIdentifiersB(-1,index);
153                         }
154
155                         m_childCollisionAlgorithms[index]->processCollision(m_compoundColObj,m_otherObj,m_dispatchInfo,m_resultOut);
156                         if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
157                         {
158                                 btVector3 worldAabbMin,worldAabbMax;
159                                 m_dispatchInfo.m_debugDraw->drawAabb(aabbMin0,aabbMax0,btVector3(1,1,1));
160                                 m_dispatchInfo.m_debugDraw->drawAabb(aabbMin1,aabbMax1,btVector3(1,1,1));
161                         }
162                         
163                         //revert back transform
164                         m_compoundColObj->internalSetTemporaryCollisionShape( tmpShape);
165                         m_compoundColObj->setWorldTransform(  orgTrans );
166                         m_compoundColObj->setInterpolationWorldTransform(orgInterpolationTrans);
167                 }
168         }
169         void            Process(const btDbvtNode* leaf)
170         {
171                 int index = leaf->dataAsInt;
172
173                 btCompoundShape* compoundShape = static_cast<btCompoundShape*>(m_compoundColObj->getCollisionShape());
174                 btCollisionShape* childShape = compoundShape->getChildShape(index);
175                 if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
176                 {
177                         btVector3 worldAabbMin,worldAabbMax;
178                         btTransform     orgTrans = m_compoundColObj->getWorldTransform();
179                         btTransformAabb(leaf->volume.Mins(),leaf->volume.Maxs(),0.,orgTrans,worldAabbMin,worldAabbMax);
180                         m_dispatchInfo.m_debugDraw->drawAabb(worldAabbMin,worldAabbMax,btVector3(1,0,0));
181                 }
182                 ProcessChildShape(childShape,index);
183
184         }
185 };
186
187
188
189
190
191
192 void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
193 {
194         btCollisionObject* colObj = m_isSwapped? body1 : body0;
195         btCollisionObject* otherObj = m_isSwapped? body0 : body1;
196
197         
198
199         btAssert (colObj->getCollisionShape()->isCompound());
200         btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
201
202         ///btCompoundShape might have changed:
203         ////make sure the internal child collision algorithm caches are still valid
204         if (compoundShape->getUpdateRevision() != m_compoundShapeRevision)
205         {
206                 ///clear and update all
207                 removeChildAlgorithms();
208                 
209                 preallocateChildAlgorithms(body0,body1);
210         }
211
212
213         btDbvt* tree = compoundShape->getDynamicAabbTree();
214         //use a dynamic aabb tree to cull potential child-overlaps
215         btCompoundLeafCallback  callback(colObj,otherObj,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold);
216
217         ///we need to refresh all contact manifolds
218         ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
219         ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
220         {
221                 int i;
222                 btManifoldArray manifoldArray;
223                 for (i=0;i<m_childCollisionAlgorithms.size();i++)
224                 {
225                         if (m_childCollisionAlgorithms[i])
226                         {
227                                 m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray);
228                                 for (int m=0;m<manifoldArray.size();m++)
229                                 {
230                                         if (manifoldArray[m]->getNumContacts())
231                                         {
232                                                 resultOut->setPersistentManifold(manifoldArray[m]);
233                                                 resultOut->refreshContactPoints();
234                                                 resultOut->setPersistentManifold(0);//??necessary?
235                                         }
236                                 }
237                                 manifoldArray.resize(0);
238                         }
239                 }
240         }
241
242         if (tree)
243         {
244
245                 btVector3 localAabbMin,localAabbMax;
246                 btTransform otherInCompoundSpace;
247                 otherInCompoundSpace = colObj->getWorldTransform().inverse() * otherObj->getWorldTransform();
248                 otherObj->getCollisionShape()->getAabb(otherInCompoundSpace,localAabbMin,localAabbMax);
249
250                 const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
251                 //process all children, that overlap with  the given AABB bounds
252                 tree->collideTV(tree->m_root,bounds,callback);
253
254         } else
255         {
256                 //iterate over all children, perform an AABB check inside ProcessChildShape
257                 int numChildren = m_childCollisionAlgorithms.size();
258                 int i;
259                 for (i=0;i<numChildren;i++)
260                 {
261                         callback.ProcessChildShape(compoundShape->getChildShape(i),i);
262                 }
263         }
264
265         {
266                                 //iterate over all children, perform an AABB check inside ProcessChildShape
267                 int numChildren = m_childCollisionAlgorithms.size();
268                 int i;
269                 btManifoldArray manifoldArray;
270         btCollisionShape* childShape = 0;
271         btTransform     orgTrans;
272         btTransform     orgInterpolationTrans;
273         btTransform     newChildWorldTrans;
274         btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;        
275         
276                 for (i=0;i<numChildren;i++)
277                 {
278                         if (m_childCollisionAlgorithms[i])
279                         {
280                                 childShape = compoundShape->getChildShape(i);
281                         //if not longer overlapping, remove the algorithm
282                 orgTrans = colObj->getWorldTransform();
283                 orgInterpolationTrans = colObj->getInterpolationWorldTransform();
284                                 const btTransform& childTrans = compoundShape->getChildTransform(i);
285                 newChildWorldTrans = orgTrans*childTrans ;
286
287                                 //perform an AABB check first
288                                 childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
289                                 otherObj->getCollisionShape()->getAabb(otherObj->getWorldTransform(),aabbMin1,aabbMax1);
290
291                                 if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
292                                 {
293                                         m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
294                                         m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
295                                         m_childCollisionAlgorithms[i] = 0;
296                                 }
297                         }
298                 }
299         }
300 }
301
302 btScalar        btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
303 {
304
305         btCollisionObject* colObj = m_isSwapped? body1 : body0;
306         btCollisionObject* otherObj = m_isSwapped? body0 : body1;
307
308         btAssert (colObj->getCollisionShape()->isCompound());
309         
310         btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
311
312         //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
313         //If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals
314         //given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means:
315         //determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1
316         //then use each overlapping node AABB against Tree0
317         //and vise versa.
318
319         btScalar hitFraction = btScalar(1.);
320
321         int numChildren = m_childCollisionAlgorithms.size();
322         int i;
323     btTransform orgTrans;
324     btScalar frac;
325         for (i=0;i<numChildren;i++)
326         {
327                 //temporarily exchange parent btCollisionShape with childShape, and recurse
328                 btCollisionShape* childShape = compoundShape->getChildShape(i);
329
330                 //backup
331         orgTrans = colObj->getWorldTransform();
332         
333                 const btTransform& childTrans = compoundShape->getChildTransform(i);
334                 //btTransform   newChildWorldTrans = orgTrans*childTrans ;
335                 colObj->setWorldTransform( orgTrans*childTrans );
336
337                 btCollisionShape* tmpShape = colObj->getCollisionShape();
338                 colObj->internalSetTemporaryCollisionShape( childShape );
339         frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut);
340                 if (frac<hitFraction)
341                 {
342                         hitFraction = frac;
343                 }
344                 //revert back
345                 colObj->internalSetTemporaryCollisionShape( tmpShape);
346                 colObj->setWorldTransform( orgTrans);
347         }
348         return hitFraction;
349
350 }
351
352
353