Bullet Collision Detection & Physics Library
btSoftMultiBodyDynamicsWorld.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
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 
17 #include "LinearMath/btQuickprof.h"
18 
19 //softbody & helpers
25 
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);
39  m_ownsSolver = true;
40  }
41 
43  m_drawNodeTree = true;
44  m_drawFaceTree = false;
45  m_drawClusterTree = false;
46  m_sbi.m_broadphase = pairCache;
47  m_sbi.m_dispatcher = dispatcher;
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 
58 }
59 
61 {
62  if (m_ownsSolver)
63  {
66  }
67 }
68 
70 {
72  {
73  BT_PROFILE("predictUnconstraintMotionSoftBody");
74  m_softBodySolver->predictMotion(float(timeStep));
75  }
76 }
77 
79 {
80  // Let the solver grab the soft bodies and if necessary optimize for it
82 
84  {
85  btAssert("Solver initialization failed\n");
86  }
87 
89 
92 
93  //self collisions
94  for (int i = 0; i < m_softBodies.size(); i++)
95  {
97  psb->defaultCollisionHandler(psb);
98  }
99 
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 
113 {
114  BT_PROFILE("solveSoftConstraints");
115 
116  if (m_softBodies.size())
117  {
119  }
120 
121  // Solve constraints solver-wise
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
132 
134  collisionFilterGroup,
135  collisionFilterMask);
136 }
137 
139 {
140  m_softBodies.remove(body);
141 
143 }
144 
146 {
147  btSoftBody* body = btSoftBody::upcast(collisionObject);
148  if (body)
149  removeSoftBody(body);
150  else
152 }
153 
155 {
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];
165  {
168  }
169 
171  {
175  }
176  }
177  }
178 }
179 
181 {
187 
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  {
201 
202  btVector3 rayDir = (rayToWorld - rayFromWorld);
203 
204  rayDir.normalize();
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 
214  }
215 
216  virtual bool process(const btBroadphaseProxy* proxy)
217  {
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  {
244  collisionObject,
245  collisionObject->getCollisionShape(),
246  collisionObject->getWorldTransform(),
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");
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  {
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 
322 {
323  int i;
324  //serialize all collision objects
325  for (i = 0; i < m_collisionObjects.size(); i++)
326  {
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 
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 }
#define btAlignedFree(ptr)
#define btAlignedAlloc(size, alignment)
#define BT_PROFILE(name)
Definition: btQuickprof.h:198
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define btAssert(x)
Definition: btScalar.h:153
#define BT_SOFTBODY_CODE
Definition: btSerializer.h:110
int size() const
return the number of elements in the array
void remove(const T &key)
void push_back(const T &_Val)
The btBroadphaseInterface class provides an interface to detect aabb-overlapping object pairs.
virtual void rayTest(const btVector3 &rayFrom, const btVector3 &rayTo, btBroadphaseRayCallback &rayCallback, const btVector3 &aabbMin=btVector3(0, 0, 0), const btVector3 &aabbMax=btVector3(0, 0, 0))=0
void * m_oldPtr
Definition: btSerializer.h:52
btCollisionConfiguration allows to configure Bullet collision detection stack allocator size,...
btCollisionObject can be used to manage collision detection objects.
virtual const char * serialize(void *dataBuffer, class btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
btTransform & getWorldTransform()
btBroadphaseProxy * getBroadphaseHandle()
int getInternalType() const
reserved for Bullet internal usage
virtual int calculateSerializeBufferSize() const
const btCollisionShape * getCollisionShape() const
The btCollisionShape class provides an interface for collision shapes that can be shared among btColl...
bool isSoftBody() const
virtual void getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const =0
getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
btBroadphaseInterface * m_broadphasePairCache
virtual void removeCollisionObject(btCollisionObject *collisionObject)
virtual void addCollisionObject(btCollisionObject *collisionObject, int collisionFilterGroup=btBroadphaseProxy::DefaultFilter, int collisionFilterMask=btBroadphaseProxy::AllFilter)
btAlignedObjectArray< btCollisionObject * > m_collisionObjects
virtual btIDebugDraw * getDebugDrawer()
int getNumCollisionObjects() const
btIDebugDraw * m_debugDrawer
void serializeContactManifolds(btSerializer *serializer)
static void rayTestSingle(const btTransform &rayFromTrans, const btTransform &rayToTrans, btCollisionObject *collisionObject, const btCollisionShape *collisionShape, const btTransform &colObjWorldTransform, RayResultCallback &resultCallback)
rayTestSingle performs a raycast call and calls the resultCallback.
void serializeCollisionObjects(btSerializer *serializer)
void serializeRigidBodies(btSerializer *serializer)
virtual void internalSingleStepSimulation(btScalar timeStep)
void serializeDynamicsWorldInfo(btSerializer *serializer)
virtual void removeCollisionObject(btCollisionObject *collisionObject)
removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise ca...
virtual void predictUnconstraintMotion(btScalar timeStep)
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:77
virtual int getDebugMode() const =0
The btMultiBodyDynamicsWorld adds Featherstone multi body dynamics to Bullet This implementation is s...
virtual void serializeMultiBodies(btSerializer *serializer)
virtual btChunk * allocate(size_t size, int numElements)=0
virtual void finishSerialization()=0
virtual void startSerialization()=0
virtual void finalizeChunk(btChunk *chunk, const char *structType, int chunkCode, void *oldPtr)=0
virtual void predictMotion(btScalar solverdt)=0
Predict motion of soft bodies into next timestep.
float getTimeScale()
Return the timescale that the simulation is using.
virtual bool checkInitialized()=0
Ensure that this solver is initialized.
virtual ~btSoftBodySolver()
virtual void updateSoftBodies()=0
Perform necessary per-step updates of soft bodies such as recomputing normals and bounding boxes.
virtual void solveConstraints(btScalar solverdt)=0
Solve constraints for a set of soft bodies.
virtual void optimize(btAlignedObjectArray< btSoftBody * > &softBodies, bool forceUpdate=false)=0
Optimize soft bodies in this solver.
The btSoftBody is an class to simulate cloth and volumetric soft bodies.
Definition: btSoftBody.h:75
void interpolateRenderMesh()
void defaultCollisionHandler(const btCollisionObjectWrapper *pcoWrap)
bool rayTest(const btVector3 &rayFrom, const btVector3 &rayTo, sRayCast &results)
Ray casting using rayFrom and rayTo in worldspace, (not direction!)
void setSoftBodySolver(btSoftBodySolver *softBodySolver)
Definition: btSoftBody.h:1117
tFaceArray m_faces
Definition: btSoftBody.h:815
static void solveClusters(const btAlignedObjectArray< btSoftBody * > &bodies)
static const btSoftBody * upcast(const btCollisionObject *colObj)
Definition: btSoftBody.h:1142
btSoftBodySolver * m_softBodySolver
Solver classes that encapsulate multiple soft bodies for solving.
virtual void predictUnconstraintMotion(btScalar timeStep)
static void rayTestSingle(const btTransform &rayFromTrans, const btTransform &rayToTrans, btCollisionObject *collisionObject, const btCollisionShape *collisionShape, const btTransform &colObjWorldTransform, RayResultCallback &resultCallback)
rayTestSingle performs a raycast call and calls the resultCallback.
virtual void internalSingleStepSimulation(btScalar timeStep)
virtual void serialize(btSerializer *serializer)
Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (see B...
void solveSoftBodiesConstraints(btScalar timeStep)
virtual void removeCollisionObject(btCollisionObject *collisionObject)
removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise ca...
void addSoftBody(btSoftBody *body, int collisionFilterGroup=btBroadphaseProxy::DefaultFilter, int collisionFilterMask=btBroadphaseProxy::AllFilter)
virtual void rayTest(const btVector3 &rayFromWorld, const btVector3 &rayToWorld, RayResultCallback &resultCallback) const
rayTest performs a raycast on all objects in the btCollisionWorld, and calls the resultCallback This ...
btSoftMultiBodyDynamicsWorld(btDispatcher *dispatcher, btBroadphaseInterface *pairCache, btMultiBodyConstraintSolver *constraintSolver, btCollisionConfiguration *collisionConfiguration, btSoftBodySolver *softBodySolver=0)
void serializeSoftBodies(btSerializer *serializer)
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:166
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:146
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:303
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
The btBroadphaseProxy is the main class that can be used with the Bullet broadphases.
btVector3 m_rayDirectionInverse
added some cached data to accelerate ray-AABB tests
LocalShapeInfo gives extra information for complex shapes Currently, only btTriangleMeshShape is avai...
RayResultCallback is used to report new raycast results.
virtual bool needsCollision(btBroadphaseProxy *proxy0) const
virtual btScalar addSingleResult(LocalRayResult &rayResult, bool normalInWorldSpace)=0
static void DrawNodeTree(btSoftBody *psb, btIDebugDraw *idraw, int mindepth=0, int maxdepth=-1)
static void DrawFaceTree(btSoftBody *psb, btIDebugDraw *idraw, int mindepth=0, int maxdepth=-1)
static void DrawClusterTree(btSoftBody *psb, btIDebugDraw *idraw, int mindepth=0, int maxdepth=-1)
static void Draw(btSoftBody *psb, btIDebugDraw *idraw, int drawflags=fDrawFlags::Std)
static void DrawFrame(btSoftBody *psb, btIDebugDraw *idraw)
btScalar air_density
Definition: btSoftBody.h:49
btDispatcher * m_dispatcher
Definition: btSoftBody.h:55
btScalar water_density
Definition: btSoftBody.h:50
btSparseSdf< 3 > m_sparsesdf
Definition: btSoftBody.h:57
btVector3 m_gravity
Definition: btSoftBody.h:56
btVector3 water_normal
Definition: btSoftBody.h:53
btScalar water_offset
Definition: btSoftBody.h:51
btBroadphaseInterface * m_broadphase
Definition: btSoftBody.h:54
eFeature::_ feature
soft body
Definition: btSoftBody.h:204
btScalar fraction
feature index
Definition: btSoftBody.h:206
int index
feature type
Definition: btSoftBody.h:205
const btSoftMultiBodyDynamicsWorld * m_world
btSoftSingleRayCallback(const btVector3 &rayFromWorld, const btVector3 &rayToWorld, const btSoftMultiBodyDynamicsWorld *world, btCollisionWorld::RayResultCallback &resultCallback)
btCollisionWorld::RayResultCallback & m_resultCallback
virtual bool process(const btBroadphaseProxy *proxy)
void Reset()
Definition: btSparseSDF.h:116
void Initialize(int hashsize=2383, int clampCells=256 *1024)
Definition: btSparseSDF.h:100