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 }