Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.h @ 1963

Last change on this file since 1963 was 1963, checked in by rgrieder, 16 years ago

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