workaround for a irregularity/bug in physics system (happens very seldom, just 1...
[blender.git] / extern / bullet / Bullet / CollisionDispatch / CollisionWorld.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 "CollisionWorld.h"
17 #include "CollisionDispatcher.h"
18 #include "CollisionDispatch/CollisionObject.h"
19 #include "CollisionShapes/CollisionShape.h"
20 #include "CollisionShapes/SphereShape.h" //for raycasting
21 #include "CollisionShapes/TriangleMeshShape.h" //for raycasting
22 #include "NarrowPhaseCollision/RaycastCallback.h"
23
24 #include "NarrowPhaseCollision/SubSimplexConvexCast.h"
25 #include "BroadphaseCollision/BroadphaseInterface.h"
26 #include "AabbUtil2.h"
27
28 #include <algorithm>
29
30 CollisionWorld::~CollisionWorld()
31 {
32         //clean up remaining objects
33         std::vector<CollisionObject*>::iterator i;
34
35         int index = 0;
36         for (i=m_collisionObjects.begin();
37         !(i==m_collisionObjects.end()); i++)
38
39         {
40                 CollisionObject* collisionObject= (*i);
41                 
42                 BroadphaseProxy* bp = collisionObject->m_broadphaseHandle;
43                 if (bp)
44                 {
45                         //
46                         // only clear the cached algorithms
47                         //
48                         GetBroadphase()->CleanProxyFromPairs(bp);
49                         GetBroadphase()->DestroyProxy(bp);
50                 }
51         }
52
53 }
54
55 void    CollisionWorld::UpdateActivationState()
56 {
57         m_dispatcher->InitUnionFind(m_collisionObjects.size());
58         
59         // put the index into m_controllers into m_tag  
60         {
61                 std::vector<CollisionObject*>::iterator i;
62                 
63                 int index = 0;
64                 for (i=m_collisionObjects.begin();
65                 !(i==m_collisionObjects.end()); i++)
66                 {
67                         
68                         CollisionObject*        collisionObject= (*i);
69                         collisionObject->m_islandTag1 = index;
70                         collisionObject->m_hitFraction = 1.f;
71                         index++;
72                         
73                 }
74         }
75         // do the union find
76         
77         m_dispatcher->FindUnions();
78         
79
80         
81 }
82
83
84
85 void    CollisionWorld::StoreIslandActivationState()
86 {
87         // put the islandId ('find' value) into m_tag   
88         {
89                 UnionFind& unionFind = m_dispatcher->GetUnionFind();
90                 
91                 std::vector<CollisionObject*>::iterator i;
92                 
93                 int index = 0;
94                 for (i=m_collisionObjects.begin();
95                 !(i==m_collisionObjects.end()); i++)
96                 {
97                         CollisionObject* collisionObject= (*i);
98                         
99                         if (collisionObject->mergesSimulationIslands())
100                         {
101                                 collisionObject->m_islandTag1 = unionFind.find(index);
102                         } else
103                         {
104                                 collisionObject->m_islandTag1 = -1;
105                         }
106                         index++;
107                 }
108         }
109 }
110
111
112
113
114
115 void    CollisionWorld::AddCollisionObject(CollisionObject* collisionObject)
116 {
117                 m_collisionObjects.push_back(collisionObject);
118
119                 //calculate new AABB
120                 SimdTransform trans = collisionObject->m_worldTransform;
121
122                 SimdVector3     minAabb;
123                 SimdVector3     maxAabb;
124                 collisionObject->m_collisionShape->GetAabb(trans,minAabb,maxAabb);
125
126                 int type = collisionObject->m_collisionShape->GetShapeType();
127                 collisionObject->m_broadphaseHandle = GetBroadphase()->CreateProxy(
128                         minAabb,
129                         maxAabb,
130                         type,
131                         collisionObject
132                         );
133                 
134
135
136
137 }
138
139 void    CollisionWorld::PerformDiscreteCollisionDetection()
140 {
141         DispatcherInfo  dispatchInfo;
142         dispatchInfo.m_timeStep = 0.f;
143         dispatchInfo.m_stepCount = 0;
144
145         //update aabb (of all moved objects)
146
147         SimdVector3 aabbMin,aabbMax;
148         for (size_t i=0;i<m_collisionObjects.size();i++)
149         {
150                 m_collisionObjects[i]->m_collisionShape->GetAabb(m_collisionObjects[i]->m_worldTransform,aabbMin,aabbMax);
151                 m_broadphase->SetAabb(m_collisionObjects[i]->m_broadphaseHandle,aabbMin,aabbMax);
152         }
153
154         m_broadphase->DispatchAllCollisionPairs(*GetDispatcher(),dispatchInfo);
155 }
156
157
158 void    CollisionWorld::RemoveCollisionObject(CollisionObject* collisionObject)
159 {
160         
161         
162         //bool removeFromBroadphase = false;
163         
164         {
165                 
166                 BroadphaseProxy* bp = collisionObject->m_broadphaseHandle;
167                 if (bp)
168                 {
169                         //
170                         // only clear the cached algorithms
171                         //
172                         GetBroadphase()->CleanProxyFromPairs(bp);
173                         GetBroadphase()->DestroyProxy(bp);
174                         collisionObject->m_broadphaseHandle = 0;
175                 }
176         }
177
178
179         std::vector<CollisionObject*>::iterator i =     std::find(m_collisionObjects.begin(), m_collisionObjects.end(), collisionObject);
180                 
181         if (!(i == m_collisionObjects.end()))
182                 {
183                         std::swap(*i, m_collisionObjects.back());
184                         m_collisionObjects.pop_back();
185                 }
186 }
187
188
189
190 void    CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3& rayToWorld, RayResultCallback& resultCallback)
191 {
192
193         
194         SimdTransform   rayFromTrans,rayToTrans;
195         rayFromTrans.setIdentity();
196         rayFromTrans.setOrigin(rayFromWorld);
197         rayToTrans.setIdentity();
198         
199         rayToTrans.setOrigin(rayToWorld);
200
201         //do culling based on aabb (rayFrom/rayTo)
202         SimdVector3 rayAabbMin = rayFromWorld;
203         SimdVector3 rayAabbMax = rayFromWorld;
204         rayAabbMin.setMin(rayToWorld);
205         rayAabbMax.setMax(rayToWorld);
206
207         SphereShape pointShape(0.0f);
208
209         /// brute force go over all objects. Once there is a broadphase, use that, or
210         /// add a raycast against aabb first.
211         
212         std::vector<CollisionObject*>::iterator iter;
213         
214         for (iter=m_collisionObjects.begin();
215         !(iter==m_collisionObjects.end()); iter++)
216         {
217                 
218                 CollisionObject*        collisionObject= (*iter);
219
220                 //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
221                 SimdVector3 collisionObjectAabbMin,collisionObjectAabbMax;
222                 collisionObject->m_collisionShape->GetAabb(collisionObject->m_worldTransform,collisionObjectAabbMin,collisionObjectAabbMax);
223
224                 //check aabb overlap
225
226                 if (TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,collisionObjectAabbMin,collisionObjectAabbMax))
227                 {
228                         if (collisionObject->m_collisionShape->IsConvex())
229                         {
230                                 ConvexCast::CastResult castResult;
231                                 castResult.m_fraction = 1.f;//??
232
233                                 ConvexShape* convexShape = (ConvexShape*) collisionObject->m_collisionShape;
234                                 VoronoiSimplexSolver    simplexSolver;
235                                 SubsimplexConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
236                                 //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
237                                 //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
238                                 
239                                 if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,collisionObject->m_worldTransform,collisionObject->m_worldTransform,castResult))
240                                 {
241                                         //add hit
242                                         if (castResult.m_normal.length2() > 0.0001f)
243                                         {
244                                                 castResult.m_normal.normalize();
245                                                 if (castResult.m_fraction < resultCallback.m_closestHitFraction)
246                                                 {
247                                                         
248
249                                                         CollisionWorld::LocalRayResult localRayResult
250                                                                 (
251                                                                         collisionObject, 
252                                                                         0,
253                                                                         castResult.m_normal,
254                                                                         castResult.m_fraction
255                                                                 );
256
257                                                         resultCallback.AddSingleResult(localRayResult);
258
259                                                 }
260                                         }
261                                 }
262                         }
263                         else
264                         {
265                                 
266                                 if (collisionObject->m_collisionShape->IsConcave())
267                                         {
268
269                                                 TriangleMeshShape* triangleMesh = (TriangleMeshShape*)collisionObject->m_collisionShape;
270                                                 
271                                                 SimdTransform worldTocollisionObject = collisionObject->m_worldTransform.inverse();
272
273                                                 SimdVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
274                                                 SimdVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
275
276                                                 //ConvexCast::CastResult
277
278                                                 struct BridgeTriangleRaycastCallback : public TriangleRaycastCallback 
279                                                 {
280                                                         RayResultCallback* m_resultCallback;
281                                                         CollisionObject*        m_collisionObject;
282                                                         TriangleMeshShape*      m_triangleMesh;
283
284                                                         BridgeTriangleRaycastCallback( const SimdVector3& from,const SimdVector3& to,
285                                                                 RayResultCallback* resultCallback, CollisionObject* collisionObject,TriangleMeshShape*  triangleMesh):
286                                                                 TriangleRaycastCallback(from,to),
287                                                                         m_resultCallback(resultCallback),
288                                                                         m_collisionObject(collisionObject),
289                                                                         m_triangleMesh(triangleMesh)
290                                                                 {
291                                                                 }
292
293
294                                                         virtual float ReportHit(const SimdVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex )
295                                                         {
296                                                                 CollisionWorld::LocalShapeInfo  shapeInfo;
297                                                                 shapeInfo.m_shapePart = partId;
298                                                                 shapeInfo.m_triangleIndex = triangleIndex;
299                                                                 
300                                                                 CollisionWorld::LocalRayResult rayResult
301                                                                 (m_collisionObject, 
302                                                                         &shapeInfo,
303                                                                         hitNormalLocal,
304                                                                         hitFraction);
305                                                                 
306                                                                 return m_resultCallback->AddSingleResult(rayResult);
307                                                                 
308                                                                 
309                                                         }
310         
311                                                 };
312
313
314                                                 BridgeTriangleRaycastCallback   rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh);
315                                                 rcb.m_hitFraction = resultCallback.m_closestHitFraction;
316
317                                                 SimdVector3 rayAabbMinLocal = rayFromLocal;
318                                                 rayAabbMinLocal.setMin(rayToLocal);
319                                                 SimdVector3 rayAabbMaxLocal = rayFromLocal;
320                                                 rayAabbMaxLocal.setMax(rayToLocal);
321
322                                                 triangleMesh->ProcessAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
323                                                                                         
324                                         }
325                                         
326
327                         }
328                 }
329         }
330
331 }