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