btSoftRigidDynamicsWorld.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 
00017 #include "btSoftRigidDynamicsWorld.h"
00018 #include "LinearMath/btQuickprof.h"
00019 
00020 //softbody & helpers
00021 #include "btSoftBody.h"
00022 #include "btSoftBodyHelpers.h"
00023 #include "btSoftBodySolvers.h"
00024 #include "btDefaultSoftBodySolver.h"
00025 #include "LinearMath/btSerializer.h"
00026 
00027 
00028 btSoftRigidDynamicsWorld::btSoftRigidDynamicsWorld(
00029         btDispatcher* dispatcher,
00030         btBroadphaseInterface* pairCache,
00031         btConstraintSolver* constraintSolver,
00032         btCollisionConfiguration* collisionConfiguration,
00033         btSoftBodySolver *softBodySolver ) : 
00034                 btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration),
00035                 m_softBodySolver( softBodySolver ),
00036                 m_ownsSolver(false)
00037 {
00038         if( !m_softBodySolver )
00039         {
00040                 void* ptr = btAlignedAlloc(sizeof(btDefaultSoftBodySolver),16);
00041                 m_softBodySolver = new(ptr) btDefaultSoftBodySolver();
00042                 m_ownsSolver = true;
00043         }
00044 
00045         m_drawFlags                     =       fDrawFlags::Std;
00046         m_drawNodeTree          =       true;
00047         m_drawFaceTree          =       false;
00048         m_drawClusterTree       =       false;
00049         m_sbi.m_broadphase = pairCache;
00050         m_sbi.m_dispatcher = dispatcher;
00051         m_sbi.m_sparsesdf.Initialize();
00052         m_sbi.m_sparsesdf.Reset();
00053 
00054         m_sbi.air_density               =       (btScalar)1.2;
00055         m_sbi.water_density     =       0;
00056         m_sbi.water_offset              =       0;
00057         m_sbi.water_normal              =       btVector3(0,0,0);
00058         m_sbi.m_gravity.setValue(0,-10,0);
00059 
00060         m_sbi.m_sparsesdf.Initialize();
00061 
00062 
00063 }
00064 
00065 btSoftRigidDynamicsWorld::~btSoftRigidDynamicsWorld()
00066 {
00067         if (m_ownsSolver)
00068         {
00069                 m_softBodySolver->~btSoftBodySolver();
00070                 btAlignedFree(m_softBodySolver);
00071         }
00072 }
00073 
00074 void    btSoftRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
00075 {
00076         btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep );
00077         {
00078                 BT_PROFILE("predictUnconstraintMotionSoftBody");
00079                 m_softBodySolver->predictMotion( timeStep );
00080         }
00081 }
00082 
00083 void    btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep )
00084 {
00085 
00086         // Let the solver grab the soft bodies and if necessary optimize for it
00087         m_softBodySolver->optimize( getSoftBodyArray() );
00088 
00089         if( !m_softBodySolver->checkInitialized() )
00090         {
00091                 btAssert( "Solver initialization failed\n" );
00092         }
00093 
00094         btDiscreteDynamicsWorld::internalSingleStepSimulation( timeStep );
00095 
00097         solveSoftBodiesConstraints( timeStep );
00098 
00099         //self collisions
00100         for ( int i=0;i<m_softBodies.size();i++)
00101         {
00102                 btSoftBody*     psb=(btSoftBody*)m_softBodies[i];
00103                 psb->defaultCollisionHandler(psb);
00104         }
00105 
00107         m_softBodySolver->updateSoftBodies( );
00108         
00109         // End solver-wise simulation step
00110         // ///////////////////////////////
00111 
00112 }
00113 
00114 void    btSoftRigidDynamicsWorld::solveSoftBodiesConstraints( btScalar timeStep )
00115 {
00116         BT_PROFILE("solveSoftConstraints");
00117 
00118         if(m_softBodies.size())
00119         {
00120                 btSoftBody::solveClusters(m_softBodies);
00121         }
00122 
00123         // Solve constraints solver-wise
00124         m_softBodySolver->solveConstraints( timeStep * m_softBodySolver->getTimeScale() );
00125 
00126 }
00127 
00128 void    btSoftRigidDynamicsWorld::addSoftBody(btSoftBody* body,short int collisionFilterGroup,short int collisionFilterMask)
00129 {
00130         m_softBodies.push_back(body);
00131 
00132         // Set the soft body solver that will deal with this body
00133         // to be the world's solver
00134         body->setSoftBodySolver( m_softBodySolver );
00135 
00136         btCollisionWorld::addCollisionObject(body,
00137                 collisionFilterGroup,
00138                 collisionFilterMask);
00139 
00140 }
00141 
00142 void    btSoftRigidDynamicsWorld::removeSoftBody(btSoftBody* body)
00143 {
00144         m_softBodies.remove(body);
00145 
00146         btCollisionWorld::removeCollisionObject(body);
00147 }
00148 
00149 void    btSoftRigidDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
00150 {
00151         btSoftBody* body = btSoftBody::upcast(collisionObject);
00152         if (body)
00153                 removeSoftBody(body);
00154         else
00155                 btDiscreteDynamicsWorld::removeCollisionObject(collisionObject);
00156 }
00157 
00158 void    btSoftRigidDynamicsWorld::debugDrawWorld()
00159 {
00160         btDiscreteDynamicsWorld::debugDrawWorld();
00161 
00162         if (getDebugDrawer())
00163         {
00164                 int i;
00165                 for (  i=0;i<this->m_softBodies.size();i++)
00166                 {
00167                         btSoftBody*     psb=(btSoftBody*)this->m_softBodies[i];
00168                         if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
00169                         {
00170                                 btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer);
00171                                 btSoftBodyHelpers::Draw(psb,m_debugDrawer,m_drawFlags);
00172                         }
00173                         
00174                         if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
00175                         {
00176                                 if(m_drawNodeTree)              btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer);
00177                                 if(m_drawFaceTree)              btSoftBodyHelpers::DrawFaceTree(psb,m_debugDrawer);
00178                                 if(m_drawClusterTree)   btSoftBodyHelpers::DrawClusterTree(psb,m_debugDrawer);
00179                         }
00180                 }               
00181         }       
00182 }
00183 
00184 
00185 
00186 
00187 struct btSoftSingleRayCallback : public btBroadphaseRayCallback
00188 {
00189         btVector3       m_rayFromWorld;
00190         btVector3       m_rayToWorld;
00191         btTransform     m_rayFromTrans;
00192         btTransform     m_rayToTrans;
00193         btVector3       m_hitNormal;
00194 
00195         const btSoftRigidDynamicsWorld* m_world;
00196         btCollisionWorld::RayResultCallback&    m_resultCallback;
00197 
00198         btSoftSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btSoftRigidDynamicsWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
00199         :m_rayFromWorld(rayFromWorld),
00200         m_rayToWorld(rayToWorld),
00201         m_world(world),
00202         m_resultCallback(resultCallback)
00203         {
00204                 m_rayFromTrans.setIdentity();
00205                 m_rayFromTrans.setOrigin(m_rayFromWorld);
00206                 m_rayToTrans.setIdentity();
00207                 m_rayToTrans.setOrigin(m_rayToWorld);
00208 
00209                 btVector3 rayDir = (rayToWorld-rayFromWorld);
00210 
00211                 rayDir.normalize ();
00213                 m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
00214                 m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
00215                 m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
00216                 m_signs[0] = m_rayDirectionInverse[0] < 0.0;
00217                 m_signs[1] = m_rayDirectionInverse[1] < 0.0;
00218                 m_signs[2] = m_rayDirectionInverse[2] < 0.0;
00219 
00220                 m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld);
00221 
00222         }
00223 
00224         
00225 
00226         virtual bool    process(const btBroadphaseProxy* proxy)
00227         {
00229                 if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
00230                         return false;
00231 
00232                 btCollisionObject*      collisionObject = (btCollisionObject*)proxy->m_clientObject;
00233 
00234                 //only perform raycast if filterMask matches
00235                 if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) 
00236                 {
00237                         //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
00238                         //btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
00239 #if 0
00240 #ifdef RECALCULATE_AABB
00241                         btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
00242                         collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
00243 #else
00244                         //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
00245                         const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
00246                         const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
00247 #endif
00248 #endif
00249                         //btScalar hitLambda = m_resultCallback.m_closestHitFraction;
00250                         //culling already done by broadphase
00251                         //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
00252                         {
00253                                 m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
00254                                         collisionObject,
00255                                                 collisionObject->getCollisionShape(),
00256                                                 collisionObject->getWorldTransform(),
00257                                                 m_resultCallback);
00258                         }
00259                 }
00260                 return true;
00261         }
00262 };
00263 
00264 void    btSoftRigidDynamicsWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
00265 {
00266         BT_PROFILE("rayTest");
00269         btSoftSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback);
00270 
00271 #ifndef USE_BRUTEFORCE_RAYBROADPHASE
00272         m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB);
00273 #else
00274         for (int i=0;i<this->getNumCollisionObjects();i++)
00275         {
00276                 rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
00277         }       
00278 #endif //USE_BRUTEFORCE_RAYBROADPHASE
00279 
00280 }
00281 
00282 
00283 void    btSoftRigidDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
00284                                           btCollisionObject* collisionObject,
00285                                           const btCollisionShape* collisionShape,
00286                                           const btTransform& colObjWorldTransform,
00287                                           RayResultCallback& resultCallback)
00288 {
00289         if (collisionShape->isSoftBody()) {
00290                 btSoftBody* softBody = btSoftBody::upcast(collisionObject);
00291                 if (softBody) {
00292                         btSoftBody::sRayCast softResult;
00293                         if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) 
00294                         {
00295                                 
00296                                 if (softResult.fraction<= resultCallback.m_closestHitFraction)
00297                                 {
00298 
00299                                         btCollisionWorld::LocalShapeInfo shapeInfo;
00300                                         shapeInfo.m_shapePart = 0;
00301                                         shapeInfo.m_triangleIndex = softResult.index;
00302                                         // get the normal
00303                                         btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin();
00304                                         btVector3 normal=-rayDir;
00305                                         normal.normalize();
00306 
00307                                         if (softResult.feature == btSoftBody::eFeature::Face)
00308                                         {
00309                                                 normal = softBody->m_faces[softResult.index].m_normal;
00310                                                 if (normal.dot(rayDir) > 0) {
00311                                                         // normal always point toward origin of the ray
00312                                                         normal = -normal;
00313                                                 }
00314                                         }
00315         
00316                                         btCollisionWorld::LocalRayResult rayResult
00317                                                 (collisionObject,
00318                                                  &shapeInfo,
00319                                                  normal,
00320                                                  softResult.fraction);
00321                                         bool    normalInWorldSpace = true;
00322                                         resultCallback.addSingleResult(rayResult,normalInWorldSpace);
00323                                 }
00324                         }
00325                 }
00326         } 
00327         else {
00328                 btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans,collisionObject,collisionShape,colObjWorldTransform,resultCallback);
00329         }
00330 }
00331 
00332 
00333 void    btSoftRigidDynamicsWorld::serializeSoftBodies(btSerializer* serializer)
00334 {
00335         int i;
00336         //serialize all collision objects
00337         for (i=0;i<m_collisionObjects.size();i++)
00338         {
00339                 btCollisionObject* colObj = m_collisionObjects[i];
00340                 if (colObj->getInternalType() & btCollisionObject::CO_SOFT_BODY)
00341                 {
00342                         int len = colObj->calculateSerializeBufferSize();
00343                         btChunk* chunk = serializer->allocate(len,1);
00344                         const char* structType = colObj->serialize(chunk->m_oldPtr, serializer);
00345                         serializer->finalizeChunk(chunk,structType,BT_SOFTBODY_CODE,colObj);
00346                 }
00347         }
00348 
00349 }
00350 
00351 void    btSoftRigidDynamicsWorld::serialize(btSerializer* serializer)
00352 {
00353 
00354         serializer->startSerialization();
00355 
00356         serializeDynamicsWorldInfo( serializer);
00357 
00358         serializeSoftBodies(serializer);
00359 
00360         serializeRigidBodies(serializer);
00361 
00362         serializeCollisionObjects(serializer);
00363 
00364         serializer->finishSerialization();
00365 }
00366 
00367