svn merge -r 13095:13148 https://svn.blender.org/svnroot/bf-blender/trunk/blender
[blender.git] / extern / bullet2 / src / BulletCollision / CollisionDispatch / btCollisionWorld.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 "btCollisionWorld.h"
17 #include "btCollisionDispatcher.h"
18 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
19 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
20 #include "BulletCollision/CollisionShapes/btConvexShape.h"
21
22 #include "BulletCollision/CollisionShapes/btSphereShape.h" //for raycasting
23 #include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" //for raycasting
24 #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
25 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
26 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
27 #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
28 #include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
29
30 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
31 #include "LinearMath/btAabbUtil2.h"
32 #include "LinearMath/btQuickprof.h"
33 #include "LinearMath/btStackAlloc.h"
34
35 //When the user doesn't provide dispatcher or broadphase, create basic versions (and delete them in destructor)
36 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
37 #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
38 #include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h"
39
40
41 btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration)
42 :m_dispatcher1(dispatcher),
43 m_broadphasePairCache(pairCache)
44 {
45         m_stackAlloc = collisionConfiguration->getStackAllocator();
46         m_dispatchInfo.m_stackAllocator = m_stackAlloc;
47 }
48
49
50 btCollisionWorld::~btCollisionWorld()
51 {
52
53         //clean up remaining objects
54         int i;
55         for (i=0;i<m_collisionObjects.size();i++)
56         {
57                 btCollisionObject* collisionObject= m_collisionObjects[i];
58                 
59                 btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
60                 if (bp)
61                 {
62                         //
63                         // only clear the cached algorithms
64                         //
65                         getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
66                         getBroadphase()->destroyProxy(bp,m_dispatcher1);
67                 }
68         }
69
70
71 }
72
73
74
75
76
77
78
79
80
81
82 void    btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
83 {
84
85         //check that the object isn't already added
86                 btAssert( m_collisionObjects.findLinearSearch(collisionObject)  == m_collisionObjects.size());
87
88                 m_collisionObjects.push_back(collisionObject);
89
90                 //calculate new AABB
91                 btTransform trans = collisionObject->getWorldTransform();
92
93                 btVector3       minAabb;
94                 btVector3       maxAabb;
95                 collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb);
96
97                 int type = collisionObject->getCollisionShape()->getShapeType();
98                 collisionObject->setBroadphaseHandle( getBroadphase()->createProxy(
99                         minAabb,
100                         maxAabb,
101                         type,
102                         collisionObject,
103                         collisionFilterGroup,
104                         collisionFilterMask,
105                         m_dispatcher1
106                         ))      ;
107
108                 
109
110
111
112 }
113
114
115
116
117 void    btCollisionWorld::performDiscreteCollisionDetection()
118 {
119         btDispatcherInfo& dispatchInfo = getDispatchInfo();
120
121         BEGIN_PROFILE("perform Broadphase Collision Detection");
122
123
124         //update aabb (of all moved objects)
125
126         btVector3 aabbMin,aabbMax;
127         for (int i=0;i<m_collisionObjects.size();i++)
128         {
129                 m_collisionObjects[i]->getCollisionShape()->getAabb(m_collisionObjects[i]->getWorldTransform(),aabbMin,aabbMax);
130                 m_broadphasePairCache->setAabb(m_collisionObjects[i]->getBroadphaseHandle(),aabbMin,aabbMax,m_dispatcher1);
131         }
132
133         m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1);
134         
135         END_PROFILE("perform Broadphase Collision Detection");
136
137         BEGIN_PROFILE("performDiscreteCollisionDetection");
138
139         btDispatcher* dispatcher = getDispatcher();
140         if (dispatcher)
141                 dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1);
142
143         END_PROFILE("performDiscreteCollisionDetection");
144
145 }
146
147
148
149 void    btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
150 {
151         
152         
153         //bool removeFromBroadphase = false;
154         
155         {
156                 
157                 btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
158                 if (bp)
159                 {
160                         //
161                         // only clear the cached algorithms
162                         //
163                         getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
164                         getBroadphase()->destroyProxy(bp,m_dispatcher1);
165                         collisionObject->setBroadphaseHandle(0);
166                 }
167         }
168
169
170         //swapremove
171         m_collisionObjects.remove(collisionObject);
172
173 }
174
175
176
177 void    btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
178                                           btCollisionObject* collisionObject,
179                                           const btCollisionShape* collisionShape,
180                                           const btTransform& colObjWorldTransform,
181                                           RayResultCallback& resultCallback,short int collisionFilterMask)
182 {
183         
184         btSphereShape pointShape(btScalar(0.0));
185         pointShape.setMargin(0.f);
186
187         objectQuerySingle(&pointShape,rayFromTrans,rayToTrans,
188                                           collisionObject,
189                                           collisionShape,
190                                           colObjWorldTransform,
191                                           resultCallback,collisionFilterMask);
192 }
193
194 void    btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& rayFromTrans,const btTransform& rayToTrans,
195                                           btCollisionObject* collisionObject,
196                                           const btCollisionShape* collisionShape,
197                                           const btTransform& colObjWorldTransform,
198                                           RayResultCallback& resultCallback,short int collisionFilterMask)
199 {
200         
201
202         if (collisionShape->isConvex())
203                         {
204                                 btConvexCast::CastResult castResult;
205                                 castResult.m_fraction = btScalar(1.);//??
206
207                                 btConvexShape* convexShape = (btConvexShape*) collisionShape;
208                                 btVoronoiSimplexSolver  simplexSolver;
209 #define  USE_SUBSIMPLEX_CONVEX_CAST 1
210 #ifdef USE_SUBSIMPLEX_CONVEX_CAST
211                                 btSubsimplexConvexCast convexCaster(castShape,convexShape,&simplexSolver);
212 #else
213                                 //btGjkConvexCast       convexCaster(castShape,convexShape,&simplexSolver);
214                                 //btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
215 #endif //#USE_SUBSIMPLEX_CONVEX_CAST
216                         
217                                 if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
218                                 {
219                                         //add hit
220                                         if (castResult.m_normal.length2() > btScalar(0.0001))
221                                         {
222                                                 
223                                                 if (castResult.m_fraction < resultCallback.m_closestHitFraction)
224                                                 {
225 #ifdef USE_SUBSIMPLEX_CONVEX_CAST
226                                                         //rotate normal into worldspace
227                                                         castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal;
228 #endif //USE_SUBSIMPLEX_CONVEX_CAST
229
230                                                         castResult.m_normal.normalize();
231                                                         btCollisionWorld::LocalRayResult localRayResult
232                                                                 (
233                                                                         collisionObject, 
234                                                                         0,
235                                                                         castResult.m_normal,
236                                                                         castResult.m_fraction
237                                                                 );
238
239                                                         bool normalInWorldSpace = true;
240                                                         resultCallback.AddSingleResult(localRayResult, normalInWorldSpace);
241
242                                                 }
243                                         }
244                                 }
245                         }
246                         else
247                         {
248                                 
249                                 if (collisionShape->isConcave())
250                                         {
251
252                                                 btTriangleMeshShape* triangleMesh = (btTriangleMeshShape*)collisionShape;
253                                                 
254                                                 btTransform worldTocollisionObject = colObjWorldTransform.inverse();
255
256                                                 btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
257                                                 btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
258
259                                                 //ConvexCast::CastResult
260
261                                                 struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback 
262                                                 {
263                                                         btCollisionWorld::RayResultCallback* m_resultCallback;
264                                                         btCollisionObject*      m_collisionObject;
265                                                         btTriangleMeshShape*    m_triangleMesh;
266
267                                                         BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
268                                                                 btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape*    triangleMesh):
269                                                                 btTriangleRaycastCallback(from,to),
270                                                                         m_resultCallback(resultCallback),
271                                                                         m_collisionObject(collisionObject),
272                                                                         m_triangleMesh(triangleMesh)
273                                                                 {
274                                                                 }
275
276
277                                                         virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
278                                                         {
279                                                                 btCollisionWorld::LocalShapeInfo        shapeInfo;
280                                                                 shapeInfo.m_shapePart = partId;
281                                                                 shapeInfo.m_triangleIndex = triangleIndex;
282                                                                 
283                                                                 btCollisionWorld::LocalRayResult rayResult
284                                                                 (m_collisionObject, 
285                                                                         &shapeInfo,
286                                                                         hitNormalLocal,
287                                                                         hitFraction);
288                                                                 
289                                                                 bool    normalInWorldSpace = false;
290                                                                 return m_resultCallback->AddSingleResult(rayResult,normalInWorldSpace);
291                                                                 
292                                                                 
293                                                         }
294         
295                                                 };
296
297
298                                                 BridgeTriangleRaycastCallback   rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh);
299                                                 rcb.m_hitFraction = resultCallback.m_closestHitFraction;
300
301                                                 btVector3 rayAabbMinLocal = rayFromLocal;
302                                                 rayAabbMinLocal.setMin(rayToLocal);
303                                                 btVector3 rayAabbMaxLocal = rayFromLocal;
304                                                 rayAabbMaxLocal.setMax(rayToLocal);
305
306                                                 triangleMesh->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
307                                                                                         
308                                         } else
309                                         {
310                                                 //todo: use AABB tree or other BVH acceleration structure!
311                                                 if (collisionShape->isCompound())
312                                                 {
313                                                         const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
314                                                         int i=0;
315                                                         for (i=0;i<compoundShape->getNumChildShapes();i++)
316                                                         {
317                                                                 btTransform childTrans = compoundShape->getChildTransform(i);
318                                                                 const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
319                                                                 btTransform childWorldTrans = colObjWorldTransform * childTrans;
320                                                                 objectQuerySingle(castShape, rayFromTrans,rayToTrans,
321                                                                         collisionObject,
322                                                                         childCollisionShape,
323                                                                         childWorldTrans,
324                                                                         resultCallback, collisionFilterMask);
325
326                                                         }
327
328
329                                                 }
330                                         }
331                         }
332 }
333
334 void    btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback,short int collisionFilterMask)
335 {
336
337         
338         btTransform     rayFromTrans,rayToTrans;
339         rayFromTrans.setIdentity();
340         rayFromTrans.setOrigin(rayFromWorld);
341         rayToTrans.setIdentity();
342         
343         rayToTrans.setOrigin(rayToWorld);
344
345         /// go over all objects, and if the ray intersects their aabb, do a ray-shape query using convexCaster (CCD)
346         
347         int i;
348         for (i=0;i<m_collisionObjects.size();i++)
349         {
350                 btCollisionObject*      collisionObject= m_collisionObjects[i];
351                 //only perform raycast if filterMask matches
352                 if(collisionObject->getBroadphaseHandle()->m_collisionFilterGroup & collisionFilterMask) { 
353                         //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
354                         btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
355                         collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
356
357                         btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing
358                         btVector3 hitNormal;
359                         if (btRayAabb(rayFromWorld,rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
360                         {
361                                 rayTestSingle(rayFromTrans,rayToTrans,
362                                         collisionObject,
363                                                 collisionObject->getCollisionShape(),
364                                                 collisionObject->getWorldTransform(),
365                                                 resultCallback);
366                         }       
367                 }
368         }
369
370 }