bullet: Update to current svn, r2636
[blender.git] / extern / bullet2 / src / BulletSoftBody / btSoftRigidDynamicsWorld.cpp
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
17 #include "btSoftRigidDynamicsWorld.h"
18 #include "LinearMath/btQuickprof.h"
19
20 //softbody & helpers
21 #include "btSoftBody.h"
22 #include "btSoftBodyHelpers.h"
23 #include "btSoftBodySolvers.h"
24 #include "btDefaultSoftBodySolver.h"
25 #include "LinearMath/btSerializer.h"
26
27
28 btSoftRigidDynamicsWorld::btSoftRigidDynamicsWorld(
29         btDispatcher* dispatcher,
30         btBroadphaseInterface* pairCache,
31         btConstraintSolver* constraintSolver,
32         btCollisionConfiguration* collisionConfiguration,
33         btSoftBodySolver *softBodySolver ) : 
34                 btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration),
35                 m_softBodySolver( softBodySolver ),
36                 m_ownsSolver(false)
37 {
38         if( !m_softBodySolver )
39         {
40                 void* ptr = btAlignedAlloc(sizeof(btDefaultSoftBodySolver),16);
41                 m_softBodySolver = new(ptr) btDefaultSoftBodySolver();
42                 m_ownsSolver = true;
43         }
44
45         m_drawFlags                     =       fDrawFlags::Std;
46         m_drawNodeTree          =       true;
47         m_drawFaceTree          =       false;
48         m_drawClusterTree       =       false;
49         m_sbi.m_broadphase = pairCache;
50         m_sbi.m_dispatcher = dispatcher;
51         m_sbi.m_sparsesdf.Initialize();
52         m_sbi.m_sparsesdf.Reset();
53
54         m_sbi.air_density               =       (btScalar)1.2;
55         m_sbi.water_density     =       0;
56         m_sbi.water_offset              =       0;
57         m_sbi.water_normal              =       btVector3(0,0,0);
58         m_sbi.m_gravity.setValue(0,-10,0);
59
60         m_sbi.m_sparsesdf.Initialize();
61
62
63 }
64
65 btSoftRigidDynamicsWorld::~btSoftRigidDynamicsWorld()
66 {
67         if (m_ownsSolver)
68         {
69                 m_softBodySolver->~btSoftBodySolver();
70                 btAlignedFree(m_softBodySolver);
71         }
72 }
73
74 void    btSoftRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
75 {
76         btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep );
77         {
78                 BT_PROFILE("predictUnconstraintMotionSoftBody");
79                 m_softBodySolver->predictMotion( timeStep );
80         }
81 }
82
83 void    btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep )
84 {
85
86         // Let the solver grab the soft bodies and if necessary optimize for it
87         m_softBodySolver->optimize( getSoftBodyArray() );
88
89         if( !m_softBodySolver->checkInitialized() )
90         {
91                 btAssert( "Solver initialization failed\n" );
92         }
93
94         btDiscreteDynamicsWorld::internalSingleStepSimulation( timeStep );
95
96         ///solve soft bodies constraints
97         solveSoftBodiesConstraints( timeStep );
98
99         //self collisions
100         for ( int i=0;i<m_softBodies.size();i++)
101         {
102                 btSoftBody*     psb=(btSoftBody*)m_softBodies[i];
103                 psb->defaultCollisionHandler(psb);
104         }
105
106         ///update soft bodies
107         m_softBodySolver->updateSoftBodies( );
108         
109         // End solver-wise simulation step
110         // ///////////////////////////////
111
112 }
113
114 void    btSoftRigidDynamicsWorld::solveSoftBodiesConstraints( btScalar timeStep )
115 {
116         BT_PROFILE("solveSoftConstraints");
117
118         if(m_softBodies.size())
119         {
120                 btSoftBody::solveClusters(m_softBodies);
121         }
122
123         // Solve constraints solver-wise
124         m_softBodySolver->solveConstraints( timeStep * m_softBodySolver->getTimeScale() );
125
126 }
127
128 void    btSoftRigidDynamicsWorld::addSoftBody(btSoftBody* body,short int collisionFilterGroup,short int collisionFilterMask)
129 {
130         m_softBodies.push_back(body);
131
132         // Set the soft body solver that will deal with this body
133         // to be the world's solver
134         body->setSoftBodySolver( m_softBodySolver );
135
136         btCollisionWorld::addCollisionObject(body,
137                 collisionFilterGroup,
138                 collisionFilterMask);
139
140 }
141
142 void    btSoftRigidDynamicsWorld::removeSoftBody(btSoftBody* body)
143 {
144         m_softBodies.remove(body);
145
146         btCollisionWorld::removeCollisionObject(body);
147 }
148
149 void    btSoftRigidDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
150 {
151         btSoftBody* body = btSoftBody::upcast(collisionObject);
152         if (body)
153                 removeSoftBody(body);
154         else
155                 btDiscreteDynamicsWorld::removeCollisionObject(collisionObject);
156 }
157
158 void    btSoftRigidDynamicsWorld::debugDrawWorld()
159 {
160         btDiscreteDynamicsWorld::debugDrawWorld();
161
162         if (getDebugDrawer())
163         {
164                 int i;
165                 for (  i=0;i<this->m_softBodies.size();i++)
166                 {
167                         btSoftBody*     psb=(btSoftBody*)this->m_softBodies[i];
168                         if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
169                         {
170                                 btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer);
171                                 btSoftBodyHelpers::Draw(psb,m_debugDrawer,m_drawFlags);
172                         }
173                         
174                         if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
175                         {
176                                 if(m_drawNodeTree)              btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer);
177                                 if(m_drawFaceTree)              btSoftBodyHelpers::DrawFaceTree(psb,m_debugDrawer);
178                                 if(m_drawClusterTree)   btSoftBodyHelpers::DrawClusterTree(psb,m_debugDrawer);
179                         }
180                 }               
181         }       
182 }
183
184
185
186
187 struct btSoftSingleRayCallback : public btBroadphaseRayCallback
188 {
189         btVector3       m_rayFromWorld;
190         btVector3       m_rayToWorld;
191         btTransform     m_rayFromTrans;
192         btTransform     m_rayToTrans;
193         btVector3       m_hitNormal;
194
195         const btSoftRigidDynamicsWorld* m_world;
196         btCollisionWorld::RayResultCallback&    m_resultCallback;
197
198         btSoftSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btSoftRigidDynamicsWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
199         :m_rayFromWorld(rayFromWorld),
200         m_rayToWorld(rayToWorld),
201         m_world(world),
202         m_resultCallback(resultCallback)
203         {
204                 m_rayFromTrans.setIdentity();
205                 m_rayFromTrans.setOrigin(m_rayFromWorld);
206                 m_rayToTrans.setIdentity();
207                 m_rayToTrans.setOrigin(m_rayToWorld);
208
209                 btVector3 rayDir = (rayToWorld-rayFromWorld);
210
211                 rayDir.normalize ();
212                 ///what about division by zero? --> just set rayDirection[i] to INF/1e30
213                 m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
214                 m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
215                 m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
216                 m_signs[0] = m_rayDirectionInverse[0] < 0.0;
217                 m_signs[1] = m_rayDirectionInverse[1] < 0.0;
218                 m_signs[2] = m_rayDirectionInverse[2] < 0.0;
219
220                 m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld);
221
222         }
223
224         
225
226         virtual bool    process(const btBroadphaseProxy* proxy)
227         {
228                 ///terminate further ray tests, once the closestHitFraction reached zero
229                 if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
230                         return false;
231
232                 btCollisionObject*      collisionObject = (btCollisionObject*)proxy->m_clientObject;
233
234                 //only perform raycast if filterMask matches
235                 if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) 
236                 {
237                         //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
238                         //btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
239 #if 0
240 #ifdef RECALCULATE_AABB
241                         btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
242                         collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
243 #else
244                         //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
245                         const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
246                         const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
247 #endif
248 #endif
249                         //btScalar hitLambda = m_resultCallback.m_closestHitFraction;
250                         //culling already done by broadphase
251                         //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
252                         {
253                                 m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
254                                         collisionObject,
255                                                 collisionObject->getCollisionShape(),
256                                                 collisionObject->getWorldTransform(),
257                                                 m_resultCallback);
258                         }
259                 }
260                 return true;
261         }
262 };
263
264 void    btSoftRigidDynamicsWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
265 {
266         BT_PROFILE("rayTest");
267         /// use the broadphase to accelerate the search for objects, based on their aabb
268         /// and for each object with ray-aabb overlap, perform an exact ray test
269         btSoftSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback);
270
271 #ifndef USE_BRUTEFORCE_RAYBROADPHASE
272         m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB);
273 #else
274         for (int i=0;i<this->getNumCollisionObjects();i++)
275         {
276                 rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
277         }       
278 #endif //USE_BRUTEFORCE_RAYBROADPHASE
279
280 }
281
282
283 void    btSoftRigidDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
284                                           btCollisionObject* collisionObject,
285                                           const btCollisionShape* collisionShape,
286                                           const btTransform& colObjWorldTransform,
287                                           RayResultCallback& resultCallback)
288 {
289         if (collisionShape->isSoftBody()) {
290                 btSoftBody* softBody = btSoftBody::upcast(collisionObject);
291                 if (softBody) {
292                         btSoftBody::sRayCast softResult;
293                         if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) 
294                         {
295                                 
296                                 if (softResult.fraction<= resultCallback.m_closestHitFraction)
297                                 {
298
299                                         btCollisionWorld::LocalShapeInfo shapeInfo;
300                                         shapeInfo.m_shapePart = 0;
301                                         shapeInfo.m_triangleIndex = softResult.index;
302                                         // get the normal
303                                         btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin();
304                                         btVector3 normal=-rayDir;
305                                         normal.normalize();
306
307                                         if (softResult.feature == btSoftBody::eFeature::Face)
308                                         {
309                                                 normal = softBody->m_faces[softResult.index].m_normal;
310                                                 if (normal.dot(rayDir) > 0) {
311                                                         // normal always point toward origin of the ray
312                                                         normal = -normal;
313                                                 }
314                                         }
315         
316                                         btCollisionWorld::LocalRayResult rayResult
317                                                 (collisionObject,
318                                                  &shapeInfo,
319                                                  normal,
320                                                  softResult.fraction);
321                                         bool    normalInWorldSpace = true;
322                                         resultCallback.addSingleResult(rayResult,normalInWorldSpace);
323                                 }
324                         }
325                 }
326         } 
327         else {
328                 btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans,collisionObject,collisionShape,colObjWorldTransform,resultCallback);
329         }
330 }
331
332
333 void    btSoftRigidDynamicsWorld::serializeSoftBodies(btSerializer* serializer)
334 {
335         int i;
336         //serialize all collision objects
337         for (i=0;i<m_collisionObjects.size();i++)
338         {
339                 btCollisionObject* colObj = m_collisionObjects[i];
340                 if (colObj->getInternalType() & btCollisionObject::CO_SOFT_BODY)
341                 {
342                         int len = colObj->calculateSerializeBufferSize();
343                         btChunk* chunk = serializer->allocate(len,1);
344                         const char* structType = colObj->serialize(chunk->m_oldPtr, serializer);
345                         serializer->finalizeChunk(chunk,structType,BT_SOFTBODY_CODE,colObj);
346                 }
347         }
348
349 }
350
351 void    btSoftRigidDynamicsWorld::serialize(btSerializer* serializer)
352 {
353
354         serializer->startSerialization();
355
356         serializeDynamicsWorldInfo( serializer);
357
358         serializeSoftBodies(serializer);
359
360         serializeRigidBodies(serializer);
361
362         serializeCollisionObjects(serializer);
363
364         serializer->finishSerialization();
365 }
366
367