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