Context Navigation

← Previous Revision
Latest Revision
Next Revision →
Blame
Revision Log

btDbvt.h @ 2178

Last change on this file since 2178 was 2119, checked in by rgrieder, 16 years ago
Merged physics branch into physics_new branch.
Property svn:eol-style set to `native`
File size: 27.9 KB

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

Note: See TracBrowser for help on using the repository browser.

Context Navigation

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

Download in other formats: