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