2
Bullet Continuous Collision Detection and Physics Library
3
Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/
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:
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.
15
///btDbvt implementation by Nathanael Presson
17
#ifndef BT_DYNAMIC_BOUNDING_VOLUME_TREE_H
18
#define BT_DYNAMIC_BOUNDING_VOLUME_TREE_H
20
#include "LinearMath/btAlignedObjectArray.h"
21
#include "LinearMath/btVector3.h"
22
#include "LinearMath/btTransform.h"
25
// Compile time configuration
29
// Implementation profiles
30
#define DBVT_IMPL_GENERIC 0 // Generic implementation
31
#define DBVT_IMPL_SSE 1 // SSE
33
// Template implementation of ICollide
34
#ifdef WIN32_AVOID_SSE_WHEN_EMBEDDED_INSIDE_BLENDER //there is always some weird compiler that breaks SSE builds
35
#if (defined (_MSC_VER) && _MSC_VER >= 1400)
36
#define DBVT_USE_TEMPLATE 1
38
#define DBVT_USE_TEMPLATE 0
41
#define DBVT_USE_TEMPLATE 0
44
// Use only intrinsics instead of inline asm
45
#define DBVT_USE_INTRINSIC_SSE 1
47
// Using memmov for collideOCL
48
#define DBVT_USE_MEMMOVE 1
50
// Enable benchmarking code
51
#define DBVT_ENABLE_BENCHMARK 0
54
#define DBVT_INLINE SIMD_FORCE_INLINE
57
#define DBVT_ALIGN __declspec(align(16))
62
// Specific methods implementation
64
#ifdef WIN32_AVOID_SSE_WHEN_EMBEDDED_INSIDE_BLENDER //there is always some weird compiler that breaks SSE builds
65
#define DBVT_SELECT_IMPL DBVT_IMPL_SSE
66
#define DBVT_MERGE_IMPL DBVT_IMPL_SSE
67
#define DBVT_INT0_IMPL DBVT_IMPL_SSE
69
#define DBVT_SELECT_IMPL DBVT_IMPL_GENERIC
70
#define DBVT_MERGE_IMPL DBVT_IMPL_GENERIC
71
#define DBVT_INT0_IMPL DBVT_IMPL_GENERIC
74
#if (DBVT_SELECT_IMPL==DBVT_IMPL_SSE)|| \
75
(DBVT_MERGE_IMPL==DBVT_IMPL_SSE)|| \
76
(DBVT_INT0_IMPL==DBVT_IMPL_SSE)
77
#include <emmintrin.h>
81
// Auto config and checks
86
#define DBVT_VIRTUAL_DTOR(a)
87
#define DBVT_PREFIX template <typename T>
88
#define DBVT_IPOLICY T& policy
89
#define DBVT_CHECKTYPE static const ICollide& typechecker=*(T*)0;
91
#define DBVT_VIRTUAL_DTOR(a) virtual ~a() {}
92
#define DBVT_VIRTUAL virtual
94
#define DBVT_IPOLICY ICollide& policy
95
#define DBVT_CHECKTYPE
99
#ifndef __CELLOS_LV2__
105
#ifndef DBVT_USE_TEMPLATE
106
#error "DBVT_USE_TEMPLATE undefined"
109
#ifndef DBVT_USE_MEMMOVE
110
#error "DBVT_USE_MEMMOVE undefined"
113
#ifndef DBVT_ENABLE_BENCHMARK
114
#error "DBVT_ENABLE_BENCHMARK undefined"
117
#ifndef DBVT_SELECT_IMPL
118
#error "DBVT_SELECT_IMPL undefined"
121
#ifndef DBVT_MERGE_IMPL
122
#error "DBVT_MERGE_IMPL undefined"
125
#ifndef DBVT_INT0_IMPL
126
#error "DBVT_INT0_IMPL undefined"
136
DBVT_INLINE btVector3 Center() const { return((mi+mx)/2); }
137
DBVT_INLINE btVector3 Lengths() const { return(mx-mi); }
138
DBVT_INLINE btVector3 Extents() const { return((mx-mi)/2); }
139
DBVT_INLINE const btVector3& Mins() const { return(mi); }
140
DBVT_INLINE const btVector3& Maxs() const { return(mx); }
141
static inline btDbvtAabbMm FromCE(const btVector3& c,const btVector3& e);
142
static inline btDbvtAabbMm FromCR(const btVector3& c,btScalar r);
143
static inline btDbvtAabbMm FromMM(const btVector3& mi,const btVector3& mx);
144
static inline btDbvtAabbMm FromPoints(const btVector3* pts,int n);
145
static inline btDbvtAabbMm FromPoints(const btVector3** ppts,int n);
146
DBVT_INLINE void Expand(const btVector3& e);
147
DBVT_INLINE void SignedExpand(const btVector3& e);
148
DBVT_INLINE bool Contain(const btDbvtAabbMm& a) const;
149
DBVT_INLINE int Classify(const btVector3& n,btScalar o,int s) const;
150
DBVT_INLINE btScalar ProjectMinimum(const btVector3& v,unsigned signs) const;
151
DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a,
152
const btDbvtAabbMm& b);
153
DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a,
154
const btDbvtAabbMm& b,
155
const btTransform& xform);
156
DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a,
158
DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a,
159
const btVector3& org,
160
const btVector3& invdir,
161
const unsigned* signs);
162
DBVT_INLINE friend btScalar Proximity( const btDbvtAabbMm& a,
163
const btDbvtAabbMm& b);
164
DBVT_INLINE friend int Select( const btDbvtAabbMm& o,
165
const btDbvtAabbMm& a,
166
const btDbvtAabbMm& b);
167
DBVT_INLINE friend void Merge( const btDbvtAabbMm& a,
168
const btDbvtAabbMm& b,
170
DBVT_INLINE friend bool NotEqual( const btDbvtAabbMm& a,
171
const btDbvtAabbMm& b);
173
DBVT_INLINE void AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const;
179
typedef btDbvtAabbMm btDbvtVolume;
186
DBVT_INLINE bool isleaf() const { return(childs[1]==0); }
187
DBVT_INLINE bool isinternal() const { return(!isleaf()); }
189
btDbvtNode* childs[2];
195
///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree).
196
///This btDbvt is used for soft body collision detection and for the btDbvtBroadphase. It has a fast insert, remove and update of nodes.
197
///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure.
206
sStkNN(const btDbvtNode* na,const btDbvtNode* nb) : a(na),b(nb) {}
210
const btDbvtNode* node;
212
sStkNP(const btDbvtNode* n,unsigned m) : node(n),mask(m) {}
216
const btDbvtNode* node;
220
sStkNPS(const btDbvtNode* n,unsigned m,btScalar v) : node(n),mask(m),value(v) {}
224
const btDbvtNode* node;
226
sStkCLN(const btDbvtNode* n,btDbvtNode* p) : node(n),parent(p) {}
228
// Policies/Interfaces
233
DBVT_VIRTUAL_DTOR(ICollide)
234
DBVT_VIRTUAL void Process(const btDbvtNode*,const btDbvtNode*) {}
235
DBVT_VIRTUAL void Process(const btDbvtNode*) {}
236
DBVT_VIRTUAL void Process(const btDbvtNode* n,btScalar) { Process(n); }
237
DBVT_VIRTUAL bool Descent(const btDbvtNode*) { return(true); }
238
DBVT_VIRTUAL bool AllLeaves(const btDbvtNode*) { return(true); }
243
virtual ~IWriter() {}
244
virtual void Prepare(const btDbvtNode* root,int numnodes)=0;
245
virtual void WriteNode(const btDbvtNode*,int index,int parent,int child0,int child1)=0;
246
virtual void WriteLeaf(const btDbvtNode*,int index,int parent)=0;
252
virtual void CloneLeaf(btDbvtNode*) {}
257
SIMPLE_STACKSIZE = 64,
258
DOUBLE_STACKSIZE = SIMPLE_STACKSIZE*2
271
bool empty() const { return(0==m_root); }
272
void optimizeBottomUp();
273
void optimizeTopDown(int bu_treshold=128);
274
void optimizeIncremental(int passes);
275
btDbvtNode* insert(const btDbvtVolume& box,void* data);
276
void update(btDbvtNode* leaf,int lookahead=-1);
277
void update(btDbvtNode* leaf,const btDbvtVolume& volume);
278
bool update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity,btScalar margin);
279
bool update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity);
280
bool update(btDbvtNode* leaf,btDbvtVolume volume,btScalar margin);
281
void remove(btDbvtNode* leaf);
282
void write(IWriter* iwriter) const;
283
void clone(btDbvt& dest,IClone* iclone=0) const;
284
static int maxdepth(const btDbvtNode* node);
285
static int countLeaves(const btDbvtNode* node);
286
static void extractLeaves(const btDbvtNode* node,btAlignedObjectArray<const btDbvtNode*>& leaves);
287
#if DBVT_ENABLE_BENCHMARK
288
static void benchmark();
290
static void benchmark(){}
292
// DBVT_IPOLICY must support ICollide policy/interface
294
static void enumNodes( const btDbvtNode* root,
297
static void enumLeaves( const btDbvtNode* root,
300
static void collideTT( const btDbvtNode* root0,
301
const btDbvtNode* root1,
304
static void collideTT( const btDbvtNode* root0,
305
const btDbvtNode* root1,
306
const btTransform& xform,
309
static void collideTT( const btDbvtNode* root0,
310
const btTransform& xform0,
311
const btDbvtNode* root1,
312
const btTransform& xform1,
315
static void collideTV( const btDbvtNode* root,
316
const btDbvtVolume& volume,
319
static void collideRAY( const btDbvtNode* root,
320
const btVector3& origin,
321
const btVector3& direction,
324
static void collideKDOP(const btDbvtNode* root,
325
const btVector3* normals,
326
const btScalar* offsets,
330
static void collideOCL( const btDbvtNode* root,
331
const btVector3* normals,
332
const btScalar* offsets,
333
const btVector3& sortaxis,
338
static void collideTU( const btDbvtNode* root,
341
static DBVT_INLINE int nearest(const int* i,const btDbvt::sStkNPS* a,btScalar v,int l,int h)
347
if(a[i[m]].value>=v) l=m+1; else h=m;
351
static DBVT_INLINE int allocate( btAlignedObjectArray<int>& ifree,
352
btAlignedObjectArray<sStkNPS>& stock,
353
const sStkNPS& value)
357
{ i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; }
359
{ i=stock.size();stock.push_back(value); }
364
btDbvt(const btDbvt&) {}
372
inline btDbvtAabbMm btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e)
375
box.mi=c-e;box.mx=c+e;
380
inline btDbvtAabbMm btDbvtAabbMm::FromCR(const btVector3& c,btScalar r)
382
return(FromCE(c,btVector3(r,r,r)));
386
inline btDbvtAabbMm btDbvtAabbMm::FromMM(const btVector3& mi,const btVector3& mx)
394
inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3* pts,int n)
397
box.mi=box.mx=pts[0];
400
box.mi.setMin(pts[i]);
401
box.mx.setMax(pts[i]);
407
inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3** ppts,int n)
410
box.mi=box.mx=*ppts[0];
413
box.mi.setMin(*ppts[i]);
414
box.mx.setMax(*ppts[i]);
420
DBVT_INLINE void btDbvtAabbMm::Expand(const btVector3& e)
426
DBVT_INLINE void btDbvtAabbMm::SignedExpand(const btVector3& e)
428
if(e.x()>0) mx.setX(mx.x()+e[0]); else mi.setX(mi.x()+e[0]);
429
if(e.y()>0) mx.setY(mx.y()+e[1]); else mi.setY(mi.y()+e[1]);
430
if(e.z()>0) mx.setZ(mx.z()+e[2]); else mi.setZ(mi.z()+e[2]);
434
DBVT_INLINE bool btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const
436
return( (mi.x()<=a.mi.x())&&
445
DBVT_INLINE int btDbvtAabbMm::Classify(const btVector3& n,btScalar o,int s) const
450
case (0+0+0): px=btVector3(mi.x(),mi.y(),mi.z());
451
pi=btVector3(mx.x(),mx.y(),mx.z());break;
452
case (1+0+0): px=btVector3(mx.x(),mi.y(),mi.z());
453
pi=btVector3(mi.x(),mx.y(),mx.z());break;
454
case (0+2+0): px=btVector3(mi.x(),mx.y(),mi.z());
455
pi=btVector3(mx.x(),mi.y(),mx.z());break;
456
case (1+2+0): px=btVector3(mx.x(),mx.y(),mi.z());
457
pi=btVector3(mi.x(),mi.y(),mx.z());break;
458
case (0+0+4): px=btVector3(mi.x(),mi.y(),mx.z());
459
pi=btVector3(mx.x(),mx.y(),mi.z());break;
460
case (1+0+4): px=btVector3(mx.x(),mi.y(),mx.z());
461
pi=btVector3(mi.x(),mx.y(),mi.z());break;
462
case (0+2+4): px=btVector3(mi.x(),mx.y(),mx.z());
463
pi=btVector3(mx.x(),mi.y(),mi.z());break;
464
case (1+2+4): px=btVector3(mx.x(),mx.y(),mx.z());
465
pi=btVector3(mi.x(),mi.y(),mi.z());break;
467
if((dot(n,px)+o)<0) return(-1);
468
if((dot(n,pi)+o)>=0) return(+1);
473
DBVT_INLINE btScalar btDbvtAabbMm::ProjectMinimum(const btVector3& v,unsigned signs) const
475
const btVector3* b[]={&mx,&mi};
476
const btVector3 p( b[(signs>>0)&1]->x(),
477
b[(signs>>1)&1]->y(),
478
b[(signs>>2)&1]->z());
483
DBVT_INLINE void btDbvtAabbMm::AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const
488
{ smi+=mx[i]*d[i];smx+=mi[i]*d[i]; }
490
{ smi+=mi[i]*d[i];smx+=mx[i]*d[i]; }
495
DBVT_INLINE bool Intersect( const btDbvtAabbMm& a,
496
const btDbvtAabbMm& b)
498
#if DBVT_INT0_IMPL == DBVT_IMPL_SSE
499
const __m128 rt(_mm_or_ps( _mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)),
500
_mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi))));
501
const __int32* pu((const __int32*)&rt);
502
return((pu[0]|pu[1]|pu[2])==0);
504
return( (a.mi.x()<=b.mx.x())&&
505
(a.mx.x()>=b.mi.x())&&
506
(a.mi.y()<=b.mx.y())&&
507
(a.mx.y()>=b.mi.y())&&
508
(a.mi.z()<=b.mx.z())&&
509
(a.mx.z()>=b.mi.z()));
514
DBVT_INLINE bool Intersect( const btDbvtAabbMm& a,
515
const btDbvtAabbMm& b,
516
const btTransform& xform)
518
const btVector3 d0=xform*b.Center()-a.Center();
519
const btVector3 d1=d0*xform.getBasis();
520
btScalar s0[2]={0,0};
521
btScalar s1[2]={dot(xform.getOrigin(),d0),s1[0]};
522
a.AddSpan(d0,s0[0],s0[1]);
523
b.AddSpan(d1,s1[0],s1[1]);
524
if(s0[0]>(s1[1])) return(false);
525
if(s0[1]<(s1[0])) return(false);
530
DBVT_INLINE bool Intersect( const btDbvtAabbMm& a,
533
return( (b.x()>=a.mi.x())&&
542
DBVT_INLINE bool Intersect( const btDbvtAabbMm& a,
543
const btVector3& org,
544
const btVector3& invdir,
545
const unsigned* signs)
548
const btVector3 b0((a.mi-org)*invdir);
549
const btVector3 b1((a.mx-org)*invdir);
550
const btVector3 tmin(btMin(b0[0],b1[0]),btMin(b0[1],b1[1]),btMin(b0[2],b1[2]));
551
const btVector3 tmax(btMax(b0[0],b1[0]),btMax(b0[1],b1[1]),btMax(b0[2],b1[2]));
552
const btScalar tin=btMax(tmin[0],btMax(tmin[1],tmin[2]));
553
const btScalar tout=btMin(tmax[0],btMin(tmax[1],tmax[2]));
556
const btVector3* bounds[2]={&a.mi,&a.mx};
557
btScalar txmin=(bounds[ signs[0]]->x()-org[0])*invdir[0];
558
btScalar txmax=(bounds[1-signs[0]]->x()-org[0])*invdir[0];
559
const btScalar tymin=(bounds[ signs[1]]->y()-org[1])*invdir[1];
560
const btScalar tymax=(bounds[1-signs[1]]->y()-org[1])*invdir[1];
561
if((txmin>tymax)||(tymin>txmax)) return(false);
562
if(tymin>txmin) txmin=tymin;
563
if(tymax<txmax) txmax=tymax;
564
const btScalar tzmin=(bounds[ signs[2]]->z()-org[2])*invdir[2];
565
const btScalar tzmax=(bounds[1-signs[2]]->z()-org[2])*invdir[2];
566
if((txmin>tzmax)||(tzmin>txmax)) return(false);
567
if(tzmin>txmin) txmin=tzmin;
568
if(tzmax<txmax) txmax=tzmax;
574
DBVT_INLINE btScalar Proximity( const btDbvtAabbMm& a,
575
const btDbvtAabbMm& b)
577
const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx);
578
return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z()));
582
DBVT_INLINE int Select( const btDbvtAabbMm& o,
583
const btDbvtAabbMm& a,
584
const btDbvtAabbMm& b)
586
#if DBVT_SELECT_IMPL == DBVT_IMPL_SSE
587
static DBVT_ALIGN const unsigned __int32 mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff};
588
// TODO: the intrinsic version is 11% slower
589
#if DBVT_USE_INTRINSIC_SSE
590
__m128 omi(_mm_load_ps(o.mi));
591
omi=_mm_add_ps(omi,_mm_load_ps(o.mx));
592
__m128 ami(_mm_load_ps(a.mi));
593
ami=_mm_add_ps(ami,_mm_load_ps(a.mx));
594
ami=_mm_sub_ps(ami,omi);
595
ami=_mm_and_ps(ami,_mm_load_ps((const float*)mask));
596
__m128 bmi(_mm_load_ps(b.mi));
597
bmi=_mm_add_ps(bmi,_mm_load_ps(b.mx));
598
bmi=_mm_sub_ps(bmi,omi);
599
bmi=_mm_and_ps(bmi,_mm_load_ps((const float*)mask));
600
__m128 t0(_mm_movehl_ps(ami,ami));
601
ami=_mm_add_ps(ami,t0);
602
ami=_mm_add_ss(ami,_mm_shuffle_ps(ami,ami,1));
603
__m128 t1(_mm_movehl_ps(bmi,bmi));
604
bmi=_mm_add_ps(bmi,t1);
605
bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1));
606
return(_mm_cmple_ss(bmi,ami).m128_u32[0]&1);
608
DBVT_ALIGN __int32 r[1];
639
return(Proximity(o,a)<Proximity(o,b)?0:1);
644
DBVT_INLINE void Merge( const btDbvtAabbMm& a,
645
const btDbvtAabbMm& b,
648
#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
649
__m128 ami(_mm_load_ps(a.mi));
650
__m128 amx(_mm_load_ps(a.mx));
651
__m128 bmi(_mm_load_ps(b.mi));
652
__m128 bmx(_mm_load_ps(b.mx));
653
ami=_mm_min_ps(ami,bmi);
654
amx=_mm_max_ps(amx,bmx);
655
_mm_store_ps(r.mi,ami);
656
_mm_store_ps(r.mx,amx);
660
if(a.mi[i]<b.mi[i]) r.mi[i]=a.mi[i]; else r.mi[i]=b.mi[i];
661
if(a.mx[i]>b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i];
667
DBVT_INLINE bool NotEqual( const btDbvtAabbMm& a,
668
const btDbvtAabbMm& b)
670
return( (a.mi.x()!=b.mi.x())||
671
(a.mi.y()!=b.mi.y())||
672
(a.mi.z()!=b.mi.z())||
673
(a.mx.x()!=b.mx.x())||
674
(a.mx.y()!=b.mx.y())||
675
(a.mx.z()!=b.mx.z()));
684
inline void btDbvt::enumNodes( const btDbvtNode* root,
688
policy.Process(root);
689
if(root->isinternal())
691
enumNodes(root->childs[0],policy);
692
enumNodes(root->childs[1],policy);
698
inline void btDbvt::enumLeaves( const btDbvtNode* root,
702
if(root->isinternal())
704
enumLeaves(root->childs[0],policy);
705
enumLeaves(root->childs[1],policy);
709
policy.Process(root);
715
inline void btDbvt::collideTT( const btDbvtNode* root0,
716
const btDbvtNode* root1,
722
btAlignedObjectArray<sStkNN> stack;
724
int treshold=DOUBLE_STACKSIZE-4;
725
stack.resize(DOUBLE_STACKSIZE);
726
stack[0]=sStkNN(root0,root1);
728
sStkNN p=stack[--depth];
731
stack.resize(stack.size()*2);
732
treshold=stack.size()-4;
736
if(p.a->isinternal())
738
stack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]);
739
stack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]);
740
stack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]);
743
else if(Intersect(p.a->volume,p.b->volume))
745
if(p.a->isinternal())
747
if(p.b->isinternal())
749
stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
750
stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
751
stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
752
stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
756
stack[depth++]=sStkNN(p.a->childs[0],p.b);
757
stack[depth++]=sStkNN(p.a->childs[1],p.b);
762
if(p.b->isinternal())
764
stack[depth++]=sStkNN(p.a,p.b->childs[0]);
765
stack[depth++]=sStkNN(p.a,p.b->childs[1]);
769
policy.Process(p.a,p.b);
779
inline void btDbvt::collideTT( const btDbvtNode* root0,
780
const btDbvtNode* root1,
781
const btTransform& xform,
787
btAlignedObjectArray<sStkNN> stack;
789
int treshold=DOUBLE_STACKSIZE-4;
790
stack.resize(DOUBLE_STACKSIZE);
791
stack[0]=sStkNN(root0,root1);
793
sStkNN p=stack[--depth];
794
if(Intersect(p.a->volume,p.b->volume,xform))
798
stack.resize(stack.size()*2);
799
treshold=stack.size()-4;
801
if(p.a->isinternal())
803
if(p.b->isinternal())
805
stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
806
stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
807
stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
808
stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
812
stack[depth++]=sStkNN(p.a->childs[0],p.b);
813
stack[depth++]=sStkNN(p.a->childs[1],p.b);
818
if(p.b->isinternal())
820
stack[depth++]=sStkNN(p.a,p.b->childs[0]);
821
stack[depth++]=sStkNN(p.a,p.b->childs[1]);
825
policy.Process(p.a,p.b);
835
inline void btDbvt::collideTT( const btDbvtNode* root0,
836
const btTransform& xform0,
837
const btDbvtNode* root1,
838
const btTransform& xform1,
841
const btTransform xform=xform0.inverse()*xform1;
842
collideTT(root0,root1,xform,policy);
847
inline void btDbvt::collideTV( const btDbvtNode* root,
848
const btDbvtVolume& vol,
854
ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol);
855
btAlignedObjectArray<const btDbvtNode*> stack;
856
stack.reserve(SIMPLE_STACKSIZE);
857
stack.push_back(root);
859
const btDbvtNode* n=stack[stack.size()-1];
861
if(Intersect(n->volume,volume))
865
stack.push_back(n->childs[0]);
866
stack.push_back(n->childs[1]);
873
} while(stack.size()>0);
879
inline void btDbvt::collideRAY( const btDbvtNode* root,
880
const btVector3& origin,
881
const btVector3& direction,
887
const btVector3 normal=direction.normalized();
888
const btVector3 invdir( 1/normal.x(),
891
const unsigned signs[]={ direction.x()<0,
894
btAlignedObjectArray<const btDbvtNode*> stack;
895
stack.reserve(SIMPLE_STACKSIZE);
896
stack.push_back(root);
898
const btDbvtNode* node=stack[stack.size()-1];
900
if(Intersect(node->volume,origin,invdir,signs))
902
if(node->isinternal())
904
stack.push_back(node->childs[0]);
905
stack.push_back(node->childs[1]);
909
policy.Process(node);
912
} while(stack.size());
918
inline void btDbvt::collideKDOP(const btDbvtNode* root,
919
const btVector3* normals,
920
const btScalar* offsets,
927
const int inside=(1<<count)-1;
928
btAlignedObjectArray<sStkNP> stack;
929
int signs[sizeof(unsigned)*8];
930
btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
931
for(int i=0;i<count;++i)
933
signs[i]= ((normals[i].x()>=0)?1:0)+
934
((normals[i].y()>=0)?2:0)+
935
((normals[i].z()>=0)?4:0);
937
stack.reserve(SIMPLE_STACKSIZE);
938
stack.push_back(sStkNP(root,0));
940
sStkNP se=stack[stack.size()-1];
943
for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
947
const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
950
case -1: out=true;break;
951
case +1: se.mask|=j;break;
957
if((se.mask!=inside)&&(se.node->isinternal()))
959
stack.push_back(sStkNP(se.node->childs[0],se.mask));
960
stack.push_back(sStkNP(se.node->childs[1],se.mask));
964
if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy);
967
} while(stack.size());
973
inline void btDbvt::collideOCL( const btDbvtNode* root,
974
const btVector3* normals,
975
const btScalar* offsets,
976
const btVector3& sortaxis,
984
const unsigned srtsgns=(sortaxis[0]>=0?1:0)+
985
(sortaxis[1]>=0?2:0)+
986
(sortaxis[2]>=0?4:0);
987
const int inside=(1<<count)-1;
988
btAlignedObjectArray<sStkNPS> stock;
989
btAlignedObjectArray<int> ifree;
990
btAlignedObjectArray<int> stack;
991
int signs[sizeof(unsigned)*8];
992
btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
993
for(int i=0;i<count;++i)
995
signs[i]= ((normals[i].x()>=0)?1:0)+
996
((normals[i].y()>=0)?2:0)+
997
((normals[i].z()>=0)?4:0);
999
stock.reserve(SIMPLE_STACKSIZE);
1000
stack.reserve(SIMPLE_STACKSIZE);
1001
ifree.reserve(SIMPLE_STACKSIZE);
1002
stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns))));
1004
const int id=stack[stack.size()-1];
1005
sStkNPS se=stock[id];
1006
stack.pop_back();ifree.push_back(id);
1010
for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
1014
const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
1017
case -1: out=true;break;
1018
case +1: se.mask|=j;break;
1024
if(policy.Descent(se.node))
1026
if(se.node->isinternal())
1028
const btDbvtNode* pns[]={ se.node->childs[0],se.node->childs[1]};
1029
sStkNPS nes[]={ sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)),
1030
sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))};
1031
const int q=nes[0].value<nes[1].value?1:0;
1036
j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size());
1038
#if DBVT_USE_MEMMOVE
1039
memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
1041
for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
1043
stack[j]=allocate(ifree,stock,nes[q]);
1045
j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size());
1047
#if DBVT_USE_MEMMOVE
1048
memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
1050
for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
1052
stack[j]=allocate(ifree,stock,nes[1-q]);
1056
stack.push_back(allocate(ifree,stock,nes[q]));
1057
stack.push_back(allocate(ifree,stock,nes[1-q]));
1062
policy.Process(se.node,se.value);
1065
} while(stack.size());
1071
inline void btDbvt::collideTU( const btDbvtNode* root,
1077
btAlignedObjectArray<const btDbvtNode*> stack;
1078
stack.reserve(SIMPLE_STACKSIZE);
1079
stack.push_back(root);
1081
const btDbvtNode* n=stack[stack.size()-1];
1083
if(policy.Descent(n))
1086
{ stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); }
1088
{ policy.Process(n); }
1090
} while(stack.size()>0);
1098
#undef DBVT_USE_MEMMOVE
1099
#undef DBVT_USE_TEMPLATE
1100
#undef DBVT_VIRTUAL_DTOR
1104
#undef DBVT_CHECKTYPE
1105
#undef DBVT_IMPL_GENERIC
1106
#undef DBVT_IMPL_SSE
1107
#undef DBVT_USE_INTRINSIC_SSE
1108
#undef DBVT_SELECT_IMPL
1109
#undef DBVT_MERGE_IMPL
1110
#undef DBVT_INT0_IMPL
added
removed
extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h
