remove bullet CMakeLists.txt files, blender uses its own.
[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
24
25 //#define USE_BRUTEFORCE_RAYBROADPHASE 1
26
27
28
29 btSoftRigidDynamicsWorld::btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
30 :btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration)
31 {
32         m_drawFlags                     =       fDrawFlags::Std;
33         m_drawNodeTree          =       true;
34         m_drawFaceTree          =       false;
35         m_drawClusterTree       =       false;
36         m_sbi.m_broadphase = pairCache;
37         m_sbi.m_dispatcher = dispatcher;
38         m_sbi.m_sparsesdf.Initialize();
39         m_sbi.m_sparsesdf.Reset();
40
41 }
42
43 btSoftRigidDynamicsWorld::~btSoftRigidDynamicsWorld()
44 {
45
46 }
47
48 void    btSoftRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
49 {
50         btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep);
51
52         for ( int i=0;i<m_softBodies.size();++i)
53         {
54                 btSoftBody*     psb= m_softBodies[i];
55
56                 psb->predictMotion(timeStep);           
57         }
58 }
59
60 void    btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep)
61 {
62         btDiscreteDynamicsWorld::internalSingleStepSimulation( timeStep );
63
64         ///solve soft bodies constraints
65         solveSoftBodiesConstraints();
66
67         //self collisions
68         for ( int i=0;i<m_softBodies.size();i++)
69         {
70                 btSoftBody*     psb=(btSoftBody*)m_softBodies[i];
71                 psb->defaultCollisionHandler(psb);
72         }
73
74         ///update soft bodies
75         updateSoftBodies();
76
77 }
78
79 void    btSoftRigidDynamicsWorld::updateSoftBodies()
80 {
81         BT_PROFILE("updateSoftBodies");
82
83         for ( int i=0;i<m_softBodies.size();i++)
84         {
85                 btSoftBody*     psb=(btSoftBody*)m_softBodies[i];
86                 psb->integrateMotion(); 
87         }
88 }
89
90 void    btSoftRigidDynamicsWorld::solveSoftBodiesConstraints()
91 {
92         BT_PROFILE("solveSoftConstraints");
93
94         if(m_softBodies.size())
95         {
96                 btSoftBody::solveClusters(m_softBodies);
97         }
98
99         for(int i=0;i<m_softBodies.size();++i)
100         {
101                 btSoftBody*     psb=(btSoftBody*)m_softBodies[i];
102                 psb->solveConstraints();
103         }       
104 }
105
106 void    btSoftRigidDynamicsWorld::addSoftBody(btSoftBody* body)
107 {
108         m_softBodies.push_back(body);
109
110         btCollisionWorld::addCollisionObject(body,
111                 btBroadphaseProxy::DefaultFilter,
112                 btBroadphaseProxy::AllFilter);
113
114 }
115
116 void    btSoftRigidDynamicsWorld::removeSoftBody(btSoftBody* body)
117 {
118         m_softBodies.remove(body);
119
120         btCollisionWorld::removeCollisionObject(body);
121 }
122
123 void    btSoftRigidDynamicsWorld::debugDrawWorld()
124 {
125         btDiscreteDynamicsWorld::debugDrawWorld();
126
127         if (getDebugDrawer())
128         {
129                 int i;
130                 for (  i=0;i<this->m_softBodies.size();i++)
131                 {
132                         btSoftBody*     psb=(btSoftBody*)this->m_softBodies[i];
133                         btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer);
134                         btSoftBodyHelpers::Draw(psb,m_debugDrawer,m_drawFlags);
135                         if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
136                         {
137                                 if(m_drawNodeTree)              btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer);
138                                 if(m_drawFaceTree)              btSoftBodyHelpers::DrawFaceTree(psb,m_debugDrawer);
139                                 if(m_drawClusterTree)   btSoftBodyHelpers::DrawClusterTree(psb,m_debugDrawer);
140                         }
141                 }               
142         }       
143 }
144
145
146 struct btSoftSingleRayCallback : public btBroadphaseRayCallback
147 {
148         btVector3       m_rayFromWorld;
149         btVector3       m_rayToWorld;
150         btTransform     m_rayFromTrans;
151         btTransform     m_rayToTrans;
152         btVector3       m_hitNormal;
153
154         const btSoftRigidDynamicsWorld* m_world;
155         btCollisionWorld::RayResultCallback&    m_resultCallback;
156
157         btSoftSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btSoftRigidDynamicsWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
158         :m_rayFromWorld(rayFromWorld),
159         m_rayToWorld(rayToWorld),
160         m_world(world),
161         m_resultCallback(resultCallback)
162         {
163                 m_rayFromTrans.setIdentity();
164                 m_rayFromTrans.setOrigin(m_rayFromWorld);
165                 m_rayToTrans.setIdentity();
166                 m_rayToTrans.setOrigin(m_rayToWorld);
167
168                 btVector3 rayDir = (rayToWorld-rayFromWorld);
169
170                 rayDir.normalize ();
171                 ///what about division by zero? --> just set rayDirection[i] to INF/1e30
172                 m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
173                 m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
174                 m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
175                 m_signs[0] = m_rayDirectionInverse[0] < 0.0;
176                 m_signs[1] = m_rayDirectionInverse[1] < 0.0;
177                 m_signs[2] = m_rayDirectionInverse[2] < 0.0;
178
179                 m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld);
180
181         }
182
183         
184
185         virtual bool    process(const btBroadphaseProxy* proxy)
186         {
187                 ///terminate further ray tests, once the closestHitFraction reached zero
188                 if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
189                         return false;
190
191                 btCollisionObject*      collisionObject = (btCollisionObject*)proxy->m_clientObject;
192
193                 //only perform raycast if filterMask matches
194                 if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) 
195                 {
196                         //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
197                         //btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
198 #if 0
199 #ifdef RECALCULATE_AABB
200                         btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
201                         collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
202 #else
203                         //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
204                         const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
205                         const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
206 #endif
207 #endif
208                         //btScalar hitLambda = m_resultCallback.m_closestHitFraction;
209                         //culling already done by broadphase
210                         //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
211                         {
212                                 m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
213                                         collisionObject,
214                                                 collisionObject->getCollisionShape(),
215                                                 collisionObject->getWorldTransform(),
216                                                 m_resultCallback);
217                         }
218                 }
219                 return true;
220         }
221 };
222
223 void    btSoftRigidDynamicsWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
224 {
225         BT_PROFILE("rayTest");
226         /// use the broadphase to accelerate the search for objects, based on their aabb
227         /// and for each object with ray-aabb overlap, perform an exact ray test
228         btSoftSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback);
229
230 #ifndef USE_BRUTEFORCE_RAYBROADPHASE
231         m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB);
232 #else
233         for (int i=0;i<this->getNumCollisionObjects();i++)
234         {
235                 rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
236         }       
237 #endif //USE_BRUTEFORCE_RAYBROADPHASE
238
239 }
240
241
242 void    btSoftRigidDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
243                                           btCollisionObject* collisionObject,
244                                           const btCollisionShape* collisionShape,
245                                           const btTransform& colObjWorldTransform,
246                                           RayResultCallback& resultCallback)
247 {
248         if (collisionShape->isSoftBody()) {
249                 btSoftBody* softBody = btSoftBody::upcast(collisionObject);
250                 if (softBody) {
251                         btSoftBody::sRayCast softResult;
252                         if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) 
253                         {
254                                 if (softResult.fraction<= resultCallback.m_closestHitFraction) 
255                                 {
256                                         btCollisionWorld::LocalShapeInfo shapeInfo;
257                                         shapeInfo.m_shapePart = 0;
258                                         shapeInfo.m_triangleIndex = softResult.index;
259                                         // get the normal
260                                         btVector3 normal = softBody->m_faces[softResult.index].m_normal;
261                                         btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin();
262                                         if (normal.dot(rayDir) > 0) {
263                                                 // normal must always point toward origin of the ray
264                                                 normal = -normal;
265                                         }
266                                         btCollisionWorld::LocalRayResult rayResult
267                                                 (collisionObject,
268                                                 &shapeInfo,
269                                                 normal,
270                                                 softResult.fraction);
271                                         bool    normalInWorldSpace = true;
272                                         resultCallback.addSingleResult(rayResult,normalInWorldSpace);
273                                 }
274                         }
275                 }
276         } 
277         else {
278                 btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans,collisionObject,collisionShape,colObjWorldTransform,resultCallback);
279         }
280 }