Applied some fixes from Bullet: issues with btDbvtBroadphase, and btSoftBody, and...
[blender-staging.git] / extern / bullet2 / src / BulletCollision / BroadphaseCollision / btDbvt.h
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 ///btDbvt implementation by Nathanael Presson
16
17 #ifndef BT_DYNAMIC_BOUNDING_VOLUME_TREE_H
18 #define BT_DYNAMIC_BOUNDING_VOLUME_TREE_H
19
20 #include "LinearMath/btAlignedObjectArray.h"
21 #include "LinearMath/btVector3.h"
22 #include "LinearMath/btTransform.h"
23
24 //
25 // Compile time configuration
26 //
27
28
29 // Implementation profiles
30 #define DBVT_IMPL_GENERIC               0       // Generic implementation       
31 #define DBVT_IMPL_SSE                   1       // SSE
32
33 // Template implementation of ICollide
34 #ifdef WIN32_AVOID_SSE_WHEN_EMBEDDED_INSIDE_BLENDER //there is always some weird compiler that breaks SSE builds
35         #if (defined (_MSC_VER) && _MSC_VER >= 1400)
36         #define DBVT_USE_TEMPLATE               1
37         #else
38         #define DBVT_USE_TEMPLATE               0
39 #endif
40 #else
41 #define DBVT_USE_TEMPLATE               0
42 #endif
43
44 // Use only intrinsics instead of inline asm
45 #define DBVT_USE_INTRINSIC_SSE  1
46
47 // Using memmov for collideOCL
48 #define DBVT_USE_MEMMOVE                1
49
50 // Enable benchmarking code
51 #define DBVT_ENABLE_BENCHMARK   0
52
53 // Inlining
54 #define DBVT_INLINE                             SIMD_FORCE_INLINE
55 // Align
56 #ifdef WIN32
57 #define DBVT_ALIGN                              __declspec(align(16))
58 #else
59 #define DBVT_ALIGN
60 #endif
61
62 // Specific methods implementation
63
64 #ifdef WIN32_AVOID_SSE_WHEN_EMBEDDED_INSIDE_BLENDER //there is always some weird compiler that breaks SSE builds
65 #define DBVT_SELECT_IMPL                DBVT_IMPL_SSE
66 #define DBVT_MERGE_IMPL                 DBVT_IMPL_SSE
67 #define DBVT_INT0_IMPL                  DBVT_IMPL_SSE
68 #else
69 #define DBVT_SELECT_IMPL                DBVT_IMPL_GENERIC
70 #define DBVT_MERGE_IMPL                 DBVT_IMPL_GENERIC
71 #define DBVT_INT0_IMPL                  DBVT_IMPL_GENERIC
72 #endif
73
74 #if     (DBVT_SELECT_IMPL==DBVT_IMPL_SSE)||     \
75         (DBVT_MERGE_IMPL==DBVT_IMPL_SSE)||      \
76         (DBVT_INT0_IMPL==DBVT_IMPL_SSE)
77 #include <emmintrin.h>
78 #endif
79
80 //
81 // Auto config and checks
82 //
83
84 #if DBVT_USE_TEMPLATE
85 #define DBVT_VIRTUAL
86 #define DBVT_VIRTUAL_DTOR(a)
87 #define DBVT_PREFIX                                     template <typename T>
88 #define DBVT_IPOLICY                            T& policy
89 #define DBVT_CHECKTYPE                          static const ICollide&  typechecker=*(T*)0;
90 #else
91 #define DBVT_VIRTUAL_DTOR(a)            virtual ~a() {}
92 #define DBVT_VIRTUAL                            virtual
93 #define DBVT_PREFIX
94 #define DBVT_IPOLICY                            ICollide& policy
95 #define DBVT_CHECKTYPE
96 #endif
97
98 #if DBVT_USE_MEMMOVE
99 #ifndef __CELLOS_LV2__
100 #include <memory.h>
101 #endif
102 #include <string.h>
103 #endif
104
105 #ifndef DBVT_USE_TEMPLATE
106 #error "DBVT_USE_TEMPLATE undefined"
107 #endif
108
109 #ifndef DBVT_USE_MEMMOVE
110 #error "DBVT_USE_MEMMOVE undefined"
111 #endif
112
113 #ifndef DBVT_ENABLE_BENCHMARK
114 #error "DBVT_ENABLE_BENCHMARK undefined"
115 #endif
116
117 #ifndef DBVT_SELECT_IMPL
118 #error "DBVT_SELECT_IMPL undefined"
119 #endif
120
121 #ifndef DBVT_MERGE_IMPL
122 #error "DBVT_MERGE_IMPL undefined"
123 #endif
124
125 #ifndef DBVT_INT0_IMPL
126 #error "DBVT_INT0_IMPL undefined"
127 #endif
128
129 //
130 // Defaults volumes
131 //
132
133 /* btDbvtAabbMm                 */ 
134 struct  btDbvtAabbMm
135 {
136 DBVT_INLINE btVector3                   Center() const  { return((mi+mx)/2); }
137 DBVT_INLINE btVector3                   Lengths() const { return(mx-mi); }
138 DBVT_INLINE btVector3                   Extents() const { return((mx-mi)/2); }
139 DBVT_INLINE const btVector3&    Mins() const    { return(mi); }
140 DBVT_INLINE const btVector3&    Maxs() const    { return(mx); }
141 static inline btDbvtAabbMm              FromCE(const btVector3& c,const btVector3& e);
142 static inline btDbvtAabbMm              FromCR(const btVector3& c,btScalar r);
143 static inline btDbvtAabbMm              FromMM(const btVector3& mi,const btVector3& mx);
144 static inline btDbvtAabbMm              FromPoints(const btVector3* pts,int n);
145 static inline btDbvtAabbMm              FromPoints(const btVector3** ppts,int n);
146 DBVT_INLINE void                                Expand(const btVector3& e);
147 DBVT_INLINE void                                SignedExpand(const btVector3& e);
148 DBVT_INLINE bool                                Contain(const btDbvtAabbMm& a) const;
149 DBVT_INLINE int                                 Classify(const btVector3& n,btScalar o,int s) const;
150 DBVT_INLINE btScalar                    ProjectMinimum(const btVector3& v,unsigned signs) const;
151 DBVT_INLINE friend bool                 Intersect(      const btDbvtAabbMm& a,
152                                                                                         const btDbvtAabbMm& b);
153 DBVT_INLINE friend bool                 Intersect(      const btDbvtAabbMm& a,
154                                                                                         const btDbvtAabbMm& b,
155                                                                                         const btTransform& xform);
156 DBVT_INLINE friend bool                 Intersect(      const btDbvtAabbMm& a,
157                                                                                         const btVector3& b);
158 DBVT_INLINE friend bool                 Intersect(      const btDbvtAabbMm& a,
159                                                                                         const btVector3& org,
160                                                                                         const btVector3& invdir,
161                                                                                         const unsigned* signs);
162 DBVT_INLINE friend btScalar             Proximity(      const btDbvtAabbMm& a,
163                                                                                         const btDbvtAabbMm& b);
164 DBVT_INLINE friend int                  Select(         const btDbvtAabbMm& o,
165                                                                                         const btDbvtAabbMm& a,
166                                                                                         const btDbvtAabbMm& b);
167 DBVT_INLINE friend void                 Merge(          const btDbvtAabbMm& a,
168                                                                                         const btDbvtAabbMm& b,
169                                                                                         btDbvtAabbMm& r);
170 DBVT_INLINE friend bool                 NotEqual(       const btDbvtAabbMm& a,
171                                                                                         const btDbvtAabbMm& b);
172 private:
173 DBVT_INLINE void                                AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const;
174 private:
175 btVector3       mi,mx;
176 };
177
178 // Types        
179 typedef btDbvtAabbMm    btDbvtVolume;
180
181 /* btDbvtNode                           */ 
182 struct  btDbvtNode
183 {
184         btDbvtVolume    volume;
185         btDbvtNode*             parent;
186         DBVT_INLINE bool        isleaf() const          { return(childs[1]==0); }
187         DBVT_INLINE bool        isinternal() const      { return(!isleaf()); }
188         union   {
189                         btDbvtNode*     childs[2];
190                         void*   data;
191                         int             dataAsInt;
192                         };
193 };
194
195 ///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree).
196 ///This btDbvt is used for soft body collision detection and for the btDbvtBroadphase. It has a fast insert, remove and update of nodes.
197 ///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure.
198 struct  btDbvt
199         {
200         /* Stack element        */ 
201         struct  sStkNN
202                 {
203                 const btDbvtNode*       a;
204                 const btDbvtNode*       b;
205                 sStkNN() {}
206                 sStkNN(const btDbvtNode* na,const btDbvtNode* nb) : a(na),b(nb) {}
207                 };
208         struct  sStkNP
209                 {
210                 const btDbvtNode*       node;
211                 int                     mask;
212                 sStkNP(const btDbvtNode* n,unsigned m) : node(n),mask(m) {}
213                 };
214         struct  sStkNPS
215                 {
216                 const btDbvtNode*       node;
217                 int                     mask;
218                 btScalar        value;
219                 sStkNPS() {}
220                 sStkNPS(const btDbvtNode* n,unsigned m,btScalar v) : node(n),mask(m),value(v) {}
221                 };
222         struct  sStkCLN
223                 {
224                 const btDbvtNode*       node;
225                 btDbvtNode*             parent;
226                 sStkCLN(const btDbvtNode* n,btDbvtNode* p) : node(n),parent(p) {}
227                 };
228         // Policies/Interfaces
229                         
230         /* ICollide     */ 
231         struct  ICollide
232                 {               
233                 DBVT_VIRTUAL_DTOR(ICollide)
234                 DBVT_VIRTUAL void       Process(const btDbvtNode*,const btDbvtNode*)            {}
235                 DBVT_VIRTUAL void       Process(const btDbvtNode*)                                      {}
236                 DBVT_VIRTUAL void       Process(const btDbvtNode* n,btScalar)                   { Process(n); }
237                 DBVT_VIRTUAL bool       Descent(const btDbvtNode*)                                      { return(true); }
238                 DBVT_VIRTUAL bool       AllLeaves(const btDbvtNode*)                                    { return(true); }
239                 };
240         /* IWriter      */ 
241         struct  IWriter
242                 {
243                 virtual ~IWriter() {}
244                 virtual void            Prepare(const btDbvtNode* root,int numnodes)=0;
245                 virtual void            WriteNode(const btDbvtNode*,int index,int parent,int child0,int child1)=0;
246                 virtual void            WriteLeaf(const btDbvtNode*,int index,int parent)=0;
247                 };
248         /* IClone       */ 
249         struct  IClone
250                 {
251                 virtual ~IClone()       {}
252                 virtual void            CloneLeaf(btDbvtNode*) {}
253                 };
254                 
255         // Constants
256         enum    {
257                         SIMPLE_STACKSIZE        =       64,
258                         DOUBLE_STACKSIZE        =       SIMPLE_STACKSIZE*2
259                         };
260                 
261         // Fields
262         btDbvtNode*             m_root;
263         btDbvtNode*             m_free;
264         int                             m_lkhd;
265         int                             m_leaves;
266         unsigned                m_opath;
267         // Methods
268                                         btDbvt();
269                                         ~btDbvt();
270         void                    clear();
271         bool                    empty() const { return(0==m_root); }
272         void                    optimizeBottomUp();
273         void                    optimizeTopDown(int bu_treshold=128);
274         void                    optimizeIncremental(int passes);
275         btDbvtNode*             insert(const btDbvtVolume& box,void* data);
276         void                    update(btDbvtNode* leaf,int lookahead=-1);
277         void                    update(btDbvtNode* leaf,const btDbvtVolume& volume);
278         bool                    update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity,btScalar margin);
279         bool                    update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity);
280         bool                    update(btDbvtNode* leaf,btDbvtVolume volume,btScalar margin);   
281         void                    remove(btDbvtNode* leaf);
282         void                    write(IWriter* iwriter) const;
283         void                    clone(btDbvt& dest,IClone* iclone=0) const;
284         static int              maxdepth(const btDbvtNode* node);
285         static int              countLeaves(const btDbvtNode* node);
286         static void             extractLeaves(const btDbvtNode* node,btAlignedObjectArray<const btDbvtNode*>& leaves);
287         #if DBVT_ENABLE_BENCHMARK
288         static void             benchmark();
289         #else
290         static void             benchmark(){}
291         #endif
292         // DBVT_IPOLICY must support ICollide policy/interface
293         DBVT_PREFIX
294         static void             enumNodes(      const btDbvtNode* root,
295                                                                 DBVT_IPOLICY);
296         DBVT_PREFIX
297         static void             enumLeaves(     const btDbvtNode* root,
298                                                                 DBVT_IPOLICY);
299         DBVT_PREFIX
300         static void             collideTT(      const btDbvtNode* root0,
301                                                                 const btDbvtNode* root1,
302                                                                 DBVT_IPOLICY);
303         DBVT_PREFIX
304         static void             collideTT(      const btDbvtNode* root0,
305                                                                 const btDbvtNode* root1,
306                                                                 const btTransform& xform,
307                                                                 DBVT_IPOLICY);
308         DBVT_PREFIX
309         static void             collideTT(      const btDbvtNode* root0,
310                                                                 const btTransform& xform0,
311                                                                 const btDbvtNode* root1,
312                                                                 const btTransform& xform1,
313                                                                 DBVT_IPOLICY);
314         DBVT_PREFIX
315         static void             collideTV(      const btDbvtNode* root,
316                                                                 const btDbvtVolume& volume,
317                                                                 DBVT_IPOLICY);
318         DBVT_PREFIX
319         static void             collideRAY(     const btDbvtNode* root,
320                                                                 const btVector3& origin,
321                                                                 const btVector3& direction,
322                                                                 DBVT_IPOLICY);
323         DBVT_PREFIX
324         static void             collideKDOP(const btDbvtNode* root,
325                                                                 const btVector3* normals,
326                                                                 const btScalar* offsets,
327                                                                 int count,
328                                                                 DBVT_IPOLICY);
329         DBVT_PREFIX
330         static void             collideOCL(     const btDbvtNode* root,
331                                                                 const btVector3* normals,
332                                                                 const btScalar* offsets,
333                                                                 const btVector3& sortaxis,
334                                                                 int count,                                                              
335                                                                 DBVT_IPOLICY,
336                                                                 bool fullsort=true);
337         DBVT_PREFIX
338         static void             collideTU(      const btDbvtNode* root,
339                                                                 DBVT_IPOLICY);
340         // Helpers      
341         static DBVT_INLINE int  nearest(const int* i,const btDbvt::sStkNPS* a,btScalar v,int l,int h)
342                 {
343                 int     m=0;
344                 while(l<h)
345                         {
346                         m=(l+h)>>1;
347                         if(a[i[m]].value>=v) l=m+1; else h=m;
348                         }
349                 return(h);
350                 }
351         static DBVT_INLINE int  allocate(       btAlignedObjectArray<int>& ifree,
352                                                                                 btAlignedObjectArray<sStkNPS>& stock,
353                                                                                 const sStkNPS& value)
354                 {
355                 int     i;
356                 if(ifree.size()>0)
357                         { i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; }
358                         else
359                         { i=stock.size();stock.push_back(value); }
360                 return(i); 
361                 }
362         //
363         private:
364                                         btDbvt(const btDbvt&)   {}      
365         };
366
367 //
368 // Inline's
369 //
370
371 //
372 inline btDbvtAabbMm                     btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e)
373 {
374 btDbvtAabbMm box;
375 box.mi=c-e;box.mx=c+e;
376 return(box);
377 }
378         
379 //
380 inline btDbvtAabbMm                     btDbvtAabbMm::FromCR(const btVector3& c,btScalar r)
381 {
382 return(FromCE(c,btVector3(r,r,r)));
383 }
384         
385 //
386 inline btDbvtAabbMm                     btDbvtAabbMm::FromMM(const btVector3& mi,const btVector3& mx)
387 {
388 btDbvtAabbMm box;
389 box.mi=mi;box.mx=mx;
390 return(box);
391 }
392         
393 //
394 inline btDbvtAabbMm                     btDbvtAabbMm::FromPoints(const btVector3* pts,int n)
395 {
396 btDbvtAabbMm box;
397 box.mi=box.mx=pts[0];
398 for(int i=1;i<n;++i)
399         {
400         box.mi.setMin(pts[i]);
401         box.mx.setMax(pts[i]);
402         }
403 return(box);
404 }
405
406 //
407 inline btDbvtAabbMm                     btDbvtAabbMm::FromPoints(const btVector3** ppts,int n)
408 {
409 btDbvtAabbMm box;
410 box.mi=box.mx=*ppts[0];
411 for(int i=1;i<n;++i)
412         {
413         box.mi.setMin(*ppts[i]);
414         box.mx.setMax(*ppts[i]);
415         }
416 return(box);
417 }
418
419 //
420 DBVT_INLINE void                btDbvtAabbMm::Expand(const btVector3& e)
421 {
422 mi-=e;mx+=e;
423 }
424         
425 //
426 DBVT_INLINE void                btDbvtAabbMm::SignedExpand(const btVector3& e)
427 {
428 if(e.x()>0) mx.setX(mx.x()+e[0]); else mi.setX(mi.x()+e[0]);
429 if(e.y()>0) mx.setY(mx.y()+e[1]); else mi.setY(mi.y()+e[1]);
430 if(e.z()>0) mx.setZ(mx.z()+e[2]); else mi.setZ(mi.z()+e[2]);
431 }
432         
433 //
434 DBVT_INLINE bool                btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const
435 {
436 return( (mi.x()<=a.mi.x())&&
437                 (mi.y()<=a.mi.y())&&
438                 (mi.z()<=a.mi.z())&&
439                 (mx.x()>=a.mx.x())&&
440                 (mx.y()>=a.mx.y())&&
441                 (mx.z()>=a.mx.z()));
442 }
443
444 //
445 DBVT_INLINE int         btDbvtAabbMm::Classify(const btVector3& n,btScalar o,int s) const
446 {
447 btVector3                       pi,px;
448 switch(s)
449         {
450         case    (0+0+0):        px=btVector3(mi.x(),mi.y(),mi.z());
451                                                 pi=btVector3(mx.x(),mx.y(),mx.z());break;
452         case    (1+0+0):        px=btVector3(mx.x(),mi.y(),mi.z());
453                                                 pi=btVector3(mi.x(),mx.y(),mx.z());break;
454         case    (0+2+0):        px=btVector3(mi.x(),mx.y(),mi.z());
455                                                 pi=btVector3(mx.x(),mi.y(),mx.z());break;
456         case    (1+2+0):        px=btVector3(mx.x(),mx.y(),mi.z());
457                                                 pi=btVector3(mi.x(),mi.y(),mx.z());break;
458         case    (0+0+4):        px=btVector3(mi.x(),mi.y(),mx.z());
459                                                 pi=btVector3(mx.x(),mx.y(),mi.z());break;
460         case    (1+0+4):        px=btVector3(mx.x(),mi.y(),mx.z());
461                                                 pi=btVector3(mi.x(),mx.y(),mi.z());break;
462         case    (0+2+4):        px=btVector3(mi.x(),mx.y(),mx.z());
463                                                 pi=btVector3(mx.x(),mi.y(),mi.z());break;
464         case    (1+2+4):        px=btVector3(mx.x(),mx.y(),mx.z());
465                                                 pi=btVector3(mi.x(),mi.y(),mi.z());break;
466         }
467 if((dot(n,px)+o)<0)             return(-1);
468 if((dot(n,pi)+o)>=0)    return(+1);
469 return(0);
470 }
471
472 //
473 DBVT_INLINE btScalar    btDbvtAabbMm::ProjectMinimum(const btVector3& v,unsigned signs) const
474 {
475 const btVector3*        b[]={&mx,&mi};
476 const btVector3         p(      b[(signs>>0)&1]->x(),
477                                                 b[(signs>>1)&1]->y(),
478                                                 b[(signs>>2)&1]->z());
479 return(dot(p,v));
480 }
481
482 //
483 DBVT_INLINE void                btDbvtAabbMm::AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const
484 {
485 for(int i=0;i<3;++i)
486         {
487         if(d[i]<0)
488                 { smi+=mx[i]*d[i];smx+=mi[i]*d[i]; }
489                 else
490                 { smi+=mi[i]*d[i];smx+=mx[i]*d[i]; }
491         }
492 }
493         
494 //
495 DBVT_INLINE bool                Intersect(      const btDbvtAabbMm& a,
496                                                                         const btDbvtAabbMm& b)
497 {
498 #if     DBVT_INT0_IMPL == DBVT_IMPL_SSE
499 const __m128    rt(_mm_or_ps(   _mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)),
500                                                                 _mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi))));
501 const __int32*  pu((const __int32*)&rt);
502 return((pu[0]|pu[1]|pu[2])==0);
503 #else
504 return( (a.mi.x()<=b.mx.x())&&
505                 (a.mx.x()>=b.mi.x())&&
506                 (a.mi.y()<=b.mx.y())&&
507                 (a.mx.y()>=b.mi.y())&&
508                 (a.mi.z()<=b.mx.z())&&          
509                 (a.mx.z()>=b.mi.z()));
510 #endif
511 }
512
513 //
514 DBVT_INLINE bool                Intersect(      const btDbvtAabbMm& a,
515                                                                         const btDbvtAabbMm& b,
516                                                                         const btTransform& xform)
517 {
518 const btVector3         d0=xform*b.Center()-a.Center();
519 const btVector3         d1=d0*xform.getBasis();
520 btScalar                        s0[2]={0,0};
521 btScalar                        s1[2]={dot(xform.getOrigin(),d0),s1[0]};
522 a.AddSpan(d0,s0[0],s0[1]);
523 b.AddSpan(d1,s1[0],s1[1]);
524 if(s0[0]>(s1[1])) return(false);
525 if(s0[1]<(s1[0])) return(false);
526 return(true);
527 }
528
529 //
530 DBVT_INLINE bool                Intersect(      const btDbvtAabbMm& a,
531                                                                         const btVector3& b)
532 {
533 return( (b.x()>=a.mi.x())&&
534                 (b.y()>=a.mi.y())&&
535                 (b.z()>=a.mi.z())&&
536                 (b.x()<=a.mx.x())&&
537                 (b.y()<=a.mx.y())&&
538                 (b.z()<=a.mx.z()));
539 }
540
541 //
542 DBVT_INLINE bool                Intersect(      const btDbvtAabbMm& a,
543                                                                         const btVector3& org,
544                                                                         const btVector3& invdir,
545                                                                         const unsigned* signs)
546 {
547 #if 0
548 const btVector3         b0((a.mi-org)*invdir);
549 const btVector3         b1((a.mx-org)*invdir);
550 const btVector3         tmin(btMin(b0[0],b1[0]),btMin(b0[1],b1[1]),btMin(b0[2],b1[2]));
551 const btVector3         tmax(btMax(b0[0],b1[0]),btMax(b0[1],b1[1]),btMax(b0[2],b1[2]));
552 const btScalar          tin=btMax(tmin[0],btMax(tmin[1],tmin[2]));
553 const btScalar          tout=btMin(tmax[0],btMin(tmax[1],tmax[2]));
554 return(tin<tout);
555 #else
556 const btVector3*        bounds[2]={&a.mi,&a.mx};
557 btScalar                        txmin=(bounds[  signs[0]]->x()-org[0])*invdir[0];
558 btScalar                        txmax=(bounds[1-signs[0]]->x()-org[0])*invdir[0];
559 const btScalar          tymin=(bounds[  signs[1]]->y()-org[1])*invdir[1];
560 const btScalar          tymax=(bounds[1-signs[1]]->y()-org[1])*invdir[1];
561 if((txmin>tymax)||(tymin>txmax)) return(false);
562 if(tymin>txmin) txmin=tymin;
563 if(tymax<txmax) txmax=tymax;
564 const btScalar          tzmin=(bounds[  signs[2]]->z()-org[2])*invdir[2];
565 const btScalar          tzmax=(bounds[1-signs[2]]->z()-org[2])*invdir[2];
566 if((txmin>tzmax)||(tzmin>txmax)) return(false);
567 if(tzmin>txmin) txmin=tzmin;
568 if(tzmax<txmax) txmax=tzmax;
569 return(txmax>0);
570 #endif
571 }
572         
573 //
574 DBVT_INLINE btScalar    Proximity(      const btDbvtAabbMm& a,
575                                                                         const btDbvtAabbMm& b)
576 {
577 const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx);
578 return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z()));
579 }
580
581 //
582 DBVT_INLINE int                 Select( const btDbvtAabbMm& o,
583                                                                 const btDbvtAabbMm& a,
584                                                                 const btDbvtAabbMm& b)
585 {
586 #if     DBVT_SELECT_IMPL == DBVT_IMPL_SSE
587 static DBVT_ALIGN const unsigned __int32        mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff};
588         // TODO: the intrinsic version is 11% slower
589         #if DBVT_USE_INTRINSIC_SSE
590         __m128  omi(_mm_load_ps(o.mi));
591         omi=_mm_add_ps(omi,_mm_load_ps(o.mx));
592         __m128  ami(_mm_load_ps(a.mi));
593         ami=_mm_add_ps(ami,_mm_load_ps(a.mx));
594         ami=_mm_sub_ps(ami,omi);
595         ami=_mm_and_ps(ami,_mm_load_ps((const float*)mask));
596         __m128  bmi(_mm_load_ps(b.mi));
597         bmi=_mm_add_ps(bmi,_mm_load_ps(b.mx));
598         bmi=_mm_sub_ps(bmi,omi);
599         bmi=_mm_and_ps(bmi,_mm_load_ps((const float*)mask));
600         __m128  t0(_mm_movehl_ps(ami,ami));
601         ami=_mm_add_ps(ami,t0);
602         ami=_mm_add_ss(ami,_mm_shuffle_ps(ami,ami,1));
603         __m128  t1(_mm_movehl_ps(bmi,bmi));
604         bmi=_mm_add_ps(bmi,t1);
605         bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1));
606         return(_mm_cmple_ss(bmi,ami).m128_u32[0]&1);
607         #else
608         DBVT_ALIGN __int32      r[1];
609         __asm
610                 {
611                 mov             eax,o
612                 mov             ecx,a
613                 mov             edx,b
614                 movaps  xmm0,[eax]
615                 movaps  xmm5,mask
616                 addps   xmm0,[eax+16]   
617                 movaps  xmm1,[ecx]
618                 movaps  xmm2,[edx]
619                 addps   xmm1,[ecx+16]
620                 addps   xmm2,[edx+16]
621                 subps   xmm1,xmm0
622                 subps   xmm2,xmm0
623                 andps   xmm1,xmm5
624                 andps   xmm2,xmm5
625                 movhlps xmm3,xmm1
626                 movhlps xmm4,xmm2
627                 addps   xmm1,xmm3
628                 addps   xmm2,xmm4
629                 pshufd  xmm3,xmm1,1
630                 pshufd  xmm4,xmm2,1
631                 addss   xmm1,xmm3
632                 addss   xmm2,xmm4
633                 cmpless xmm2,xmm1
634                 movss   r,xmm2
635                 }
636         return(r[0]&1);
637         #endif
638 #else
639 return(Proximity(o,a)<Proximity(o,b)?0:1);
640 #endif
641 }
642
643 //
644 DBVT_INLINE void                Merge(  const btDbvtAabbMm& a,
645                                                                 const btDbvtAabbMm& b,
646                                                                 btDbvtAabbMm& r)
647 {
648 #if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
649 __m128  ami(_mm_load_ps(a.mi));
650 __m128  amx(_mm_load_ps(a.mx));
651 __m128  bmi(_mm_load_ps(b.mi));
652 __m128  bmx(_mm_load_ps(b.mx));
653 ami=_mm_min_ps(ami,bmi);
654 amx=_mm_max_ps(amx,bmx);
655 _mm_store_ps(r.mi,ami);
656 _mm_store_ps(r.mx,amx);
657 #else
658 for(int i=0;i<3;++i)
659         {
660         if(a.mi[i]<b.mi[i]) r.mi[i]=a.mi[i]; else r.mi[i]=b.mi[i];
661         if(a.mx[i]>b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i];
662         }
663 #endif
664 }
665
666 //
667 DBVT_INLINE bool                NotEqual(       const btDbvtAabbMm& a,
668                                                                         const btDbvtAabbMm& b)
669 {
670 return( (a.mi.x()!=b.mi.x())||
671                 (a.mi.y()!=b.mi.y())||
672                 (a.mi.z()!=b.mi.z())||
673                 (a.mx.x()!=b.mx.x())||
674                 (a.mx.y()!=b.mx.y())||
675                 (a.mx.z()!=b.mx.z()));
676 }
677
678 //
679 // Inline's
680 //
681
682 //
683 DBVT_PREFIX
684 inline void             btDbvt::enumNodes(      const btDbvtNode* root,
685                                                                         DBVT_IPOLICY)
686 {
687 DBVT_CHECKTYPE
688 policy.Process(root);
689 if(root->isinternal())
690         {
691         enumNodes(root->childs[0],policy);
692         enumNodes(root->childs[1],policy);
693         }
694 }
695
696 //
697 DBVT_PREFIX
698 inline void             btDbvt::enumLeaves(     const btDbvtNode* root,
699                                                                         DBVT_IPOLICY)
700 {
701 DBVT_CHECKTYPE
702 if(root->isinternal())
703         {
704         enumLeaves(root->childs[0],policy);
705         enumLeaves(root->childs[1],policy);
706         }
707         else
708         {
709         policy.Process(root);
710         }
711 }
712
713 //
714 DBVT_PREFIX
715 inline void             btDbvt::collideTT(      const btDbvtNode* root0,
716                                                                         const btDbvtNode* root1,
717                                                                         DBVT_IPOLICY)
718 {
719 DBVT_CHECKTYPE
720 if(root0&&root1)
721         {
722         btAlignedObjectArray<sStkNN>    stack;
723         int                                                             depth=1;
724         int                                                             treshold=DOUBLE_STACKSIZE-4;
725         stack.resize(DOUBLE_STACKSIZE);
726         stack[0]=sStkNN(root0,root1);
727         do      {               
728                 sStkNN  p=stack[--depth];
729                 if(depth>treshold)
730                         {
731                         stack.resize(stack.size()*2);
732                         treshold=stack.size()-4;
733                         }
734                 if(p.a==p.b)
735                         {
736                         if(p.a->isinternal())
737                                 {
738                                 stack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]);
739                                 stack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]);
740                                 stack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]);
741                                 }
742                         }
743                 else if(Intersect(p.a->volume,p.b->volume))
744                         {
745                         if(p.a->isinternal())
746                                 {
747                                 if(p.b->isinternal())
748                                         {
749                                         stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
750                                         stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
751                                         stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
752                                         stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
753                                         }
754                                         else
755                                         {
756                                         stack[depth++]=sStkNN(p.a->childs[0],p.b);
757                                         stack[depth++]=sStkNN(p.a->childs[1],p.b);
758                                         }
759                                 }
760                                 else
761                                 {
762                                 if(p.b->isinternal())
763                                         {
764                                         stack[depth++]=sStkNN(p.a,p.b->childs[0]);
765                                         stack[depth++]=sStkNN(p.a,p.b->childs[1]);
766                                         }
767                                         else
768                                         {
769                                         policy.Process(p.a,p.b);
770                                         }
771                                 }
772                         }
773                 } while(depth);
774         }
775 }
776
777 //
778 DBVT_PREFIX
779 inline void             btDbvt::collideTT(      const btDbvtNode* root0,
780                                                                         const btDbvtNode* root1,
781                                                                         const btTransform& xform,
782                                                                         DBVT_IPOLICY)
783 {
784 DBVT_CHECKTYPE
785 if(root0&&root1)
786         {
787         btAlignedObjectArray<sStkNN>    stack;
788         int                                                             depth=1;
789         int                                                             treshold=DOUBLE_STACKSIZE-4;
790         stack.resize(DOUBLE_STACKSIZE);
791         stack[0]=sStkNN(root0,root1);
792         do      {
793                 sStkNN  p=stack[--depth];
794                 if(Intersect(p.a->volume,p.b->volume,xform))
795                         {
796                         if(depth>treshold)
797                                 {
798                                 stack.resize(stack.size()*2);
799                                 treshold=stack.size()-4;
800                                 }
801                         if(p.a->isinternal())
802                                 {
803                                 if(p.b->isinternal())
804                                         {                                       
805                                         stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
806                                         stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
807                                         stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
808                                         stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
809                                         }
810                                         else
811                                         {
812                                         stack[depth++]=sStkNN(p.a->childs[0],p.b);
813                                         stack[depth++]=sStkNN(p.a->childs[1],p.b);
814                                         }
815                                 }
816                                 else
817                                 {
818                                 if(p.b->isinternal())
819                                         {
820                                         stack[depth++]=sStkNN(p.a,p.b->childs[0]);
821                                         stack[depth++]=sStkNN(p.a,p.b->childs[1]);
822                                         }
823                                         else
824                                         {
825                                         policy.Process(p.a,p.b);
826                                         }
827                                 }
828                         }
829                 } while(depth);
830         }
831 }
832
833 //
834 DBVT_PREFIX
835 inline void             btDbvt::collideTT(      const btDbvtNode* root0,
836                                                                         const btTransform& xform0,
837                                                                         const btDbvtNode* root1,
838                                                                         const btTransform& xform1,
839                                                                         DBVT_IPOLICY)
840 {
841 const btTransform       xform=xform0.inverse()*xform1;
842 collideTT(root0,root1,xform,policy);
843 }
844
845 //
846 DBVT_PREFIX
847 inline void             btDbvt::collideTV(      const btDbvtNode* root,
848                                                                         const btDbvtVolume& vol,
849                                                                         DBVT_IPOLICY)
850 {
851 DBVT_CHECKTYPE
852 if(root)
853         {
854         ATTRIBUTE_ALIGNED16(btDbvtVolume)               volume(vol);
855         btAlignedObjectArray<const btDbvtNode*> stack;
856         stack.reserve(SIMPLE_STACKSIZE);
857         stack.push_back(root);
858         do      {
859                 const btDbvtNode*       n=stack[stack.size()-1];
860                 stack.pop_back();
861                 if(Intersect(n->volume,volume))
862                         {
863                         if(n->isinternal())
864                                 {
865                                 stack.push_back(n->childs[0]);
866                                 stack.push_back(n->childs[1]);
867                                 }
868                                 else
869                                 {
870                                 policy.Process(n);
871                                 }
872                         }
873                 } while(stack.size()>0);
874         }
875 }
876
877 //
878 DBVT_PREFIX
879 inline void             btDbvt::collideRAY(     const btDbvtNode* root,
880                                                                         const btVector3& origin,
881                                                                         const btVector3& direction,
882                                                                         DBVT_IPOLICY)
883 {
884 DBVT_CHECKTYPE
885 if(root)
886         {
887         const btVector3 normal=direction.normalized();
888         const btVector3 invdir( 1/normal.x(),
889                                                         1/normal.y(),
890                                                         1/normal.z());
891         const unsigned  signs[]={       direction.x()<0,
892                                                                 direction.y()<0,
893                                                                 direction.z()<0};
894         btAlignedObjectArray<const btDbvtNode*> stack;
895         stack.reserve(SIMPLE_STACKSIZE);
896         stack.push_back(root);
897         do      {
898                 const btDbvtNode*       node=stack[stack.size()-1];
899                 stack.pop_back();
900                 if(Intersect(node->volume,origin,invdir,signs))
901                         {
902                         if(node->isinternal())
903                                 {
904                                 stack.push_back(node->childs[0]);
905                                 stack.push_back(node->childs[1]);
906                                 }
907                                 else
908                                 {
909                                 policy.Process(node);
910                                 }
911                         }
912                 } while(stack.size());
913         }
914 }
915
916 //
917 DBVT_PREFIX
918 inline void             btDbvt::collideKDOP(const btDbvtNode* root,
919                                                                         const btVector3* normals,
920                                                                         const btScalar* offsets,
921                                                                         int count,
922                                                                         DBVT_IPOLICY)
923 {
924 DBVT_CHECKTYPE
925 if(root)
926         {
927         const int                                               inside=(1<<count)-1;
928         btAlignedObjectArray<sStkNP>    stack;
929         int                                                             signs[sizeof(unsigned)*8];
930         btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
931         for(int i=0;i<count;++i)
932                 {
933                 signs[i]=       ((normals[i].x()>=0)?1:0)+
934                                         ((normals[i].y()>=0)?2:0)+
935                                         ((normals[i].z()>=0)?4:0);
936                 }
937         stack.reserve(SIMPLE_STACKSIZE);
938         stack.push_back(sStkNP(root,0));
939         do      {
940                 sStkNP  se=stack[stack.size()-1];
941                 bool    out=false;
942                 stack.pop_back();
943                 for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
944                         {
945                         if(0==(se.mask&j))
946                                 {
947                                 const int       side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
948                                 switch(side)
949                                         {
950                                         case    -1:     out=true;break;
951                                         case    +1:     se.mask|=j;break;
952                                         }
953                                 }
954                         }
955                 if(!out)
956                         {
957                         if((se.mask!=inside)&&(se.node->isinternal()))
958                                 {
959                                 stack.push_back(sStkNP(se.node->childs[0],se.mask));
960                                 stack.push_back(sStkNP(se.node->childs[1],se.mask));
961                                 }
962                                 else
963                                 {
964                                 if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy);
965                                 }
966                         }
967                 } while(stack.size());
968         }
969 }
970
971 //
972 DBVT_PREFIX
973 inline void             btDbvt::collideOCL(     const btDbvtNode* root,
974                                                                         const btVector3* normals,
975                                                                         const btScalar* offsets,
976                                                                         const btVector3& sortaxis,
977                                                                         int count,
978                                                                         DBVT_IPOLICY,
979                                                                         bool fsort)
980 {
981 DBVT_CHECKTYPE
982 if(root)
983         {
984         const unsigned                                  srtsgns=(sortaxis[0]>=0?1:0)+
985                                                                                         (sortaxis[1]>=0?2:0)+
986                                                                                         (sortaxis[2]>=0?4:0);
987         const int                                               inside=(1<<count)-1;
988         btAlignedObjectArray<sStkNPS>   stock;
989         btAlignedObjectArray<int>               ifree;
990         btAlignedObjectArray<int>               stack;
991         int                                                             signs[sizeof(unsigned)*8];
992         btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
993         for(int i=0;i<count;++i)
994                 {
995                 signs[i]=       ((normals[i].x()>=0)?1:0)+
996                                         ((normals[i].y()>=0)?2:0)+
997                                         ((normals[i].z()>=0)?4:0);
998                 }
999         stock.reserve(SIMPLE_STACKSIZE);
1000         stack.reserve(SIMPLE_STACKSIZE);
1001         ifree.reserve(SIMPLE_STACKSIZE);
1002         stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns))));
1003         do      {
1004                 const int       id=stack[stack.size()-1];
1005                 sStkNPS         se=stock[id];
1006                 stack.pop_back();ifree.push_back(id);
1007                 if(se.mask!=inside)
1008                         {
1009                         bool    out=false;
1010                         for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
1011                                 {
1012                                 if(0==(se.mask&j))
1013                                         {
1014                                         const int       side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
1015                                         switch(side)
1016                                                 {
1017                                                 case    -1:     out=true;break;
1018                                                 case    +1:     se.mask|=j;break;
1019                                                 }
1020                                         }
1021                                 }
1022                         if(out) continue;
1023                         }
1024                 if(policy.Descent(se.node))
1025                         {
1026                         if(se.node->isinternal())
1027                                 {
1028                                 const btDbvtNode* pns[]={       se.node->childs[0],se.node->childs[1]};
1029                                 sStkNPS         nes[]={ sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)),
1030                                                                         sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))};
1031                                 const int       q=nes[0].value<nes[1].value?1:0;                                
1032                                 int                     j=stack.size();
1033                                 if(fsort&&(j>0))
1034                                         {
1035                                         /* Insert 0     */ 
1036                                         j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size());
1037                                         stack.push_back(0);
1038                                         #if DBVT_USE_MEMMOVE
1039                                         memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
1040                                         #else
1041                                         for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
1042                                         #endif
1043                                         stack[j]=allocate(ifree,stock,nes[q]);
1044                                         /* Insert 1     */ 
1045                                         j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size());
1046                                         stack.push_back(0);
1047                                         #if DBVT_USE_MEMMOVE
1048                                         memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
1049                                         #else
1050                                         for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
1051                                         #endif
1052                                         stack[j]=allocate(ifree,stock,nes[1-q]);
1053                                         }
1054                                         else
1055                                         {
1056                                         stack.push_back(allocate(ifree,stock,nes[q]));
1057                                         stack.push_back(allocate(ifree,stock,nes[1-q]));
1058                                         }
1059                                 }
1060                                 else
1061                                 {
1062                                 policy.Process(se.node,se.value);
1063                                 }
1064                         }
1065                 } while(stack.size());
1066         }
1067 }
1068
1069 //
1070 DBVT_PREFIX
1071 inline void             btDbvt::collideTU(      const btDbvtNode* root,
1072                                                                         DBVT_IPOLICY)
1073 {
1074 DBVT_CHECKTYPE
1075 if(root)
1076         {
1077         btAlignedObjectArray<const btDbvtNode*> stack;
1078         stack.reserve(SIMPLE_STACKSIZE);
1079         stack.push_back(root);
1080         do      {
1081                 const btDbvtNode*       n=stack[stack.size()-1];
1082                 stack.pop_back();
1083                 if(policy.Descent(n))
1084                         {
1085                         if(n->isinternal())
1086                                 { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); }
1087                                 else
1088                                 { policy.Process(n); }
1089                         }
1090                 } while(stack.size()>0);
1091         }
1092 }
1093
1094 //
1095 // PP Cleanup
1096 //
1097
1098 #undef DBVT_USE_MEMMOVE
1099 #undef DBVT_USE_TEMPLATE
1100 #undef DBVT_VIRTUAL_DTOR
1101 #undef DBVT_VIRTUAL
1102 #undef DBVT_PREFIX
1103 #undef DBVT_IPOLICY
1104 #undef DBVT_CHECKTYPE
1105 #undef DBVT_IMPL_GENERIC
1106 #undef DBVT_IMPL_SSE
1107 #undef DBVT_USE_INTRINSIC_SSE
1108 #undef DBVT_SELECT_IMPL
1109 #undef DBVT_MERGE_IMPL
1110 #undef DBVT_INT0_IMPL
1111
1112 #endif