extern/bullet2/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
  24
  25 //#define USE_BRUTEFORCE_RAYBROADPHASE 1
  26
  27
  28
  29 btSoftRigidDynamicsWorld::btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
  30 :btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration)
  31 {
  32         m_drawFlags                     =       fDrawFlags::Std;
  33         m_drawNodeTree          =       true;
  34         m_drawFaceTree          =       false;
  35         m_drawClusterTree       =       false;
  36         m_sbi.m_broadphase = pairCache;
  37         m_sbi.m_dispatcher = dispatcher;
  38         m_sbi.m_sparsesdf.Initialize();
  39         m_sbi.m_sparsesdf.Reset();
  40
  41 }
  42
  43 btSoftRigidDynamicsWorld::~btSoftRigidDynamicsWorld()
  44 {
  45
  46 }
  47
  48 void    btSoftRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
  49 {
  50         btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep);
  51
  52         for ( int i=0;i<m_softBodies.size();++i)
  53         {
  54                 btSoftBody*     psb= m_softBodies[i];
  55
  56                 psb->predictMotion(timeStep);
  57         }
  58 }
  59
  60 void    btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep)
  61 {
  62         btDiscreteDynamicsWorld::internalSingleStepSimulation( timeStep );
  63
  64         ///solve soft bodies constraints
  65         solveSoftBodiesConstraints();
  66
  67         //self collisions
  68         for ( int i=0;i<m_softBodies.size();i++)
  69         {
  70                 btSoftBody*     psb=(btSoftBody*)m_softBodies[i];
  71                 psb->defaultCollisionHandler(psb);
  72         }
  73
  74         ///update soft bodies
  75         updateSoftBodies();
  76
  77 }
  78
  79 void    btSoftRigidDynamicsWorld::updateSoftBodies()
  80 {
  81         BT_PROFILE("updateSoftBodies");
  82
  83         for ( int i=0;i<m_softBodies.size();i++)
  84         {
  85                 btSoftBody*     psb=(btSoftBody*)m_softBodies[i];
  86                 psb->integrateMotion();
  87         }
  88 }
  89
  90 void    btSoftRigidDynamicsWorld::solveSoftBodiesConstraints()
  91 {
  92         BT_PROFILE("solveSoftConstraints");
  93
  94         if(m_softBodies.size())
  95         {
  96                 btSoftBody::solveClusters(m_softBodies);
  97         }
  98
  99         for(int i=0;i<m_softBodies.size();++i)
 100         {
 101                 btSoftBody*     psb=(btSoftBody*)m_softBodies[i];
 102                 psb->solveConstraints();
 103         }
 104 }
 105
 106 void    btSoftRigidDynamicsWorld::addSoftBody(btSoftBody* body)
 107 {
 108         m_softBodies.push_back(body);
 109
 110         btCollisionWorld::addCollisionObject(body,
 111                 btBroadphaseProxy::DefaultFilter,
 112                 btBroadphaseProxy::AllFilter);
 113
 114 }
 115
 116 void    btSoftRigidDynamicsWorld::removeSoftBody(btSoftBody* body)
 117 {
 118         m_softBodies.remove(body);
 119
 120         btCollisionWorld::removeCollisionObject(body);
 121 }
 122
 123 void    btSoftRigidDynamicsWorld::debugDrawWorld()
 124 {
 125         btDiscreteDynamicsWorld::debugDrawWorld();
 126
 127         if (getDebugDrawer())
 128         {
 129                 int i;
 130                 for (  i=0;i<this->m_softBodies.size();i++)
 131                 {
 132                         btSoftBody*     psb=(btSoftBody*)this->m_softBodies[i];
 133                         btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer);
 134                         btSoftBodyHelpers::Draw(psb,m_debugDrawer,m_drawFlags);
 135                         if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
 136                         {
 137                                 if(m_drawNodeTree)              btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer);
 138                                 if(m_drawFaceTree)              btSoftBodyHelpers::DrawFaceTree(psb,m_debugDrawer);
 139                                 if(m_drawClusterTree)   btSoftBodyHelpers::DrawClusterTree(psb,m_debugDrawer);
 140                         }
 141                 }
 142         }
 143 }
 144
 145
 146 struct btSoftSingleRayCallback : public btBroadphaseRayCallback
 147 {
 148         btVector3       m_rayFromWorld;
 149         btVector3       m_rayToWorld;
 150         btTransform     m_rayFromTrans;
 151         btTransform     m_rayToTrans;
 152         btVector3       m_hitNormal;
 153
 154         const btSoftRigidDynamicsWorld* m_world;
 155         btCollisionWorld::RayResultCallback&    m_resultCallback;
 156
 157         btSoftSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btSoftRigidDynamicsWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
 158         :m_rayFromWorld(rayFromWorld),
 159         m_rayToWorld(rayToWorld),
 160         m_world(world),
 161         m_resultCallback(resultCallback)
 162         {
 163                 m_rayFromTrans.setIdentity();
 164                 m_rayFromTrans.setOrigin(m_rayFromWorld);
 165                 m_rayToTrans.setIdentity();
 166                 m_rayToTrans.setOrigin(m_rayToWorld);
 167
 168                 btVector3 rayDir = (rayToWorld-rayFromWorld);
 169
 170                 rayDir.normalize ();
 171                 ///what about division by zero? --> just set rayDirection[i] to INF/1e30
 172                 m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
 173                 m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
 174                 m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
 175                 m_signs[0] = m_rayDirectionInverse[0] < 0.0;
 176                 m_signs[1] = m_rayDirectionInverse[1] < 0.0;
 177                 m_signs[2] = m_rayDirectionInverse[2] < 0.0;
 178
 179                 m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld);
 180
 181         }
 182
 183
 184
 185         virtual bool    process(const btBroadphaseProxy* proxy)
 186         {
 187                 ///terminate further ray tests, once the closestHitFraction reached zero
 188                 if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
 189                         return false;
 190
 191                 btCollisionObject*      collisionObject = (btCollisionObject*)proxy->m_clientObject;
 192
 193                 //only perform raycast if filterMask matches
 194                 if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
 195                 {
 196                         //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
 197                         //btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
 198 #if 0
 199 #ifdef RECALCULATE_AABB
 200                         btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
 201                         collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
 202 #else
 203                         //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
 204                         const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
 205                         const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
 206 #endif
 207 #endif
 208                         //btScalar hitLambda = m_resultCallback.m_closestHitFraction;
 209                         //culling already done by broadphase
 210                         //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
 211                         {
 212                                 m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
 213                                         collisionObject,
 214                                                 collisionObject->getCollisionShape(),
 215                                                 collisionObject->getWorldTransform(),
 216                                                 m_resultCallback);
 217                         }
 218                 }
 219                 return true;
 220         }
 221 };
 222
 223 void    btSoftRigidDynamicsWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
 224 {
 225         BT_PROFILE("rayTest");
 226         /// use the broadphase to accelerate the search for objects, based on their aabb
 227         /// and for each object with ray-aabb overlap, perform an exact ray test
 228         btSoftSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback);
 229
 230 #ifndef USE_BRUTEFORCE_RAYBROADPHASE
 231         m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB);
 232 #else
 233         for (int i=0;i<this->getNumCollisionObjects();i++)
 234         {
 235                 rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
 236         }
 237 #endif //USE_BRUTEFORCE_RAYBROADPHASE
 238
 239 }
 240
 241
 242 void    btSoftRigidDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
 243                                           btCollisionObject* collisionObject,
 244                                           const btCollisionShape* collisionShape,
 245                                           const btTransform& colObjWorldTransform,
 246                                           RayResultCallback& resultCallback)
 247 {
 248         if (collisionShape->isSoftBody()) {
 249                 btSoftBody* softBody = btSoftBody::upcast(collisionObject);
 250                 if (softBody) {
 251                         btSoftBody::sRayCast softResult;
 252                         if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult))
 253                         {
 254                                 if (softResult.fraction<= resultCallback.m_closestHitFraction)
 255                                 {
 256                                         btCollisionWorld::LocalShapeInfo shapeInfo;
 257                                         shapeInfo.m_shapePart = 0;
 258                                         shapeInfo.m_triangleIndex = softResult.index;
 259                                         // get the normal
 260                                         btVector3 normal = softBody->m_faces[softResult.index].m_normal;
 261                                         btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin();
 262                                         if (normal.dot(rayDir) > 0) {
 263                                                 // normal must always point toward origin of the ray
 264                                                 normal = -normal;
 265                                         }
 266                                         btCollisionWorld::LocalRayResult rayResult
 267                                                 (collisionObject,
 268                                                 &shapeInfo,
 269                                                 normal,
 270                                                 softResult.fraction);
 271                                         bool    normalInWorldSpace = true;
 272                                         resultCallback.addSingleResult(rayResult,normalInWorldSpace);
 273                                 }
 274                         }
 275                 }
 276         }
 277         else {
 278                 btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans,collisionObject,collisionShape,colObjWorldTransform,resultCallback);
 279         }
 280 }