dali-physics/third-party/bullet3/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp

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