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