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         void solveConstraints(btScalar timeStep);
  66
  67         void updateActivationState(btScalar timeStep);
  68
  69         void clearGravity();
  70
  71 public:
  72         btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0);
  73
  74         virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.));
  75
  76         virtual void debugDrawWorld();
  77
  78         void setSolverCallback(btSolverCallback cb)
  79         {
  80                 m_solverCallback = cb;
  81         }
  82
  83         virtual ~btDeformableMultiBodyDynamicsWorld();
  84
  85         virtual btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld()
  86         {
  87                 return (btMultiBodyDynamicsWorld*)(this);
  88         }
  89
  90         virtual const btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() const
  91         {
  92                 return (const btMultiBodyDynamicsWorld*)(this);
  93         }
  94
  95         virtual btDynamicsWorldType getWorldType() const
  96         {
  97                 return BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD;
  98         }
  99
 100         virtual void predictUnconstraintMotion(btScalar timeStep);
 101
 102         virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter);
 103
 104         btSoftBodyArray& getSoftBodyArray()
 105         {
 106                 return m_softBodies;
 107         }
 108
 109         const btSoftBodyArray& getSoftBodyArray() const
 110         {
 111                 return m_softBodies;
 112         }
 113
 114         btSoftBodyWorldInfo& getWorldInfo()
 115         {
 116                 return m_sbi;
 117         }
 118
 119         const btSoftBodyWorldInfo& getWorldInfo() const
 120         {
 121                 return m_sbi;
 122         }
 123
 124         virtual void setGravity(const btVector3& gravity);
 125
 126         void reinitialize(btScalar timeStep);
 127
 128         void applyRigidBodyGravity(btScalar timeStep);
 129
 130         void beforeSolverCallbacks(btScalar timeStep);
 131
 132         void afterSolverCallbacks(btScalar timeStep);
 133
 134         void addForce(btSoftBody* psb, btDeformableLagrangianForce* force);
 135
 136         void removeForce(btSoftBody* psb, btDeformableLagrangianForce* force);
 137
 138         void removeSoftBodyForce(btSoftBody* psb);
 139
 140         void removeSoftBody(btSoftBody* body);
 141
 142         void removeCollisionObject(btCollisionObject* collisionObject);
 143
 144         int getDrawFlags() const { return (m_drawFlags); }
 145         void setDrawFlags(int f) { m_drawFlags = f; }
 146
 147         void setupConstraints();
 148
 149         void performDeformableCollisionDetection();
 150
 151         void solveMultiBodyConstraints();
 152
 153         void solveContactConstraints();
 154
 155         void sortConstraints();
 156
 157         void softBodySelfCollision();
 158
 159         void setImplicit(bool implicit)
 160         {
 161                 m_implicit = implicit;
 162         }
 163
 164         void setLineSearch(bool lineSearch)
 165         {
 166                 m_lineSearch = lineSearch;
 167         }
 168
 169         void setUseProjection(bool useProjection)
 170         {
 171                 m_useProjection = useProjection;
 172         }
 173
 174         void applyRepulsionForce(btScalar timeStep);
 175
 176         void performGeometricCollisions(btScalar timeStep);
 177
 178         struct btDeformableSingleRayCallback : public btBroadphaseRayCallback
 179         {
 180                 btVector3 m_rayFromWorld;
 181                 btVector3 m_rayToWorld;
 182                 btTransform m_rayFromTrans;
 183                 btTransform m_rayToTrans;
 184                 btVector3 m_hitNormal;
 185
 186                 const btDeformableMultiBodyDynamicsWorld* m_world;
 187                 btCollisionWorld::RayResultCallback& m_resultCallback;
 188
 189                 btDeformableSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btDeformableMultiBodyDynamicsWorld* world, btCollisionWorld::RayResultCallback& resultCallback)
 190                         : m_rayFromWorld(rayFromWorld),
 191                           m_rayToWorld(rayToWorld),
 192                           m_world(world),
 193                           m_resultCallback(resultCallback)
 194                 {
 195                         m_rayFromTrans.setIdentity();
 196                         m_rayFromTrans.setOrigin(m_rayFromWorld);
 197                         m_rayToTrans.setIdentity();
 198                         m_rayToTrans.setOrigin(m_rayToWorld);
 199
 200                         btVector3 rayDir = (rayToWorld - rayFromWorld);
 201
 202                         rayDir.normalize();
 203                         ///what about division by zero? --> just set rayDirection[i] to INF/1e30
 204                         m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
 205                         m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
 206                         m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
 207                         m_signs[0] = m_rayDirectionInverse[0] < 0.0;
 208                         m_signs[1] = m_rayDirectionInverse[1] < 0.0;
 209                         m_signs[2] = m_rayDirectionInverse[2] < 0.0;
 210
 211                         m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld);
 212                 }
 213
 214                 virtual bool process(const btBroadphaseProxy* proxy)
 215                 {
 216                         ///terminate further ray tests, once the closestHitFraction reached zero
 217                         if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
 218                                 return false;
 219
 220                         btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
 221
 222                         //only perform raycast if filterMask matches
 223                         if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
 224                         {
 225                                 //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
 226                                 //btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
 227 #if 0
 228 #ifdef RECALCULATE_AABB
 229                 btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
 230                 collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
 231 #else
 232                 //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
 233                 const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
 234                 const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
 235 #endif
 236 #endif
 237                                 //btScalar hitLambda = m_resultCallback.m_closestHitFraction;
 238                                 //culling already done by broadphase
 239                                 //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
 240                                 {
 241                                         m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans,
 242                                                                                    collisionObject,
 243                                                                                    collisionObject->getCollisionShape(),
 244                                                                                    collisionObject->getWorldTransform(),
 245                                                                                    m_resultCallback);
 246                                 }
 247                         }
 248                         return true;
 249                 }
 250         };
 251
 252         void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
 253         {
 254                 BT_PROFILE("rayTest");
 255                 /// use the broadphase to accelerate the search for objects, based on their aabb
 256                 /// and for each object with ray-aabb overlap, perform an exact ray test
 257                 btDeformableSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback);
 258
 259 #ifndef USE_BRUTEFORCE_RAYBROADPHASE
 260                 m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB);
 261 #else
 262                 for (int i = 0; i < this->getNumCollisionObjects(); i++)
 263                 {
 264                         rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
 265                 }
 266 #endif  //USE_BRUTEFORCE_RAYBROADPHASE
 267         }
 268
 269         void rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans,
 270                                            btCollisionObject* collisionObject,
 271                                            const btCollisionShape* collisionShape,
 272                                            const btTransform& colObjWorldTransform,
 273                                            RayResultCallback& resultCallback) const
 274         {
 275                 if (collisionShape->isSoftBody())
 276                 {
 277                         btSoftBody* softBody = btSoftBody::upcast(collisionObject);
 278                         if (softBody)
 279                         {
 280                                 btSoftBody::sRayCast softResult;
 281                                 if (softBody->rayFaceTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult))
 282                                 {
 283                                         if (softResult.fraction <= resultCallback.m_closestHitFraction)
 284                                         {
 285                                                 btCollisionWorld::LocalShapeInfo shapeInfo;
 286                                                 shapeInfo.m_shapePart = 0;
 287                                                 shapeInfo.m_triangleIndex = softResult.index;
 288                                                 // get the normal
 289                                                 btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin();
 290                                                 btVector3 normal = -rayDir;
 291                                                 normal.normalize();
 292                                                 {
 293                                                         normal = softBody->m_faces[softResult.index].m_normal;
 294                                                         if (normal.dot(rayDir) > 0)
 295                                                         {
 296                                                                 // normal always point toward origin of the ray
 297                                                                 normal = -normal;
 298                                                         }
 299                                                 }
 300
 301                                                 btCollisionWorld::LocalRayResult rayResult(collisionObject,
 302                                                                                                                                    &shapeInfo,
 303                                                                                                                                    normal,
 304                                                                                                                                    softResult.fraction);
 305                                                 bool normalInWorldSpace = true;
 306                                                 resultCallback.addSingleResult(rayResult, normalInWorldSpace);
 307                                         }
 308                                 }
 309                         }
 310                 }
 311                 else
 312                 {
 313                         btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, collisionObject, collisionShape, colObjWorldTransform, resultCallback);
 314                 }
 315         }
 316 };
 317
 318 #endif  //BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H