src/BulletSoftBody/btSoftRigidDynamicsWorld.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 "btSoftRigidDynamicsWorld.h"
  18 #include "LinearMath/btQuickprof.h"
  19
  20 //softbody & helpers
  21 #include "btSoftBody.h"
  22 #include "btSoftBodyHelpers.h"
  23 #include "btSoftBodySolvers.h"
  24 #include "btDefaultSoftBodySolver.h"
  25 #include "LinearMath/btSerializer.h"
  26
  27
  28 btSoftRigidDynamicsWorld::btSoftRigidDynamicsWorld(
  29         btDispatcher* dispatcher,
  30         btBroadphaseInterface* pairCache,
  31         btConstraintSolver* constraintSolver,
  32         btCollisionConfiguration* collisionConfiguration,
  33         btSoftBodySolver *softBodySolver ) :
  34                 btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration),
  35                 m_softBodySolver( softBodySolver ),
  36                 m_ownsSolver(false)
  37 {
  38         if( !m_softBodySolver )
  39         {
  40                 void* ptr = btAlignedAlloc(sizeof(btDefaultSoftBodySolver),16);
  41                 m_softBodySolver = new(ptr) btDefaultSoftBodySolver();
  42                 m_ownsSolver = true;
  43         }
  44
  45         m_drawFlags                     =       fDrawFlags::Std;
  46         m_drawNodeTree          =       true;
  47         m_drawFaceTree          =       false;
  48         m_drawClusterTree       =       false;
  49         m_sbi.m_broadphase = pairCache;
  50         m_sbi.m_dispatcher = dispatcher;
  51         m_sbi.m_sparsesdf.Initialize();
  52         m_sbi.m_sparsesdf.Reset();
  53
  54         m_sbi.air_density               =       (btScalar)1.2;
  55         m_sbi.water_density     =       0;
  56         m_sbi.water_offset              =       0;
  57         m_sbi.water_normal              =       btVector3(0,0,0);
  58         m_sbi.m_gravity.setValue(0,-10,0);
  59
  60         m_sbi.m_sparsesdf.Initialize();
  61
  62
  63 }
  64
  65 btSoftRigidDynamicsWorld::~btSoftRigidDynamicsWorld()
  66 {
  67         if (m_ownsSolver)
  68         {
  69                 m_softBodySolver->~btSoftBodySolver();
  70                 btAlignedFree(m_softBodySolver);
  71         }
  72 }
  73
  74 void    btSoftRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
  75 {
  76         btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep );
  77         {
  78                 BT_PROFILE("predictUnconstraintMotionSoftBody");
  79                 m_softBodySolver->predictMotion( timeStep );
  80         }
  81 }
  82
  83 void    btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep )
  84 {
  85
  86         // Let the solver grab the soft bodies and if necessary optimize for it
  87         m_softBodySolver->optimize( getSoftBodyArray() );
  88
  89         if( !m_softBodySolver->checkInitialized() )
  90         {
  91                 btAssert( "Solver initialization failed\n" );
  92         }
  93
  94         btDiscreteDynamicsWorld::internalSingleStepSimulation( timeStep );
  95
  96         ///solve soft bodies constraints
  97         solveSoftBodiesConstraints( timeStep );
  98
  99         //self collisions
 100         for ( int i=0;i<m_softBodies.size();i++)
 101         {
 102                 btSoftBody*     psb=(btSoftBody*)m_softBodies[i];
 103                 psb->defaultCollisionHandler(psb);
 104         }
 105
 106         ///update soft bodies
 107         m_softBodySolver->updateSoftBodies( );
 108
 109         // End solver-wise simulation step
 110         // ///////////////////////////////
 111
 112 }
 113
 114 void    btSoftRigidDynamicsWorld::solveSoftBodiesConstraints( btScalar timeStep )
 115 {
 116         BT_PROFILE("solveSoftConstraints");
 117
 118         if(m_softBodies.size())
 119         {
 120                 btSoftBody::solveClusters(m_softBodies);
 121         }
 122
 123         // Solve constraints solver-wise
 124         m_softBodySolver->solveConstraints( timeStep * m_softBodySolver->getTimeScale() );
 125
 126 }
 127
 128 void    btSoftRigidDynamicsWorld::addSoftBody(btSoftBody* body,short int collisionFilterGroup,short int collisionFilterMask)
 129 {
 130         m_softBodies.push_back(body);
 131
 132         // Set the soft body solver that will deal with this body
 133         // to be the world's solver
 134         body->setSoftBodySolver( m_softBodySolver );
 135
 136         btCollisionWorld::addCollisionObject(body,
 137                 collisionFilterGroup,
 138                 collisionFilterMask);
 139
 140 }
 141
 142 void    btSoftRigidDynamicsWorld::removeSoftBody(btSoftBody* body)
 143 {
 144         m_softBodies.remove(body);
 145
 146         btCollisionWorld::removeCollisionObject(body);
 147 }
 148
 149 void    btSoftRigidDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
 150 {
 151         btSoftBody* body = btSoftBody::upcast(collisionObject);
 152         if (body)
 153                 removeSoftBody(body);
 154         else
 155                 btDiscreteDynamicsWorld::removeCollisionObject(collisionObject);
 156 }
 157
 158 void    btSoftRigidDynamicsWorld::debugDrawWorld()
 159 {
 160         btDiscreteDynamicsWorld::debugDrawWorld();
 161
 162         if (getDebugDrawer())
 163         {
 164                 int i;
 165                 for (  i=0;i<this->m_softBodies.size();i++)
 166                 {
 167                         btSoftBody*     psb=(btSoftBody*)this->m_softBodies[i];
 168                         if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
 169                         {
 170                                 btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer);
 171                                 btSoftBodyHelpers::Draw(psb,m_debugDrawer,m_drawFlags);
 172                         }
 173
 174                         if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
 175                         {
 176                                 if(m_drawNodeTree)              btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer);
 177                                 if(m_drawFaceTree)              btSoftBodyHelpers::DrawFaceTree(psb,m_debugDrawer);
 178                                 if(m_drawClusterTree)   btSoftBodyHelpers::DrawClusterTree(psb,m_debugDrawer);
 179                         }
 180                 }
 181         }
 182 }
 183
 184
 185
 186
 187 struct btSoftSingleRayCallback : public btBroadphaseRayCallback
 188 {
 189         btVector3       m_rayFromWorld;
 190         btVector3       m_rayToWorld;
 191         btTransform     m_rayFromTrans;
 192         btTransform     m_rayToTrans;
 193         btVector3       m_hitNormal;
 194
 195         const btSoftRigidDynamicsWorld* m_world;
 196         btCollisionWorld::RayResultCallback&    m_resultCallback;
 197
 198         btSoftSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btSoftRigidDynamicsWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
 199         :m_rayFromWorld(rayFromWorld),
 200         m_rayToWorld(rayToWorld),
 201         m_world(world),
 202         m_resultCallback(resultCallback)
 203         {
 204                 m_rayFromTrans.setIdentity();
 205                 m_rayFromTrans.setOrigin(m_rayFromWorld);
 206                 m_rayToTrans.setIdentity();
 207                 m_rayToTrans.setOrigin(m_rayToWorld);
 208
 209                 btVector3 rayDir = (rayToWorld-rayFromWorld);
 210
 211                 rayDir.normalize ();
 212                 ///what about division by zero? --> just set rayDirection[i] to INF/1e30
 213                 m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
 214                 m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
 215                 m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
 216                 m_signs[0] = m_rayDirectionInverse[0] < 0.0;
 217                 m_signs[1] = m_rayDirectionInverse[1] < 0.0;
 218                 m_signs[2] = m_rayDirectionInverse[2] < 0.0;
 219
 220                 m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld);
 221
 222         }
 223
 224
 225
 226         virtual bool    process(const btBroadphaseProxy* proxy)
 227         {
 228                 ///terminate further ray tests, once the closestHitFraction reached zero
 229                 if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
 230                         return false;
 231
 232                 btCollisionObject*      collisionObject = (btCollisionObject*)proxy->m_clientObject;
 233
 234                 //only perform raycast if filterMask matches
 235                 if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
 236                 {
 237                         //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
 238                         //btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
 239 #if 0
 240 #ifdef RECALCULATE_AABB
 241                         btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
 242                         collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
 243 #else
 244                         //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
 245                         const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
 246                         const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
 247 #endif
 248 #endif
 249                         //btScalar hitLambda = m_resultCallback.m_closestHitFraction;
 250                         //culling already done by broadphase
 251                         //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
 252                         {
 253                                 m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
 254                                         collisionObject,
 255                                                 collisionObject->getCollisionShape(),
 256                                                 collisionObject->getWorldTransform(),
 257                                                 m_resultCallback);
 258                         }
 259                 }
 260                 return true;
 261         }
 262 };
 263
 264 void    btSoftRigidDynamicsWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
 265 {
 266         BT_PROFILE("rayTest");
 267         /// use the broadphase to accelerate the search for objects, based on their aabb
 268         /// and for each object with ray-aabb overlap, perform an exact ray test
 269         btSoftSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback);
 270
 271 #ifndef USE_BRUTEFORCE_RAYBROADPHASE
 272         m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB);
 273 #else
 274         for (int i=0;i<this->getNumCollisionObjects();i++)
 275         {
 276                 rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
 277         }
 278 #endif //USE_BRUTEFORCE_RAYBROADPHASE
 279
 280 }
 281
 282
 283 void    btSoftRigidDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
 284                                           btCollisionObject* collisionObject,
 285                                           const btCollisionShape* collisionShape,
 286                                           const btTransform& colObjWorldTransform,
 287                                           RayResultCallback& resultCallback)
 288 {
 289         if (collisionShape->isSoftBody()) {
 290                 btSoftBody* softBody = btSoftBody::upcast(collisionObject);
 291                 if (softBody) {
 292                         btSoftBody::sRayCast softResult;
 293                         if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult))
 294                         {
 295
 296                                 if (softResult.fraction<= resultCallback.m_closestHitFraction)
 297                                 {
 298
 299                                         btCollisionWorld::LocalShapeInfo shapeInfo;
 300                                         shapeInfo.m_shapePart = 0;
 301                                         shapeInfo.m_triangleIndex = softResult.index;
 302                                         // get the normal
 303                                         btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin();
 304                                         btVector3 normal=-rayDir;
 305                                         normal.normalize();
 306
 307                                         if (softResult.feature == btSoftBody::eFeature::Face)
 308                                         {
 309                                                 normal = softBody->m_faces[softResult.index].m_normal;
 310                                                 if (normal.dot(rayDir) > 0) {
 311                                                         // normal always point toward origin of the ray
 312                                                         normal = -normal;
 313                                                 }
 314                                         }
 315
 316                                         btCollisionWorld::LocalRayResult rayResult
 317                                                 (collisionObject,
 318                                                  &shapeInfo,
 319                                                  normal,
 320                                                  softResult.fraction);
 321                                         bool    normalInWorldSpace = true;
 322                                         resultCallback.addSingleResult(rayResult,normalInWorldSpace);
 323                                 }
 324                         }
 325                 }
 326         }
 327         else {
 328                 btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans,collisionObject,collisionShape,colObjWorldTransform,resultCallback);
 329         }
 330 }
 331
 332
 333 void    btSoftRigidDynamicsWorld::serializeSoftBodies(btSerializer* serializer)
 334 {
 335         int i;
 336         //serialize all collision objects
 337         for (i=0;i<m_collisionObjects.size();i++)
 338         {
 339                 btCollisionObject* colObj = m_collisionObjects[i];
 340                 if (colObj->getInternalType() & btCollisionObject::CO_SOFT_BODY)
 341                 {
 342                         int len = colObj->calculateSerializeBufferSize();
 343                         btChunk* chunk = serializer->allocate(len,1);
 344                         const char* structType = colObj->serialize(chunk->m_oldPtr, serializer);
 345                         serializer->finalizeChunk(chunk,structType,BT_SOFTBODY_CODE,colObj);
 346                 }
 347         }
 348
 349 }
 350
 351 void    btSoftRigidDynamicsWorld::serialize(btSerializer* serializer)
 352 {
 353
 354         serializer->startSerialization();
 355
 356         serializeDynamicsWorldInfo( serializer);
 357
 358         serializeSoftBodies(serializer);
 359
 360         serializeRigidBodies(serializer);
 361
 362         serializeCollisionObjects(serializer);
 363
 364         serializer->finishSerialization();
 365 }
 366
 367