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