Applied some fixes from Bullet: issues with btDbvtBroadphase, and btSoftBody, and...
[blender-staging.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
23 btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped)
24 :btCollisionAlgorithm(ci),
25 m_isSwapped(isSwapped),
26 m_sharedManifold(ci.m_manifold)
27 {
28         m_ownsManifold = false;
29
30         btCollisionObject* colObj = m_isSwapped? body1 : body0;
31         btCollisionObject* otherObj = m_isSwapped? body0 : body1;
32         assert (colObj->getCollisionShape()->isCompound());
33         
34         btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
35         int numChildren = compoundShape->getNumChildShapes();
36         int i;
37         
38         m_childCollisionAlgorithms.resize(numChildren);
39         for (i=0;i<numChildren;i++)
40         {
41                 if (compoundShape->getDynamicAabbTree())
42                 {
43                         m_childCollisionAlgorithms[i] = 0;
44                 } else
45                 {
46                         btCollisionShape* tmpShape = colObj->getCollisionShape();
47                         btCollisionShape* childShape = compoundShape->getChildShape(i);
48                         colObj->internalSetTemporaryCollisionShape( childShape );
49                         m_childCollisionAlgorithms[i] = ci.m_dispatcher1->findAlgorithm(colObj,otherObj,m_sharedManifold);
50                         colObj->internalSetTemporaryCollisionShape( tmpShape );
51                 }
52         }
53 }
54
55
56 btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm()
57 {
58         int numChildren = m_childCollisionAlgorithms.size();
59         int i;
60         for (i=0;i<numChildren;i++)
61         {
62                 if (m_childCollisionAlgorithms[i])
63                 {
64                         m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
65                         m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
66                 }
67         }
68 }
69
70
71
72
73 struct  btCompoundLeafCallback : btDbvt::ICollide
74 {
75
76 public:
77
78         btCollisionObject* m_compoundColObj;
79         btCollisionObject* m_otherObj;
80         btDispatcher* m_dispatcher;
81         const btDispatcherInfo& m_dispatchInfo;
82         btManifoldResult*       m_resultOut;
83         btCollisionAlgorithm**  m_childCollisionAlgorithms;
84         btPersistentManifold*   m_sharedManifold;
85
86
87
88
89         btCompoundLeafCallback (btCollisionObject* compoundObj,btCollisionObject* otherObj,btDispatcher* dispatcher,const btDispatcherInfo& dispatchInfo,btManifoldResult*      resultOut,btCollisionAlgorithm**        childCollisionAlgorithms,btPersistentManifold*  sharedManifold)
90                 :m_compoundColObj(compoundObj),m_otherObj(otherObj),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut),
91                 m_childCollisionAlgorithms(childCollisionAlgorithms),
92                 m_sharedManifold(sharedManifold)
93         {
94
95         }
96
97
98         void    ProcessChildShape(btCollisionShape* childShape,int index)
99         {
100                 
101                 btCompoundShape* compoundShape = static_cast<btCompoundShape*>(m_compoundColObj->getCollisionShape());
102
103
104                 //backup
105                 btTransform     orgTrans = m_compoundColObj->getWorldTransform();
106                 btTransform     orgInterpolationTrans = m_compoundColObj->getInterpolationWorldTransform();
107                 const btTransform& childTrans = compoundShape->getChildTransform(index);
108                 btTransform     newChildWorldTrans = orgTrans*childTrans ;
109
110                 //perform an AABB check first
111                 btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
112                 childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
113                 m_otherObj->getCollisionShape()->getAabb(m_otherObj->getWorldTransform(),aabbMin1,aabbMax1);
114
115                 if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
116                 {
117
118                         m_compoundColObj->setWorldTransform( newChildWorldTrans);
119                         m_compoundColObj->setInterpolationWorldTransform(newChildWorldTrans);
120
121                         //the contactpoint is still projected back using the original inverted worldtrans
122                         btCollisionShape* tmpShape = m_compoundColObj->getCollisionShape();
123                         m_compoundColObj->internalSetTemporaryCollisionShape( childShape );
124
125                         if (!m_childCollisionAlgorithms[index])
126                                 m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(m_compoundColObj,m_otherObj,m_sharedManifold);
127
128                         m_childCollisionAlgorithms[index]->processCollision(m_compoundColObj,m_otherObj,m_dispatchInfo,m_resultOut);
129                         if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
130                         {
131                                 btVector3 worldAabbMin,worldAabbMax;
132                                 m_dispatchInfo.m_debugDraw->drawAabb(aabbMin0,aabbMax0,btVector3(1,1,1));
133                                 m_dispatchInfo.m_debugDraw->drawAabb(aabbMin1,aabbMax1,btVector3(1,1,1));
134                         }
135                         
136                         //revert back transform
137                         m_compoundColObj->internalSetTemporaryCollisionShape( tmpShape);
138                         m_compoundColObj->setWorldTransform(  orgTrans );
139                         m_compoundColObj->setInterpolationWorldTransform(orgInterpolationTrans);
140                 }
141         }
142         void            Process(const btDbvtNode* leaf)
143         {
144                 int index = leaf->dataAsInt;
145
146                 btCompoundShape* compoundShape = static_cast<btCompoundShape*>(m_compoundColObj->getCollisionShape());
147                 btCollisionShape* childShape = compoundShape->getChildShape(index);
148                 if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
149                 {
150                         btVector3 worldAabbMin,worldAabbMax;
151                         btTransform     orgTrans = m_compoundColObj->getWorldTransform();
152                         btTransformAabb(leaf->volume.Mins(),leaf->volume.Maxs(),0.,orgTrans,worldAabbMin,worldAabbMax);
153                         m_dispatchInfo.m_debugDraw->drawAabb(worldAabbMin,worldAabbMax,btVector3(1,0,0));
154                 }
155                 ProcessChildShape(childShape,index);
156
157         }
158 };
159
160
161
162
163
164
165 void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
166 {
167         btCollisionObject* colObj = m_isSwapped? body1 : body0;
168         btCollisionObject* otherObj = m_isSwapped? body0 : body1;
169
170         assert (colObj->getCollisionShape()->isCompound());
171         btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
172
173         btDbvt* tree = compoundShape->getDynamicAabbTree();
174         //use a dynamic aabb tree to cull potential child-overlaps
175         btCompoundLeafCallback  callback(colObj,otherObj,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold);
176
177
178         if (tree)
179         {
180
181                 btVector3 localAabbMin,localAabbMax;
182                 btTransform otherInCompoundSpace;
183                 otherInCompoundSpace = colObj->getWorldTransform().inverse() * otherObj->getWorldTransform();
184                 otherObj->getCollisionShape()->getAabb(otherInCompoundSpace,localAabbMin,localAabbMax);
185
186                 const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
187                 //process all children, that overlap with  the given AABB bounds
188                 tree->collideTV(tree->m_root,bounds,callback);
189
190         } else
191         {
192                 //iterate over all children, perform an AABB check inside ProcessChildShape
193                 int numChildren = m_childCollisionAlgorithms.size();
194                 int i;
195                 for (i=0;i<numChildren;i++)
196                 {
197                         callback.ProcessChildShape(compoundShape->getChildShape(i),i);
198                 }
199         }
200
201         {
202                                 //iterate over all children, perform an AABB check inside ProcessChildShape
203                 int numChildren = m_childCollisionAlgorithms.size();
204                 int i;
205                 btManifoldArray manifoldArray;
206
207                 for (i=0;i<numChildren;i++)
208                 {
209                         if (m_childCollisionAlgorithms[i])
210                         {
211                                 btCollisionShape* childShape = compoundShape->getChildShape(i);
212                         //if not longer overlapping, remove the algorithm
213                                 btTransform     orgTrans = colObj->getWorldTransform();
214                                 btTransform     orgInterpolationTrans = colObj->getInterpolationWorldTransform();
215                                 const btTransform& childTrans = compoundShape->getChildTransform(i);
216                                 btTransform     newChildWorldTrans = orgTrans*childTrans ;
217
218                                 //perform an AABB check first
219                                 btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
220                                 childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
221                                 otherObj->getCollisionShape()->getAabb(otherObj->getWorldTransform(),aabbMin1,aabbMax1);
222
223                                 if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
224                                 {
225                                         m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
226                                         m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
227                                         m_childCollisionAlgorithms[i] = 0;
228                                 }
229
230                         }
231                         
232                 }
233
234                 
235
236         }
237 }
238
239 btScalar        btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
240 {
241
242         btCollisionObject* colObj = m_isSwapped? body1 : body0;
243         btCollisionObject* otherObj = m_isSwapped? body0 : body1;
244
245         assert (colObj->getCollisionShape()->isCompound());
246         
247         btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
248
249         //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
250         //If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals
251         //given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means:
252         //determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1
253         //then use each overlapping node AABB against Tree0
254         //and vise versa.
255
256         btScalar hitFraction = btScalar(1.);
257
258         int numChildren = m_childCollisionAlgorithms.size();
259         int i;
260         for (i=0;i<numChildren;i++)
261         {
262                 //temporarily exchange parent btCollisionShape with childShape, and recurse
263                 btCollisionShape* childShape = compoundShape->getChildShape(i);
264
265                 //backup
266                 btTransform     orgTrans = colObj->getWorldTransform();
267         
268                 const btTransform& childTrans = compoundShape->getChildTransform(i);
269                 //btTransform   newChildWorldTrans = orgTrans*childTrans ;
270                 colObj->setWorldTransform( orgTrans*childTrans );
271
272                 btCollisionShape* tmpShape = colObj->getCollisionShape();
273                 colObj->internalSetTemporaryCollisionShape( childShape );
274                 btScalar frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut);
275                 if (frac<hitFraction)
276                 {
277                         hitFraction = frac;
278                 }
279                 //revert back
280                 colObj->internalSetTemporaryCollisionShape( tmpShape);
281                 colObj->setWorldTransform( orgTrans);
282         }
283         return hitFraction;
284
285 }
286
287