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 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
  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, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
  38 : btCollisionAlgorithm(ci),
  39 m_isSwapped(isSwapped),
  40 m_btSoftBodyTriangleCallback(ci.m_dispatcher1,body0Wrap,body1Wrap,isSwapped)
  41 {
  42 }
  43
  44
  45
  46 btSoftBodyConcaveCollisionAlgorithm::~btSoftBodyConcaveCollisionAlgorithm()
  47 {
  48 }
  49
  50
  51
  52 btSoftBodyTriangleCallback::btSoftBodyTriangleCallback(btDispatcher*  dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped):
  53 m_dispatcher(dispatcher),
  54 m_dispatchInfoPtr(0)
  55 {
  56         m_softBody = (isSwapped? (btSoftBody*)body1Wrap->getCollisionObject():(btSoftBody*)body0Wrap->getCollisionObject());
  57         m_triBody = isSwapped? body0Wrap->getCollisionObject():body1Wrap->getCollisionObject();
  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
  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                 const btTransform& tr = m_triBody->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(m_triBody->getCollisionShape()->getUserPointer());
 119
 120                 btCollisionObjectWrapper softBody(0,m_softBody->getCollisionShape(),m_softBody,m_softBody->getWorldTransform());
 121                 //btCollisionObjectWrapper triBody(0,tm, ob, btTransform::getIdentity());//ob->getWorldTransform());//??
 122                 btCollisionObjectWrapper triBody(0,tm, m_triBody, m_triBody->getWorldTransform());
 123
 124                 btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0);//m_manifoldPtr);
 125
 126                 colAlgo->processCollision(&softBody,&triBody,*m_dispatchInfoPtr,m_resultOut);
 127                 colAlgo->~btCollisionAlgorithm();
 128                 ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
 129
 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(m_triBody->getCollisionShape()->getUserPointer());
 162
 163
 164                 btCollisionObjectWrapper softBody(0,m_softBody->getCollisionShape(),m_softBody,m_softBody->getWorldTransform());
 165                 btCollisionObjectWrapper triBody(0,tm, m_triBody, m_triBody->getWorldTransform());//btTransform::getIdentity());//??
 166
 167                 btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0);//m_manifoldPtr);
 168
 169                 colAlgo->processCollision(&softBody,&triBody,*m_dispatchInfoPtr,m_resultOut);
 170                 colAlgo->~btCollisionAlgorithm();
 171                 ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
 172
 173                 triIndex.m_childShape = tm;
 174                 m_shapeCache.insert(triKey,triIndex);
 175
 176         }
 177
 178
 179
 180 }
 181
 182
 183
 184 void    btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btCollisionObjectWrapper* triBodyWrap, const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 185 {
 186         m_dispatchInfoPtr = &dispatchInfo;
 187         m_collisionMarginTriangle = collisionMarginTriangle+btScalar(BT_SOFTBODY_TRIANGLE_EXTRUSION);
 188         m_resultOut = resultOut;
 189
 190
 191         btVector3       aabbWorldSpaceMin,aabbWorldSpaceMax;
 192         m_softBody->getAabb(aabbWorldSpaceMin,aabbWorldSpaceMax);
 193         btVector3 halfExtents = (aabbWorldSpaceMax-aabbWorldSpaceMin)*btScalar(0.5);
 194         btVector3 softBodyCenter = (aabbWorldSpaceMax+aabbWorldSpaceMin)*btScalar(0.5);
 195
 196         btTransform softTransform;
 197         softTransform.setIdentity();
 198         softTransform.setOrigin(softBodyCenter);
 199
 200         btTransform convexInTriangleSpace;
 201         convexInTriangleSpace = triBodyWrap->getWorldTransform().inverse() * softTransform;
 202         btTransformAabb(halfExtents,m_collisionMarginTriangle,convexInTriangleSpace,m_aabbMin,m_aabbMax);
 203 }
 204
 205 void btSoftBodyConcaveCollisionAlgorithm::clearCache()
 206 {
 207         m_btSoftBodyTriangleCallback.clearCache();
 208
 209 }
 210
 211 void btSoftBodyConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 212 {
 213
 214
 215         //btCollisionObject* convexBody = m_isSwapped ? body1 : body0;
 216         const btCollisionObjectWrapper* triBody = m_isSwapped ? body0Wrap : body1Wrap;
 217
 218         if (triBody->getCollisionShape()->isConcave())
 219         {
 220
 221
 222                 const btCollisionObject*        triOb = triBody->getCollisionObject();
 223                 const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>( triOb->getCollisionShape());
 224
 225                 //      if (convexBody->getCollisionShape()->isConvex())
 226                 {
 227                         btScalar collisionMarginTriangle = concaveShape->getMargin();
 228
 229                         //                      resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
 230                         m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,triBody,dispatchInfo,resultOut);
 231
 232
 233                         concaveShape->processAllTriangles( &m_btSoftBodyTriangleCallback,m_btSoftBodyTriangleCallback.getAabbMin(),m_btSoftBodyTriangleCallback.getAabbMax());
 234
 235                         //      resultOut->refreshContactPoints();
 236
 237                 }
 238
 239         }
 240
 241 }
 242
 243
 244 btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 245 {
 246         (void)resultOut;
 247         (void)dispatchInfo;
 248         btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
 249         btCollisionObject* triBody = m_isSwapped ? body0 : body1;
 250
 251
 252         //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
 253
 254         //only perform CCD above a certain threshold, this prevents blocking on the long run
 255         //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
 256         btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
 257         if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
 258         {
 259                 return btScalar(1.);
 260         }
 261
 262         //const btVector3& from = convexbody->m_worldTransform.getOrigin();
 263         //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
 264         //todo: only do if the motion exceeds the 'radius'
 265
 266         btTransform triInv = triBody->getWorldTransform().inverse();
 267         btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
 268         btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
 269
 270         struct LocalTriangleSphereCastCallback  : public btTriangleCallback
 271         {
 272                 btTransform m_ccdSphereFromTrans;
 273                 btTransform m_ccdSphereToTrans;
 274                 btTransform     m_meshTransform;
 275
 276                 btScalar        m_ccdSphereRadius;
 277                 btScalar        m_hitFraction;
 278
 279
 280                 LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
 281                         :m_ccdSphereFromTrans(from),
 282                         m_ccdSphereToTrans(to),
 283                         m_ccdSphereRadius(ccdSphereRadius),
 284                         m_hitFraction(hitFraction)
 285                 {
 286                 }
 287
 288
 289                 virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
 290                 {
 291                         (void)partId;
 292                         (void)triangleIndex;
 293                         //do a swept sphere for now
 294                         btTransform ident;
 295                         ident.setIdentity();
 296                         btConvexCast::CastResult castResult;
 297                         castResult.m_fraction = m_hitFraction;
 298                         btSphereShape   pointShape(m_ccdSphereRadius);
 299                         btTriangleShape triShape(triangle[0],triangle[1],triangle[2]);
 300                         btVoronoiSimplexSolver  simplexSolver;
 301                         btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
 302                         //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
 303                         //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
 304                         //local space?
 305
 306                         if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
 307                                 ident,ident,castResult))
 308                         {
 309                                 if (m_hitFraction > castResult.m_fraction)
 310                                         m_hitFraction = castResult.m_fraction;
 311                         }
 312
 313                 }
 314
 315         };
 316
 317
 318
 319
 320
 321         if (triBody->getCollisionShape()->isConcave())
 322         {
 323                 btVector3 rayAabbMin = convexFromLocal.getOrigin();
 324                 rayAabbMin.setMin(convexToLocal.getOrigin());
 325                 btVector3 rayAabbMax = convexFromLocal.getOrigin();
 326                 rayAabbMax.setMax(convexToLocal.getOrigin());
 327                 btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
 328                 rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
 329                 rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
 330
 331                 btScalar curHitFraction = btScalar(1.); //is this available?
 332                 LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
 333                         convexbody->getCcdSweptSphereRadius(),curHitFraction);
 334
 335                 raycastCallback.m_hitFraction = convexbody->getHitFraction();
 336
 337                 btCollisionObject* concavebody = triBody;
 338
 339                 btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
 340
 341                 if (triangleMesh)
 342                 {
 343                         triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
 344                 }
 345
 346
 347
 348                 if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
 349                 {
 350                         convexbody->setHitFraction( raycastCallback.m_hitFraction);
 351                         return raycastCallback.m_hitFraction;
 352                 }
 353         }
 354
 355         return btScalar(1.);
 356
 357 }