3bcfe492107a2adb6b86fcc3728be6b250dc826e
[blender.git] / extern / bullet2 / src / BulletSoftBody / btSoftBodyConcaveCollisionAlgorithm.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
17 #include "btSoftBodyConcaveCollisionAlgorithm.h"
18 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
19 #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
20 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
21 #include "BulletCollision/CollisionShapes/btConcaveShape.h"
22 #include "BulletCollision/CollisionDispatch/btManifoldResult.h"
23 #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
24 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
25 #include "BulletCollision/CollisionShapes/btSphereShape.h"
26 #include "BulletCollision/CollisionShapes/btTetrahedronShape.h"
27 #include "BulletCollision/CollisionShapes/btConvexHullShape.h"
28
29
30
31 #include "LinearMath/btIDebugDraw.h"
32 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
33 #include "BulletSoftBody/btSoftBody.h"
34
35 #define BT_SOFTBODY_TRIANGLE_EXTRUSION btScalar(0.06)//make this configurable
36
37 btSoftBodyConcaveCollisionAlgorithm::btSoftBodyConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1,bool isSwapped)
38 : btCollisionAlgorithm(ci),
39 m_isSwapped(isSwapped),
40 m_btSoftBodyTriangleCallback(ci.m_dispatcher1,body0,body1,isSwapped)
41 {
42 }
43
44
45
46 btSoftBodyConcaveCollisionAlgorithm::~btSoftBodyConcaveCollisionAlgorithm()
47 {
48 }
49
50
51
52 btSoftBodyTriangleCallback::btSoftBodyTriangleCallback(btDispatcher*  dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped):
53           m_dispatcher(dispatcher),
54         m_dispatchInfoPtr(0)
55 {
56         m_softBody = (btSoftBody*) (isSwapped? body1:body0);
57         m_triBody = isSwapped? body0:body1;
58         
59           //
60           // create the manifold from the dispatcher 'manifold pool'
61           //
62 //        m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody);
63
64           clearCache();
65 }
66
67 btSoftBodyTriangleCallback::~btSoftBodyTriangleCallback()
68 {
69         clearCache();
70 //      m_dispatcher->releaseManifold( m_manifoldPtr );
71   
72 }
73   
74
75 void    btSoftBodyTriangleCallback::clearCache()
76 {
77         //m_dispatcher->clearManifold(m_manifoldPtr);
78 };
79
80
81 static const int maxParts = 1;
82 static const int maxTriangleIndex = 100*100;
83
84 btCollisionShape* shapeCache[maxParts][maxTriangleIndex];
85
86
87 void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex)
88 {
89         static bool hackedFirst = true;
90         if (hackedFirst)
91         {
92                 hackedFirst = false;
93                 int i,j;
94                 for (i=0;i<maxParts;i++)
95                 {
96                         for (j=0;j<maxTriangleIndex;j++)
97                         {
98                                 shapeCache[i][j]=0;
99                         }
100                 }
101         }
102  
103         //just for debugging purposes
104         //printf("triangle %d",m_triangleCount++);
105 btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBody);
106         btCollisionAlgorithmConstructionInfo ci;
107         ci.m_dispatcher1 = m_dispatcher;
108
109                 ///debug drawing of the overlapping triangles
110         if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0)
111         {
112                 btVector3 color(255,255,0);
113                 btTransform& tr = ob->getWorldTransform();
114                 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color);
115                 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color);
116                 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color);
117
118                 //btVector3 center = triangle[0] + triangle[1]+triangle[2];
119                 //center *= btScalar(0.333333);
120                 //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(center),color);
121                 //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(center),color);
122                 //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(center),color);
123
124         }
125
126         if (shapeCache[partId][triangleIndex])
127         {
128                 btCollisionShape* tm = shapeCache[partId][triangleIndex];
129
130                 //copy over user pointers to temporary shape
131                 tm->setUserPointer(ob->getRootCollisionShape()->getUserPointer());
132                 
133                 btCollisionShape* tmpShape = ob->getCollisionShape();
134                 ob->internalSetTemporaryCollisionShape( tm );
135                 
136
137                 btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_softBody,m_triBody,0);//m_manifoldPtr);
138                 
139                 colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
140                 colAlgo->~btCollisionAlgorithm();
141                 ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
142                 ob->internalSetTemporaryCollisionShape( tmpShape);
143                 return;
144         }
145
146         //aabb filter is already applied!       
147
148
149         
150
151         
152
153
154
155         //btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject);
156         
157 //      if (m_softBody->getCollisionShape()->getShapeType()==
158         {
159 //              btVector3 other;
160                 btVector3 normal = (triangle[1]-triangle[0]).cross(triangle[2]-triangle[0]);
161                 normal.normalize();
162                 normal*= BT_SOFTBODY_TRIANGLE_EXTRUSION;
163 //              other=(triangle[0]+triangle[1]+triangle[2])*0.333333f;
164 //              other+=normal*22.f;
165                 btVector3       pts[6] = {triangle[0]+normal,
166                         triangle[1]+normal,
167                 triangle[2]+normal,
168                 triangle[0]-normal,
169                 triangle[1]-normal,
170                 triangle[2]-normal};
171
172                 btConvexHullShape* tm = new btConvexHullShape(&pts[0].getX(),6);
173
174
175 //              btBU_Simplex1to4 tm(triangle[0],triangle[1],triangle[2],other);
176                 
177                 //btTriangleShape tm(triangle[0],triangle[1],triangle[2]);      
178         //      tm.setMargin(m_collisionMarginTriangle);
179                 
180                 //copy over user pointers to temporary shape
181                 tm->setUserPointer(ob->getRootCollisionShape()->getUserPointer());
182                 
183                 btCollisionShape* tmpShape = ob->getCollisionShape();
184                 ob->internalSetTemporaryCollisionShape( tm );
185                 
186
187                 btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_softBody,m_triBody,0);//m_manifoldPtr);
188                 ///this should use the btDispatcher, so the actual registered algorithm is used
189                 //              btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody);
190
191                 //m_resultOut->setShapeIdentifiers(-1,-1,partId,triangleIndex);
192         //      cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex);
193 //              cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
194                 colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
195                 colAlgo->~btCollisionAlgorithm();
196                 ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
197                 ob->internalSetTemporaryCollisionShape( tmpShape );
198 //              delete tm;
199
200                 shapeCache[partId][triangleIndex] = tm;
201
202         }
203
204         
205
206 }
207
208
209
210 void    btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
211 {
212         m_dispatchInfoPtr = &dispatchInfo;
213         m_collisionMarginTriangle = collisionMarginTriangle+btScalar(BT_SOFTBODY_TRIANGLE_EXTRUSION);
214         m_resultOut = resultOut;
215
216         
217         btVector3       aabbWorldSpaceMin,aabbWorldSpaceMax;
218         m_softBody->getAabb(aabbWorldSpaceMin,aabbWorldSpaceMax);
219         btVector3 halfExtents = (aabbWorldSpaceMax-aabbWorldSpaceMin)*btScalar(0.5);
220         btVector3 softBodyCenter = (aabbWorldSpaceMax+aabbWorldSpaceMin)*btScalar(0.5);
221
222         btTransform softTransform;
223         softTransform.setIdentity();
224         softTransform.setOrigin(softBodyCenter);
225
226         btTransform convexInTriangleSpace;
227         convexInTriangleSpace = m_triBody->getWorldTransform().inverse() * softTransform;
228         btTransformAabb(halfExtents,m_collisionMarginTriangle,convexInTriangleSpace,m_aabbMin,m_aabbMax);
229 }
230
231 void btSoftBodyConcaveCollisionAlgorithm::clearCache()
232 {
233         m_btSoftBodyTriangleCallback.clearCache();
234
235 }
236
237 void btSoftBodyConcaveCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
238 {
239         
240         
241         btCollisionObject* convexBody = m_isSwapped ? body1 : body0;
242         btCollisionObject* triBody = m_isSwapped ? body0 : body1;
243
244         if (triBody->getCollisionShape()->isConcave())
245         {
246
247
248                 btCollisionObject*      triOb = triBody;
249                 btConcaveShape* concaveShape = static_cast<btConcaveShape*>( triOb->getCollisionShape());
250                 
251         //      if (convexBody->getCollisionShape()->isConvex())
252                 {
253                         btScalar collisionMarginTriangle = concaveShape->getMargin();
254                                         
255 //                      resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
256                         m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,resultOut);
257
258                         //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
259                         //m_dispatcher->clearManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
260
261 //                      m_btSoftBodyTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody);
262
263
264                         concaveShape->processAllTriangles( &m_btSoftBodyTriangleCallback,m_btSoftBodyTriangleCallback.getAabbMin(),m_btSoftBodyTriangleCallback.getAabbMax());
265                         
266                 //      resultOut->refreshContactPoints();
267         
268                 }
269         
270         }
271
272 }
273
274
275 btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
276 {
277         (void)resultOut;
278         (void)dispatchInfo;
279         btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
280         btCollisionObject* triBody = m_isSwapped ? body0 : body1;
281
282
283         //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
284
285         //only perform CCD above a certain threshold, this prevents blocking on the long run
286         //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
287         btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
288         if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
289         {
290                 return btScalar(1.);
291         }
292
293         //const btVector3& from = convexbody->m_worldTransform.getOrigin();
294         //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
295         //todo: only do if the motion exceeds the 'radius'
296
297         btTransform triInv = triBody->getWorldTransform().inverse();
298         btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
299         btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
300
301         struct LocalTriangleSphereCastCallback  : public btTriangleCallback
302         {
303                 btTransform m_ccdSphereFromTrans;
304                 btTransform m_ccdSphereToTrans;
305                 btTransform     m_meshTransform;
306
307                 btScalar        m_ccdSphereRadius;
308                 btScalar        m_hitFraction;
309         
310
311                 LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
312                         :m_ccdSphereFromTrans(from),
313                         m_ccdSphereToTrans(to),
314                         m_ccdSphereRadius(ccdSphereRadius),
315                         m_hitFraction(hitFraction)
316                 {                       
317                 }
318                 
319                 
320                 virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
321                 {
322                         (void)partId;
323                         (void)triangleIndex;
324                         //do a swept sphere for now
325                         btTransform ident;
326                         ident.setIdentity();
327                         btConvexCast::CastResult castResult;
328                         castResult.m_fraction = m_hitFraction;
329                         btSphereShape   pointShape(m_ccdSphereRadius);
330                         btTriangleShape triShape(triangle[0],triangle[1],triangle[2]);
331                         btVoronoiSimplexSolver  simplexSolver;
332                         btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
333                         //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
334                         //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
335                         //local space?
336
337                         if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
338                                 ident,ident,castResult))
339                         {
340                                 if (m_hitFraction > castResult.m_fraction)
341                                         m_hitFraction = castResult.m_fraction;
342                         }
343
344                 }
345
346         };
347
348
349         
350
351         
352         if (triBody->getCollisionShape()->isConcave())
353         {
354                 btVector3 rayAabbMin = convexFromLocal.getOrigin();
355                 rayAabbMin.setMin(convexToLocal.getOrigin());
356                 btVector3 rayAabbMax = convexFromLocal.getOrigin();
357                 rayAabbMax.setMax(convexToLocal.getOrigin());
358                 btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
359                 rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
360                 rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
361
362                 btScalar curHitFraction = btScalar(1.); //is this available?
363                 LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
364                         convexbody->getCcdSweptSphereRadius(),curHitFraction);
365
366                 raycastCallback.m_hitFraction = convexbody->getHitFraction();
367
368                 btCollisionObject* concavebody = triBody;
369
370                 btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
371                 
372                 if (triangleMesh)
373                 {
374                         triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
375                 }
376         
377
378
379                 if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
380                 {
381                         convexbody->setHitFraction( raycastCallback.m_hitFraction);
382                         return raycastCallback.m_hitFraction;
383                 }
384         }
385
386         return btScalar(1.);
387
388 }