Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/external/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @ 6829

Last change on this file since 6829 was 5781, checked in by rgrieder, 15 years ago

Reverted trunk again. We might want to find a way to delete these revisions again (x3n's changes are still available as diff in the commit mails).

  • Property svn:eol-style set to native
File size: 12.1 KB
Line 
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. 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
24btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped)
25:btActivatingCollisionAlgorithm(ci,body0,body1),
26m_isSwapped(isSwapped),
27m_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
40void    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
68void    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
82btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm()
83{
84        removeChildAlgorithms();
85}
86
87
88
89
90struct  btCompoundLeafCallback : btDbvt::ICollide
91{
92
93public:
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               
118                btCompoundShape* compoundShape = static_cast<btCompoundShape*>(m_compoundColObj->getCollisionShape());
119
120
121                //backup
122                btTransform     orgTrans = m_compoundColObj->getWorldTransform();
123                btTransform     orgInterpolationTrans = m_compoundColObj->getInterpolationWorldTransform();
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_otherObj->getCollisionShape()->getAabb(m_otherObj->getWorldTransform(),aabbMin1,aabbMax1);
131
132                if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
133                {
134
135                        m_compoundColObj->setWorldTransform( newChildWorldTrans);
136                        m_compoundColObj->setInterpolationWorldTransform(newChildWorldTrans);
137
138                        //the contactpoint is still projected back using the original inverted worldtrans
139                        btCollisionShape* tmpShape = m_compoundColObj->getCollisionShape();
140                        m_compoundColObj->internalSetTemporaryCollisionShape( childShape );
141
142                        if (!m_childCollisionAlgorithms[index])
143                                m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(m_compoundColObj,m_otherObj,m_sharedManifold);
144
145                        m_childCollisionAlgorithms[index]->processCollision(m_compoundColObj,m_otherObj,m_dispatchInfo,m_resultOut);
146                        if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
147                        {
148                                btVector3 worldAabbMin,worldAabbMax;
149                                m_dispatchInfo.m_debugDraw->drawAabb(aabbMin0,aabbMax0,btVector3(1,1,1));
150                                m_dispatchInfo.m_debugDraw->drawAabb(aabbMin1,aabbMax1,btVector3(1,1,1));
151                        }
152                       
153                        //revert back transform
154                        m_compoundColObj->internalSetTemporaryCollisionShape( tmpShape);
155                        m_compoundColObj->setWorldTransform(  orgTrans );
156                        m_compoundColObj->setInterpolationWorldTransform(orgInterpolationTrans);
157                }
158        }
159        void            Process(const btDbvtNode* leaf)
160        {
161                int index = leaf->dataAsInt;
162
163                btCompoundShape* compoundShape = static_cast<btCompoundShape*>(m_compoundColObj->getCollisionShape());
164                btCollisionShape* childShape = compoundShape->getChildShape(index);
165                if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
166                {
167                        btVector3 worldAabbMin,worldAabbMax;
168                        btTransform     orgTrans = m_compoundColObj->getWorldTransform();
169                        btTransformAabb(leaf->volume.Mins(),leaf->volume.Maxs(),0.,orgTrans,worldAabbMin,worldAabbMax);
170                        m_dispatchInfo.m_debugDraw->drawAabb(worldAabbMin,worldAabbMax,btVector3(1,0,0));
171                }
172                ProcessChildShape(childShape,index);
173
174        }
175};
176
177
178
179
180
181
182void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
183{
184        btCollisionObject* colObj = m_isSwapped? body1 : body0;
185        btCollisionObject* otherObj = m_isSwapped? body0 : body1;
186
187       
188
189        btAssert (colObj->getCollisionShape()->isCompound());
190        btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
191
192        ///btCompoundShape might have changed:
193        ////make sure the internal child collision algorithm caches are still valid
194        if (compoundShape->getUpdateRevision() != m_compoundShapeRevision)
195        {
196                ///clear and update all
197                removeChildAlgorithms();
198               
199                preallocateChildAlgorithms(body0,body1);
200        }
201
202
203        btDbvt* tree = compoundShape->getDynamicAabbTree();
204        //use a dynamic aabb tree to cull potential child-overlaps
205        btCompoundLeafCallback  callback(colObj,otherObj,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold);
206
207        ///we need to refresh all contact manifolds
208        ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
209        ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
210        {
211                int i;
212                btManifoldArray manifoldArray;
213                for (i=0;i<m_childCollisionAlgorithms.size();i++)
214                {
215                        if (m_childCollisionAlgorithms[i])
216                        {
217                                m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray);
218                                for (int m=0;m<manifoldArray.size();m++)
219                                {
220                                        if (manifoldArray[m]->getNumContacts())
221                                        {
222                                                resultOut->setPersistentManifold(manifoldArray[m]);
223                                                resultOut->refreshContactPoints();
224                                                resultOut->setPersistentManifold(0);//??necessary?
225                                        }
226                                }
227                                manifoldArray.clear();
228                        }
229                }
230        }
231
232        if (tree)
233        {
234
235                btVector3 localAabbMin,localAabbMax;
236                btTransform otherInCompoundSpace;
237                otherInCompoundSpace = colObj->getWorldTransform().inverse() * otherObj->getWorldTransform();
238                otherObj->getCollisionShape()->getAabb(otherInCompoundSpace,localAabbMin,localAabbMax);
239
240                const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
241                //process all children, that overlap with  the given AABB bounds
242                tree->collideTV(tree->m_root,bounds,callback);
243
244        } else
245        {
246                //iterate over all children, perform an AABB check inside ProcessChildShape
247                int numChildren = m_childCollisionAlgorithms.size();
248                int i;
249                for (i=0;i<numChildren;i++)
250                {
251                        callback.ProcessChildShape(compoundShape->getChildShape(i),i);
252                }
253        }
254
255        {
256                                //iterate over all children, perform an AABB check inside ProcessChildShape
257                int numChildren = m_childCollisionAlgorithms.size();
258                int i;
259                btManifoldArray manifoldArray;
260
261                for (i=0;i<numChildren;i++)
262                {
263                        if (m_childCollisionAlgorithms[i])
264                        {
265                                btCollisionShape* childShape = compoundShape->getChildShape(i);
266                        //if not longer overlapping, remove the algorithm
267                                btTransform     orgTrans = colObj->getWorldTransform();
268                                btTransform     orgInterpolationTrans = colObj->getInterpolationWorldTransform();
269                                const btTransform& childTrans = compoundShape->getChildTransform(i);
270                                btTransform     newChildWorldTrans = orgTrans*childTrans ;
271
272                                //perform an AABB check first
273                                btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
274                                childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
275                                otherObj->getCollisionShape()->getAabb(otherObj->getWorldTransform(),aabbMin1,aabbMax1);
276
277                                if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
278                                {
279                                        m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
280                                        m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
281                                        m_childCollisionAlgorithms[i] = 0;
282                                }
283
284                        }
285                       
286                }
287
288               
289
290        }
291}
292
293btScalar        btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
294{
295
296        btCollisionObject* colObj = m_isSwapped? body1 : body0;
297        btCollisionObject* otherObj = m_isSwapped? body0 : body1;
298
299        btAssert (colObj->getCollisionShape()->isCompound());
300       
301        btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
302
303        //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
304        //If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals
305        //given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means:
306        //determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1
307        //then use each overlapping node AABB against Tree0
308        //and vise versa.
309
310        btScalar hitFraction = btScalar(1.);
311
312        int numChildren = m_childCollisionAlgorithms.size();
313        int i;
314        for (i=0;i<numChildren;i++)
315        {
316                //temporarily exchange parent btCollisionShape with childShape, and recurse
317                btCollisionShape* childShape = compoundShape->getChildShape(i);
318
319                //backup
320                btTransform     orgTrans = colObj->getWorldTransform();
321       
322                const btTransform& childTrans = compoundShape->getChildTransform(i);
323                //btTransform   newChildWorldTrans = orgTrans*childTrans ;
324                colObj->setWorldTransform( orgTrans*childTrans );
325
326                btCollisionShape* tmpShape = colObj->getCollisionShape();
327                colObj->internalSetTemporaryCollisionShape( childShape );
328                btScalar frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut);
329                if (frac<hitFraction)
330                {
331                        hitFraction = frac;
332                }
333                //revert back
334                colObj->internalSetTemporaryCollisionShape( tmpShape);
335                colObj->setWorldTransform( orgTrans);
336        }
337        return hitFraction;
338
339}
340
341
342
Note: See TracBrowser for help on using the repository browser.