Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

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

Last change on this file since 2379 was 2192, checked in by rgrieder, 16 years ago
Reverted all changes of attempt to update physics branch.
Property svn:eol-style set to `native`
File size: 27.9 KB

Rev	Line
[1963]	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
[1972]	35	#if (defined (_MSC_VER) && _MSC_VER >= 1400)
	36	#define DBVT_USE_TEMPLATE 1
	37	#else
	38	#define DBVT_USE_TEMPLATE 0
[1963]	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	{
[1972]	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);
[1963]	173	private:
[1972]	174	DBVT_INLINE void AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const;
[1963]	175	private:
[1972]	176	btVector3 mi,mx;
[1963]	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 {
[1972]	190	btDbvtNode* childs[2];
	191	void* data;
	192	int dataAsInt;
	193	};
[1963]	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
[1972]	200	{
[1963]	201	/* Stack element */
	202	struct sStkNN
[1972]	203	{
[1963]	204	const btDbvtNode* a;
	205	const btDbvtNode* b;
	206	sStkNN() {}
	207	sStkNN(const btDbvtNode* na,const btDbvtNode* nb) : a(na),b(nb) {}
[1972]	208	};
[1963]	209	struct sStkNP
[1972]	210	{
[1963]	211	const btDbvtNode* node;
	212	int mask;
	213	sStkNP(const btDbvtNode* n,unsigned m) : node(n),mask(m) {}
[1972]	214	};
[1963]	215	struct sStkNPS
[1972]	216	{
[1963]	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) {}
[1972]	222	};
[1963]	223	struct sStkCLN
[1972]	224	{
[1963]	225	const btDbvtNode* node;
	226	btDbvtNode* parent;
	227	sStkCLN(const btDbvtNode* n,btDbvtNode* p) : node(n),parent(p) {}
[1972]	228	};
[1963]	229	// Policies/Interfaces
[1972]	230
[1963]	231	/* ICollide */
	232	struct ICollide
[1972]	233	{
[1963]	234	DBVT_VIRTUAL_DTOR(ICollide)
[1972]	235	DBVT_VIRTUAL void Process(const btDbvtNode,const btDbvtNode) {}
[1963]	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); }
[1972]	240	};
[1963]	241	/* IWriter */
	242	struct IWriter
[1972]	243	{
[1963]	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;
[1972]	248	};
[1963]	249	/* IClone */
	250	struct IClone
[1972]	251	{
[1963]	252	virtual ~IClone() {}
	253	virtual void CloneLeaf(btDbvtNode*) {}
[1972]	254	};
	255
[1963]	256	// Constants
	257	enum {
[1972]	258	SIMPLE_STACKSIZE = 64,
	259	DOUBLE_STACKSIZE = SIMPLE_STACKSIZE*2
	260	};
	261
[1963]	262	// Fields
	263	btDbvtNode* m_root;
	264	btDbvtNode* m_free;
	265	int m_lkhd;
	266	int m_leaves;
	267	unsigned m_opath;
	268	// Methods
[1972]	269	btDbvt();
	270	~btDbvt();
[1963]	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);
[1972]	288	#if DBVT_ENABLE_BENCHMARK
[1963]	289	static void benchmark();
[1972]	290	#else
[1963]	291	static void benchmark(){}
[1972]	292	#endif
[1963]	293	// DBVT_IPOLICY must support ICollide policy/interface
	294	DBVT_PREFIX
[1972]	295	static void enumNodes( const btDbvtNode* root,
	296	DBVT_IPOLICY);
[1963]	297	DBVT_PREFIX
[1972]	298	static void enumLeaves( const btDbvtNode* root,
	299	DBVT_IPOLICY);
[1963]	300	DBVT_PREFIX
[1972]	301	static void collideTT( const btDbvtNode* root0,
	302	const btDbvtNode* root1,
	303	DBVT_IPOLICY);
[1963]	304	DBVT_PREFIX
[1972]	305	static void collideTT( const btDbvtNode* root0,
	306	const btDbvtNode* root1,
	307	const btTransform& xform,
	308	DBVT_IPOLICY);
[1963]	309	DBVT_PREFIX
[1972]	310	static void collideTT( const btDbvtNode* root0,
	311	const btTransform& xform0,
	312	const btDbvtNode* root1,
	313	const btTransform& xform1,
	314	DBVT_IPOLICY);
[1963]	315	DBVT_PREFIX
[1972]	316	static void collideTV( const btDbvtNode* root,
	317	const btDbvtVolume& volume,
	318	DBVT_IPOLICY);
[1963]	319	DBVT_PREFIX
[1972]	320	static void collideRAY( const btDbvtNode* root,
	321	const btVector3& origin,
	322	const btVector3& direction,
	323	DBVT_IPOLICY);
[1963]	324	DBVT_PREFIX
[1972]	325	static void collideKDOP(const btDbvtNode* root,
	326	const btVector3* normals,
	327	const btScalar* offsets,
	328	int count,
	329	DBVT_IPOLICY);
[1963]	330	DBVT_PREFIX
[1972]	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);
[1963]	338	DBVT_PREFIX
[1972]	339	static void collideTU( const btDbvtNode* root,
	340	DBVT_IPOLICY);
[1963]	341	// Helpers
	342	static DBVT_INLINE int nearest(const int* i,const btDbvt::sStkNPS* a,btScalar v,int l,int h)
[1972]	343	{
[1963]	344	int m=0;
	345	while(l<h)
[1972]	346	{
[1963]	347	m=(l+h)>>1;
	348	if(a[i[m]].value>=v) l=m+1; else h=m;
[1972]	349	}
	350	return(h);
[1963]	351	}
	352	static DBVT_INLINE int allocate( btAlignedObjectArray<int>& ifree,
[1972]	353	btAlignedObjectArray<sStkNPS>& stock,
	354	const sStkNPS& value)
	355	{
[1963]	356	int i;
	357	if(ifree.size()>0)
[1972]	358	{ i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; }
	359	else
	360	{ i=stock.size();stock.push_back(value); }
[1963]	361	return(i);
[1972]	362	}
[1963]	363	//
[1972]	364	private:
	365	btDbvt(const btDbvt&) {}
	366	};
[1963]	367
	368	//
	369	// Inline's
	370	//
	371
	372	//
	373	inline btDbvtAabbMm btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e)
	374	{
[1972]	375	btDbvtAabbMm box;
	376	box.mi=c-e;box.mx=c+e;
	377	return(box);
[1963]	378	}
[1972]	379
[1963]	380	//
	381	inline btDbvtAabbMm btDbvtAabbMm::FromCR(const btVector3& c,btScalar r)
	382	{
[1972]	383	return(FromCE(c,btVector3(r,r,r)));
[1963]	384	}
[1972]	385
[1963]	386	//
	387	inline btDbvtAabbMm btDbvtAabbMm::FromMM(const btVector3& mi,const btVector3& mx)
	388	{
[1972]	389	btDbvtAabbMm box;
	390	box.mi=mi;box.mx=mx;
	391	return(box);
[1963]	392	}
[1972]	393
[1963]	394	//
	395	inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3* pts,int n)
	396	{
[1972]	397	btDbvtAabbMm box;
	398	box.mi=box.mx=pts[0];
	399	for(int i=1;i<n;++i)
[1963]	400	{
[1972]	401	box.mi.setMin(pts[i]);
	402	box.mx.setMax(pts[i]);
[1963]	403	}
[1972]	404	return(box);
[1963]	405	}
	406
	407	//
	408	inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3** ppts,int n)
	409	{
[1972]	410	btDbvtAabbMm box;
	411	box.mi=box.mx=*ppts[0];
	412	for(int i=1;i<n;++i)
[1963]	413	{
[1972]	414	box.mi.setMin(*ppts[i]);
	415	box.mx.setMax(*ppts[i]);
[1963]	416	}
[1972]	417	return(box);
[1963]	418	}
	419
	420	//
	421	DBVT_INLINE void btDbvtAabbMm::Expand(const btVector3& e)
	422	{
[1972]	423	mi-=e;mx+=e;
[1963]	424	}
[1972]	425
[1963]	426	//
	427	DBVT_INLINE void btDbvtAabbMm::SignedExpand(const btVector3& e)
	428	{
[1972]	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]);
[1963]	432	}
[1972]	433
[1963]	434	//
	435	DBVT_INLINE bool btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const
	436	{
[1972]	437	return( (mi.x()<=a.mi.x())&&
[1963]	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	{
[1972]	448	btVector3 pi,px;
	449	switch(s)
[1963]	450	{
	451	case (0+0+0): px=btVector3(mi.x(),mi.y(),mi.z());
[1972]	452	pi=btVector3(mx.x(),mx.y(),mx.z());break;
[1963]	453	case (1+0+0): px=btVector3(mx.x(),mi.y(),mi.z());
[1972]	454	pi=btVector3(mi.x(),mx.y(),mx.z());break;
[1963]	455	case (0+2+0): px=btVector3(mi.x(),mx.y(),mi.z());
[1972]	456	pi=btVector3(mx.x(),mi.y(),mx.z());break;
[1963]	457	case (1+2+0): px=btVector3(mx.x(),mx.y(),mi.z());
[1972]	458	pi=btVector3(mi.x(),mi.y(),mx.z());break;
[1963]	459	case (0+0+4): px=btVector3(mi.x(),mi.y(),mx.z());
[1972]	460	pi=btVector3(mx.x(),mx.y(),mi.z());break;
[1963]	461	case (1+0+4): px=btVector3(mx.x(),mi.y(),mx.z());
[1972]	462	pi=btVector3(mi.x(),mx.y(),mi.z());break;
[1963]	463	case (0+2+4): px=btVector3(mi.x(),mx.y(),mx.z());
[1972]	464	pi=btVector3(mx.x(),mi.y(),mi.z());break;
[1963]	465	case (1+2+4): px=btVector3(mx.x(),mx.y(),mx.z());
[1972]	466	pi=btVector3(mi.x(),mi.y(),mi.z());break;
[1963]	467	}
[1972]	468	if((dot(n,px)+o)<0) return(-1);
	469	if((dot(n,pi)+o)>=0) return(+1);
	470	return(0);
[1963]	471	}
	472
	473	//
	474	DBVT_INLINE btScalar btDbvtAabbMm::ProjectMinimum(const btVector3& v,unsigned signs) const
	475	{
[1972]	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));
[1963]	481	}
	482
	483	//
	484	DBVT_INLINE void btDbvtAabbMm::AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const
	485	{
[1972]	486	for(int i=0;i<3;++i)
[1963]	487	{
[1972]	488	if(d[i]<0)
[1963]	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	}
[1972]	494
[1963]	495	//
	496	DBVT_INLINE bool Intersect( const btDbvtAabbMm& a,
[1972]	497	const btDbvtAabbMm& b)
[1963]	498	{
	499	#if DBVT_INT0_IMPL == DBVT_IMPL_SSE
[1972]	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);
[1963]	504	#else
[1972]	505	return( (a.mi.x()<=b.mx.x())&&
[1963]	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,
[1972]	516	const btDbvtAabbMm& b,
	517	const btTransform& xform)
[1963]	518	{
[1972]	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);
[1963]	528	}
	529
	530	//
	531	DBVT_INLINE bool Intersect( const btDbvtAabbMm& a,
[1972]	532	const btVector3& b)
[1963]	533	{
[1972]	534	return( (b.x()>=a.mi.x())&&
[1963]	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	//
[1972]	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	//
[1963]	575	DBVT_INLINE btScalar Proximity( const btDbvtAabbMm& a,
[1972]	576	const btDbvtAabbMm& b)
[1963]	577	{
[1972]	578	const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx);
	579	return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z()));
[1963]	580	}
	581
	582	//
	583	DBVT_INLINE int Select( const btDbvtAabbMm& o,
[1972]	584	const btDbvtAabbMm& a,
	585	const btDbvtAabbMm& b)
[1963]	586	{
	587	#if DBVT_SELECT_IMPL == DBVT_IMPL_SSE
[1972]	588	static DBVT_ALIGN const unsigned __int32 mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff};
[1963]	589	// TODO: the intrinsic version is 11% slower
[1972]	590	#if DBVT_USE_INTRINSIC_SSE
[1963]	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);
[1972]	608	#else
[1963]	609	DBVT_ALIGN __int32 r[1];
	610	__asm
[1972]	611	{
[1963]	612	mov eax,o
[1972]	613	mov ecx,a
	614	mov edx,b
	615	movaps xmm0,[eax]
[1963]	616	movaps xmm5,mask
[1972]	617	addps xmm0,[eax+16]
[1963]	618	movaps xmm1,[ecx]
	619	movaps xmm2,[edx]
	620	addps xmm1,[ecx+16]
	621	addps xmm2,[edx+16]
	622	subps xmm1,xmm0
[1972]	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	}
[1963]	637	return(r[0]&1);
[1972]	638	#endif
[1963]	639	#else
[1972]	640	return(Proximity(o,a)<Proximity(o,b)?0:1);
[1963]	641	#endif
	642	}
	643
	644	//
	645	DBVT_INLINE void Merge( const btDbvtAabbMm& a,
[1972]	646	const btDbvtAabbMm& b,
	647	btDbvtAabbMm& r)
[1963]	648	{
	649	#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
[1972]	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);
[1963]	658	#else
[1972]	659	for(int i=0;i<3;++i)
[1963]	660	{
[1972]	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];
[1963]	663	}
	664	#endif
	665	}
	666
	667	//
	668	DBVT_INLINE bool NotEqual( const btDbvtAabbMm& a,
[1972]	669	const btDbvtAabbMm& b)
[1963]	670	{
[1972]	671	return( (a.mi.x()!=b.mi.x())\|\|
[1963]	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,
[1972]	686	DBVT_IPOLICY)
[1963]	687	{
[1972]	688	DBVT_CHECKTYPE
	689	policy.Process(root);
	690	if(root->isinternal())
[1963]	691	{
[1972]	692	enumNodes(root->childs[0],policy);
	693	enumNodes(root->childs[1],policy);
[1963]	694	}
	695	}
	696
	697	//
	698	DBVT_PREFIX
	699	inline void btDbvt::enumLeaves( const btDbvtNode* root,
[1972]	700	DBVT_IPOLICY)
[1963]	701	{
[1972]	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	}
[1963]	712	}
	713
	714	//
	715	DBVT_PREFIX
	716	inline void btDbvt::collideTT( const btDbvtNode* root0,
[1972]	717	const btDbvtNode* root1,
	718	DBVT_IPOLICY)
[1963]	719	{
[1972]	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())
[1963]	738	{
[1972]	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]);
[1963]	742	}
[1972]	743	}
	744	else if(Intersect(p.a->volume,p.b->volume))
	745	{
	746	if(p.a->isinternal())
[1963]	747	{
[1972]	748	if(p.b->isinternal())
[1963]	749	{
[1972]	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]);
[1963]	754	}
[1972]	755	else
	756	{
	757	stack[depth++]=sStkNN(p.a->childs[0],p.b);
	758	stack[depth++]=sStkNN(p.a->childs[1],p.b);
	759	}
[1963]	760	}
[1972]	761	else
[1963]	762	{
[1972]	763	if(p.b->isinternal())
[1963]	764	{
[1972]	765	stack[depth++]=sStkNN(p.a,p.b->childs[0]);
	766	stack[depth++]=sStkNN(p.a,p.b->childs[1]);
[1963]	767	}
	768	else
	769	{
[1972]	770	policy.Process(p.a,p.b);
[1963]	771	}
	772	}
[1972]	773	}
	774	} while(depth);
	775	}
[1963]	776	}
	777
	778	//
	779	DBVT_PREFIX
	780	inline void btDbvt::collideTT( const btDbvtNode* root0,
[1972]	781	const btDbvtNode* root1,
	782	const btTransform& xform,
	783	DBVT_IPOLICY)
[1963]	784	{
[1972]	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)
[1963]	798	{
[1972]	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
[1963]	812	{
[1972]	813	stack[depth++]=sStkNN(p.a->childs[0],p.b);
	814	stack[depth++]=sStkNN(p.a->childs[1],p.b);
[1963]	815	}
[1972]	816	}
	817	else
	818	{
	819	if(p.b->isinternal())
[1963]	820	{
[1972]	821	stack[depth++]=sStkNN(p.a,p.b->childs[0]);
	822	stack[depth++]=sStkNN(p.a,p.b->childs[1]);
[1963]	823	}
	824	else
	825	{
[1972]	826	policy.Process(p.a,p.b);
[1963]	827	}
	828	}
[1972]	829	}
	830	} while(depth);
	831	}
[1963]	832	}
	833
	834	//
	835	DBVT_PREFIX
	836	inline void btDbvt::collideTT( const btDbvtNode* root0,
[1972]	837	const btTransform& xform0,
	838	const btDbvtNode* root1,
	839	const btTransform& xform1,
	840	DBVT_IPOLICY)
[1963]	841	{
[1972]	842	const btTransform xform=xform0.inverse()*xform1;
	843	collideTT(root0,root1,xform,policy);
[1963]	844	}
	845
	846	//
	847	DBVT_PREFIX
	848	inline void btDbvt::collideTV( const btDbvtNode* root,
[1972]	849	const btDbvtVolume& vol,
	850	DBVT_IPOLICY)
[1963]	851	{
[1972]	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())
[1963]	865	{
[1972]	866	stack.push_back(n->childs[0]);
	867	stack.push_back(n->childs[1]);
[1963]	868	}
[1972]	869	else
	870	{
	871	policy.Process(n);
	872	}
	873	}
	874	} while(stack.size()>0);
	875	}
[1963]	876	}
	877
	878	//
	879	DBVT_PREFIX
[1972]	880	inline void btDbvt::collideRAY( const btDbvtNode* root,
	881	const btVector3& origin,
	882	const btVector3& direction,
	883	DBVT_IPOLICY)
[1963]	884	{
[1972]	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())
[1963]	904	{
[1972]	905	stack.push_back(node->childs[0]);
	906	stack.push_back(node->childs[1]);
[1963]	907	}
[1972]	908	else
	909	{
	910	policy.Process(node);
	911	}
	912	}
	913	} while(stack.size());
	914	}
[1963]	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	{
[1972]	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)
[1963]	933	{
[1972]	934	signs[i]= ((normals[i].x()>=0)?1:0)+
[1963]	935	((normals[i].y()>=0)?2:0)+
	936	((normals[i].z()>=0)?4:0);
[1972]	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))
[1963]	947	{
[1972]	948	const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
	949	switch(side)
[1963]	950	{
[1972]	951	case -1: out=true;break;
	952	case +1: se.mask\|=j;break;
[1963]	953	}
	954	}
[1972]	955	}
	956	if(!out)
	957	{
	958	if((se.mask!=inside)&&(se.node->isinternal()))
[1963]	959	{
[1972]	960	stack.push_back(sStkNP(se.node->childs[0],se.mask));
	961	stack.push_back(sStkNP(se.node->childs[1],se.mask));
[1963]	962	}
[1972]	963	else
	964	{
	965	if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy);
	966	}
	967	}
	968	} while(stack.size());
	969	}
[1963]	970	}
	971
	972	//
	973	DBVT_PREFIX
	974	inline void btDbvt::collideOCL( const btDbvtNode* root,
[1972]	975	const btVector3* normals,
	976	const btScalar* offsets,
	977	const btVector3& sortaxis,
	978	int count,
	979	DBVT_IPOLICY,
	980	bool fsort)
[1963]	981	{
[1972]	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)
[1963]	995	{
[1972]	996	signs[i]= ((normals[i].x()>=0)?1:0)+
[1963]	997	((normals[i].y()>=0)?2:0)+
	998	((normals[i].z()>=0)?4:0);
[1972]	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)
[1963]	1012	{
[1972]	1013	if(0==(se.mask&j))
[1963]	1014	{
[1972]	1015	const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
	1016	switch(side)
[1963]	1017	{
[1972]	1018	case -1: out=true;break;
	1019	case +1: se.mask\|=j;break;
[1963]	1020	}
	1021	}
	1022	}
[1972]	1023	if(out) continue;
	1024	}
	1025	if(policy.Descent(se.node))
	1026	{
	1027	if(se.node->isinternal())
[1963]	1028	{
[1972]	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))
[1963]	1035	{
[1972]	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]);
[1963]	1054	}
	1055	else
	1056	{
[1972]	1057	stack.push_back(allocate(ifree,stock,nes[q]));
	1058	stack.push_back(allocate(ifree,stock,nes[1-q]));
[1963]	1059	}
	1060	}
[1972]	1061	else
	1062	{
	1063	policy.Process(se.node,se.value);
	1064	}
	1065	}
	1066	} while(stack.size());
	1067	}
[1963]	1068	}
	1069
	1070	//
	1071	DBVT_PREFIX
	1072	inline void btDbvt::collideTU( const btDbvtNode* root,
[1972]	1073	DBVT_IPOLICY)
[1963]	1074	{
[1972]	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	}
[1963]	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.

Download in other formats: