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