Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/kicklib/src/external/bullet/BulletCollision/BroadphaseCollision/btDbvt.h @ 8501

Last change on this file since 8501 was 7983, checked in by rgrieder, 14 years ago
Updated Bullet Physics Engine from v2.74 to v2.77
Property svn:eol-style set to `native`
File size: 31.2 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
[7983]	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
[7983]	60	#if defined (BT_USE_SSE) && defined (_WIN32)
[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
[7983]	95	#if !defined( __CELLOS_LV2__) && !defined(__MWERKS__)
[1963]	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	}
[7983]	487	if((btDot(n,px)+o)<0) return(-1);
	488	if((btDot(n,pi)+o)>=0) return(+1);
[2430]	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());
[7983]	499	return(btDot(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	{
[7983]	950	(void) rayTo;
[2430]	951	DBVT_CHECKTYPE
	952	if(root)
[1972]	953	{
[2430]	954	btVector3 resultNormal;
	955
	956	int depth=1;
	957	int treshold=DOUBLE_STACKSIZE-2;
	958	btAlignedObjectArray<const btDbvtNode*> stack;
	959	stack.resize(DOUBLE_STACKSIZE);
	960	stack[0]=root;
	961	btVector3 bounds[2];
	962	do
	963	{
	964	const btDbvtNode* node=stack[--depth];
[7983]	965	bounds[0] = node->volume.Mins()-aabbMax;
	966	bounds[1] = node->volume.Maxs()-aabbMin;
[2430]	967	btScalar tmin=1.f,lambda_min=0.f;
	968	unsigned int result1=false;
	969	result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
	970	if(result1)
[1972]	971	{
[2430]	972	if(node->isinternal())
[1963]	973	{
[2430]	974	if(depth>treshold)
	975	{
	976	stack.resize(stack.size()*2);
	977	treshold=stack.size()-2;
	978	}
	979	stack[depth++]=node->childs[0];
	980	stack[depth++]=node->childs[1];
[1963]	981	}
[1972]	982	else
	983	{
[2430]	984	policy.Process(node);
[1972]	985	}
	986	}
[2430]	987	} while(depth);
[1972]	988	}
[1963]	989	}
	990
	991	//
	992	DBVT_PREFIX
[2430]	993	inline void btDbvt::rayTest( const btDbvtNode* root,
	994	const btVector3& rayFrom,
	995	const btVector3& rayTo,
	996	DBVT_IPOLICY)
[1963]	997	{
[2430]	998	DBVT_CHECKTYPE
	999	if(root)
	1000	{
	1001	btVector3 rayDir = (rayTo-rayFrom);
	1002	rayDir.normalize ();
	1003
[7983]	1004	///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
[2430]	1005	btVector3 rayDirectionInverse;
[7983]	1006	rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
	1007	rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
	1008	rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
[2430]	1009	unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0};
	1010
	1011	btScalar lambda_max = rayDir.dot(rayTo-rayFrom);
	1012
	1013	btVector3 resultNormal;
	1014
	1015	btAlignedObjectArray<const btDbvtNode*> stack;
	1016
	1017	int depth=1;
	1018	int treshold=DOUBLE_STACKSIZE-2;
	1019
	1020	stack.resize(DOUBLE_STACKSIZE);
	1021	stack[0]=root;
	1022	btVector3 bounds[2];
	1023	do {
	1024	const btDbvtNode* node=stack[--depth];
	1025
	1026	bounds[0] = node->volume.Mins();
	1027	bounds[1] = node->volume.Maxs();
	1028
	1029	btScalar tmin=1.f,lambda_min=0.f;
	1030	unsigned int result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
	1031
	1032	#ifdef COMPARE_BTRAY_AABB2
	1033	btScalar param=1.f;
	1034	bool result2 = btRayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal);
	1035	btAssert(result1 == result2);
	1036	#endif //TEST_BTRAY_AABB2
	1037
	1038	if(result1)
[1963]	1039	{
[2430]	1040	if(node->isinternal())
	1041	{
	1042	if(depth>treshold)
	1043	{
	1044	stack.resize(stack.size()*2);
	1045	treshold=stack.size()-2;
	1046	}
	1047	stack[depth++]=node->childs[0];
	1048	stack[depth++]=node->childs[1];
	1049	}
	1050	else
	1051	{
	1052	policy.Process(node);
	1053	}
[1963]	1054	}
[2430]	1055	} while(depth);
	1056
	1057	}
[1963]	1058	}
	1059
	1060	//
	1061	DBVT_PREFIX
	1062	inline void btDbvt::collideKDOP(const btDbvtNode* root,
	1063	const btVector3* normals,
	1064	const btScalar* offsets,
	1065	int count,
	1066	DBVT_IPOLICY)
	1067	{
[2430]	1068	DBVT_CHECKTYPE
	1069	if(root)
[1963]	1070	{
[2430]	1071	const int inside=(1<<count)-1;
	1072	btAlignedObjectArray<sStkNP> stack;
	1073	int signs[sizeof(unsigned)*8];
	1074	btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
	1075	for(int i=0;i<count;++i)
	1076	{
	1077	signs[i]= ((normals[i].x()>=0)?1:0)+
[1963]	1078	((normals[i].y()>=0)?2:0)+
	1079	((normals[i].z()>=0)?4:0);
[2430]	1080	}
	1081	stack.reserve(SIMPLE_STACKSIZE);
	1082	stack.push_back(sStkNP(root,0));
	1083	do {
	1084	sStkNP se=stack[stack.size()-1];
	1085	bool out=false;
	1086	stack.pop_back();
	1087	for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
[1963]	1088	{
[2430]	1089	if(0==(se.mask&j))
[1963]	1090	{
[2430]	1091	const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
	1092	switch(side)
	1093	{
	1094	case -1: out=true;break;
	1095	case +1: se.mask\|=j;break;
	1096	}
[1963]	1097	}
	1098	}
[2430]	1099	if(!out)
[1963]	1100	{
[2430]	1101	if((se.mask!=inside)&&(se.node->isinternal()))
	1102	{
	1103	stack.push_back(sStkNP(se.node->childs[0],se.mask));
	1104	stack.push_back(sStkNP(se.node->childs[1],se.mask));
	1105	}
	1106	else
	1107	{
	1108	if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy);
	1109	}
[1963]	1110	}
[2430]	1111	} while(stack.size());
	1112	}
[1963]	1113	}
	1114
	1115	//
	1116	DBVT_PREFIX
	1117	inline void btDbvt::collideOCL( const btDbvtNode* root,
[2430]	1118	const btVector3* normals,
	1119	const btScalar* offsets,
	1120	const btVector3& sortaxis,
	1121	int count,
	1122	DBVT_IPOLICY,
	1123	bool fsort)
[1963]	1124	{
[2430]	1125	DBVT_CHECKTYPE
	1126	if(root)
[1963]	1127	{
[2430]	1128	const unsigned srtsgns=(sortaxis[0]>=0?1:0)+
	1129	(sortaxis[1]>=0?2:0)+
	1130	(sortaxis[2]>=0?4:0);
	1131	const int inside=(1<<count)-1;
	1132	btAlignedObjectArray<sStkNPS> stock;
	1133	btAlignedObjectArray<int> ifree;
	1134	btAlignedObjectArray<int> stack;
	1135	int signs[sizeof(unsigned)*8];
	1136	btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
	1137	for(int i=0;i<count;++i)
	1138	{
	1139	signs[i]= ((normals[i].x()>=0)?1:0)+
[1963]	1140	((normals[i].y()>=0)?2:0)+
	1141	((normals[i].z()>=0)?4:0);
[2430]	1142	}
	1143	stock.reserve(SIMPLE_STACKSIZE);
	1144	stack.reserve(SIMPLE_STACKSIZE);
	1145	ifree.reserve(SIMPLE_STACKSIZE);
	1146	stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns))));
	1147	do {
	1148	const int id=stack[stack.size()-1];
	1149	sStkNPS se=stock[id];
	1150	stack.pop_back();ifree.push_back(id);
	1151	if(se.mask!=inside)
[1963]	1152	{
[2430]	1153	bool out=false;
	1154	for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
[1963]	1155	{
[2430]	1156	if(0==(se.mask&j))
[1963]	1157	{
[2430]	1158	const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
	1159	switch(side)
	1160	{
	1161	case -1: out=true;break;
	1162	case +1: se.mask\|=j;break;
	1163	}
[1963]	1164	}
	1165	}
[2430]	1166	if(out) continue;
[1963]	1167	}
[2430]	1168	if(policy.Descent(se.node))
[1963]	1169	{
[2430]	1170	if(se.node->isinternal())
[1963]	1171	{
[2430]	1172	const btDbvtNode* pns[]={ se.node->childs[0],se.node->childs[1]};
	1173	sStkNPS nes[]={ sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)),
	1174	sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))};
	1175	const int q=nes[0].value<nes[1].value?1:0;
	1176	int j=stack.size();
	1177	if(fsort&&(j>0))
	1178	{
	1179	/* Insert 0 */
	1180	j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size());
	1181	stack.push_back(0);
	1182	#if DBVT_USE_MEMMOVE
	1183	memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
	1184	#else
	1185	for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
	1186	#endif
	1187	stack[j]=allocate(ifree,stock,nes[q]);
	1188	/* Insert 1 */
	1189	j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size());
	1190	stack.push_back(0);
	1191	#if DBVT_USE_MEMMOVE
	1192	memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
	1193	#else
	1194	for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
	1195	#endif
	1196	stack[j]=allocate(ifree,stock,nes[1-q]);
	1197	}
	1198	else
	1199	{
	1200	stack.push_back(allocate(ifree,stock,nes[q]));
	1201	stack.push_back(allocate(ifree,stock,nes[1-q]));
	1202	}
[1963]	1203	}
	1204	else
	1205	{
[2430]	1206	policy.Process(se.node,se.value);
[1963]	1207	}
	1208	}
[2430]	1209	} while(stack.size());
	1210	}
[1963]	1211	}
	1212
	1213	//
	1214	DBVT_PREFIX
	1215	inline void btDbvt::collideTU( const btDbvtNode* root,
[2430]	1216	DBVT_IPOLICY)
[1963]	1217	{
[2430]	1218	DBVT_CHECKTYPE
	1219	if(root)
	1220	{
	1221	btAlignedObjectArray<const btDbvtNode*> stack;
	1222	stack.reserve(SIMPLE_STACKSIZE);
	1223	stack.push_back(root);
	1224	do {
	1225	const btDbvtNode* n=stack[stack.size()-1];
	1226	stack.pop_back();
	1227	if(policy.Descent(n))
	1228	{
	1229	if(n->isinternal())
	1230	{ stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); }
	1231	else
	1232	{ policy.Process(n); }
	1233	}
	1234	} while(stack.size()>0);
	1235	}
[1963]	1236	}
	1237
	1238	//
	1239	// PP Cleanup
	1240	//
	1241
	1242	#undef DBVT_USE_MEMMOVE
	1243	#undef DBVT_USE_TEMPLATE
	1244	#undef DBVT_VIRTUAL_DTOR
	1245	#undef DBVT_VIRTUAL
	1246	#undef DBVT_PREFIX
	1247	#undef DBVT_IPOLICY
	1248	#undef DBVT_CHECKTYPE
	1249	#undef DBVT_IMPL_GENERIC
	1250	#undef DBVT_IMPL_SSE
	1251	#undef DBVT_USE_INTRINSIC_SSE
	1252	#undef DBVT_SELECT_IMPL
	1253	#undef DBVT_MERGE_IMPL
	1254	#undef DBVT_INT0_IMPL
	1255
	1256	#endif

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

Download in other formats: