00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "btSoftRigidDynamicsWorld.h"
00018 #include "LinearMath/btQuickprof.h"
00019
00020
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
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
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
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
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
00133
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
00235 if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
00236 {
00237
00238
00239 #if 0
00240 #ifdef RECALCULATE_AABB
00241 btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
00242 collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
00243 #else
00244
00245 const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
00246 const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
00247 #endif
00248 #endif
00249
00250
00251
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
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
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
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