Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/presentationFS14/src/external/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @ 10203

Last change on this file since 10203 was 10203, checked in by landauf, 5 years ago

removed unused code and debug output

  • Property svn:eol-style set to native
File size: 12.5 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                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
192void 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
302btScalar        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
Note: See TracBrowser for help on using the repository browser.