Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/questsystem5/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.h @ 2948

Last change on this file since 2948 was 2908, checked in by dafrick, 16 years ago
Reverted to revision 2906 (because I'm too stupid to merge correctly, 2nd try will follow shortly. )
Property svn:eol-style set to `native`
File size: 31.7 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
[2908]	60	#if (defined (WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION))
[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
[2908]	85	#define DBVT_CHECKTYPE static const ICollide& typechecker=(T)0;
[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);
	149	DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a,
[2908]	150	const btDbvtAabbMm& b,
	151	const btTransform& xform);
	152	DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a,
[2430]	153	const btVector3& b);
	154
	155	DBVT_INLINE friend btScalar Proximity( const btDbvtAabbMm& a,
	156	const btDbvtAabbMm& b);
	157	DBVT_INLINE friend int Select( const btDbvtAabbMm& o,
	158	const btDbvtAabbMm& a,
	159	const btDbvtAabbMm& b);
	160	DBVT_INLINE friend void Merge( const btDbvtAabbMm& a,
	161	const btDbvtAabbMm& b,
	162	btDbvtAabbMm& r);
	163	DBVT_INLINE friend bool NotEqual( const btDbvtAabbMm& a,
	164	const btDbvtAabbMm& b);
[1963]	165	private:
[2430]	166	DBVT_INLINE void AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const;
[1963]	167	private:
[2430]	168	btVector3 mi,mx;
[1963]	169	};
	170
	171	// Types
	172	typedef btDbvtAabbMm btDbvtVolume;
	173
	174	/* btDbvtNode */
	175	struct btDbvtNode
	176	{
	177	btDbvtVolume volume;
	178	btDbvtNode* parent;
	179	DBVT_INLINE bool isleaf() const { return(childs[1]==0); }
	180	DBVT_INLINE bool isinternal() const { return(!isleaf()); }
[2430]	181	union
	182	{
	183	btDbvtNode* childs[2];
	184	void* data;
	185	int dataAsInt;
	186	};
[1963]	187	};
	188
	189	///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree).
	190	///This btDbvt is used for soft body collision detection and for the btDbvtBroadphase. It has a fast insert, remove and update of nodes.
	191	///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure.
	192	struct btDbvt
[2430]	193	{
[1963]	194	/* Stack element */
	195	struct sStkNN
[2430]	196	{
[1963]	197	const btDbvtNode* a;
	198	const btDbvtNode* b;
	199	sStkNN() {}
	200	sStkNN(const btDbvtNode* na,const btDbvtNode* nb) : a(na),b(nb) {}
[2430]	201	};
[1963]	202	struct sStkNP
[2430]	203	{
[1963]	204	const btDbvtNode* node;
	205	int mask;
	206	sStkNP(const btDbvtNode* n,unsigned m) : node(n),mask(m) {}
[2430]	207	};
[1963]	208	struct sStkNPS
[2430]	209	{
[1963]	210	const btDbvtNode* node;
	211	int mask;
	212	btScalar value;
	213	sStkNPS() {}
	214	sStkNPS(const btDbvtNode* n,unsigned m,btScalar v) : node(n),mask(m),value(v) {}
[2430]	215	};
[1963]	216	struct sStkCLN
[2430]	217	{
[1963]	218	const btDbvtNode* node;
	219	btDbvtNode* parent;
	220	sStkCLN(const btDbvtNode* n,btDbvtNode* p) : node(n),parent(p) {}
[2430]	221	};
[1963]	222	// Policies/Interfaces
[2430]	223
[1963]	224	/* ICollide */
	225	struct ICollide
[2430]	226	{
[1963]	227	DBVT_VIRTUAL_DTOR(ICollide)
[2430]	228	DBVT_VIRTUAL void Process(const btDbvtNode,const btDbvtNode) {}
[1963]	229	DBVT_VIRTUAL void Process(const btDbvtNode*) {}
	230	DBVT_VIRTUAL void Process(const btDbvtNode* n,btScalar) { Process(n); }
	231	DBVT_VIRTUAL bool Descent(const btDbvtNode*) { return(true); }
	232	DBVT_VIRTUAL bool AllLeaves(const btDbvtNode*) { return(true); }
[2430]	233	};
[1963]	234	/* IWriter */
	235	struct IWriter
[2430]	236	{
[1963]	237	virtual ~IWriter() {}
	238	virtual void Prepare(const btDbvtNode* root,int numnodes)=0;
	239	virtual void WriteNode(const btDbvtNode*,int index,int parent,int child0,int child1)=0;
	240	virtual void WriteLeaf(const btDbvtNode*,int index,int parent)=0;
[2430]	241	};
[1963]	242	/* IClone */
	243	struct IClone
[2430]	244	{
[1963]	245	virtual ~IClone() {}
	246	virtual void CloneLeaf(btDbvtNode*) {}
[2430]	247	};
	248
[1963]	249	// Constants
	250	enum {
[2430]	251	SIMPLE_STACKSIZE = 64,
	252	DOUBLE_STACKSIZE = SIMPLE_STACKSIZE*2
	253	};
	254
[1963]	255	// Fields
	256	btDbvtNode* m_root;
	257	btDbvtNode* m_free;
	258	int m_lkhd;
	259	int m_leaves;
	260	unsigned m_opath;
[2430]	261
	262
	263	btAlignedObjectArray<sStkNN> m_stkStack;
	264
	265
[1963]	266	// Methods
[2430]	267	btDbvt();
	268	~btDbvt();
[1963]	269	void clear();
	270	bool empty() const { return(0==m_root); }
	271	void optimizeBottomUp();
	272	void optimizeTopDown(int bu_treshold=128);
	273	void optimizeIncremental(int passes);
	274	btDbvtNode* insert(const btDbvtVolume& box,void* data);
	275	void update(btDbvtNode* leaf,int lookahead=-1);
[2430]	276	void update(btDbvtNode* leaf,btDbvtVolume& volume);
	277	bool update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity,btScalar margin);
	278	bool update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity);
	279	bool update(btDbvtNode* leaf,btDbvtVolume& volume,btScalar margin);
[1963]	280	void remove(btDbvtNode* leaf);
	281	void write(IWriter* iwriter) const;
	282	void clone(btDbvt& dest,IClone* iclone=0) const;
	283	static int maxdepth(const btDbvtNode* node);
	284	static int countLeaves(const btDbvtNode* node);
	285	static void extractLeaves(const btDbvtNode* node,btAlignedObjectArray<const btDbvtNode*>& leaves);
[2430]	286	#if DBVT_ENABLE_BENCHMARK
[1963]	287	static void benchmark();
[2430]	288	#else
[1963]	289	static void benchmark(){}
[2430]	290	#endif
[1963]	291	// DBVT_IPOLICY must support ICollide policy/interface
	292	DBVT_PREFIX
[2430]	293	static void enumNodes( const btDbvtNode* root,
	294	DBVT_IPOLICY);
[1963]	295	DBVT_PREFIX
[2430]	296	static void enumLeaves( const btDbvtNode* root,
	297	DBVT_IPOLICY);
[1963]	298	DBVT_PREFIX
[2430]	299	void collideTT( const btDbvtNode* root0,
	300	const btDbvtNode* root1,
	301	DBVT_IPOLICY);
	302
[1963]	303	DBVT_PREFIX
[2430]	304	void collideTTpersistentStack( const btDbvtNode* root0,
	305	const btDbvtNode* root1,
	306	DBVT_IPOLICY);
[2908]	307
[1963]	308	DBVT_PREFIX
[2430]	309	void collideTT( const btDbvtNode* root0,
	310	const btDbvtNode* root1,
	311	const btTransform& xform,
	312	DBVT_IPOLICY);
[1963]	313	DBVT_PREFIX
[2430]	314	void collideTT( const btDbvtNode* root0,
	315	const btTransform& xform0,
	316	const btDbvtNode* root1,
	317	const btTransform& xform1,
	318	DBVT_IPOLICY);
[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
[2908]	533	//
	534	DBVT_INLINE bool Intersect( const btDbvtAabbMm& a,
	535	const btDbvtAabbMm& b,
	536	const btTransform& xform)
	537	{
	538	const btVector3 d0=xform*b.Center()-a.Center();
	539	const btVector3 d1=d0*xform.getBasis();
	540	btScalar s0[2]={0,0};
	541	btScalar s1[2]={dot(xform.getOrigin(),d0),s1[0]};
	542	a.AddSpan(d0,s0[0],s0[1]);
	543	b.AddSpan(d1,s1[0],s1[1]);
	544	if(s0[0]>(s1[1])) return(false);
	545	if(s0[1]<(s1[0])) return(false);
	546	return(true);
	547	}
[1963]	548
	549	//
	550	DBVT_INLINE bool Intersect( const btDbvtAabbMm& a,
[2430]	551	const btVector3& b)
[1963]	552	{
[2430]	553	return( (b.x()>=a.mi.x())&&
[1963]	554	(b.y()>=a.mi.y())&&
	555	(b.z()>=a.mi.z())&&
	556	(b.x()<=a.mx.x())&&
	557	(b.y()<=a.mx.y())&&
	558	(b.z()<=a.mx.z()));
	559	}
	560
[2430]	561
	562
	563
	564
	565	//////////////////////////////////////
	566
	567
[1963]	568	//
	569	DBVT_INLINE btScalar Proximity( const btDbvtAabbMm& a,
[2430]	570	const btDbvtAabbMm& b)
[1963]	571	{
[2430]	572	const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx);
	573	return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z()));
[1963]	574	}
	575
[2430]	576
	577
[1963]	578	//
	579	DBVT_INLINE int Select( const btDbvtAabbMm& o,
[2430]	580	const btDbvtAabbMm& a,
	581	const btDbvtAabbMm& b)
[1963]	582	{
	583	#if DBVT_SELECT_IMPL == DBVT_IMPL_SSE
[2430]	584	static ATTRIBUTE_ALIGNED16(const unsigned __int32) mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff};
	585	///@todo: the intrinsic version is 11% slower
	586	#if DBVT_USE_INTRINSIC_SSE
	587
	588	union btSSEUnion ///NOTE: if we use more intrinsics, move btSSEUnion into the LinearMath directory
	589	{
	590	__m128 ssereg;
	591	float floats[4];
	592	int ints[4];
	593	};
	594
[1963]	595	__m128 omi(_mm_load_ps(o.mi));
	596	omi=_mm_add_ps(omi,_mm_load_ps(o.mx));
	597	__m128 ami(_mm_load_ps(a.mi));
	598	ami=_mm_add_ps(ami,_mm_load_ps(a.mx));
	599	ami=_mm_sub_ps(ami,omi);
	600	ami=_mm_and_ps(ami,_mm_load_ps((const float*)mask));
	601	__m128 bmi(_mm_load_ps(b.mi));
	602	bmi=_mm_add_ps(bmi,_mm_load_ps(b.mx));
	603	bmi=_mm_sub_ps(bmi,omi);
	604	bmi=_mm_and_ps(bmi,_mm_load_ps((const float*)mask));
	605	__m128 t0(_mm_movehl_ps(ami,ami));
	606	ami=_mm_add_ps(ami,t0);
	607	ami=_mm_add_ss(ami,_mm_shuffle_ps(ami,ami,1));
[2430]	608	__m128 t1(_mm_movehl_ps(bmi,bmi));
[1963]	609	bmi=_mm_add_ps(bmi,t1);
	610	bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1));
[2430]	611
	612	btSSEUnion tmp;
	613	tmp.ssereg = _mm_cmple_ss(bmi,ami);
	614	return tmp.ints[0]&1;
	615
	616	#else
	617	ATTRIBUTE_ALIGNED16(__int32 r[1]);
[1963]	618	__asm
[2430]	619	{
[1963]	620	mov eax,o
[2430]	621	mov ecx,a
	622	mov edx,b
	623	movaps xmm0,[eax]
[1963]	624	movaps xmm5,mask
[2430]	625	addps xmm0,[eax+16]
[1963]	626	movaps xmm1,[ecx]
	627	movaps xmm2,[edx]
	628	addps xmm1,[ecx+16]
	629	addps xmm2,[edx+16]
	630	subps xmm1,xmm0
[2430]	631	subps xmm2,xmm0
	632	andps xmm1,xmm5
	633	andps xmm2,xmm5
	634	movhlps xmm3,xmm1
	635	movhlps xmm4,xmm2
	636	addps xmm1,xmm3
	637	addps xmm2,xmm4
	638	pshufd xmm3,xmm1,1
	639	pshufd xmm4,xmm2,1
	640	addss xmm1,xmm3
	641	addss xmm2,xmm4
	642	cmpless xmm2,xmm1
	643	movss r,xmm2
	644	}
[1963]	645	return(r[0]&1);
[2430]	646	#endif
[1963]	647	#else
[2430]	648	return(Proximity(o,a)<Proximity(o,b)?0:1);
[1963]	649	#endif
	650	}
	651
	652	//
	653	DBVT_INLINE void Merge( const btDbvtAabbMm& a,
[2430]	654	const btDbvtAabbMm& b,
	655	btDbvtAabbMm& r)
[1963]	656	{
	657	#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
[2430]	658	__m128 ami(_mm_load_ps(a.mi));
	659	__m128 amx(_mm_load_ps(a.mx));
	660	__m128 bmi(_mm_load_ps(b.mi));
	661	__m128 bmx(_mm_load_ps(b.mx));
	662	ami=_mm_min_ps(ami,bmi);
	663	amx=_mm_max_ps(amx,bmx);
	664	_mm_store_ps(r.mi,ami);
	665	_mm_store_ps(r.mx,amx);
[1963]	666	#else
[2430]	667	for(int i=0;i<3;++i)
[1963]	668	{
[2430]	669	if(a.mi[i]<b.mi[i]) r.mi[i]=a.mi[i]; else r.mi[i]=b.mi[i];
	670	if(a.mx[i]>b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i];
[1963]	671	}
	672	#endif
	673	}
	674
	675	//
	676	DBVT_INLINE bool NotEqual( const btDbvtAabbMm& a,
[2430]	677	const btDbvtAabbMm& b)
[1963]	678	{
[2430]	679	return( (a.mi.x()!=b.mi.x())\|\|
[1963]	680	(a.mi.y()!=b.mi.y())\|\|
	681	(a.mi.z()!=b.mi.z())\|\|
	682	(a.mx.x()!=b.mx.x())\|\|
	683	(a.mx.y()!=b.mx.y())\|\|
	684	(a.mx.z()!=b.mx.z()));
	685	}
	686
	687	//
	688	// Inline's
	689	//
	690
	691	//
	692	DBVT_PREFIX
	693	inline void btDbvt::enumNodes( const btDbvtNode* root,
[2430]	694	DBVT_IPOLICY)
[1963]	695	{
[2430]	696	DBVT_CHECKTYPE
	697	policy.Process(root);
	698	if(root->isinternal())
[1963]	699	{
[2430]	700	enumNodes(root->childs[0],policy);
	701	enumNodes(root->childs[1],policy);
[1963]	702	}
	703	}
	704
	705	//
	706	DBVT_PREFIX
	707	inline void btDbvt::enumLeaves( const btDbvtNode* root,
[2430]	708	DBVT_IPOLICY)
[1963]	709	{
[2430]	710	DBVT_CHECKTYPE
	711	if(root->isinternal())
	712	{
	713	enumLeaves(root->childs[0],policy);
	714	enumLeaves(root->childs[1],policy);
	715	}
	716	else
	717	{
	718	policy.Process(root);
	719	}
[1963]	720	}
	721
	722	//
	723	DBVT_PREFIX
	724	inline void btDbvt::collideTT( const btDbvtNode* root0,
[2430]	725	const btDbvtNode* root1,
	726	DBVT_IPOLICY)
[1963]	727	{
[2430]	728	DBVT_CHECKTYPE
	729	if(root0&&root1)
	730	{
	731	int depth=1;
	732	int treshold=DOUBLE_STACKSIZE-4;
	733	btAlignedObjectArray<sStkNN> stkStack;
	734	stkStack.resize(DOUBLE_STACKSIZE);
	735	stkStack[0]=sStkNN(root0,root1);
	736	do {
	737	sStkNN p=stkStack[--depth];
	738	if(depth>treshold)
[1963]	739	{
[2430]	740	stkStack.resize(stkStack.size()*2);
	741	treshold=stkStack.size()-4;
[1963]	742	}
[2430]	743	if(p.a==p.b)
[1963]	744	{
[2430]	745	if(p.a->isinternal())
[1963]	746	{
[2430]	747	stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]);
	748	stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]);
	749	stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]);
[1963]	750	}
	751	}
[2430]	752	else if(Intersect(p.a->volume,p.b->volume))
[1963]	753	{
[2430]	754	if(p.a->isinternal())
[1963]	755	{
[2430]	756	if(p.b->isinternal())
	757	{
	758	stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
	759	stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
	760	stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
	761	stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
	762	}
	763	else
	764	{
	765	stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
	766	stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
	767	}
[1963]	768	}
	769	else
	770	{
[2430]	771	if(p.b->isinternal())
	772	{
	773	stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
	774	stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
	775	}
	776	else
	777	{
	778	policy.Process(p.a,p.b);
	779	}
[1963]	780	}
	781	}
[2430]	782	} while(depth);
	783	}
[1963]	784	}
	785
[2430]	786
	787
[1963]	788	DBVT_PREFIX
[2430]	789	inline void btDbvt::collideTTpersistentStack( const btDbvtNode* root0,
	790	const btDbvtNode* root1,
	791	DBVT_IPOLICY)
[1963]	792	{
[2430]	793	DBVT_CHECKTYPE
	794	if(root0&&root1)
	795	{
	796	int depth=1;
	797	int treshold=DOUBLE_STACKSIZE-4;
	798
	799	m_stkStack.resize(DOUBLE_STACKSIZE);
	800	m_stkStack[0]=sStkNN(root0,root1);
	801	do {
	802	sStkNN p=m_stkStack[--depth];
	803	if(depth>treshold)
[1963]	804	{
[2430]	805	m_stkStack.resize(m_stkStack.size()*2);
	806	treshold=m_stkStack.size()-4;
[1972]	807	}
[2430]	808	if(p.a==p.b)
[1972]	809	{
[2430]	810	if(p.a->isinternal())
	811	{
	812	m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]);
	813	m_stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]);
	814	m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]);
[1972]	815	}
[2430]	816	}
	817	else if(Intersect(p.a->volume,p.b->volume))
	818	{
	819	if(p.a->isinternal())
	820	{
	821	if(p.b->isinternal())
	822	{
	823	m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
	824	m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
	825	m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
	826	m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
	827	}
	828	else
	829	{
	830	m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
	831	m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
	832	}
	833	}
[1972]	834	else
[1963]	835	{
[2430]	836	if(p.b->isinternal())
	837	{
	838	m_stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
	839	m_stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
	840	}
	841	else
	842	{
	843	policy.Process(p.a,p.b);
	844	}
[1963]	845	}
[1972]	846	}
[2430]	847	} while(depth);
	848	}
	849	}
	850
[2908]	851
[2430]	852	//
	853	DBVT_PREFIX
	854	inline void btDbvt::collideTT( const btDbvtNode* root0,
	855	const btDbvtNode* root1,
	856	const btTransform& xform,
	857	DBVT_IPOLICY)
	858	{
	859	DBVT_CHECKTYPE
	860	if(root0&&root1)
	861	{
	862	int depth=1;
	863	int treshold=DOUBLE_STACKSIZE-4;
	864	btAlignedObjectArray<sStkNN> stkStack;
	865	stkStack.resize(DOUBLE_STACKSIZE);
	866	stkStack[0]=sStkNN(root0,root1);
	867	do {
	868	sStkNN p=stkStack[--depth];
	869	if(Intersect(p.a->volume,p.b->volume,xform))
[1972]	870	{
[2430]	871	if(depth>treshold)
[1963]	872	{
[2430]	873	stkStack.resize(stkStack.size()*2);
	874	treshold=stkStack.size()-4;
[1963]	875	}
[2430]	876	if(p.a->isinternal())
	877	{
	878	if(p.b->isinternal())
	879	{
	880	stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
	881	stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
	882	stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
	883	stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
	884	}
	885	else
	886	{
	887	stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
	888	stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
	889	}
	890	}
[1963]	891	else
	892	{
[2430]	893	if(p.b->isinternal())
	894	{
	895	stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
	896	stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
	897	}
	898	else
	899	{
	900	policy.Process(p.a,p.b);
	901	}
[1963]	902	}
	903	}
[2430]	904	} while(depth);
	905	}
[1963]	906	}
[2908]	907
[1963]	908	//
	909	DBVT_PREFIX
	910	inline void btDbvt::collideTT( const btDbvtNode* root0,
[2430]	911	const btTransform& xform0,
	912	const btDbvtNode* root1,
	913	const btTransform& xform1,
	914	DBVT_IPOLICY)
[1963]	915	{
[2430]	916	const btTransform xform=xform0.inverse()*xform1;
	917	collideTT(root0,root1,xform,policy);
[1963]	918	}
	919
	920	//
	921	DBVT_PREFIX
	922	inline void btDbvt::collideTV( const btDbvtNode* root,
[2430]	923	const btDbvtVolume& vol,
	924	DBVT_IPOLICY)
[1963]	925	{
[2430]	926	DBVT_CHECKTYPE
	927	if(root)
	928	{
	929	ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol);
	930	btAlignedObjectArray<const btDbvtNode*> stack;
	931	stack.resize(0);
	932	stack.reserve(SIMPLE_STACKSIZE);
	933	stack.push_back(root);
	934	do {
	935	const btDbvtNode* n=stack[stack.size()-1];
	936	stack.pop_back();
	937	if(Intersect(n->volume,volume))
	938	{
	939	if(n->isinternal())
	940	{
	941	stack.push_back(n->childs[0]);
	942	stack.push_back(n->childs[1]);
	943	}
	944	else
	945	{
	946	policy.Process(n);
	947	}
	948	}
	949	} while(stack.size()>0);
	950	}
	951	}
	952
	953	DBVT_PREFIX
	954	inline void btDbvt::rayTestInternal( const btDbvtNode* root,
	955	const btVector3& rayFrom,
	956	const btVector3& rayTo,
	957	const btVector3& rayDirectionInverse,
	958	unsigned int signs[3],
	959	btScalar lambda_max,
	960	const btVector3& aabbMin,
	961	const btVector3& aabbMax,
	962	DBVT_IPOLICY) const
	963	{
	964	DBVT_CHECKTYPE
	965	if(root)
[1972]	966	{
[2430]	967	btVector3 resultNormal;
	968
	969	int depth=1;
	970	int treshold=DOUBLE_STACKSIZE-2;
	971	btAlignedObjectArray<const btDbvtNode*> stack;
	972	stack.resize(DOUBLE_STACKSIZE);
	973	stack[0]=root;
	974	btVector3 bounds[2];
	975	do
	976	{
	977	const btDbvtNode* node=stack[--depth];
	978	bounds[0] = node->volume.Mins()+aabbMin;
	979	bounds[1] = node->volume.Maxs()+aabbMax;
	980	btScalar tmin=1.f,lambda_min=0.f;
	981	unsigned int result1=false;
	982	result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
	983	if(result1)
[1972]	984	{
[2430]	985	if(node->isinternal())
[1963]	986	{
[2430]	987	if(depth>treshold)
	988	{
	989	stack.resize(stack.size()*2);
	990	treshold=stack.size()-2;
	991	}
	992	stack[depth++]=node->childs[0];
	993	stack[depth++]=node->childs[1];
[1963]	994	}
[1972]	995	else
	996	{
[2430]	997	policy.Process(node);
[1972]	998	}
	999	}
[2430]	1000	} while(depth);
[1972]	1001	}
[1963]	1002	}
	1003
	1004	//
	1005	DBVT_PREFIX
[2430]	1006	inline void btDbvt::rayTest( const btDbvtNode* root,
	1007	const btVector3& rayFrom,
	1008	const btVector3& rayTo,
	1009	DBVT_IPOLICY)
[1963]	1010	{
[2430]	1011	DBVT_CHECKTYPE
	1012	if(root)
	1013	{
	1014	btVector3 rayDir = (rayTo-rayFrom);
	1015	rayDir.normalize ();
	1016
	1017	///what about division by zero? --> just set rayDirection[i] to INF/1e30
	1018	btVector3 rayDirectionInverse;
	1019	rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
	1020	rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
	1021	rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
	1022	unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0};
	1023
	1024	btScalar lambda_max = rayDir.dot(rayTo-rayFrom);
	1025
	1026	btVector3 resultNormal;
	1027
	1028	btAlignedObjectArray<const btDbvtNode*> stack;
	1029
	1030	int depth=1;
	1031	int treshold=DOUBLE_STACKSIZE-2;
	1032
	1033	stack.resize(DOUBLE_STACKSIZE);
	1034	stack[0]=root;
	1035	btVector3 bounds[2];
	1036	do {
	1037	const btDbvtNode* node=stack[--depth];
	1038
	1039	bounds[0] = node->volume.Mins();
	1040	bounds[1] = node->volume.Maxs();
	1041
	1042	btScalar tmin=1.f,lambda_min=0.f;
	1043	unsigned int result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
	1044
	1045	#ifdef COMPARE_BTRAY_AABB2
	1046	btScalar param=1.f;
	1047	bool result2 = btRayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal);
	1048	btAssert(result1 == result2);
	1049	#endif //TEST_BTRAY_AABB2
	1050
	1051	if(result1)
[1963]	1052	{
[2430]	1053	if(node->isinternal())
	1054	{
	1055	if(depth>treshold)
	1056	{
	1057	stack.resize(stack.size()*2);
	1058	treshold=stack.size()-2;
	1059	}
	1060	stack[depth++]=node->childs[0];
	1061	stack[depth++]=node->childs[1];
	1062	}
	1063	else
	1064	{
	1065	policy.Process(node);
	1066	}
[1963]	1067	}
[2430]	1068	} while(depth);
	1069
	1070	}
[1963]	1071	}
	1072
	1073	//
	1074	DBVT_PREFIX
	1075	inline void btDbvt::collideKDOP(const btDbvtNode* root,
	1076	const btVector3* normals,
	1077	const btScalar* offsets,
	1078	int count,
	1079	DBVT_IPOLICY)
	1080	{
[2430]	1081	DBVT_CHECKTYPE
	1082	if(root)
[1963]	1083	{
[2430]	1084	const int inside=(1<<count)-1;
	1085	btAlignedObjectArray<sStkNP> stack;
	1086	int signs[sizeof(unsigned)*8];
	1087	btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
	1088	for(int i=0;i<count;++i)
	1089	{
	1090	signs[i]= ((normals[i].x()>=0)?1:0)+
[1963]	1091	((normals[i].y()>=0)?2:0)+
	1092	((normals[i].z()>=0)?4:0);
[2430]	1093	}
	1094	stack.reserve(SIMPLE_STACKSIZE);
	1095	stack.push_back(sStkNP(root,0));
	1096	do {
	1097	sStkNP se=stack[stack.size()-1];
	1098	bool out=false;
	1099	stack.pop_back();
	1100	for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
[1963]	1101	{
[2430]	1102	if(0==(se.mask&j))
[1963]	1103	{
[2430]	1104	const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
	1105	switch(side)
	1106	{
	1107	case -1: out=true;break;
	1108	case +1: se.mask\|=j;break;
	1109	}
[1963]	1110	}
	1111	}
[2430]	1112	if(!out)
[1963]	1113	{
[2430]	1114	if((se.mask!=inside)&&(se.node->isinternal()))
	1115	{
	1116	stack.push_back(sStkNP(se.node->childs[0],se.mask));
	1117	stack.push_back(sStkNP(se.node->childs[1],se.mask));
	1118	}
	1119	else
	1120	{
	1121	if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy);
	1122	}
[1963]	1123	}
[2430]	1124	} while(stack.size());
	1125	}
[1963]	1126	}
	1127
	1128	//
	1129	DBVT_PREFIX
	1130	inline void btDbvt::collideOCL( const btDbvtNode* root,
[2430]	1131	const btVector3* normals,
	1132	const btScalar* offsets,
	1133	const btVector3& sortaxis,
	1134	int count,
	1135	DBVT_IPOLICY,
	1136	bool fsort)
[1963]	1137	{
[2430]	1138	DBVT_CHECKTYPE
	1139	if(root)
[1963]	1140	{
[2430]	1141	const unsigned srtsgns=(sortaxis[0]>=0?1:0)+
	1142	(sortaxis[1]>=0?2:0)+
	1143	(sortaxis[2]>=0?4:0);
	1144	const int inside=(1<<count)-1;
	1145	btAlignedObjectArray<sStkNPS> stock;
	1146	btAlignedObjectArray<int> ifree;
	1147	btAlignedObjectArray<int> stack;
	1148	int signs[sizeof(unsigned)*8];
	1149	btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
	1150	for(int i=0;i<count;++i)
	1151	{
	1152	signs[i]= ((normals[i].x()>=0)?1:0)+
[1963]	1153	((normals[i].y()>=0)?2:0)+
	1154	((normals[i].z()>=0)?4:0);
[2430]	1155	}
	1156	stock.reserve(SIMPLE_STACKSIZE);
	1157	stack.reserve(SIMPLE_STACKSIZE);
	1158	ifree.reserve(SIMPLE_STACKSIZE);
	1159	stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns))));
	1160	do {
	1161	const int id=stack[stack.size()-1];
	1162	sStkNPS se=stock[id];
	1163	stack.pop_back();ifree.push_back(id);
	1164	if(se.mask!=inside)
[1963]	1165	{
[2430]	1166	bool out=false;
	1167	for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
[1963]	1168	{
[2430]	1169	if(0==(se.mask&j))
[1963]	1170	{
[2430]	1171	const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
	1172	switch(side)
	1173	{
	1174	case -1: out=true;break;
	1175	case +1: se.mask\|=j;break;
	1176	}
[1963]	1177	}
	1178	}
[2430]	1179	if(out) continue;
[1963]	1180	}
[2430]	1181	if(policy.Descent(se.node))
[1963]	1182	{
[2430]	1183	if(se.node->isinternal())
[1963]	1184	{
[2430]	1185	const btDbvtNode* pns[]={ se.node->childs[0],se.node->childs[1]};
	1186	sStkNPS nes[]={ sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)),
	1187	sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))};
	1188	const int q=nes[0].value<nes[1].value?1:0;
	1189	int j=stack.size();
	1190	if(fsort&&(j>0))
	1191	{
	1192	/* Insert 0 */
	1193	j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size());
	1194	stack.push_back(0);
	1195	#if DBVT_USE_MEMMOVE
	1196	memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
	1197	#else
	1198	for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
	1199	#endif
	1200	stack[j]=allocate(ifree,stock,nes[q]);
	1201	/* Insert 1 */
	1202	j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size());
	1203	stack.push_back(0);
	1204	#if DBVT_USE_MEMMOVE
	1205	memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
	1206	#else
	1207	for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
	1208	#endif
	1209	stack[j]=allocate(ifree,stock,nes[1-q]);
	1210	}
	1211	else
	1212	{
	1213	stack.push_back(allocate(ifree,stock,nes[q]));
	1214	stack.push_back(allocate(ifree,stock,nes[1-q]));
	1215	}
[1963]	1216	}
	1217	else
	1218	{
[2430]	1219	policy.Process(se.node,se.value);
[1963]	1220	}
	1221	}
[2430]	1222	} while(stack.size());
	1223	}
[1963]	1224	}
	1225
	1226	//
	1227	DBVT_PREFIX
	1228	inline void btDbvt::collideTU( const btDbvtNode* root,
[2430]	1229	DBVT_IPOLICY)
[1963]	1230	{
[2430]	1231	DBVT_CHECKTYPE
	1232	if(root)
	1233	{
	1234	btAlignedObjectArray<const btDbvtNode*> stack;
	1235	stack.reserve(SIMPLE_STACKSIZE);
	1236	stack.push_back(root);
	1237	do {
	1238	const btDbvtNode* n=stack[stack.size()-1];
	1239	stack.pop_back();
	1240	if(policy.Descent(n))
	1241	{
	1242	if(n->isinternal())
	1243	{ stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); }
	1244	else
	1245	{ policy.Process(n); }
	1246	}
	1247	} while(stack.size()>0);
	1248	}
[1963]	1249	}
	1250
	1251	//
	1252	// PP Cleanup
	1253	//
	1254
	1255	#undef DBVT_USE_MEMMOVE
	1256	#undef DBVT_USE_TEMPLATE
	1257	#undef DBVT_VIRTUAL_DTOR
	1258	#undef DBVT_VIRTUAL
	1259	#undef DBVT_PREFIX
	1260	#undef DBVT_IPOLICY
	1261	#undef DBVT_CHECKTYPE
	1262	#undef DBVT_IMPL_GENERIC
	1263	#undef DBVT_IMPL_SSE
	1264	#undef DBVT_USE_INTRINSIC_SSE
	1265	#undef DBVT_SELECT_IMPL
	1266	#undef DBVT_MERGE_IMPL
	1267	#undef DBVT_INT0_IMPL
	1268
	1269	#endif

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

Download in other formats: