dali-physics/third-party/bullet3/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h

   1 /*
   2  Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
   3
   4  Bullet Continuous Collision Detection and Physics Library
   5  Copyright (c) 2019 Google Inc. http://bulletphysics.org
   6  This software is provided 'as-is', without any express or implied warranty.
   7  In no event will the authors be held liable for any damages arising from the use of this software.
   8  Permission is granted to anyone to use this software for any purpose,
   9  including commercial applications, and to alter it and redistribute it freely,
  10  subject to the following restrictions:
  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 #ifndef BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H
  17 #define BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H
  18
  19 #include "btSoftMultiBodyDynamicsWorld.h"
  20 #include "btDeformableLagrangianForce.h"
  21 #include "btDeformableMassSpringForce.h"
  22 // #include "btDeformableBodySolver.h"
  23 #include "btDeformableMultiBodyConstraintSolver.h"
  24 #include "btSoftBodyHelpers.h"
  25 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
  26 #include <functional>
  27 typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
  28
  29 class btDeformableBodySolver;
  30 class btDeformableLagrangianForce;
  31 struct MultiBodyInplaceSolverIslandCallback;
  32 struct DeformableBodyInplaceSolverIslandCallback;
  33 class btDeformableMultiBodyConstraintSolver;
  34
  35 typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
  36
  37 class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld
  38 {
  39         typedef btAlignedObjectArray<btVector3> TVStack;
  40         ///Solver classes that encapsulate multiple deformable bodies for solving
  41         btDeformableBodySolver* m_deformableBodySolver;
  42         btSoftBodyArray m_softBodies;
  43         int m_drawFlags;
  44         bool m_drawNodeTree;
  45         bool m_drawFaceTree;
  46         bool m_drawClusterTree;
  47         btSoftBodyWorldInfo m_sbi;
  48         btScalar m_internalTime;
  49         int m_ccdIterations;
  50         bool m_implicit;
  51         bool m_lineSearch;
  52         bool m_useProjection;
  53         DeformableBodyInplaceSolverIslandCallback* m_solverDeformableBodyIslandCallback;
  54
  55         typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world);
  56         btSolverCallback m_solverCallback;
  57
  58 protected:
  59         virtual void internalSingleStepSimulation(btScalar timeStep);
  60
  61         virtual void integrateTransforms(btScalar timeStep);
  62
  63         void positionCorrection(btScalar timeStep);
  64
  65         // Modified by Samsung to compile under GCC13. 2024-01-10. eunkiki.hong@samsung.com
  66         using btMultiBodyDynamicsWorld::solveConstraints;
  67         void solveConstraints(btScalar timeStep);
  68
  69         void updateActivationState(btScalar timeStep);
  70
  71         void clearGravity();
  72
  73 public:
  74         btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0);
  75
  76         virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.));
  77
  78         virtual void debugDrawWorld();
  79
  80         void setSolverCallback(btSolverCallback cb)
  81         {
  82                 m_solverCallback = cb;
  83         }
  84
  85         virtual ~btDeformableMultiBodyDynamicsWorld();
  86
  87         virtual btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld()
  88         {
  89                 return (btMultiBodyDynamicsWorld*)(this);
  90         }
  91
  92         virtual const btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() const
  93         {
  94                 return (const btMultiBodyDynamicsWorld*)(this);
  95         }
  96
  97         virtual btDynamicsWorldType getWorldType() const
  98         {
  99                 return BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD;
 100         }
 101
 102         virtual void predictUnconstraintMotion(btScalar timeStep);
 103
 104         virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter);
 105
 106         btSoftBodyArray& getSoftBodyArray()
 107         {
 108                 return m_softBodies;
 109         }
 110
 111         const btSoftBodyArray& getSoftBodyArray() const
 112         {
 113                 return m_softBodies;
 114         }
 115
 116         btSoftBodyWorldInfo& getWorldInfo()
 117         {
 118                 return m_sbi;
 119         }
 120
 121         const btSoftBodyWorldInfo& getWorldInfo() const
 122         {
 123                 return m_sbi;
 124         }
 125
 126         virtual void setGravity(const btVector3& gravity);
 127
 128         void reinitialize(btScalar timeStep);
 129
 130         void applyRigidBodyGravity(btScalar timeStep);
 131
 132         void beforeSolverCallbacks(btScalar timeStep);
 133
 134         void afterSolverCallbacks(btScalar timeStep);
 135
 136         void addForce(btSoftBody* psb, btDeformableLagrangianForce* force);
 137
 138         void removeForce(btSoftBody* psb, btDeformableLagrangianForce* force);
 139
 140         void removeSoftBodyForce(btSoftBody* psb);
 141
 142         void removeSoftBody(btSoftBody* body);
 143
 144         void removeCollisionObject(btCollisionObject* collisionObject);
 145
 146         int getDrawFlags() const { return (m_drawFlags); }
 147         void setDrawFlags(int f) { m_drawFlags = f; }
 148
 149         void setupConstraints();
 150
 151         void performDeformableCollisionDetection();
 152
 153         void solveMultiBodyConstraints();
 154
 155         void solveContactConstraints();
 156
 157         void sortConstraints();
 158
 159         void softBodySelfCollision();
 160
 161         void setImplicit(bool implicit)
 162         {
 163                 m_implicit = implicit;
 164         }
 165
 166         void setLineSearch(bool lineSearch)
 167         {
 168                 m_lineSearch = lineSearch;
 169         }
 170
 171         void setUseProjection(bool useProjection)
 172         {
 173                 m_useProjection = useProjection;
 174         }
 175
 176         void applyRepulsionForce(btScalar timeStep);
 177
 178         void performGeometricCollisions(btScalar timeStep);
 179
 180         struct btDeformableSingleRayCallback : 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 btDeformableMultiBodyDynamicsWorld* m_world;
 189                 btCollisionWorld::RayResultCallback& m_resultCallback;
 190
 191                 btDeformableSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btDeformableMultiBodyDynamicsWorld* 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 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                 btDeformableSingleRayCallback 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 rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans,
 272                                            btCollisionObject* collisionObject,
 273                                            const btCollisionShape* collisionShape,
 274                                            const btTransform& colObjWorldTransform,
 275                                            RayResultCallback& resultCallback) const
 276         {
 277                 if (collisionShape->isSoftBody())
 278                 {
 279                         btSoftBody* softBody = btSoftBody::upcast(collisionObject);
 280                         if (softBody)
 281                         {
 282                                 btSoftBody::sRayCast softResult;
 283                                 if (softBody->rayFaceTest(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                                                         normal = softBody->m_faces[softResult.index].m_normal;
 296                                                         if (normal.dot(rayDir) > 0)
 297                                                         {
 298                                                                 // normal always point toward origin of the ray
 299                                                                 normal = -normal;
 300                                                         }
 301                                                 }
 302
 303                                                 btCollisionWorld::LocalRayResult rayResult(collisionObject,
 304                                                                                                                                    &shapeInfo,
 305                                                                                                                                    normal,
 306                                                                                                                                    softResult.fraction);
 307                                                 bool normalInWorldSpace = true;
 308                                                 resultCallback.addSingleResult(rayResult, normalInWorldSpace);
 309                                         }
 310                                 }
 311                         }
 312                 }
 313                 else
 314                 {
 315                         btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, collisionObject, collisionShape, colObjWorldTransform, resultCallback);
 316                 }
 317         }
 318 };
 319
 320 #endif  //BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H