svn merge -r 16593:16648 https://svn.blender.org/svnroot/bf-blender/trunk/blender
[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        =       true;//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         }
184 return(proxy);
185 }
186
187 //
188 void                                                    btDbvtBroadphase::destroyProxy( btBroadphaseProxy* absproxy,
189                                                                                                                                 btDispatcher* dispatcher)
190 {
191 btDbvtProxy*    proxy=(btDbvtProxy*)absproxy;
192 if(proxy->stage==STAGECOUNT)
193         m_sets[1].remove(proxy->leaf);
194         else
195         m_sets[0].remove(proxy->leaf);
196 listremove(proxy,m_stageRoots[proxy->stage]);
197 m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher);
198 btAlignedFree(proxy);
199 m_needcleanup=true;
200 }
201
202 //
203 void                                                    btDbvtBroadphase::setAabb(              btBroadphaseProxy* absproxy,
204                                                                                                                                 const btVector3& aabbMin,
205                                                                                                                                 const btVector3& aabbMax,
206                                                                                                                                 btDispatcher* /*dispatcher*/)
207 {
208 btDbvtProxy*                                            proxy=(btDbvtProxy*)absproxy;
209 ATTRIBUTE_ALIGNED16(btDbvtVolume)       aabb=btDbvtVolume::FromMM(aabbMin,aabbMax);
210 #if DBVT_BP_PREVENTFALSEUPDATE
211 if(NotEqual(aabb,proxy->leaf->volume))
212 #endif
213         {
214         bool    docollide=false;
215         if(proxy->stage==STAGECOUNT)
216                 {/* fixed -> dynamic set        */ 
217                 m_sets[1].remove(proxy->leaf);
218                 proxy->leaf=m_sets[0].insert(aabb,proxy);
219                 docollide=true;
220                 }
221                 else
222                 {/* dynamic set                         */ 
223                 ++m_updates_call;
224                 if(Intersect(proxy->leaf->volume,aabb))
225                         {/* Moving                              */ 
226                         const btVector3 delta=aabbMin-proxy->aabb.Mins();
227                         btVector3               velocity(aabb.Extents()*m_prediction);
228                         if(delta[0]<0) velocity[0]=-velocity[0];
229                         if(delta[1]<0) velocity[1]=-velocity[1];
230                         if(delta[2]<0) velocity[2]=-velocity[2];
231                         if      (
232                                 #ifdef DBVT_BP_MARGIN                           
233                                 m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN)
234                                 #else
235                                 m_sets[0].update(proxy->leaf,aabb,velocity)
236                                 #endif
237                                 )
238                                 {
239                                 ++m_updates_done;
240                                 docollide=true;
241                                 }
242                         }
243                         else
244                         {/* Teleporting                 */ 
245                         m_sets[0].update(proxy->leaf,aabb);
246                         ++m_updates_done;
247                         docollide=true;
248                         }       
249                 }
250         listremove(proxy,m_stageRoots[proxy->stage]);
251         proxy->aabb             =       aabb;
252         proxy->stage    =       m_stageCurrent;
253         listappend(proxy,m_stageRoots[m_stageCurrent]);
254         if(docollide)
255                 {
256                 m_needcleanup=true;
257                 if(!m_deferedcollide)
258                         {
259                         btDbvtTreeCollider      collider(this);
260                         btDbvt::collideTT(m_sets[1].m_root,proxy->leaf,collider);
261                         btDbvt::collideTT(m_sets[0].m_root,proxy->leaf,collider);
262                         }
263                 }       
264         }
265 }
266
267 //
268 void                                                    btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
269 {
270 collide(dispatcher);
271 #if DBVT_BP_PROFILE
272 if(0==(m_pid%DBVT_BP_PROFILING_RATE))
273         {       
274         printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leaves,m_sets[0].m_leaves,m_paircache->getNumOverlappingPairs());
275         unsigned int    total=m_profiling.m_total;
276         if(total<=0) total=1;
277         printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/DBVT_BP_PROFILING_RATE);
278         printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/DBVT_BP_PROFILING_RATE);
279         printf("cleanup:   %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/DBVT_BP_PROFILING_RATE);
280         printf("total:     %uus\r\n",total/DBVT_BP_PROFILING_RATE);
281         const unsigned long     sum=m_profiling.m_ddcollide+
282                                                         m_profiling.m_fdcollide+
283                                                         m_profiling.m_cleanup;
284         printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/DBVT_BP_PROFILING_RATE);
285         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));
286         clear(m_profiling);
287         m_clock.reset();
288         }
289 #endif
290 }
291
292 //
293 void                                                    btDbvtBroadphase::collide(btDispatcher* dispatcher)
294 {
295 SPC(m_profiling.m_total);
296 /* optimize                             */ 
297 m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100);
298 if(m_fixedleft)
299         {
300         const int count=1+(m_sets[1].m_leaves*m_fupdates)/100;
301         m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100);
302         m_fixedleft=btMax<int>(0,m_fixedleft-count);
303         }
304 /* dynamic -> fixed set */ 
305 m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT;
306 btDbvtProxy*    current=m_stageRoots[m_stageCurrent];
307 if(current)
308         {
309         btDbvtTreeCollider      collider(this);
310         do      {
311                 btDbvtProxy*    next=current->links[1];
312                 listremove(current,m_stageRoots[current->stage]);
313                 listappend(current,m_stageRoots[STAGECOUNT]);
314                 #if DBVT_BP_ACCURATESLEEPING
315                 m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher);
316                 collider.proxy=current;
317                 btDbvt::collideTV(m_sets[0].m_root,current->aabb,collider);
318                 btDbvt::collideTV(m_sets[1].m_root,current->aabb,collider);
319                 #endif
320                 m_sets[0].remove(current->leaf);
321                 current->leaf   =       m_sets[1].insert(current->aabb,current);
322                 current->stage  =       STAGECOUNT;     
323                 current                 =       next;
324                 } while(current);
325         m_fixedleft=m_sets[1].m_leaves;
326         m_needcleanup=true;
327         }
328 /* collide dynamics             */ 
329         {
330         btDbvtTreeCollider      collider(this);
331         if(m_deferedcollide)
332                 {
333                 SPC(m_profiling.m_fdcollide);
334                 btDbvt::collideTT(m_sets[0].m_root,m_sets[1].m_root,collider);
335                 }
336         if(m_deferedcollide)
337                 {
338                 SPC(m_profiling.m_ddcollide);
339                 btDbvt::collideTT(m_sets[0].m_root,m_sets[0].m_root,collider);
340                 }
341         }
342 /* clean up                             */ 
343 if(m_needcleanup)
344         {
345         SPC(m_profiling.m_cleanup);
346         btBroadphasePairArray&  pairs=m_paircache->getOverlappingPairArray();
347         if(pairs.size()>0)
348                 {
349                 const int       ci=pairs.size();
350                 int                     ni=btMin(ci,btMax<int>(m_newpairs,(ci*m_cupdates)/100));
351                 for(int i=0;i<ni;++i)
352                         {
353                         btBroadphasePair&       p=pairs[(m_cid+i)%ci];
354                         btDbvtProxy*            pa=(btDbvtProxy*)p.m_pProxy0;
355                         btDbvtProxy*            pb=(btDbvtProxy*)p.m_pProxy1;
356                         if(!Intersect(pa->leaf->volume,pb->leaf->volume))
357                                 {
358                                 #if DBVT_BP_SORTPAIRS
359                                 if(pa>pb) btSwap(pa,pb);
360                                 #endif
361                                 m_paircache->removeOverlappingPair(pa,pb,dispatcher);
362                                 --ni;--i;
363                                 }
364                         }
365                 if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0;
366                 }
367         }
368 ++m_pid;
369 m_newpairs=1;
370 m_needcleanup=false;
371 if(m_updates_call>0)
372         { m_updates_ratio=m_updates_done/(btScalar)m_updates_call; }
373         else
374         { m_updates_ratio=0; }
375 m_updates_done/=2;
376 m_updates_call/=2;
377 }
378
379 //
380 void                                                    btDbvtBroadphase::optimize()
381 {
382 m_sets[0].optimizeTopDown();
383 m_sets[1].optimizeTopDown();
384 }
385
386 //
387 btOverlappingPairCache*                 btDbvtBroadphase::getOverlappingPairCache()
388 {
389 return(m_paircache);
390 }
391
392 //
393 const btOverlappingPairCache*   btDbvtBroadphase::getOverlappingPairCache() const
394 {
395 return(m_paircache);
396 }
397
398 //
399 void                                                    btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
400 {
401
402         ATTRIBUTE_ALIGNED16(btDbvtVolume)       bounds;
403
404 if(!m_sets[0].empty())
405         if(!m_sets[1].empty())  Merge(  m_sets[0].m_root->volume,
406                                                                         m_sets[1].m_root->volume,bounds);
407                                                         else
408                                                         bounds=m_sets[0].m_root->volume;
409 else if(!m_sets[1].empty())     bounds=m_sets[1].m_root->volume;
410                                                         else
411                                                         bounds=btDbvtVolume::FromCR(btVector3(0,0,0),0);
412 aabbMin=bounds.Mins();
413 aabbMax=bounds.Maxs();
414 }
415
416 //
417 void                                                    btDbvtBroadphase::printStats()
418 {}
419
420 //
421 #if DBVT_BP_ENABLE_BENCHMARK
422
423 struct  btBroadphaseBenchmark
424         {
425         struct  Experiment
426                 {
427                 const char*                     name;
428                 int                                     object_count;
429                 int                                     update_count;
430                 int                                     spawn_count;
431                 int                                     iterations;
432                 btScalar                        speed;
433                 btScalar                        amplitude;
434                 };
435         struct  Object
436                 {
437                 btVector3                       center;
438                 btVector3                       extents;
439                 btBroadphaseProxy*      proxy;
440                 btScalar                        time;
441                 void                            update(btScalar speed,btScalar amplitude,btBroadphaseInterface* pbi)
442                         {
443                         time            +=      speed;
444                         center[0]       =       btCos(time*(btScalar)2.17)*amplitude+
445                                                         btSin(time)*amplitude/2;
446                         center[1]       =       btCos(time*(btScalar)1.38)*amplitude+
447                                                         btSin(time)*amplitude;
448                         center[2]       =       btSin(time*(btScalar)0.777)*amplitude;
449                         pbi->setAabb(proxy,center-extents,center+extents,0);
450                         }
451                 };
452         static int              UnsignedRand(int range=RAND_MAX-1)      { return(rand()%(range+1)); }
453         static btScalar UnitRand()                                                      { return(UnsignedRand(16384)/(btScalar)16384); }
454         static void             OutputTime(const char* name,btClock& c,unsigned count=0)
455                 {
456                 const unsigned long     us=c.getTimeMicroseconds();
457                 const unsigned long     ms=(us+500)/1000;
458                 const btScalar          sec=us/(btScalar)(1000*1000);
459                 if(count>0)
460                         printf("%s : %u us (%u ms), %.2f/s\r\n",name,us,ms,count/sec);
461                         else
462                         printf("%s : %u us (%u ms)\r\n",name,us,ms);
463                 }
464         };
465
466 void                                                    btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi)
467 {
468 static const btBroadphaseBenchmark::Experiment          experiments[]=
469         {
470         {"1024o.10%",1024,10,0,8192,(btScalar)0.005,(btScalar)100},
471         /*{"4096o.10%",4096,10,0,8192,(btScalar)0.005,(btScalar)100},
472         {"8192o.10%",8192,10,0,8192,(btScalar)0.005,(btScalar)100},*/
473         };
474 static const int                                                                                nexperiments=sizeof(experiments)/sizeof(experiments[0]);
475 btAlignedObjectArray<btBroadphaseBenchmark::Object*>    objects;
476 btClock                                                                                                 wallclock;
477 /* Begin                        */ 
478 for(int iexp=0;iexp<nexperiments;++iexp)
479         {
480         const btBroadphaseBenchmark::Experiment&        experiment=experiments[iexp];
481         const int                                                                       object_count=experiment.object_count;
482         const int                                                                       update_count=(object_count*experiment.update_count)/100;
483         const int                                                                       spawn_count=(object_count*experiment.spawn_count)/100;
484         const btScalar                                                          speed=experiment.speed; 
485         const btScalar                                                          amplitude=experiment.amplitude;
486         printf("Experiment #%u '%s':\r\n",iexp,experiment.name);
487         printf("\tObjects: %u\r\n",object_count);
488         printf("\tUpdate: %u\r\n",update_count);
489         printf("\tSpawn: %u\r\n",spawn_count);
490         printf("\tSpeed: %f\r\n",speed);
491         printf("\tAmplitude: %f\r\n",amplitude);
492         srand(180673);
493         /* Create objects       */ 
494         wallclock.reset();
495         objects.reserve(object_count);
496         for(int i=0;i<object_count;++i)
497                 {
498                 btBroadphaseBenchmark::Object*  po=new btBroadphaseBenchmark::Object();
499                 po->center[0]=btBroadphaseBenchmark::UnitRand()*50;
500                 po->center[1]=btBroadphaseBenchmark::UnitRand()*50;
501                 po->center[2]=btBroadphaseBenchmark::UnitRand()*50;
502                 po->extents[0]=btBroadphaseBenchmark::UnitRand()*2+2;
503                 po->extents[1]=btBroadphaseBenchmark::UnitRand()*2+2;
504                 po->extents[2]=btBroadphaseBenchmark::UnitRand()*2+2;
505                 po->time=btBroadphaseBenchmark::UnitRand()*2000;
506                 po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0);
507                 objects.push_back(po);
508                 }
509         btBroadphaseBenchmark::OutputTime("\tInitialization",wallclock);
510         /* First update         */ 
511         wallclock.reset();
512         for(int i=0;i<objects.size();++i)
513                 {
514                 objects[i]->update(speed,amplitude,pbi);
515                 }
516         btBroadphaseBenchmark::OutputTime("\tFirst update",wallclock);
517         /* Updates                      */ 
518         wallclock.reset();
519         for(int i=0;i<experiment.iterations;++i)
520                 {
521                 for(int j=0;j<update_count;++j)
522                         {                               
523                         objects[j]->update(speed,amplitude,pbi);
524                         }
525                 pbi->calculateOverlappingPairs(0);
526                 }
527         btBroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations);
528         /* Clean up                     */ 
529         wallclock.reset();
530         for(int i=0;i<objects.size();++i)
531                 {
532                 pbi->destroyProxy(objects[i]->proxy,0);
533                 delete objects[i];
534                 }
535         objects.resize(0);
536         btBroadphaseBenchmark::OutputTime("\tRelease",wallclock);
537         }
538
539 }
540 #else
541 void                                                    btDbvtBroadphase::benchmark(btBroadphaseInterface*)
542 {}
543 #endif
544
545 #if DBVT_BP_PROFILE
546 #undef  SPC
547 #endif