Finally upgraded to latest Bullet subversion, about to release 2.71. Some recent...
[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.3)
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;
214         m_resultOut = resultOut;
215
216         //recalc aabbs
217 //      btTransform softbodyInTriangleSpace;
218 //      softbodyInTriangleSpace = m_triBody->getWorldTransform().inverse() * m_softBody->getWorldTransform();
219 //      btCollisionShape* convexShape = static_cast<btCollisionShape*>(m_convexBody->getCollisionShape());
220         //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
221         btVector3       aabbWorldSpaceMin,aabbWorldSpaceMax;
222         m_softBody->getAabb(aabbWorldSpaceMin,aabbWorldSpaceMax);
223
224         btVector3 halfExtents = (aabbWorldSpaceMax-aabbWorldSpaceMin)*btScalar(0.5);
225         btVector3 softBodyCenter = (aabbWorldSpaceMax+aabbWorldSpaceMin)*btScalar(0.5);
226
227         btTransform     triInverse = m_triBody->getWorldTransform().inverse();
228
229         btMatrix3x3 abs_b = triInverse.getBasis().absolute();  
230         btPoint3 center = softBodyCenter + triInverse.getOrigin();
231
232         btVector3 extent = btVector3(abs_b[0].dot(halfExtents),
233                    abs_b[1].dot(halfExtents),
234                   abs_b[2].dot(halfExtents));
235 //      extent += btVector3(getMargin(),getMargin(),getMargin());
236
237         m_aabbMin = center - extent;
238         m_aabbMax = center + extent;
239
240         btScalar extraMargin = collisionMarginTriangle+btScalar(BT_SOFTBODY_TRIANGLE_EXTRUSION);
241         btVector3 extra(extraMargin,extraMargin,extraMargin);
242
243         m_aabbMax += extra;
244         m_aabbMin -= extra;
245
246 /*      btVector3 extra(2,2,2);
247         m_aabbMin = aabbWorldSpaceMin-extra;
248         m_aabbMax = aabbWorldSpaceMax+extra;
249 */
250
251 }
252
253 void btSoftBodyConcaveCollisionAlgorithm::clearCache()
254 {
255         m_btSoftBodyTriangleCallback.clearCache();
256
257 }
258
259 void btSoftBodyConcaveCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
260 {
261         
262         
263         btCollisionObject* convexBody = m_isSwapped ? body1 : body0;
264         btCollisionObject* triBody = m_isSwapped ? body0 : body1;
265
266         if (triBody->getCollisionShape()->isConcave())
267         {
268
269
270                 btCollisionObject*      triOb = triBody;
271                 btConcaveShape* concaveShape = static_cast<btConcaveShape*>( triOb->getCollisionShape());
272                 
273         //      if (convexBody->getCollisionShape()->isConvex())
274                 {
275                         btScalar collisionMarginTriangle = concaveShape->getMargin();
276                                         
277 //                      resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
278                         m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,resultOut);
279
280                         //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
281                         //m_dispatcher->clearManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
282
283 //                      m_btSoftBodyTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody);
284
285
286                         concaveShape->processAllTriangles( &m_btSoftBodyTriangleCallback,m_btSoftBodyTriangleCallback.getAabbMin(),m_btSoftBodyTriangleCallback.getAabbMax());
287                         
288                 //      resultOut->refreshContactPoints();
289         
290                 }
291         
292         }
293
294 }
295
296
297 btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
298 {
299         (void)resultOut;
300         (void)dispatchInfo;
301         btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
302         btCollisionObject* triBody = m_isSwapped ? body0 : body1;
303
304
305         //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
306
307         //only perform CCD above a certain threshold, this prevents blocking on the long run
308         //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
309         btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
310         if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
311         {
312                 return btScalar(1.);
313         }
314
315         //const btVector3& from = convexbody->m_worldTransform.getOrigin();
316         //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
317         //todo: only do if the motion exceeds the 'radius'
318
319         btTransform triInv = triBody->getWorldTransform().inverse();
320         btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
321         btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
322
323         struct LocalTriangleSphereCastCallback  : public btTriangleCallback
324         {
325                 btTransform m_ccdSphereFromTrans;
326                 btTransform m_ccdSphereToTrans;
327                 btTransform     m_meshTransform;
328
329                 btScalar        m_ccdSphereRadius;
330                 btScalar        m_hitFraction;
331         
332
333                 LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
334                         :m_ccdSphereFromTrans(from),
335                         m_ccdSphereToTrans(to),
336                         m_ccdSphereRadius(ccdSphereRadius),
337                         m_hitFraction(hitFraction)
338                 {                       
339                 }
340                 
341                 
342                 virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
343                 {
344                         (void)partId;
345                         (void)triangleIndex;
346                         //do a swept sphere for now
347                         btTransform ident;
348                         ident.setIdentity();
349                         btConvexCast::CastResult castResult;
350                         castResult.m_fraction = m_hitFraction;
351                         btSphereShape   pointShape(m_ccdSphereRadius);
352                         btTriangleShape triShape(triangle[0],triangle[1],triangle[2]);
353                         btVoronoiSimplexSolver  simplexSolver;
354                         btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
355                         //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
356                         //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
357                         //local space?
358
359                         if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
360                                 ident,ident,castResult))
361                         {
362                                 if (m_hitFraction > castResult.m_fraction)
363                                         m_hitFraction = castResult.m_fraction;
364                         }
365
366                 }
367
368         };
369
370
371         
372
373         
374         if (triBody->getCollisionShape()->isConcave())
375         {
376                 btVector3 rayAabbMin = convexFromLocal.getOrigin();
377                 rayAabbMin.setMin(convexToLocal.getOrigin());
378                 btVector3 rayAabbMax = convexFromLocal.getOrigin();
379                 rayAabbMax.setMax(convexToLocal.getOrigin());
380                 btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
381                 rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
382                 rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
383
384                 btScalar curHitFraction = btScalar(1.); //is this available?
385                 LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
386                         convexbody->getCcdSweptSphereRadius(),curHitFraction);
387
388                 raycastCallback.m_hitFraction = convexbody->getHitFraction();
389
390                 btCollisionObject* concavebody = triBody;
391
392                 btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
393                 
394                 if (triangleMesh)
395                 {
396                         triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
397                 }
398         
399
400
401                 if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
402                 {
403                         convexbody->setHitFraction( raycastCallback.m_hitFraction);
404                         return raycastCallback.m_hitFraction;
405                 }
406         }
407
408         return btScalar(1.);
409
410 }