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