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() &btIDebugDraw::DBG_DrawWireframe)
  99         {
 100                 btVector3 color(1,1,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->setShapeIdentifiersB(partId,triangleIndex);
 172                 //              cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
 173                 colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
 174                 colAlgo->~btCollisionAlgorithm();
 175                 ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
 176
 177
 178                 ob->internalSetTemporaryCollisionShape( tmpShape );
 179                 triIndex.m_childShape = tm;
 180                 m_shapeCache.insert(triKey,triIndex);
 181
 182         }
 183
 184
 185
 186 }
 187
 188
 189
 190 void    btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 191 {
 192         m_dispatchInfoPtr = &dispatchInfo;
 193         m_collisionMarginTriangle = collisionMarginTriangle+btScalar(BT_SOFTBODY_TRIANGLE_EXTRUSION);
 194         m_resultOut = resultOut;
 195
 196
 197         btVector3       aabbWorldSpaceMin,aabbWorldSpaceMax;
 198         m_softBody->getAabb(aabbWorldSpaceMin,aabbWorldSpaceMax);
 199         btVector3 halfExtents = (aabbWorldSpaceMax-aabbWorldSpaceMin)*btScalar(0.5);
 200         btVector3 softBodyCenter = (aabbWorldSpaceMax+aabbWorldSpaceMin)*btScalar(0.5);
 201
 202         btTransform softTransform;
 203         softTransform.setIdentity();
 204         softTransform.setOrigin(softBodyCenter);
 205
 206         btTransform convexInTriangleSpace;
 207         convexInTriangleSpace = m_triBody->getWorldTransform().inverse() * softTransform;
 208         btTransformAabb(halfExtents,m_collisionMarginTriangle,convexInTriangleSpace,m_aabbMin,m_aabbMax);
 209 }
 210
 211 void btSoftBodyConcaveCollisionAlgorithm::clearCache()
 212 {
 213         m_btSoftBodyTriangleCallback.clearCache();
 214
 215 }
 216
 217 void btSoftBodyConcaveCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 218 {
 219
 220
 221         //btCollisionObject* convexBody = m_isSwapped ? body1 : body0;
 222         btCollisionObject* triBody = m_isSwapped ? body0 : body1;
 223
 224         if (triBody->getCollisionShape()->isConcave())
 225         {
 226
 227
 228                 btCollisionObject*      triOb = triBody;
 229                 btConcaveShape* concaveShape = static_cast<btConcaveShape*>( triOb->getCollisionShape());
 230
 231                 //      if (convexBody->getCollisionShape()->isConvex())
 232                 {
 233                         btScalar collisionMarginTriangle = concaveShape->getMargin();
 234
 235                         //                      resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
 236                         m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,resultOut);
 237
 238                         //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
 239                         //m_dispatcher->clearManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
 240
 241                         //                      m_btSoftBodyTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody);
 242
 243
 244                         concaveShape->processAllTriangles( &m_btSoftBodyTriangleCallback,m_btSoftBodyTriangleCallback.getAabbMin(),m_btSoftBodyTriangleCallback.getAabbMax());
 245
 246                         //      resultOut->refreshContactPoints();
 247
 248                 }
 249
 250         }
 251
 252 }
 253
 254
 255 btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 256 {
 257         (void)resultOut;
 258         (void)dispatchInfo;
 259         btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
 260         btCollisionObject* triBody = m_isSwapped ? body0 : body1;
 261
 262
 263         //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
 264
 265         //only perform CCD above a certain threshold, this prevents blocking on the long run
 266         //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
 267         btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
 268         if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
 269         {
 270                 return btScalar(1.);
 271         }
 272
 273         //const btVector3& from = convexbody->m_worldTransform.getOrigin();
 274         //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
 275         //todo: only do if the motion exceeds the 'radius'
 276
 277         btTransform triInv = triBody->getWorldTransform().inverse();
 278         btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
 279         btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
 280
 281         struct LocalTriangleSphereCastCallback  : public btTriangleCallback
 282         {
 283                 btTransform m_ccdSphereFromTrans;
 284                 btTransform m_ccdSphereToTrans;
 285                 btTransform     m_meshTransform;
 286
 287                 btScalar        m_ccdSphereRadius;
 288                 btScalar        m_hitFraction;
 289
 290
 291                 LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
 292                         :m_ccdSphereFromTrans(from),
 293                         m_ccdSphereToTrans(to),
 294                         m_ccdSphereRadius(ccdSphereRadius),
 295                         m_hitFraction(hitFraction)
 296                 {
 297                 }
 298
 299
 300                 virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
 301                 {
 302                         (void)partId;
 303                         (void)triangleIndex;
 304                         //do a swept sphere for now
 305                         btTransform ident;
 306                         ident.setIdentity();
 307                         btConvexCast::CastResult castResult;
 308                         castResult.m_fraction = m_hitFraction;
 309                         btSphereShape   pointShape(m_ccdSphereRadius);
 310                         btTriangleShape triShape(triangle[0],triangle[1],triangle[2]);
 311                         btVoronoiSimplexSolver  simplexSolver;
 312                         btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
 313                         //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
 314                         //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
 315                         //local space?
 316
 317                         if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
 318                                 ident,ident,castResult))
 319                         {
 320                                 if (m_hitFraction > castResult.m_fraction)
 321                                         m_hitFraction = castResult.m_fraction;
 322                         }
 323
 324                 }
 325
 326         };
 327
 328
 329
 330
 331
 332         if (triBody->getCollisionShape()->isConcave())
 333         {
 334                 btVector3 rayAabbMin = convexFromLocal.getOrigin();
 335                 rayAabbMin.setMin(convexToLocal.getOrigin());
 336                 btVector3 rayAabbMax = convexFromLocal.getOrigin();
 337                 rayAabbMax.setMax(convexToLocal.getOrigin());
 338                 btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
 339                 rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
 340                 rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
 341
 342                 btScalar curHitFraction = btScalar(1.); //is this available?
 343                 LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
 344                         convexbody->getCcdSweptSphereRadius(),curHitFraction);
 345
 346                 raycastCallback.m_hitFraction = convexbody->getHitFraction();
 347
 348                 btCollisionObject* concavebody = triBody;
 349
 350                 btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
 351
 352                 if (triangleMesh)
 353                 {
 354                         triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
 355                 }
 356
 357
 358
 359                 if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
 360                 {
 361                         convexbody->setHitFraction( raycastCallback.m_hitFraction);
 362                         return raycastCallback.m_hitFraction;
 363                 }
 364         }
 365
 366         return btScalar(1.);
 367
 368 }