fb905c31eb41337b4748f6d13489838ad5924caa
[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                 }
175         }
176
177
178         std::vector<CollisionObject*>::iterator i =     std::find(m_collisionObjects.begin(), m_collisionObjects.end(), collisionObject);
179                 
180         if (!(i == m_collisionObjects.end()))
181                 {
182                         std::swap(*i, m_collisionObjects.back());
183                         m_collisionObjects.pop_back();
184                 }
185 }
186
187
188
189 void    CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3& rayToWorld, RayResultCallback& resultCallback)
190 {
191
192         
193         SimdTransform   rayFromTrans,rayToTrans;
194         rayFromTrans.setIdentity();
195         rayFromTrans.setOrigin(rayFromWorld);
196         rayToTrans.setIdentity();
197         
198         rayToTrans.setOrigin(rayToWorld);
199
200         //do culling based on aabb (rayFrom/rayTo)
201         SimdVector3 rayAabbMin = rayFromWorld;
202         SimdVector3 rayAabbMax = rayFromWorld;
203         rayAabbMin.setMin(rayToWorld);
204         rayAabbMax.setMax(rayToWorld);
205
206         SphereShape pointShape(0.0f);
207
208         /// brute force go over all objects. Once there is a broadphase, use that, or
209         /// add a raycast against aabb first.
210         
211         std::vector<CollisionObject*>::iterator iter;
212         
213         for (iter=m_collisionObjects.begin();
214         !(iter==m_collisionObjects.end()); iter++)
215         {
216                 
217                 CollisionObject*        collisionObject= (*iter);
218
219                 //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
220                 SimdVector3 collisionObjectAabbMin,collisionObjectAabbMax;
221                 collisionObject->m_collisionShape->GetAabb(collisionObject->m_worldTransform,collisionObjectAabbMin,collisionObjectAabbMax);
222
223                 //check aabb overlap
224
225                 if (TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,collisionObjectAabbMin,collisionObjectAabbMax))
226                 {
227                         if (collisionObject->m_collisionShape->IsConvex())
228                         {
229                                 ConvexCast::CastResult castResult;
230                                 castResult.m_fraction = 1.f;//??
231
232                                 ConvexShape* convexShape = (ConvexShape*) collisionObject->m_collisionShape;
233                                 VoronoiSimplexSolver    simplexSolver;
234                                 SubsimplexConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
235                                 //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
236                                 //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
237                                 
238                                 if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,collisionObject->m_worldTransform,collisionObject->m_worldTransform,castResult))
239                                 {
240                                         //add hit
241                                         if (castResult.m_normal.length2() > 0.0001f)
242                                         {
243                                                 castResult.m_normal.normalize();
244                                                 if (castResult.m_fraction < resultCallback.m_closestHitFraction)
245                                                 {
246                                                         
247
248                                                         CollisionWorld::LocalRayResult localRayResult
249                                                                 (
250                                                                         collisionObject, 
251                                                                         0,
252                                                                         castResult.m_normal,
253                                                                         castResult.m_fraction
254                                                                 );
255
256                                                         resultCallback.AddSingleResult(localRayResult);
257
258                                                 }
259                                         }
260                                 }
261                         }
262                         else
263                         {
264                                 
265                                 if (collisionObject->m_collisionShape->IsConcave())
266                                         {
267
268                                                 TriangleMeshShape* triangleMesh = (TriangleMeshShape*)collisionObject->m_collisionShape;
269                                                 
270                                                 SimdTransform worldTocollisionObject = collisionObject->m_worldTransform.inverse();
271
272                                                 SimdVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
273                                                 SimdVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
274
275                                                 //ConvexCast::CastResult
276
277                                                 struct BridgeTriangleRaycastCallback : public TriangleRaycastCallback 
278                                                 {
279                                                         RayResultCallback* m_resultCallback;
280                                                         CollisionObject*        m_collisionObject;
281                                                         TriangleMeshShape*      m_triangleMesh;
282
283                                                         BridgeTriangleRaycastCallback( const SimdVector3& from,const SimdVector3& to,
284                                                                 RayResultCallback* resultCallback, CollisionObject* collisionObject,TriangleMeshShape*  triangleMesh):
285                                                                 TriangleRaycastCallback(from,to),
286                                                                         m_resultCallback(resultCallback),
287                                                                         m_collisionObject(collisionObject),
288                                                                         m_triangleMesh(triangleMesh)
289                                                                 {
290                                                                 }
291
292
293                                                         virtual float ReportHit(const SimdVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex )
294                                                         {
295                                                                 CollisionWorld::LocalShapeInfo  shapeInfo;
296                                                                 shapeInfo.m_shapePart = partId;
297                                                                 shapeInfo.m_triangleIndex = triangleIndex;
298                                                                 
299                                                                 CollisionWorld::LocalRayResult rayResult
300                                                                 (m_collisionObject, 
301                                                                         &shapeInfo,
302                                                                         hitNormalLocal,
303                                                                         hitFraction);
304                                                                 
305                                                                 return m_resultCallback->AddSingleResult(rayResult);
306                                                                 
307                                                                 
308                                                         }
309         
310                                                 };
311
312
313                                                 BridgeTriangleRaycastCallback   rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh);
314                                                 rcb.m_hitFraction = resultCallback.m_closestHitFraction;
315
316                                                 SimdVector3 rayAabbMinLocal = rayFromLocal;
317                                                 rayAabbMinLocal.setMin(rayToLocal);
318                                                 SimdVector3 rayAabbMaxLocal = rayFromLocal;
319                                                 rayAabbMaxLocal.setMax(rayToLocal);
320
321                                                 triangleMesh->ProcessAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
322                                                                                         
323                                         }
324                                         
325
326                         }
327                 }
328         }
329
330 }