Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/trunk/src/external/bullet/BulletCollision/BroadphaseCollision/btDbvt.h @ 6725

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

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

Download in other formats: