Index: code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.h	(revision 2192)
+++ code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.h	(revision 2430)
@@ -21,4 +21,5 @@
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btTransform.h"
+#include "LinearMath/btAabbUtil2.h"
 
 //
@@ -33,12 +34,12 @@
 // Template implementation of ICollide
 #ifdef WIN32
-	#if (defined (_MSC_VER) && _MSC_VER >= 1400)
-	#define	DBVT_USE_TEMPLATE		1
-	#else
-	#define	DBVT_USE_TEMPLATE		0
-#endif
+#if (defined (_MSC_VER) && _MSC_VER >= 1400)
+#define	DBVT_USE_TEMPLATE		1
 #else
 #define	DBVT_USE_TEMPLATE		0
 #endif
+#else
+#define	DBVT_USE_TEMPLATE		0
+#endif
 
 // Use only intrinsics instead of inline asm
@@ -53,10 +54,4 @@
 // Inlining
 #define DBVT_INLINE				SIMD_FORCE_INLINE
-// Align
-#ifdef WIN32
-#define DBVT_ALIGN				__declspec(align(16))
-#else
-#define DBVT_ALIGN
-#endif
 
 // Specific methods implementation
@@ -135,44 +130,41 @@
 struct	btDbvtAabbMm
 {
-DBVT_INLINE btVector3			Center() const	{ return((mi+mx)/2); }
-DBVT_INLINE btVector3			Lengths() const	{ return(mx-mi); }
-DBVT_INLINE btVector3			Extents() const	{ return((mx-mi)/2); }
-DBVT_INLINE const btVector3&	Mins() const	{ return(mi); }
-DBVT_INLINE const btVector3&	Maxs() const	{ return(mx); }
-static inline btDbvtAabbMm		FromCE(const btVector3& c,const btVector3& e);
-static inline btDbvtAabbMm		FromCR(const btVector3& c,btScalar r);
-static inline btDbvtAabbMm		FromMM(const btVector3& mi,const btVector3& mx);
-static inline btDbvtAabbMm		FromPoints(const btVector3* pts,int n);
-static inline btDbvtAabbMm		FromPoints(const btVector3** ppts,int n);
-DBVT_INLINE void				Expand(const btVector3& e);
-DBVT_INLINE void				SignedExpand(const btVector3& e);
-DBVT_INLINE bool				Contain(const btDbvtAabbMm& a) const;
-DBVT_INLINE int					Classify(const btVector3& n,btScalar o,int s) const;
-DBVT_INLINE btScalar			ProjectMinimum(const btVector3& v,unsigned signs) const;
-DBVT_INLINE friend bool			Intersect(	const btDbvtAabbMm& a,
-											const btDbvtAabbMm& b);
-DBVT_INLINE friend bool			Intersect(	const btDbvtAabbMm& a,
-											const btDbvtAabbMm& b,
-											const btTransform& xform);
-DBVT_INLINE friend bool			Intersect(	const btDbvtAabbMm& a,
-											const btVector3& b);
-DBVT_INLINE friend bool			Intersect(	const btDbvtAabbMm& a,
-											const btVector3& org,
-											const btVector3& invdir,
-											const unsigned* signs);
-DBVT_INLINE friend btScalar		Proximity(	const btDbvtAabbMm& a,
-											const btDbvtAabbMm& b);
-DBVT_INLINE friend int			Select(		const btDbvtAabbMm& o,
-											const btDbvtAabbMm& a,
-											const btDbvtAabbMm& b);
-DBVT_INLINE friend void			Merge(		const btDbvtAabbMm& a,
-											const btDbvtAabbMm& b,
-											btDbvtAabbMm& r);
-DBVT_INLINE friend bool			NotEqual(	const btDbvtAabbMm& a,
-											const btDbvtAabbMm& b);
+	DBVT_INLINE btVector3			Center() const	{ return((mi+mx)/2); }
+	DBVT_INLINE btVector3			Lengths() const	{ return(mx-mi); }
+	DBVT_INLINE btVector3			Extents() const	{ return((mx-mi)/2); }
+	DBVT_INLINE const btVector3&	Mins() const	{ return(mi); }
+	DBVT_INLINE const btVector3&	Maxs() const	{ return(mx); }
+	static inline btDbvtAabbMm		FromCE(const btVector3& c,const btVector3& e);
+	static inline btDbvtAabbMm		FromCR(const btVector3& c,btScalar r);
+	static inline btDbvtAabbMm		FromMM(const btVector3& mi,const btVector3& mx);
+	static inline btDbvtAabbMm		FromPoints(const btVector3* pts,int n);
+	static inline btDbvtAabbMm		FromPoints(const btVector3** ppts,int n);
+	DBVT_INLINE void				Expand(const btVector3& e);
+	DBVT_INLINE void				SignedExpand(const btVector3& e);
+	DBVT_INLINE bool				Contain(const btDbvtAabbMm& a) const;
+	DBVT_INLINE int					Classify(const btVector3& n,btScalar o,int s) const;
+	DBVT_INLINE btScalar			ProjectMinimum(const btVector3& v,unsigned signs) const;
+	DBVT_INLINE friend bool			Intersect(	const btDbvtAabbMm& a,
+		const btDbvtAabbMm& b);
+	DBVT_INLINE friend bool			Intersect(	const btDbvtAabbMm& a,
+		const btDbvtAabbMm& b,
+		const btTransform& xform);
+	DBVT_INLINE friend bool			Intersect(	const btDbvtAabbMm& a,
+		const btVector3& b);
+
+	DBVT_INLINE friend btScalar		Proximity(	const btDbvtAabbMm& a,
+		const btDbvtAabbMm& b);
+	DBVT_INLINE friend int			Select(		const btDbvtAabbMm& o,
+		const btDbvtAabbMm& a,
+		const btDbvtAabbMm& b);
+	DBVT_INLINE friend void			Merge(		const btDbvtAabbMm& a,
+		const btDbvtAabbMm& b,
+		btDbvtAabbMm& r);
+	DBVT_INLINE friend bool			NotEqual(	const btDbvtAabbMm& a,
+		const btDbvtAabbMm& b);
 private:
-DBVT_INLINE void				AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const;
+	DBVT_INLINE void				AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const;
 private:
-btVector3	mi,mx;
+	btVector3	mi,mx;
 };
 
@@ -187,9 +179,10 @@
 	DBVT_INLINE bool	isleaf() const		{ return(childs[1]==0); }
 	DBVT_INLINE bool	isinternal() const	{ return(!isleaf()); }
-	union	{
-			btDbvtNode*	childs[2];
-			void*	data;
-			int		dataAsInt;
-			};
+	union
+	{
+		btDbvtNode*	childs[2];
+		void*	data;
+		int		dataAsInt;
+	};
 };
 
@@ -198,21 +191,21 @@
 ///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure.
 struct	btDbvt
-	{
+{
 	/* Stack element	*/ 
 	struct	sStkNN
-		{
+	{
 		const btDbvtNode*	a;
 		const btDbvtNode*	b;
 		sStkNN() {}
 		sStkNN(const btDbvtNode* na,const btDbvtNode* nb) : a(na),b(nb) {}
-		};
+	};
 	struct	sStkNP
-		{
+	{
 		const btDbvtNode*	node;
 		int			mask;
 		sStkNP(const btDbvtNode* n,unsigned m) : node(n),mask(m) {}
-		};
+	};
 	struct	sStkNPS
-		{
+	{
 		const btDbvtNode*	node;
 		int			mask;
@@ -220,44 +213,44 @@
 		sStkNPS() {}
 		sStkNPS(const btDbvtNode* n,unsigned m,btScalar v) : node(n),mask(m),value(v) {}
-		};
+	};
 	struct	sStkCLN
-		{
+	{
 		const btDbvtNode*	node;
 		btDbvtNode*		parent;
 		sStkCLN(const btDbvtNode* n,btDbvtNode* p) : node(n),parent(p) {}
-		};
+	};
 	// Policies/Interfaces
-			
+
 	/* ICollide	*/ 
 	struct	ICollide
-		{		
+	{		
 		DBVT_VIRTUAL_DTOR(ICollide)
-		DBVT_VIRTUAL void	Process(const btDbvtNode*,const btDbvtNode*)		{}
+			DBVT_VIRTUAL void	Process(const btDbvtNode*,const btDbvtNode*)		{}
 		DBVT_VIRTUAL void	Process(const btDbvtNode*)					{}
 		DBVT_VIRTUAL void	Process(const btDbvtNode* n,btScalar)			{ Process(n); }
 		DBVT_VIRTUAL bool	Descent(const btDbvtNode*)					{ return(true); }
 		DBVT_VIRTUAL bool	AllLeaves(const btDbvtNode*)					{ return(true); }
-		};
+	};
 	/* IWriter	*/ 
 	struct	IWriter
-		{
+	{
 		virtual ~IWriter() {}
 		virtual void		Prepare(const btDbvtNode* root,int numnodes)=0;
 		virtual void		WriteNode(const btDbvtNode*,int index,int parent,int child0,int child1)=0;
 		virtual void		WriteLeaf(const btDbvtNode*,int index,int parent)=0;
-		};
+	};
 	/* IClone	*/ 
 	struct	IClone
-		{
+	{
 		virtual ~IClone()	{}
 		virtual void		CloneLeaf(btDbvtNode*) {}
-		};
-		
+	};
+
 	// Constants
 	enum	{
-			SIMPLE_STACKSIZE	=	64,
-			DOUBLE_STACKSIZE	=	SIMPLE_STACKSIZE*2
-			};
-		
+		SIMPLE_STACKSIZE	=	64,
+		DOUBLE_STACKSIZE	=	SIMPLE_STACKSIZE*2
+	};
+
 	// Fields
 	btDbvtNode*		m_root;
@@ -266,7 +259,12 @@
 	int				m_leaves;
 	unsigned		m_opath;
+
+	
+	btAlignedObjectArray<sStkNN>	m_stkStack;
+
+
 	// Methods
-					btDbvt();
-					~btDbvt();
+	btDbvt();
+	~btDbvt();
 	void			clear();
 	bool			empty() const { return(0==m_root); }
@@ -276,8 +274,8 @@
 	btDbvtNode*		insert(const btDbvtVolume& box,void* data);
 	void			update(btDbvtNode* leaf,int lookahead=-1);
-	void			update(btDbvtNode* leaf,const btDbvtVolume& volume);
-	bool			update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity,btScalar margin);
-	bool			update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity);
-	bool			update(btDbvtNode* leaf,btDbvtVolume volume,btScalar margin);	
+	void			update(btDbvtNode* leaf,btDbvtVolume& volume);
+	bool			update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity,btScalar margin);
+	bool			update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity);
+	bool			update(btDbvtNode* leaf,btDbvtVolume& volume,btScalar margin);	
 	void			remove(btDbvtNode* leaf);
 	void			write(IWriter* iwriter) const;
@@ -286,83 +284,104 @@
 	static int		countLeaves(const btDbvtNode* node);
 	static void		extractLeaves(const btDbvtNode* node,btAlignedObjectArray<const btDbvtNode*>& leaves);
-	#if DBVT_ENABLE_BENCHMARK
+#if DBVT_ENABLE_BENCHMARK
 	static void		benchmark();
-	#else
+#else
 	static void		benchmark(){}
-	#endif
+#endif
 	// DBVT_IPOLICY must support ICollide policy/interface
 	DBVT_PREFIX
-	static void		enumNodes(	const btDbvtNode* root,
-								DBVT_IPOLICY);
+		static void		enumNodes(	const btDbvtNode* root,
+		DBVT_IPOLICY);
 	DBVT_PREFIX
-	static void		enumLeaves(	const btDbvtNode* root,
-								DBVT_IPOLICY);
+		static void		enumLeaves(	const btDbvtNode* root,
+		DBVT_IPOLICY);
 	DBVT_PREFIX
-	static void		collideTT(	const btDbvtNode* root0,
-								const btDbvtNode* root1,
-								DBVT_IPOLICY);
+		void		collideTT(	const btDbvtNode* root0,
+		const btDbvtNode* root1,
+		DBVT_IPOLICY);
+
 	DBVT_PREFIX
-	static void		collideTT(	const btDbvtNode* root0,
-								const btDbvtNode* root1,
-								const btTransform& xform,
-								DBVT_IPOLICY);
+		void		collideTTpersistentStack(	const btDbvtNode* root0,
+		  const btDbvtNode* root1,
+		  DBVT_IPOLICY);
+
 	DBVT_PREFIX
-	static void		collideTT(	const btDbvtNode* root0,
-								const btTransform& xform0,
-								const btDbvtNode* root1,
-								const btTransform& xform1,
-								DBVT_IPOLICY);
+		void		collideTT(	const btDbvtNode* root0,
+		const btDbvtNode* root1,
+		const btTransform& xform,
+		DBVT_IPOLICY);
 	DBVT_PREFIX
-	static void		collideTV(	const btDbvtNode* root,
-								const btDbvtVolume& volume,
-								DBVT_IPOLICY);
+		void		collideTT(	const btDbvtNode* root0,
+		const btTransform& xform0,
+		const btDbvtNode* root1,
+		const btTransform& xform1,
+		DBVT_IPOLICY);
 	DBVT_PREFIX
-	static void		collideRAY(	const btDbvtNode* root,
-								const btVector3& origin,
-								const btVector3& direction,
-								DBVT_IPOLICY);
+		void		collideTV(	const btDbvtNode* root,
+		const btDbvtVolume& volume,
+		DBVT_IPOLICY);
+	///rayTest is a re-entrant ray test, and can be called in parallel as long as the btAlignedAlloc is thread-safe (uses locking etc)
+	///rayTest is slower than rayTestInternal, because it builds a local stack, using memory allocations, and it recomputes signs/rayDirectionInverses each time
 	DBVT_PREFIX
-	static void		collideKDOP(const btDbvtNode* root,
-								const btVector3* normals,
-								const btScalar* offsets,
-								int count,
-								DBVT_IPOLICY);
+		static void		rayTest(	const btDbvtNode* root,
+		const btVector3& rayFrom,
+		const btVector3& rayTo,
+		DBVT_IPOLICY);
+	///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
+	///rayTestInternal is used by btDbvtBroadphase to accelerate world ray casts
 	DBVT_PREFIX
-	static void		collideOCL(	const btDbvtNode* root,
-								const btVector3* normals,
-								const btScalar* offsets,
-								const btVector3& sortaxis,
-								int count,								
-								DBVT_IPOLICY,
-								bool fullsort=true);
+		void		rayTestInternal(	const btDbvtNode* root,
+								const btVector3& rayFrom,
+								const btVector3& rayTo,
+								const btVector3& rayDirectionInverse,
+								unsigned int signs[3],
+								btScalar lambda_max,
+								const btVector3& aabbMin,
+								const btVector3& aabbMax,
+								DBVT_IPOLICY) const;
+
 	DBVT_PREFIX
-	static void		collideTU(	const btDbvtNode* root,
-								DBVT_IPOLICY);
+		static void		collideKDOP(const btDbvtNode* root,
+		const btVector3* normals,
+		const btScalar* offsets,
+		int count,
+		DBVT_IPOLICY);
+	DBVT_PREFIX
+		static void		collideOCL(	const btDbvtNode* root,
+		const btVector3* normals,
+		const btScalar* offsets,
+		const btVector3& sortaxis,
+		int count,								
+		DBVT_IPOLICY,
+		bool fullsort=true);
+	DBVT_PREFIX
+		static void		collideTU(	const btDbvtNode* root,
+		DBVT_IPOLICY);
 	// Helpers	
 	static DBVT_INLINE int	nearest(const int* i,const btDbvt::sStkNPS* a,btScalar v,int l,int h)
-		{
+	{
 		int	m=0;
 		while(l<h)
-			{
+		{
 			m=(l+h)>>1;
 			if(a[i[m]].value>=v) l=m+1; else h=m;
-			}
+		}
 		return(h);
-		}
+	}
 	static DBVT_INLINE int	allocate(	btAlignedObjectArray<int>& ifree,
-										btAlignedObjectArray<sStkNPS>& stock,
-										const sStkNPS& value)
-		{
+		btAlignedObjectArray<sStkNPS>& stock,
+		const sStkNPS& value)
+	{
 		int	i;
 		if(ifree.size()>0)
-			{ i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; }
-			else
-			{ i=stock.size();stock.push_back(value); }
+		{ i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; }
+		else
+		{ i=stock.size();stock.push_back(value); }
 		return(i); 
-		}
+	}
 	//
-	private:
-					btDbvt(const btDbvt&)	{}	
-	};
+private:
+	btDbvt(const btDbvt&)	{}	
+};
 
 //
@@ -373,34 +392,34 @@
 inline btDbvtAabbMm			btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e)
 {
-btDbvtAabbMm box;
-box.mi=c-e;box.mx=c+e;
-return(box);
-}
-	
+	btDbvtAabbMm box;
+	box.mi=c-e;box.mx=c+e;
+	return(box);
+}
+
 //
 inline btDbvtAabbMm			btDbvtAabbMm::FromCR(const btVector3& c,btScalar r)
 {
-return(FromCE(c,btVector3(r,r,r)));
-}
-	
+	return(FromCE(c,btVector3(r,r,r)));
+}
+
 //
 inline btDbvtAabbMm			btDbvtAabbMm::FromMM(const btVector3& mi,const btVector3& mx)
 {
-btDbvtAabbMm box;
-box.mi=mi;box.mx=mx;
-return(box);
-}
-	
+	btDbvtAabbMm box;
+	box.mi=mi;box.mx=mx;
+	return(box);
+}
+
 //
 inline btDbvtAabbMm			btDbvtAabbMm::FromPoints(const btVector3* pts,int n)
 {
-btDbvtAabbMm box;
-box.mi=box.mx=pts[0];
-for(int i=1;i<n;++i)
-	{
-	box.mi.setMin(pts[i]);
-	box.mx.setMax(pts[i]);
+	btDbvtAabbMm box;
+	box.mi=box.mx=pts[0];
+	for(int i=1;i<n;++i)
+	{
+		box.mi.setMin(pts[i]);
+		box.mx.setMax(pts[i]);
 	}
-return(box);
+	return(box);
 }
 
@@ -408,12 +427,12 @@
 inline btDbvtAabbMm			btDbvtAabbMm::FromPoints(const btVector3** ppts,int n)
 {
-btDbvtAabbMm box;
-box.mi=box.mx=*ppts[0];
-for(int i=1;i<n;++i)
-	{
-	box.mi.setMin(*ppts[i]);
-	box.mx.setMax(*ppts[i]);
+	btDbvtAabbMm box;
+	box.mi=box.mx=*ppts[0];
+	for(int i=1;i<n;++i)
+	{
+		box.mi.setMin(*ppts[i]);
+		box.mx.setMax(*ppts[i]);
 	}
-return(box);
+	return(box);
 }
 
@@ -421,19 +440,19 @@
 DBVT_INLINE void		btDbvtAabbMm::Expand(const btVector3& e)
 {
-mi-=e;mx+=e;
-}
-	
+	mi-=e;mx+=e;
+}
+
 //
 DBVT_INLINE void		btDbvtAabbMm::SignedExpand(const btVector3& e)
 {
-if(e.x()>0) mx.setX(mx.x()+e[0]); else mi.setX(mi.x()+e[0]);
-if(e.y()>0) mx.setY(mx.y()+e[1]); else mi.setY(mi.y()+e[1]);
-if(e.z()>0) mx.setZ(mx.z()+e[2]); else mi.setZ(mi.z()+e[2]);
-}
-	
+	if(e.x()>0) mx.setX(mx.x()+e[0]); else mi.setX(mi.x()+e[0]);
+	if(e.y()>0) mx.setY(mx.y()+e[1]); else mi.setY(mi.y()+e[1]);
+	if(e.z()>0) mx.setZ(mx.z()+e[2]); else mi.setZ(mi.z()+e[2]);
+}
+
 //
 DBVT_INLINE bool		btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const
 {
-return(	(mi.x()<=a.mi.x())&&
+	return(	(mi.x()<=a.mi.x())&&
 		(mi.y()<=a.mi.y())&&
 		(mi.z()<=a.mi.z())&&
@@ -446,27 +465,27 @@
 DBVT_INLINE int		btDbvtAabbMm::Classify(const btVector3& n,btScalar o,int s) const
 {
-btVector3			pi,px;
-switch(s)
+	btVector3			pi,px;
+	switch(s)
 	{
 	case	(0+0+0):	px=btVector3(mi.x(),mi.y(),mi.z());
-						pi=btVector3(mx.x(),mx.y(),mx.z());break;
+		pi=btVector3(mx.x(),mx.y(),mx.z());break;
 	case	(1+0+0):	px=btVector3(mx.x(),mi.y(),mi.z());
-						pi=btVector3(mi.x(),mx.y(),mx.z());break;
+		pi=btVector3(mi.x(),mx.y(),mx.z());break;
 	case	(0+2+0):	px=btVector3(mi.x(),mx.y(),mi.z());
-						pi=btVector3(mx.x(),mi.y(),mx.z());break;
+		pi=btVector3(mx.x(),mi.y(),mx.z());break;
 	case	(1+2+0):	px=btVector3(mx.x(),mx.y(),mi.z());
-						pi=btVector3(mi.x(),mi.y(),mx.z());break;
+		pi=btVector3(mi.x(),mi.y(),mx.z());break;
 	case	(0+0+4):	px=btVector3(mi.x(),mi.y(),mx.z());
-						pi=btVector3(mx.x(),mx.y(),mi.z());break;
+		pi=btVector3(mx.x(),mx.y(),mi.z());break;
 	case	(1+0+4):	px=btVector3(mx.x(),mi.y(),mx.z());
-						pi=btVector3(mi.x(),mx.y(),mi.z());break;
+		pi=btVector3(mi.x(),mx.y(),mi.z());break;
 	case	(0+2+4):	px=btVector3(mi.x(),mx.y(),mx.z());
-						pi=btVector3(mx.x(),mi.y(),mi.z());break;
+		pi=btVector3(mx.x(),mi.y(),mi.z());break;
 	case	(1+2+4):	px=btVector3(mx.x(),mx.y(),mx.z());
-						pi=btVector3(mi.x(),mi.y(),mi.z());break;
+		pi=btVector3(mi.x(),mi.y(),mi.z());break;
 	}
-if((dot(n,px)+o)<0)		return(-1);
-if((dot(n,pi)+o)>=0)	return(+1);
-return(0);
+	if((dot(n,px)+o)<0)		return(-1);
+	if((dot(n,pi)+o)>=0)	return(+1);
+	return(0);
 }
 
@@ -474,9 +493,9 @@
 DBVT_INLINE btScalar	btDbvtAabbMm::ProjectMinimum(const btVector3& v,unsigned signs) const
 {
-const btVector3*	b[]={&mx,&mi};
-const btVector3		p(	b[(signs>>0)&1]->x(),
-						b[(signs>>1)&1]->y(),
-						b[(signs>>2)&1]->z());
-return(dot(p,v));
+	const btVector3*	b[]={&mx,&mi};
+	const btVector3		p(	b[(signs>>0)&1]->x(),
+		b[(signs>>1)&1]->y(),
+		b[(signs>>2)&1]->z());
+	return(dot(p,v));
 }
 
@@ -484,7 +503,7 @@
 DBVT_INLINE void		btDbvtAabbMm::AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const
 {
-for(int i=0;i<3;++i)
-	{
-	if(d[i]<0)
+	for(int i=0;i<3;++i)
+	{
+		if(d[i]<0)
 		{ smi+=mx[i]*d[i];smx+=mi[i]*d[i]; }
 		else
@@ -492,16 +511,16 @@
 	}
 }
-	
+
 //
 DBVT_INLINE bool		Intersect(	const btDbvtAabbMm& a,
-									const btDbvtAabbMm& b)
+								  const btDbvtAabbMm& b)
 {
 #if	DBVT_INT0_IMPL == DBVT_IMPL_SSE
-const __m128	rt(_mm_or_ps(	_mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)),
-								_mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi))));
-const __int32*	pu((const __int32*)&rt);
-return((pu[0]|pu[1]|pu[2])==0);
+	const __m128	rt(_mm_or_ps(	_mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)),
+		_mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi))));
+	const __int32*	pu((const __int32*)&rt);
+	return((pu[0]|pu[1]|pu[2])==0);
 #else
-return(	(a.mi.x()<=b.mx.x())&&
+	return(	(a.mi.x()<=b.mx.x())&&
 		(a.mx.x()>=b.mi.x())&&
 		(a.mi.y()<=b.mx.y())&&
@@ -514,23 +533,23 @@
 //
 DBVT_INLINE bool		Intersect(	const btDbvtAabbMm& a,
-									const btDbvtAabbMm& b,
-									const btTransform& xform)
-{
-const btVector3		d0=xform*b.Center()-a.Center();
-const btVector3		d1=d0*xform.getBasis();
-btScalar			s0[2]={0,0};
-btScalar			s1[2]={dot(xform.getOrigin(),d0),s1[0]};
-a.AddSpan(d0,s0[0],s0[1]);
-b.AddSpan(d1,s1[0],s1[1]);
-if(s0[0]>(s1[1])) return(false);
-if(s0[1]<(s1[0])) return(false);
-return(true);
+								  const btDbvtAabbMm& b,
+								  const btTransform& xform)
+{
+	const btVector3		d0=xform*b.Center()-a.Center();
+	const btVector3		d1=d0*xform.getBasis();
+	btScalar			s0[2]={0,0};
+	btScalar			s1[2]={dot(xform.getOrigin(),d0),s1[0]};
+	a.AddSpan(d0,s0[0],s0[1]);
+	b.AddSpan(d1,s1[0],s1[1]);
+	if(s0[0]>(s1[1])) return(false);
+	if(s0[1]<(s1[0])) return(false);
+	return(true);
 }
 
 //
 DBVT_INLINE bool		Intersect(	const btDbvtAabbMm& a,
-									const btVector3& b)
-{
-return(	(b.x()>=a.mi.x())&&
+								  const btVector3& b)
+{
+	return(	(b.x()>=a.mi.x())&&
 		(b.y()>=a.mi.y())&&
 		(b.z()>=a.mi.z())&&
@@ -540,53 +559,38 @@
 }
 
-//
-DBVT_INLINE bool		Intersect(	const btDbvtAabbMm& a,
-									const btVector3& org,
-									const btVector3& invdir,
-									const unsigned* signs)
-{
-#if 0
-const btVector3		b0((a.mi-org)*invdir);
-const btVector3		b1((a.mx-org)*invdir);
-const btVector3		tmin(btMin(b0[0],b1[0]),btMin(b0[1],b1[1]),btMin(b0[2],b1[2]));
-const btVector3		tmax(btMax(b0[0],b1[0]),btMax(b0[1],b1[1]),btMax(b0[2],b1[2]));
-const btScalar		tin=btMax(tmin[0],btMax(tmin[1],tmin[2]));
-const btScalar		tout=btMin(tmax[0],btMin(tmax[1],tmax[2]));
-return(tin<tout);
-#else
-const btVector3*	bounds[2]={&a.mi,&a.mx};
-btScalar			txmin=(bounds[  signs[0]]->x()-org[0])*invdir[0];
-btScalar			txmax=(bounds[1-signs[0]]->x()-org[0])*invdir[0];
-const btScalar		tymin=(bounds[  signs[1]]->y()-org[1])*invdir[1];
-const btScalar		tymax=(bounds[1-signs[1]]->y()-org[1])*invdir[1];
-if((txmin>tymax)||(tymin>txmax)) return(false);
-if(tymin>txmin) txmin=tymin;
-if(tymax<txmax) txmax=tymax;
-const btScalar		tzmin=(bounds[  signs[2]]->z()-org[2])*invdir[2];
-const btScalar		tzmax=(bounds[1-signs[2]]->z()-org[2])*invdir[2];
-if((txmin>tzmax)||(tzmin>txmax)) return(false);
-if(tzmin>txmin) txmin=tzmin;
-if(tzmax<txmax) txmax=tzmax;
-return(txmax>0);
-#endif
-}
-	
+
+
+
+
+//////////////////////////////////////
+
+
 //
 DBVT_INLINE btScalar	Proximity(	const btDbvtAabbMm& a,
-									const btDbvtAabbMm& b)
-{
-const btVector3	d=(a.mi+a.mx)-(b.mi+b.mx);
-return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z()));
-}
+								  const btDbvtAabbMm& b)
+{
+	const btVector3	d=(a.mi+a.mx)-(b.mi+b.mx);
+	return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z()));
+}
+
+
 
 //
 DBVT_INLINE int			Select(	const btDbvtAabbMm& o,
-								const btDbvtAabbMm& a,
-								const btDbvtAabbMm& b)
+							   const btDbvtAabbMm& a,
+							   const btDbvtAabbMm& b)
 {
 #if	DBVT_SELECT_IMPL == DBVT_IMPL_SSE
-static DBVT_ALIGN const unsigned __int32	mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff};
-	// TODO: the intrinsic version is 11% slower
-	#if DBVT_USE_INTRINSIC_SSE
+	static ATTRIBUTE_ALIGNED16(const unsigned __int32)	mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff};
+	///@todo: the intrinsic version is 11% slower
+#if DBVT_USE_INTRINSIC_SSE
+
+	union btSSEUnion ///NOTE: if we use more intrinsics, move btSSEUnion into the LinearMath directory
+	{
+	   __m128		ssereg;
+	   float		floats[4];
+	   int			ints[4];
+	};
+
 	__m128	omi(_mm_load_ps(o.mi));
 	omi=_mm_add_ps(omi,_mm_load_ps(o.mx));
@@ -602,18 +606,22 @@
 	ami=_mm_add_ps(ami,t0);
 	ami=_mm_add_ss(ami,_mm_shuffle_ps(ami,ami,1));
-	__m128	t1(_mm_movehl_ps(bmi,bmi));
+	__m128 t1(_mm_movehl_ps(bmi,bmi));
 	bmi=_mm_add_ps(bmi,t1);
 	bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1));
-	return(_mm_cmple_ss(bmi,ami).m128_u32[0]&1);
-	#else
-	DBVT_ALIGN __int32	r[1];
+	
+	btSSEUnion tmp;
+	tmp.ssereg = _mm_cmple_ss(bmi,ami);
+	return tmp.ints[0]&1;
+
+#else
+	ATTRIBUTE_ALIGNED16(__int32	r[1]);
 	__asm
-		{
+	{
 		mov		eax,o
-		mov		ecx,a
-		mov		edx,b
-		movaps	xmm0,[eax]
+			mov		ecx,a
+			mov		edx,b
+			movaps	xmm0,[eax]
 		movaps	xmm5,mask
-		addps	xmm0,[eax+16]	
+			addps	xmm0,[eax+16]	
 		movaps	xmm1,[ecx]
 		movaps	xmm2,[edx]
@@ -621,22 +629,22 @@
 		addps	xmm2,[edx+16]
 		subps	xmm1,xmm0
-		subps	xmm2,xmm0
-		andps	xmm1,xmm5
-		andps	xmm2,xmm5
-		movhlps	xmm3,xmm1
-		movhlps	xmm4,xmm2
-		addps	xmm1,xmm3
-		addps	xmm2,xmm4
-		pshufd	xmm3,xmm1,1
-		pshufd	xmm4,xmm2,1
-		addss	xmm1,xmm3
-		addss	xmm2,xmm4
-		cmpless	xmm2,xmm1
-		movss	r,xmm2
-		}
+			subps	xmm2,xmm0
+			andps	xmm1,xmm5
+			andps	xmm2,xmm5
+			movhlps	xmm3,xmm1
+			movhlps	xmm4,xmm2
+			addps	xmm1,xmm3
+			addps	xmm2,xmm4
+			pshufd	xmm3,xmm1,1
+			pshufd	xmm4,xmm2,1
+			addss	xmm1,xmm3
+			addss	xmm2,xmm4
+			cmpless	xmm2,xmm1
+			movss	r,xmm2
+	}
 	return(r[0]&1);
-	#endif
+#endif
 #else
-return(Proximity(o,a)<Proximity(o,b)?0:1);
+	return(Proximity(o,a)<Proximity(o,b)?0:1);
 #endif
 }
@@ -644,21 +652,21 @@
 //
 DBVT_INLINE void		Merge(	const btDbvtAabbMm& a,
-								const btDbvtAabbMm& b,
-								btDbvtAabbMm& r)
+							  const btDbvtAabbMm& b,
+							  btDbvtAabbMm& r)
 {
 #if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
-__m128	ami(_mm_load_ps(a.mi));
-__m128	amx(_mm_load_ps(a.mx));
-__m128	bmi(_mm_load_ps(b.mi));
-__m128	bmx(_mm_load_ps(b.mx));
-ami=_mm_min_ps(ami,bmi);
-amx=_mm_max_ps(amx,bmx);
-_mm_store_ps(r.mi,ami);
-_mm_store_ps(r.mx,amx);
+	__m128	ami(_mm_load_ps(a.mi));
+	__m128	amx(_mm_load_ps(a.mx));
+	__m128	bmi(_mm_load_ps(b.mi));
+	__m128	bmx(_mm_load_ps(b.mx));
+	ami=_mm_min_ps(ami,bmi);
+	amx=_mm_max_ps(amx,bmx);
+	_mm_store_ps(r.mi,ami);
+	_mm_store_ps(r.mx,amx);
 #else
-for(int i=0;i<3;++i)
-	{
-	if(a.mi[i]<b.mi[i]) r.mi[i]=a.mi[i]; else r.mi[i]=b.mi[i];
-	if(a.mx[i]>b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i];
+	for(int i=0;i<3;++i)
+	{
+		if(a.mi[i]<b.mi[i]) r.mi[i]=a.mi[i]; else r.mi[i]=b.mi[i];
+		if(a.mx[i]>b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i];
 	}
 #endif
@@ -667,7 +675,7 @@
 //
 DBVT_INLINE bool		NotEqual(	const btDbvtAabbMm& a,
-									const btDbvtAabbMm& b)
-{
-return(	(a.mi.x()!=b.mi.x())||
+								 const btDbvtAabbMm& b)
+{
+	return(	(a.mi.x()!=b.mi.x())||
 		(a.mi.y()!=b.mi.y())||
 		(a.mi.z()!=b.mi.z())||
@@ -684,12 +692,12 @@
 DBVT_PREFIX
 inline void		btDbvt::enumNodes(	const btDbvtNode* root,
-									DBVT_IPOLICY)
-{
-DBVT_CHECKTYPE
-policy.Process(root);
-if(root->isinternal())
-	{
-	enumNodes(root->childs[0],policy);
-	enumNodes(root->childs[1],policy);
+								  DBVT_IPOLICY)
+{
+	DBVT_CHECKTYPE
+		policy.Process(root);
+	if(root->isinternal())
+	{
+		enumNodes(root->childs[0],policy);
+		enumNodes(root->childs[1],policy);
 	}
 }
@@ -698,16 +706,16 @@
 DBVT_PREFIX
 inline void		btDbvt::enumLeaves(	const btDbvtNode* root,
-									DBVT_IPOLICY)
-{
-DBVT_CHECKTYPE
-if(root->isinternal())
-	{
-	enumLeaves(root->childs[0],policy);
-	enumLeaves(root->childs[1],policy);
-	}
-	else
-	{
-	policy.Process(root);
-	}
+								   DBVT_IPOLICY)
+{
+	DBVT_CHECKTYPE
+		if(root->isinternal())
+		{
+			enumLeaves(root->childs[0],policy);
+			enumLeaves(root->childs[1],policy);
+		}
+		else
+		{
+			policy.Process(root);
+		}
 }
 
@@ -715,59 +723,277 @@
 DBVT_PREFIX
 inline void		btDbvt::collideTT(	const btDbvtNode* root0,
-									const btDbvtNode* root1,
-									DBVT_IPOLICY)
-{
-DBVT_CHECKTYPE
-if(root0&&root1)
-	{
-	btAlignedObjectArray<sStkNN>	stack;
-	int								depth=1;
-	int								treshold=DOUBLE_STACKSIZE-4;
-	stack.resize(DOUBLE_STACKSIZE);
-	stack[0]=sStkNN(root0,root1);
-	do	{		
-		sStkNN	p=stack[--depth];
-		if(depth>treshold)
+								  const btDbvtNode* root1,
+								  DBVT_IPOLICY)
+{
+	DBVT_CHECKTYPE
+		if(root0&&root1)
+		{
+			int								depth=1;
+			int								treshold=DOUBLE_STACKSIZE-4;
+			btAlignedObjectArray<sStkNN>	stkStack;
+			stkStack.resize(DOUBLE_STACKSIZE);
+			stkStack[0]=sStkNN(root0,root1);
+			do	{		
+				sStkNN	p=stkStack[--depth];
+				if(depth>treshold)
+				{
+					stkStack.resize(stkStack.size()*2);
+					treshold=stkStack.size()-4;
+				}
+				if(p.a==p.b)
+				{
+					if(p.a->isinternal())
+					{
+						stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]);
+						stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]);
+						stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]);
+					}
+				}
+				else if(Intersect(p.a->volume,p.b->volume))
+				{
+					if(p.a->isinternal())
+					{
+						if(p.b->isinternal())
+						{
+							stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
+							stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
+							stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
+							stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
+						}
+						else
+						{
+							stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
+							stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
+						}
+					}
+					else
+					{
+						if(p.b->isinternal())
+						{
+							stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
+							stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
+						}
+						else
+						{
+							policy.Process(p.a,p.b);
+						}
+					}
+				}
+			} while(depth);
+		}
+}
+
+
+
+DBVT_PREFIX
+inline void		btDbvt::collideTTpersistentStack(	const btDbvtNode* root0,
+								  const btDbvtNode* root1,
+								  DBVT_IPOLICY)
+{
+	DBVT_CHECKTYPE
+		if(root0&&root1)
+		{
+			int								depth=1;
+			int								treshold=DOUBLE_STACKSIZE-4;
+			
+			m_stkStack.resize(DOUBLE_STACKSIZE);
+			m_stkStack[0]=sStkNN(root0,root1);
+			do	{		
+				sStkNN	p=m_stkStack[--depth];
+				if(depth>treshold)
+				{
+					m_stkStack.resize(m_stkStack.size()*2);
+					treshold=m_stkStack.size()-4;
+				}
+				if(p.a==p.b)
+				{
+					if(p.a->isinternal())
+					{
+						m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]);
+						m_stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]);
+						m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]);
+					}
+				}
+				else if(Intersect(p.a->volume,p.b->volume))
+				{
+					if(p.a->isinternal())
+					{
+						if(p.b->isinternal())
+						{
+							m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
+							m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
+							m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
+							m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
+						}
+						else
+						{
+							m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
+							m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
+						}
+					}
+					else
+					{
+						if(p.b->isinternal())
+						{
+							m_stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
+							m_stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
+						}
+						else
+						{
+							policy.Process(p.a,p.b);
+						}
+					}
+				}
+			} while(depth);
+		}
+}
+
+
+//
+DBVT_PREFIX
+inline void		btDbvt::collideTT(	const btDbvtNode* root0,
+								  const btDbvtNode* root1,
+								  const btTransform& xform,
+								  DBVT_IPOLICY)
+{
+	DBVT_CHECKTYPE
+		if(root0&&root1)
+		{
+			int								depth=1;
+			int								treshold=DOUBLE_STACKSIZE-4;
+			btAlignedObjectArray<sStkNN>	stkStack;
+			stkStack.resize(DOUBLE_STACKSIZE);
+			stkStack[0]=sStkNN(root0,root1);
+			do	{
+				sStkNN	p=stkStack[--depth];
+				if(Intersect(p.a->volume,p.b->volume,xform))
+				{
+					if(depth>treshold)
+					{
+						stkStack.resize(stkStack.size()*2);
+						treshold=stkStack.size()-4;
+					}
+					if(p.a->isinternal())
+					{
+						if(p.b->isinternal())
+						{					
+							stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
+							stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
+							stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
+							stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
+						}
+						else
+						{
+							stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
+							stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
+						}
+					}
+					else
+					{
+						if(p.b->isinternal())
+						{
+							stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
+							stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
+						}
+						else
+						{
+							policy.Process(p.a,p.b);
+						}
+					}
+				}
+			} while(depth);
+		}
+}
+
+//
+DBVT_PREFIX
+inline void		btDbvt::collideTT(	const btDbvtNode* root0,
+								  const btTransform& xform0,
+								  const btDbvtNode* root1,
+								  const btTransform& xform1,
+								  DBVT_IPOLICY)
+{
+	const btTransform	xform=xform0.inverse()*xform1;
+	collideTT(root0,root1,xform,policy);
+}
+
+//
+DBVT_PREFIX
+inline void		btDbvt::collideTV(	const btDbvtNode* root,
+								  const btDbvtVolume& vol,
+								  DBVT_IPOLICY)
+{
+	DBVT_CHECKTYPE
+		if(root)
+		{
+			ATTRIBUTE_ALIGNED16(btDbvtVolume)		volume(vol);
+			btAlignedObjectArray<const btDbvtNode*>	stack;
+			stack.resize(0);
+			stack.reserve(SIMPLE_STACKSIZE);
+			stack.push_back(root);
+			do	{
+				const btDbvtNode*	n=stack[stack.size()-1];
+				stack.pop_back();
+				if(Intersect(n->volume,volume))
+				{
+					if(n->isinternal())
+					{
+						stack.push_back(n->childs[0]);
+						stack.push_back(n->childs[1]);
+					}
+					else
+					{
+						policy.Process(n);
+					}
+				}
+			} while(stack.size()>0);
+		}
+}
+
+DBVT_PREFIX
+inline void		btDbvt::rayTestInternal(	const btDbvtNode* root,
+								const btVector3& rayFrom,
+								const btVector3& rayTo,
+								const btVector3& rayDirectionInverse,
+								unsigned int signs[3],
+								btScalar lambda_max,
+								const btVector3& aabbMin,
+								const btVector3& aabbMax,
+								DBVT_IPOLICY) const
+{
+	DBVT_CHECKTYPE
+	if(root)
+	{
+		btVector3 resultNormal;
+
+		int								depth=1;
+		int								treshold=DOUBLE_STACKSIZE-2;
+		btAlignedObjectArray<const btDbvtNode*>	stack;
+		stack.resize(DOUBLE_STACKSIZE);
+		stack[0]=root;
+		btVector3 bounds[2];
+		do	
+		{
+			const btDbvtNode*	node=stack[--depth];
+			bounds[0] = node->volume.Mins()+aabbMin;
+			bounds[1] = node->volume.Maxs()+aabbMax;
+			btScalar tmin=1.f,lambda_min=0.f;
+			unsigned int result1=false;
+			result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
+			if(result1)
 			{
-			stack.resize(stack.size()*2);
-			treshold=stack.size()-4;
-			}
-		if(p.a==p.b)
-			{
-			if(p.a->isinternal())
-				{
-				stack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]);
-				stack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]);
-				stack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]);
-				}
-			}
-		else if(Intersect(p.a->volume,p.b->volume))
-			{
-			if(p.a->isinternal())
-				{
-				if(p.b->isinternal())
-					{
-					stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
-					stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
-					stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
-					stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
-					}
-					else
-					{
-					stack[depth++]=sStkNN(p.a->childs[0],p.b);
-					stack[depth++]=sStkNN(p.a->childs[1],p.b);
-					}
+				if(node->isinternal())
+				{
+					if(depth>treshold)
+					{
+						stack.resize(stack.size()*2);
+						treshold=stack.size()-2;
+					}
+					stack[depth++]=node->childs[0];
+					stack[depth++]=node->childs[1];
 				}
 				else
 				{
-				if(p.b->isinternal())
-					{
-					stack[depth++]=sStkNN(p.a,p.b->childs[0]);
-					stack[depth++]=sStkNN(p.a,p.b->childs[1]);
-					}
-					else
-					{
-					policy.Process(p.a,p.b);
-					}
+					policy.Process(node);
 				}
 			}
@@ -778,139 +1004,69 @@
 //
 DBVT_PREFIX
-inline void		btDbvt::collideTT(	const btDbvtNode* root0,
-									const btDbvtNode* root1,
-									const btTransform& xform,
-									DBVT_IPOLICY)
-{
-DBVT_CHECKTYPE
-if(root0&&root1)
-	{
-	btAlignedObjectArray<sStkNN>	stack;
-	int								depth=1;
-	int								treshold=DOUBLE_STACKSIZE-4;
-	stack.resize(DOUBLE_STACKSIZE);
-	stack[0]=sStkNN(root0,root1);
-	do	{
-		sStkNN	p=stack[--depth];
-		if(Intersect(p.a->volume,p.b->volume,xform))
-			{
-			if(depth>treshold)
-				{
-				stack.resize(stack.size()*2);
-				treshold=stack.size()-4;
-				}
-			if(p.a->isinternal())
-				{
-				if(p.b->isinternal())
-					{					
-					stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
-					stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
-					stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
-					stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
+inline void		btDbvt::rayTest(	const btDbvtNode* root,
+								const btVector3& rayFrom,
+								const btVector3& rayTo,
+								DBVT_IPOLICY)
+{
+	DBVT_CHECKTYPE
+		if(root)
+		{
+			btVector3 rayDir = (rayTo-rayFrom);
+			rayDir.normalize ();
+
+			///what about division by zero? --> just set rayDirection[i] to INF/1e30
+			btVector3 rayDirectionInverse;
+			rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
+			rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
+			rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
+			unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0};
+
+			btScalar lambda_max = rayDir.dot(rayTo-rayFrom);
+
+			btVector3 resultNormal;
+
+			btAlignedObjectArray<const btDbvtNode*>	stack;
+
+			int								depth=1;
+			int								treshold=DOUBLE_STACKSIZE-2;
+
+			stack.resize(DOUBLE_STACKSIZE);
+			stack[0]=root;
+			btVector3 bounds[2];
+			do	{
+				const btDbvtNode*	node=stack[--depth];
+
+				bounds[0] = node->volume.Mins();
+				bounds[1] = node->volume.Maxs();
+				
+				btScalar tmin=1.f,lambda_min=0.f;
+				unsigned int result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
+
+#ifdef COMPARE_BTRAY_AABB2
+				btScalar param=1.f;
+				bool result2 = btRayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal);
+				btAssert(result1 == result2);
+#endif //TEST_BTRAY_AABB2
+
+				if(result1)
+				{
+					if(node->isinternal())
+					{
+						if(depth>treshold)
+						{
+							stack.resize(stack.size()*2);
+							treshold=stack.size()-2;
+						}
+						stack[depth++]=node->childs[0];
+						stack[depth++]=node->childs[1];
 					}
 					else
 					{
-					stack[depth++]=sStkNN(p.a->childs[0],p.b);
-					stack[depth++]=sStkNN(p.a->childs[1],p.b);
-					}
-				}
-				else
-				{
-				if(p.b->isinternal())
-					{
-					stack[depth++]=sStkNN(p.a,p.b->childs[0]);
-					stack[depth++]=sStkNN(p.a,p.b->childs[1]);
-					}
-					else
-					{
-					policy.Process(p.a,p.b);
-					}
-				}
-			}
-		} while(depth);
-	}
-}
-
-//
-DBVT_PREFIX
-inline void		btDbvt::collideTT(	const btDbvtNode* root0,
-									const btTransform& xform0,
-									const btDbvtNode* root1,
-									const btTransform& xform1,
-									DBVT_IPOLICY)
-{
-const btTransform	xform=xform0.inverse()*xform1;
-collideTT(root0,root1,xform,policy);
-}
-
-//
-DBVT_PREFIX
-inline void		btDbvt::collideTV(	const btDbvtNode* root,
-									const btDbvtVolume& vol,
-									DBVT_IPOLICY)
-{
-DBVT_CHECKTYPE
-if(root)
-	{
-	ATTRIBUTE_ALIGNED16(btDbvtVolume)		volume(vol);
-	btAlignedObjectArray<const btDbvtNode*>	stack;
-	stack.reserve(SIMPLE_STACKSIZE);
-	stack.push_back(root);
-	do	{
-		const btDbvtNode*	n=stack[stack.size()-1];
-		stack.pop_back();
-		if(Intersect(n->volume,volume))
-			{
-			if(n->isinternal())
-				{
-				stack.push_back(n->childs[0]);
-				stack.push_back(n->childs[1]);
-				}
-				else
-				{
-				policy.Process(n);
-				}
-			}
-		} while(stack.size()>0);
-	}
-}
-
-//
-DBVT_PREFIX
-inline void		btDbvt::collideRAY(	const btDbvtNode* root,
-									const btVector3& origin,
-									const btVector3& direction,
-									DBVT_IPOLICY)
-{
-DBVT_CHECKTYPE
-if(root)
-	{
-	const btVector3	normal=direction.normalized();
-	const btVector3	invdir(	1/normal.x(),
-							1/normal.y(),
-							1/normal.z());
-	const unsigned	signs[]={	direction.x()<0,
-								direction.y()<0,
-								direction.z()<0};
-	btAlignedObjectArray<const btDbvtNode*>	stack;
-	stack.reserve(SIMPLE_STACKSIZE);
-	stack.push_back(root);
-	do	{
-		const btDbvtNode*	node=stack[stack.size()-1];
-		stack.pop_back();
-		if(Intersect(node->volume,origin,invdir,signs))
-			{
-			if(node->isinternal())
-				{
-				stack.push_back(node->childs[0]);
-				stack.push_back(node->childs[1]);
-				}
-				else
-				{
-				policy.Process(node);
-				}
-			}
-		} while(stack.size());
-	}
+						policy.Process(node);
+					}
+				}
+			} while(depth);
+
+		}
 }
 
@@ -923,49 +1079,49 @@
 									DBVT_IPOLICY)
 {
-DBVT_CHECKTYPE
-if(root)
-	{
-	const int						inside=(1<<count)-1;
-	btAlignedObjectArray<sStkNP>	stack;
-	int								signs[sizeof(unsigned)*8];
-	btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
-	for(int i=0;i<count;++i)
+	DBVT_CHECKTYPE
+		if(root)
 		{
-		signs[i]=	((normals[i].x()>=0)?1:0)+
+			const int						inside=(1<<count)-1;
+			btAlignedObjectArray<sStkNP>	stack;
+			int								signs[sizeof(unsigned)*8];
+			btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
+			for(int i=0;i<count;++i)
+			{
+				signs[i]=	((normals[i].x()>=0)?1:0)+
 					((normals[i].y()>=0)?2:0)+
 					((normals[i].z()>=0)?4:0);
+			}
+			stack.reserve(SIMPLE_STACKSIZE);
+			stack.push_back(sStkNP(root,0));
+			do	{
+				sStkNP	se=stack[stack.size()-1];
+				bool	out=false;
+				stack.pop_back();
+				for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
+				{
+					if(0==(se.mask&j))
+					{
+						const int	side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
+						switch(side)
+						{
+						case	-1:	out=true;break;
+						case	+1:	se.mask|=j;break;
+						}
+					}
+				}
+				if(!out)
+				{
+					if((se.mask!=inside)&&(se.node->isinternal()))
+					{
+						stack.push_back(sStkNP(se.node->childs[0],se.mask));
+						stack.push_back(sStkNP(se.node->childs[1],se.mask));
+					}
+					else
+					{
+						if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy);
+					}
+				}
+			} while(stack.size());
 		}
-	stack.reserve(SIMPLE_STACKSIZE);
-	stack.push_back(sStkNP(root,0));
-	do	{
-		sStkNP	se=stack[stack.size()-1];
-		bool	out=false;
-		stack.pop_back();
-		for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
-			{
-			if(0==(se.mask&j))
-				{
-				const int	side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
-				switch(side)
-					{
-					case	-1:	out=true;break;
-					case	+1:	se.mask|=j;break;
-					}
-				}
-			}
-		if(!out)
-			{
-			if((se.mask!=inside)&&(se.node->isinternal()))
-				{
-				stack.push_back(sStkNP(se.node->childs[0],se.mask));
-				stack.push_back(sStkNP(se.node->childs[1],se.mask));
-				}
-				else
-				{
-				if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy);
-				}
-			}
-		} while(stack.size());
-	}
 }
 
@@ -973,97 +1129,97 @@
 DBVT_PREFIX
 inline void		btDbvt::collideOCL(	const btDbvtNode* root,
-									const btVector3* normals,
-									const btScalar* offsets,
-									const btVector3& sortaxis,
-									int count,
-									DBVT_IPOLICY,
-									bool fsort)
-{
-DBVT_CHECKTYPE
-if(root)
-	{
-	const unsigned					srtsgns=(sortaxis[0]>=0?1:0)+
-											(sortaxis[1]>=0?2:0)+
-											(sortaxis[2]>=0?4:0);
-	const int						inside=(1<<count)-1;
-	btAlignedObjectArray<sStkNPS>	stock;
-	btAlignedObjectArray<int>		ifree;
-	btAlignedObjectArray<int>		stack;
-	int								signs[sizeof(unsigned)*8];
-	btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
-	for(int i=0;i<count;++i)
+								   const btVector3* normals,
+								   const btScalar* offsets,
+								   const btVector3& sortaxis,
+								   int count,
+								   DBVT_IPOLICY,
+								   bool fsort)
+{
+	DBVT_CHECKTYPE
+		if(root)
 		{
-		signs[i]=	((normals[i].x()>=0)?1:0)+
+			const unsigned					srtsgns=(sortaxis[0]>=0?1:0)+
+				(sortaxis[1]>=0?2:0)+
+				(sortaxis[2]>=0?4:0);
+			const int						inside=(1<<count)-1;
+			btAlignedObjectArray<sStkNPS>	stock;
+			btAlignedObjectArray<int>		ifree;
+			btAlignedObjectArray<int>		stack;
+			int								signs[sizeof(unsigned)*8];
+			btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
+			for(int i=0;i<count;++i)
+			{
+				signs[i]=	((normals[i].x()>=0)?1:0)+
 					((normals[i].y()>=0)?2:0)+
 					((normals[i].z()>=0)?4:0);
+			}
+			stock.reserve(SIMPLE_STACKSIZE);
+			stack.reserve(SIMPLE_STACKSIZE);
+			ifree.reserve(SIMPLE_STACKSIZE);
+			stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns))));
+			do	{
+				const int	id=stack[stack.size()-1];
+				sStkNPS		se=stock[id];
+				stack.pop_back();ifree.push_back(id);
+				if(se.mask!=inside)
+				{
+					bool	out=false;
+					for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
+					{
+						if(0==(se.mask&j))
+						{
+							const int	side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
+							switch(side)
+							{
+							case	-1:	out=true;break;
+							case	+1:	se.mask|=j;break;
+							}
+						}
+					}
+					if(out) continue;
+				}
+				if(policy.Descent(se.node))
+				{
+					if(se.node->isinternal())
+					{
+						const btDbvtNode* pns[]={	se.node->childs[0],se.node->childs[1]};
+						sStkNPS		nes[]={	sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)),
+							sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))};
+						const int	q=nes[0].value<nes[1].value?1:0;				
+						int			j=stack.size();
+						if(fsort&&(j>0))
+						{
+							/* Insert 0	*/ 
+							j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size());
+							stack.push_back(0);
+#if DBVT_USE_MEMMOVE
+							memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
+#else
+							for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
+#endif
+							stack[j]=allocate(ifree,stock,nes[q]);
+							/* Insert 1	*/ 
+							j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size());
+							stack.push_back(0);
+#if DBVT_USE_MEMMOVE
+							memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
+#else
+							for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
+#endif
+							stack[j]=allocate(ifree,stock,nes[1-q]);
+						}
+						else
+						{
+							stack.push_back(allocate(ifree,stock,nes[q]));
+							stack.push_back(allocate(ifree,stock,nes[1-q]));
+						}
+					}
+					else
+					{
+						policy.Process(se.node,se.value);
+					}
+				}
+			} while(stack.size());
 		}
-	stock.reserve(SIMPLE_STACKSIZE);
-	stack.reserve(SIMPLE_STACKSIZE);
-	ifree.reserve(SIMPLE_STACKSIZE);
-	stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns))));
-	do	{
-		const int	id=stack[stack.size()-1];
-		sStkNPS		se=stock[id];
-		stack.pop_back();ifree.push_back(id);
-		if(se.mask!=inside)
-			{
-			bool	out=false;
-			for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
-				{
-				if(0==(se.mask&j))
-					{
-					const int	side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
-					switch(side)
-						{
-						case	-1:	out=true;break;
-						case	+1:	se.mask|=j;break;
-						}
-					}
-				}
-			if(out) continue;
-			}
-		if(policy.Descent(se.node))
-			{
-			if(se.node->isinternal())
-				{
-				const btDbvtNode* pns[]={	se.node->childs[0],se.node->childs[1]};
-				sStkNPS		nes[]={	sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)),
-									sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))};
-				const int	q=nes[0].value<nes[1].value?1:0;				
-				int			j=stack.size();
-				if(fsort&&(j>0))
-					{
-					/* Insert 0	*/ 
-					j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size());
-					stack.push_back(0);
-					#if DBVT_USE_MEMMOVE
-					memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
-					#else
-					for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
-					#endif
-					stack[j]=allocate(ifree,stock,nes[q]);
-					/* Insert 1	*/ 
-					j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size());
-					stack.push_back(0);
-					#if DBVT_USE_MEMMOVE
-					memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
-					#else
-					for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
-					#endif
-					stack[j]=allocate(ifree,stock,nes[1-q]);
-					}
-					else
-					{
-					stack.push_back(allocate(ifree,stock,nes[q]));
-					stack.push_back(allocate(ifree,stock,nes[1-q]));
-					}
-				}
-				else
-				{
-				policy.Process(se.node,se.value);
-				}
-			}
-		} while(stack.size());
-	}
 }
 
@@ -1071,24 +1227,24 @@
 DBVT_PREFIX
 inline void		btDbvt::collideTU(	const btDbvtNode* root,
-									DBVT_IPOLICY)
-{
-DBVT_CHECKTYPE
-if(root)
-	{
-	btAlignedObjectArray<const btDbvtNode*>	stack;
-	stack.reserve(SIMPLE_STACKSIZE);
-	stack.push_back(root);
-	do	{
-		const btDbvtNode*	n=stack[stack.size()-1];
-		stack.pop_back();
-		if(policy.Descent(n))
-			{
-			if(n->isinternal())
-				{ stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); }
-				else
-				{ policy.Process(n); }
-			}
-		} while(stack.size()>0);
-	}
+								  DBVT_IPOLICY)
+{
+	DBVT_CHECKTYPE
+		if(root)
+		{
+			btAlignedObjectArray<const btDbvtNode*>	stack;
+			stack.reserve(SIMPLE_STACKSIZE);
+			stack.push_back(root);
+			do	{
+				const btDbvtNode*	n=stack[stack.size()-1];
+				stack.pop_back();
+				if(policy.Descent(n))
+				{
+					if(n->isinternal())
+					{ stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); }
+					else
+					{ policy.Process(n); }
+				}
+			} while(stack.size()>0);
+		}
 }
 
