BGE real-time soft bodies, step 2 / 3: create a btSoftBody. Next step is hooking...
[blender-staging.git] / extern / bullet2 / src / BulletCollision / BroadphaseCollision / btDbvtBroadphase.cpp
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2007 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 ///btDbvtBroadphase implementation by Nathanael Presson
16
17 #include "btDbvtBroadphase.h"
18
19 //
20 // Profiling
21 //
22
23 #if DBVT_BP_PROFILE||DBVT_BP_ENABLE_BENCHMARK
24 #include <stdio.h>
25 #endif
26
27 #if DBVT_BP_PROFILE
28 struct  ProfileScope
29         {
30         __forceinline ProfileScope(btClock& clock,unsigned long& value) :
31                 m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds())
32                 {
33                 }
34         __forceinline ~ProfileScope()
35                 {
36                 (*m_value)+=m_clock->getTimeMicroseconds()-m_base;
37                 }
38         btClock*                m_clock;
39         unsigned long*  m_value;
40         unsigned long   m_base;
41         };
42 #define SPC(_value_)    ProfileScope    spc_scope(m_clock,_value_)
43 #else
44 #define SPC(_value_)
45 #endif
46
47 //
48 // Helpers
49 //
50
51 //
52 template <typename T>
53 static inline void      listappend(T* item,T*& list)
54 {
55 item->links[0]=0;
56 item->links[1]=list;
57 if(list) list->links[0]=item;
58 list=item;
59 }
60
61 //
62 template <typename T>
63 static inline void      listremove(T* item,T*& list)
64 {
65 if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1];
66 if(item->links[1]) item->links[1]->links[0]=item->links[0];
67 }
68
69 //
70 template <typename T>
71 static inline int       listcount(T* root)
72 {
73 int     n=0;
74 while(root) { ++n;root=root->links[1]; }
75 return(n);
76 }
77
78 //
79 template <typename T>
80 static inline void      clear(T& value)
81 {
82 static const struct ZeroDummy : T {} zerodummy;
83 value=zerodummy;
84 }
85
86 //
87 // Colliders
88 //
89
90 /* Tree collider        */ 
91 struct  btDbvtTreeCollider : btDbvt::ICollide
92 {
93 btDbvtBroadphase*       pbp;
94 btDbvtProxy*            proxy;
95                 btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {}
96 void    Process(const btDbvtNode* na,const btDbvtNode* nb)
97         {
98         if(na!=nb)
99                 {
100                 btDbvtProxy*    pa=(btDbvtProxy*)na->data;
101                 btDbvtProxy*    pb=(btDbvtProxy*)nb->data;
102                 #if DBVT_BP_SORTPAIRS
103                 if(pa>pb) btSwap(pa,pb);
104                 #endif
105                 pbp->m_paircache->addOverlappingPair(pa,pb);
106                 ++pbp->m_newpairs;
107                 }
108         }
109 void    Process(const btDbvtNode* n)
110         {
111         Process(n,proxy->leaf);
112         }
113 };
114
115 //
116 // btDbvtBroadphase
117 //
118
119 //
120 btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache)
121 {
122 m_deferedcollide        =       false;
123 m_needcleanup           =       true;
124 m_releasepaircache      =       (paircache!=0)?false:true;
125 m_prediction            =       1/(btScalar)2;
126 m_stageCurrent          =       0;
127 m_fixedleft                     =       0;
128 m_fupdates                      =       1;
129 m_dupdates                      =       0;
130 m_cupdates                      =       10;
131 m_newpairs                      =       1;
132 m_updates_call          =       0;
133 m_updates_done          =       0;
134 m_updates_ratio         =       0;
135 m_paircache                     =       paircache?
136                                                 paircache       :
137                                                 new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
138 m_gid                           =       0;
139 m_pid                           =       0;
140 m_cid                           =       0;
141 for(int i=0;i<=STAGECOUNT;++i)
142         {
143         m_stageRoots[i]=0;
144         }
145 #if DBVT_BP_PROFILE
146 clear(m_profiling);
147 #endif
148 }
149
150 //
151 btDbvtBroadphase::~btDbvtBroadphase()
152 {
153 if(m_releasepaircache) 
154 {
155         m_paircache->~btOverlappingPairCache();
156         btAlignedFree(m_paircache);
157 }
158 }
159
160 //
161 btBroadphaseProxy*                              btDbvtBroadphase::createProxy(  const btVector3& aabbMin,
162                                                                                                                                 const btVector3& aabbMax,
163                                                                                                                                 int /*shapeType*/,
164                                                                                                                                 void* userPtr,
165                                                                                                                                 short int collisionFilterGroup,
166                                                                                                                                 short int collisionFilterMask,
167                                                                                                                                 btDispatcher* /*dispatcher*/,
168                                                                                                                                 void* /*multiSapProxy*/)
169 {
170 btDbvtProxy*            proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy(  userPtr,
171                                                                                                                                                                         collisionFilterGroup,
172                                                                                                                                                                         collisionFilterMask);
173 proxy->aabb                     =       btDbvtVolume::FromMM(aabbMin,aabbMax);
174 proxy->stage            =       m_stageCurrent;
175 proxy->m_uniqueId       =       ++m_gid;
176 proxy->leaf                     =       m_sets[0].insert(proxy->aabb,proxy);
177 listappend(proxy,m_stageRoots[m_stageCurrent]);
178 if(!m_deferedcollide)
179         {
180         btDbvtTreeCollider      collider(this);
181         collider.proxy=proxy;
182         btDbvt::collideTV(m_sets[0].m_root,proxy->aabb,collider);
183         btDbvt::collideTV(m_sets[1].m_root,proxy->aabb,collider);
184         }
185 return(proxy);
186 }
187
188 //
189 void                                                    btDbvtBroadphase::destroyProxy( btBroadphaseProxy* absproxy,
190                                                                                                                                 btDispatcher* dispatcher)
191 {
192 btDbvtProxy*    proxy=(btDbvtProxy*)absproxy;
193 if(proxy->stage==STAGECOUNT)
194         m_sets[1].remove(proxy->leaf);
195         else
196         m_sets[0].remove(proxy->leaf);
197 listremove(proxy,m_stageRoots[proxy->stage]);
198 m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher);
199 btAlignedFree(proxy);
200 m_needcleanup=true;
201 }
202
203 //
204 void                                                    btDbvtBroadphase::setAabb(              btBroadphaseProxy* absproxy,
205                                                                                                                                 const btVector3& aabbMin,
206                                                                                                                                 const btVector3& aabbMax,
207                                                                                                                                 btDispatcher* /*dispatcher*/)
208 {
209 btDbvtProxy*                                            proxy=(btDbvtProxy*)absproxy;
210 ATTRIBUTE_ALIGNED16(btDbvtVolume)       aabb=btDbvtVolume::FromMM(aabbMin,aabbMax);
211 #if DBVT_BP_PREVENTFALSEUPDATE
212 if(NotEqual(aabb,proxy->leaf->volume))
213 #endif
214         {
215         bool    docollide=false;
216         if(proxy->stage==STAGECOUNT)
217                 {/* fixed -> dynamic set        */ 
218                 m_sets[1].remove(proxy->leaf);
219                 proxy->leaf=m_sets[0].insert(aabb,proxy);
220                 docollide=true;
221                 }
222                 else
223                 {/* dynamic set                         */ 
224                 ++m_updates_call;
225                 if(Intersect(proxy->leaf->volume,aabb))
226                         {/* Moving                              */ 
227                         const btVector3 delta=aabbMin-proxy->aabb.Mins();
228                         btVector3               velocity(aabb.Extents()*m_prediction);
229                         if(delta[0]<0) velocity[0]=-velocity[0];
230                         if(delta[1]<0) velocity[1]=-velocity[1];
231                         if(delta[2]<0) velocity[2]=-velocity[2];
232                         if      (
233                                 #ifdef DBVT_BP_MARGIN                           
234                                 m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN)
235                                 #else
236                                 m_sets[0].update(proxy->leaf,aabb,velocity)
237                                 #endif
238                                 )
239                                 {
240                                 ++m_updates_done;
241                                 docollide=true;
242                                 }
243                         }
244                         else
245                         {/* Teleporting                 */ 
246                         m_sets[0].update(proxy->leaf,aabb);
247                         ++m_updates_done;
248                         docollide=true;
249                         }       
250                 }
251         listremove(proxy,m_stageRoots[proxy->stage]);
252         proxy->aabb             =       aabb;
253         proxy->stage    =       m_stageCurrent;
254         listappend(proxy,m_stageRoots[m_stageCurrent]);
255         if(docollide)
256                 {
257                 m_needcleanup=true;
258                 if(!m_deferedcollide)
259                         {
260                         btDbvtTreeCollider      collider(this);
261                         btDbvt::collideTT(m_sets[1].m_root,proxy->leaf,collider);
262                         btDbvt::collideTT(m_sets[0].m_root,proxy->leaf,collider);
263                         }
264                 }       
265         }
266 }
267
268 //
269 void                                                    btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
270 {
271 collide(dispatcher);
272 #if DBVT_BP_PROFILE
273 if(0==(m_pid%DBVT_BP_PROFILING_RATE))
274         {       
275         printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leaves,m_sets[0].m_leaves,m_paircache->getNumOverlappingPairs());
276         unsigned int    total=m_profiling.m_total;
277         if(total<=0) total=1;
278         printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/DBVT_BP_PROFILING_RATE);
279         printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/DBVT_BP_PROFILING_RATE);
280         printf("cleanup:   %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/DBVT_BP_PROFILING_RATE);
281         printf("total:     %uus\r\n",total/DBVT_BP_PROFILING_RATE);
282         const unsigned long     sum=m_profiling.m_ddcollide+
283                                                         m_profiling.m_fdcollide+
284                                                         m_profiling.m_cleanup;
285         printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/DBVT_BP_PROFILING_RATE);
286         printf("job counts: %u%%\r\n",(m_profiling.m_jobcount*100)/((m_sets[0].m_leaves+m_sets[1].m_leaves)*DBVT_BP_PROFILING_RATE));
287         clear(m_profiling);
288         m_clock.reset();
289         }
290 #endif
291 }
292
293 //
294 void                                                    btDbvtBroadphase::collide(btDispatcher* dispatcher)
295 {
296 SPC(m_profiling.m_total);
297 /* optimize                             */ 
298 m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100);
299 if(m_fixedleft)
300         {
301         const int count=1+(m_sets[1].m_leaves*m_fupdates)/100;
302         m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100);
303         m_fixedleft=btMax<int>(0,m_fixedleft-count);
304         }
305 /* dynamic -> fixed set */ 
306 m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT;
307 btDbvtProxy*    current=m_stageRoots[m_stageCurrent];
308 if(current)
309         {
310         btDbvtTreeCollider      collider(this);
311         do      {
312                 btDbvtProxy*    next=current->links[1];
313                 listremove(current,m_stageRoots[current->stage]);
314                 listappend(current,m_stageRoots[STAGECOUNT]);
315                 #if DBVT_BP_ACCURATESLEEPING
316                 m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher);
317                 collider.proxy=current;
318                 btDbvt::collideTV(m_sets[0].m_root,current->aabb,collider);
319                 btDbvt::collideTV(m_sets[1].m_root,current->aabb,collider);
320                 #endif
321                 m_sets[0].remove(current->leaf);
322                 current->leaf   =       m_sets[1].insert(current->aabb,current);
323                 current->stage  =       STAGECOUNT;     
324                 current                 =       next;
325                 } while(current);
326         m_fixedleft=m_sets[1].m_leaves;
327         m_needcleanup=true;
328         }
329 /* collide dynamics             */ 
330         {
331         btDbvtTreeCollider      collider(this);
332         if(m_deferedcollide)
333                 {
334                 SPC(m_profiling.m_fdcollide);
335                 btDbvt::collideTT(m_sets[0].m_root,m_sets[1].m_root,collider);
336                 }
337         if(m_deferedcollide)
338                 {
339                 SPC(m_profiling.m_ddcollide);
340                 btDbvt::collideTT(m_sets[0].m_root,m_sets[0].m_root,collider);
341                 }
342         }
343 /* clean up                             */ 
344 if(m_needcleanup)
345         {
346         SPC(m_profiling.m_cleanup);
347         btBroadphasePairArray&  pairs=m_paircache->getOverlappingPairArray();
348         if(pairs.size()>0)
349                 {
350                 const int       ci=pairs.size();
351                 int                     ni=btMin(ci,btMax<int>(m_newpairs,(ci*m_cupdates)/100));
352                 for(int i=0;i<ni;++i)
353                         {
354                         btBroadphasePair&       p=pairs[(m_cid+i)%ci];
355                         btDbvtProxy*            pa=(btDbvtProxy*)p.m_pProxy0;
356                         btDbvtProxy*            pb=(btDbvtProxy*)p.m_pProxy1;
357                         if(!Intersect(pa->leaf->volume,pb->leaf->volume))
358                                 {
359                                 #if DBVT_BP_SORTPAIRS
360                                 if(pa>pb) btSwap(pa,pb);
361                                 #endif
362                                 m_paircache->removeOverlappingPair(pa,pb,dispatcher);
363                                 --ni;--i;
364                                 }
365                         }
366                 if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0;
367                 }
368         }
369 ++m_pid;
370 m_newpairs=1;
371 m_needcleanup=false;
372 if(m_updates_call>0)
373         { m_updates_ratio=m_updates_done/(btScalar)m_updates_call; }
374         else
375         { m_updates_ratio=0; }
376 m_updates_done/=2;
377 m_updates_call/=2;
378 }
379
380 //
381 void                                                    btDbvtBroadphase::optimize()
382 {
383 m_sets[0].optimizeTopDown();
384 m_sets[1].optimizeTopDown();
385 }
386
387 //
388 btOverlappingPairCache*                 btDbvtBroadphase::getOverlappingPairCache()
389 {
390 return(m_paircache);
391 }
392
393 //
394 const btOverlappingPairCache*   btDbvtBroadphase::getOverlappingPairCache() const
395 {
396 return(m_paircache);
397 }
398
399 //
400 void                                                    btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
401 {
402
403         ATTRIBUTE_ALIGNED16(btDbvtVolume)       bounds;
404
405 if(!m_sets[0].empty())
406         if(!m_sets[1].empty())  Merge(  m_sets[0].m_root->volume,
407                                                                         m_sets[1].m_root->volume,bounds);
408                                                         else
409                                                         bounds=m_sets[0].m_root->volume;
410 else if(!m_sets[1].empty())     bounds=m_sets[1].m_root->volume;
411                                                         else
412                                                         bounds=btDbvtVolume::FromCR(btVector3(0,0,0),0);
413 aabbMin=bounds.Mins();
414 aabbMax=bounds.Maxs();
415 }
416
417 //
418 void                                                    btDbvtBroadphase::printStats()
419 {}
420
421 //
422 #if DBVT_BP_ENABLE_BENCHMARK
423
424 struct  btBroadphaseBenchmark
425         {
426         struct  Experiment
427                 {
428                 const char*                     name;
429                 int                                     object_count;
430                 int                                     update_count;
431                 int                                     spawn_count;
432                 int                                     iterations;
433                 btScalar                        speed;
434                 btScalar                        amplitude;
435                 };
436         struct  Object
437                 {
438                 btVector3                       center;
439                 btVector3                       extents;
440                 btBroadphaseProxy*      proxy;
441                 btScalar                        time;
442                 void                            update(btScalar speed,btScalar amplitude,btBroadphaseInterface* pbi)
443                         {
444                         time            +=      speed;
445                         center[0]       =       btCos(time*(btScalar)2.17)*amplitude+
446                                                         btSin(time)*amplitude/2;
447                         center[1]       =       btCos(time*(btScalar)1.38)*amplitude+
448                                                         btSin(time)*amplitude;
449                         center[2]       =       btSin(time*(btScalar)0.777)*amplitude;
450                         pbi->setAabb(proxy,center-extents,center+extents,0);
451                         }
452                 };
453         static int              UnsignedRand(int range=RAND_MAX-1)      { return(rand()%(range+1)); }
454         static btScalar UnitRand()                                                      { return(UnsignedRand(16384)/(btScalar)16384); }
455         static void             OutputTime(const char* name,btClock& c,unsigned count=0)
456                 {
457                 const unsigned long     us=c.getTimeMicroseconds();
458                 const unsigned long     ms=(us+500)/1000;
459                 const btScalar          sec=us/(btScalar)(1000*1000);
460                 if(count>0)
461                         printf("%s : %u us (%u ms), %.2f/s\r\n",name,us,ms,count/sec);
462                         else
463                         printf("%s : %u us (%u ms)\r\n",name,us,ms);
464                 }
465         };
466
467 void                                                    btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi)
468 {
469 static const btBroadphaseBenchmark::Experiment          experiments[]=
470         {
471         {"1024o.10%",1024,10,0,8192,(btScalar)0.005,(btScalar)100},
472         /*{"4096o.10%",4096,10,0,8192,(btScalar)0.005,(btScalar)100},
473         {"8192o.10%",8192,10,0,8192,(btScalar)0.005,(btScalar)100},*/
474         };
475 static const int                                                                                nexperiments=sizeof(experiments)/sizeof(experiments[0]);
476 btAlignedObjectArray<btBroadphaseBenchmark::Object*>    objects;
477 btClock                                                                                                 wallclock;
478 /* Begin                        */ 
479 for(int iexp=0;iexp<nexperiments;++iexp)
480         {
481         const btBroadphaseBenchmark::Experiment&        experiment=experiments[iexp];
482         const int                                                                       object_count=experiment.object_count;
483         const int                                                                       update_count=(object_count*experiment.update_count)/100;
484         const int                                                                       spawn_count=(object_count*experiment.spawn_count)/100;
485         const btScalar                                                          speed=experiment.speed; 
486         const btScalar                                                          amplitude=experiment.amplitude;
487         printf("Experiment #%u '%s':\r\n",iexp,experiment.name);
488         printf("\tObjects: %u\r\n",object_count);
489         printf("\tUpdate: %u\r\n",update_count);
490         printf("\tSpawn: %u\r\n",spawn_count);
491         printf("\tSpeed: %f\r\n",speed);
492         printf("\tAmplitude: %f\r\n",amplitude);
493         srand(180673);
494         /* Create objects       */ 
495         wallclock.reset();
496         objects.reserve(object_count);
497         for(int i=0;i<object_count;++i)
498                 {
499                 btBroadphaseBenchmark::Object*  po=new btBroadphaseBenchmark::Object();
500                 po->center[0]=btBroadphaseBenchmark::UnitRand()*50;
501                 po->center[1]=btBroadphaseBenchmark::UnitRand()*50;
502                 po->center[2]=btBroadphaseBenchmark::UnitRand()*50;
503                 po->extents[0]=btBroadphaseBenchmark::UnitRand()*2+2;
504                 po->extents[1]=btBroadphaseBenchmark::UnitRand()*2+2;
505                 po->extents[2]=btBroadphaseBenchmark::UnitRand()*2+2;
506                 po->time=btBroadphaseBenchmark::UnitRand()*2000;
507                 po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0);
508                 objects.push_back(po);
509                 }
510         btBroadphaseBenchmark::OutputTime("\tInitialization",wallclock);
511         /* First update         */ 
512         wallclock.reset();
513         for(int i=0;i<objects.size();++i)
514                 {
515                 objects[i]->update(speed,amplitude,pbi);
516                 }
517         btBroadphaseBenchmark::OutputTime("\tFirst update",wallclock);
518         /* Updates                      */ 
519         wallclock.reset();
520         for(int i=0;i<experiment.iterations;++i)
521                 {
522                 for(int j=0;j<update_count;++j)
523                         {                               
524                         objects[j]->update(speed,amplitude,pbi);
525                         }
526                 pbi->calculateOverlappingPairs(0);
527                 }
528         btBroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations);
529         /* Clean up                     */ 
530         wallclock.reset();
531         for(int i=0;i<objects.size();++i)
532                 {
533                 pbi->destroyProxy(objects[i]->proxy,0);
534                 delete objects[i];
535                 }
536         objects.resize(0);
537         btBroadphaseBenchmark::OutputTime("\tRelease",wallclock);
538         }
539
540 }
541 #else
542 void                                                    btDbvtBroadphase::benchmark(btBroadphaseInterface*)
543 {}
544 #endif
545
546 #if DBVT_BP_PROFILE
547 #undef  SPC
548 #endif